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 <to.jafar@gmail.com>

* update refs

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
pull/22831/head
Jafar Al-Gharaibeh 2021-11-08 16:40:26 -06:00 committed by GitHub
parent 8933859e35
commit 8c38e9c49e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 14 additions and 8 deletions

View File

@ -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

View File

@ -1 +1 @@
59877743b76ee0a07e882342ff7ba8b492b4fe02
086f33a769cfba4bf1e1bcc80fd14455a2b2da46