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
parent
8933859e35
commit
8c38e9c49e
|
@ -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
|
||||
|
||||
|
|
|
@ -1 +1 @@
|
|||
59877743b76ee0a07e882342ff7ba8b492b4fe02
|
||||
086f33a769cfba4bf1e1bcc80fd14455a2b2da46
|
Loading…
Reference in New Issue