diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index f2d198338d..8ff39ceaeb 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -12,6 +12,14 @@ ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float: + if val > val_steady + hyst_gap: + val_steady = val - hyst_gap + elif val < val_steady - hyst_gap: + val_steady = val + hyst_gap + return val_steady + + def create_button_event(cur_but: int, prev_but: int, buttons_dict: Dict[int, capnp.lib.capnp._EnumModule], unpressed: int = 0) -> capnp.lib.capnp._DynamicStructBuilder: if cur_but != unpressed: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 1614003e87..bc6c31e12c 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -9,7 +9,7 @@ from common.basedir import BASEDIR from common.conversions import Conversions as CV from common.kalman.simple_kalman import KF1D from common.realtime import DT_CTRL -from selfdrive.car import create_button_enable_events, gen_empty_fingerprint +from selfdrive.car import apply_hysteresis, create_button_enable_events, gen_empty_fingerprint from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX from selfdrive.controls.lib.events import Events from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -171,6 +171,12 @@ class CarInterfaceBase(ABC): else: self.v_ego_cluster_seen = True + # Many cars apply hysteresis to the ego dash speed + if self.CS is not None: + ret.vEgoCluster = apply_hysteresis(ret.vEgoCluster, self.CS.out.vEgoCluster, self.CS.cluster_speed_hyst_gap) + if abs(ret.vEgo) < self.CS.cluster_min_speed: + ret.vEgoCluster = 0.0 + if ret.cruiseState.speedCluster == 0: ret.cruiseState.speedCluster = ret.cruiseState.speed @@ -272,6 +278,8 @@ class CarStateBase(ABC): self.right_blinker_cnt = 0 self.left_blinker_prev = False self.right_blinker_prev = False + self.cluster_speed_hyst_gap = 0.0 + self.cluster_min_speed = 0.0 # min speed before dropping to 0 # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]]) # R = 0.3