From 8c38e9c49e23d6e1030d9c302ae2cc404e5cc6b7 Mon Sep 17 00:00:00 2001 From: Jafar Al-Gharaibeh Date: Mon, 8 Nov 2021 16:40:26 -0600 Subject: [PATCH] Mazda: fix occasional steer fault at initial lkas enable (#22806) * Mazda: fix occasional steer fault at initial lkas enable Signed-off-by: Jafar Al-Gharaibeh * update refs Co-authored-by: Adeeb Shihadeh --- selfdrive/car/mazda/carstate.py | 20 +++++++++++++------- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 0a1815948f..4140a4e079 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -15,7 +15,7 @@ class CarState(CarStateBase): self.crz_btns_counter = 0 self.acc_active_last = False self.low_speed_alert = False - self.lkas_allowed = False + self.lkas_allowed_speed = False def update(self, cp, cp_cam): @@ -59,11 +59,15 @@ class CarState(CarStateBase): ret.gas = cp.vl["ENGINE_DATA"]["PEDAL_GAS"] ret.gasPressed = ret.gas > 0 + # Either due to low speed or hands off + lkas_blocked = cp.vl["STEER_RATE"]["LKAS_BLOCK"] == 1 + # LKAS is enabled at 52kph going up and disabled at 45kph going down - if speed_kph > LKAS_LIMITS.ENABLE_SPEED: - self.lkas_allowed = True + # wait for LKAS_BLOCK signal to clear when going up since it lags behind the speed sometimes + if speed_kph > LKAS_LIMITS.ENABLE_SPEED and not lkas_blocked: + self.lkas_allowed_speed = True elif speed_kph < LKAS_LIMITS.DISABLE_SPEED: - self.lkas_allowed = False + self.lkas_allowed_speed = False # TODO: the signal used for available seems to be the adaptive cruise signal, instead of the main on # it should be used for carState.cruiseState.nonAdaptive instead @@ -72,14 +76,16 @@ class CarState(CarStateBase): ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS if ret.cruiseState.enabled: - if not self.lkas_allowed and self.acc_active_last: + if not self.lkas_allowed_speed and self.acc_active_last: self.low_speed_alert = True else: self.low_speed_alert = False # Check if LKAS is disabled due to lack of driver torque when all other states indicate - # it should be enabled (steer lockout) - ret.steerWarning = self.lkas_allowed and (cp.vl["STEER_RATE"]["LKAS_BLOCK"] == 1) + # it should be enabled (steer lockout). Don't warn until we actually get lkas active + # and lose it again, i.e, after initial lkas activation + + ret.steerWarning = self.lkas_allowed_speed and lkas_blocked self.acc_active_last = ret.cruiseState.enabled diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 6fc3129b70..6c223b2ad6 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -59877743b76ee0a07e882342ff7ba8b492b4fe02 \ No newline at end of file +086f33a769cfba4bf1e1bcc80fd14455a2b2da46 \ No newline at end of file