add controls mismatch lateral event

cereal/log.capnp

@@ -198,6 +198,7 @@ struct OnroadEvent @0xc4fa6047f024e718 {
     silentDoorOpen @161;
     silentSeatbeltNotLatched @162;
     silentParkBrake @163;
+    controlsMismatchLateral @164;
 
     soundsUnavailableDEPRECATED @47;
   }

selfdrive/selfdrived/events.py

@@ -1063,6 +1063,11 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"),
   },
 
+  EventName.controlsMismatchLateral: {
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch: Lateral"),
+    ET.NO_ENTRY: NoEntryAlert("Controls Mismatch: Lateral"),
+  },
+
 }
 
 

selfdrive/selfdrived/selfdrived.py

@@ -248,7 +248,6 @@ class SelfdriveD:
       else:
         safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
 
-      # TODO-SP: add controlsMismatchLat for controlsAllowedLat
       # safety mismatch allows some time for pandad to set the safety mode and publish it back from panda
       if (safety_mismatch and self.sm.frame*DT_CTRL > 10.) or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200:
         self.events.add(EventName.controlsMismatch)
@@ -476,7 +475,7 @@ class SelfdriveD:
     if not self.CP.passive and self.initialized:
       self.enabled, self.active = self.state_machine.update(self.events)
     if not self.CP.notCar:
-      self.mads.update(CS)
+      self.mads.update(CS, self.sm)
     self.update_alerts(CS)
 
     self.publish_selfdriveState(CS)

sunnypilot/mads/mads.py

@@ -24,7 +24,7 @@ THE SOFTWARE.
 Last updated: July 29, 2024
 """
 
-from cereal import car, log, custom
+from cereal import messaging, car, log, custom
 
 from opendbc.car.hyundai.values import HyundaiFlags
 from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
@@ -35,6 +35,9 @@ from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SIL
 State = custom.SelfdriveStateSP.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
 ButtonType = car.CarState.ButtonEvent.Type
 EventName = log.OnroadEvent.EventName
+SafetyModel = car.CarParams.SafetyModel
+
+IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
 
 
 class ModularAssistiveDrivingSystem:
@@ -44,6 +47,7 @@ class ModularAssistiveDrivingSystem:
     self.enabled = False
     self.active = False
     self.available = False
+    self.mismatch_counter = 0
     self.allow_always = False
     self.selfdrive = selfdrive
     self.selfdrive.enabled_prev = False
@@ -130,10 +134,23 @@ class ModularAssistiveDrivingSystem:
     self.events.remove(EventName.wrongCruiseMode)
     self.events.remove(EventName.wrongCarMode)
 
-  def update(self, CS: car.CarState):
+    if self.mismatch_counter >= 200:
+      self.events.add(EventName.controlsMismatchLateral)
+
+  def data_sample(self, sm: messaging.SubMaster):
+    if not self.enabled:
+      self.mismatch_counter = 0
+
+    if self.enabled and any(not ps.controlsAllowedLat for ps in sm['pandaStates']
+                            if ps.safetyModel not in IGNORED_SAFETY_MODES):
+      self.mismatch_counter += 1
+
+  def update(self, CS: car.CarState, sm: messaging.SubMaster):
     if not self.enabled_toggle:
       return
 
+    self.data_sample(sm)
+
     self.update_events(CS)
 
     if not self.selfdrive.CP.passive and self.selfdrive.initialized: