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>

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
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-59877743b76ee0a07e882342ff7ba8b492b4fe02
+086f33a769cfba4bf1e1bcc80fd14455a2b2da46