diff --git a/cereal/log.capnp b/cereal/log.capnp index 782832cd0d..744343d337 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -198,6 +198,7 @@ struct OnroadEvent @0xc4fa6047f024e718 { silentDoorOpen @161; silentSeatbeltNotLatched @162; silentParkBrake @163; + controlsMismatchLateral @164; soundsUnavailableDEPRECATED @47; } diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 77cccf6d9c..34046b8caa 100755 --- a/selfdrive/selfdrived/events.py +++ b/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"), + }, + } diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 6e87bb1777..c2b608943e 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/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) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 3ea5346097..49f7b8e935 100644 --- a/sunnypilot/mads/mads.py +++ b/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: