diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 6d471071d6..fc196528d7 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -188,7 +188,7 @@ class CarState(CarStateBase): ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1 ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"] ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"] - ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD + ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5) ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0 ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"], diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 0458178ee3..ad1fd22b90 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -8,7 +8,7 @@ from cereal import car from common.basedir import BASEDIR from common.conversions import Conversions as CV from common.kalman.simple_kalman import KF1D -from common.numpy_fast import interp +from common.numpy_fast import clip, interp from common.realtime import DT_CTRL from selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_center_deadzone @@ -327,6 +327,7 @@ class CarStateBase(ABC): self.cruise_buttons = 0 self.left_blinker_cnt = 0 self.right_blinker_cnt = 0 + self.steering_pressed_cnt = 0 self.left_blinker_prev = False self.right_blinker_prev = False self.cluster_speed_hyst_gap = 0.0 @@ -364,6 +365,12 @@ class CarStateBase(ABC): self.right_blinker_cnt = blinker_time if right_blinker_lamp else max(self.right_blinker_cnt - 1, 0) return self.left_blinker_cnt > 0, self.right_blinker_cnt > 0 + def update_steering_pressed(self, steering_pressed, steering_pressed_min_count): + """Applies filtering on steering pressed for noisy driver torque signals.""" + self.steering_pressed_cnt += 1 if steering_pressed else -1 + self.steering_pressed_cnt = clip(self.steering_pressed_cnt, 0, steering_pressed_min_count * 2) + return self.steering_pressed_cnt > steering_pressed_min_count + def update_blinker_from_stalk(self, blinker_time: int, left_blinker_stalk: bool, right_blinker_stalk: bool): """Update blinkers from stalk position. When stalk is seen the blinker will be on for at least blinker_time, or until the stalk is turned off, whichever is longer. If the opposite stalk direction is seen the blinker diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 57e3a05f4d..342ab98b57 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -faa85c0cb3609fc43674b263ae885c60f136ba6c \ No newline at end of file +5f83a3ba20ec757ea7a5ef9b4159ac99cb77af0f