sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8

openpilot/sunnypilot/mads/helpers.py

@@ -0,0 +1,73 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from openpilot.common.params import Params
+from opendbc.car import structs
+from opendbc.safety import ALTERNATIVE_EXPERIENCE
+from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP, HyundaiSafetyFlagsSP
+from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
+
+
+MADS_NO_ACC_MAIN_BUTTON = ("rivian", "tesla")
+
+
+class MadsSteeringModeOnBrake:
+  REMAIN_ACTIVE = 0
+  PAUSE = 1
+  DISENGAGE = 2
+
+
+def get_mads_limited_brands(CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> bool:
+  if CP.brand == 'rivian':
+    return True
+  if CP.brand == 'tesla':
+    return not CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS
+
+  return False
+
+
+def read_steering_mode_param(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params):
+  if get_mads_limited_brands(CP, CP_SP):
+    return MadsSteeringModeOnBrake.DISENGAGE
+
+  return params.get("MadsSteeringMode", return_default=True)
+
+
+def set_alternative_experience(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params):
+  enabled = params.get_bool("Mads")
+  steering_mode = read_steering_mode_param(CP, CP_SP, params)
+
+  if enabled:
+    CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS
+
+    if steering_mode == MadsSteeringModeOnBrake.DISENGAGE:
+      CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.MADS_DISENGAGE_LATERAL_ON_BRAKE
+    elif steering_mode == MadsSteeringModeOnBrake.PAUSE:
+      CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.MADS_PAUSE_LATERAL_ON_BRAKE
+
+
+def set_car_specific_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params):
+  if CP.brand == "hyundai":
+    # TODO-SP: This should be separated from MADS module for future implementations
+    #          Use "HyundaiLongitudinalMainCruiseToggleable" param
+    hyundai_cruise_main_toggleable = True
+    if hyundai_cruise_main_toggleable:
+      CP_SP.flags |= HyundaiFlagsSP.LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE.value
+      CP_SP.safetyParam |= HyundaiSafetyFlagsSP.LONG_MAIN_CRUISE_TOGGLEABLE
+
+  # MADS Partial Support
+  # MADS is currently partially supported for these platforms due to lack of consistent states to engage controls
+  # Only MadsSteeringModeOnBrake.DISENGAGE is supported for these platforms
+  # TODO-SP: To enable MADS full support for Rivian and most Tesla, identify consistent signals for MADS toggling
+  mads_partial_support = get_mads_limited_brands(CP, CP_SP)
+  if mads_partial_support:
+    params.put("MadsSteeringMode", 2, block=True)
+    params.put_bool("MadsUnifiedEngagementMode", True, block=True)
+
+  # no ACC MAIN button for these brands
+  if CP.brand in MADS_NO_ACC_MAIN_BUTTON:
+    params.remove("MadsMainCruiseAllowed")

openpilot/sunnypilot/mads/mads.py

@@ -0,0 +1,222 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from openpilot.cereal import log, custom
+
+from opendbc.car import structs
+from opendbc.car.hyundai.values import HyundaiFlags
+from openpilot.common.params import Params
+from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, MADS_NO_ACC_MAIN_BUTTON
+from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT
+
+State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
+ButtonType = structs.CarState.ButtonEvent.Type
+EventName = log.OnroadEvent.EventName
+EventNameSP = custom.OnroadEventSP.EventName
+GearShifter = structs.CarState.GearShifter
+SafetyModel = structs.CarParams.SafetyModel
+
+SET_SPEED_BUTTONS = (ButtonType.accelCruise, ButtonType.resumeCruise, ButtonType.decelCruise, ButtonType.setCruise)
+IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
+
+
+class ModularAssistiveDrivingSystem:
+  def __init__(self, selfdrive):
+    self.CP = selfdrive.CP
+    self.CP_SP = selfdrive.CP_SP
+    self.params = selfdrive.params
+
+    self.enabled = False
+    self.active = False
+    self.available = False
+    self.lateral_mismatch_counter = 0
+    self.allow_always = False
+    self.no_main_cruise = False
+    self.selfdrive = selfdrive
+    self.selfdrive.enabled_prev = False
+    self.state_machine = StateMachine(self)
+    self.events = self.selfdrive.events
+    self.events_sp = self.selfdrive.events_sp
+    self.disengage_on_accelerator = Params().get_bool("DisengageOnAccelerator")
+    if self.CP.brand == "hyundai":
+      if self.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD):
+        self.allow_always = True
+    if self.CP.brand == "tesla":
+      self.allow_always = True
+
+    if self.CP.brand in MADS_NO_ACC_MAIN_BUTTON:
+      self.no_main_cruise = True
+
+    # read params on init
+    self.enabled_toggle = self.params.get_bool("Mads")
+    self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed")
+    self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.CP_SP, self.params)
+    self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode")
+
+  def read_params(self):
+    self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed")
+    self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode")
+
+  def pedal_pressed_non_gas_pressed(self, CS: structs.CarState) -> bool:
+    # ignore `pedalPressed` events caused by gas presses
+    if self.events.has(EventName.pedalPressed) and not (CS.gasPressed and not self.selfdrive.CS_prev.gasPressed and self.disengage_on_accelerator):
+      return True
+
+    return False
+
+  def should_silent_lkas_enable(self, CS: structs.CarState) -> bool:
+    if self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE and (CS.brakePressed or CS.regenBraking or self.pedal_pressed_non_gas_pressed(CS)):
+      return False
+
+    if self.events_sp.contains_in_list(GEARS_ALLOW_PAUSED_SILENT):
+      return False
+
+    return True
+
+  def block_unified_engagement_mode(self) -> bool:
+    # UEM disabled
+    if not self.unified_engagement_mode:
+      return True
+
+    if self.enabled:
+      return True
+
+    if self.selfdrive.enabled and self.selfdrive.enabled_prev:
+      return True
+
+    return False
+
+  def get_wrong_car_mode(self, alert_only: bool) -> None:
+    if alert_only:
+      if self.events.has(EventName.wrongCarMode):
+        self.replace_event(EventName.wrongCarMode, EventNameSP.wrongCarModeAlertOnly)
+    else:
+      self.events.remove(EventName.wrongCarMode)
+
+  def transition_paused_state(self):
+    if self.state_machine.state != State.paused:
+      self.events_sp.add(EventNameSP.silentLkasDisable)
+
+  def replace_event(self, old_event: int, new_event: int):
+    self.events.remove(old_event)
+    self.events_sp.add(new_event)
+
+  def data_sample(self):
+    # When the safety and selfdrived do not agree on controls_allowed_lateral
+    # we want to disengage sunnypilot. However the status from the panda goes through
+    # another socket other than the CAN messages and one can arrive earlier than the other.
+    # Therefore we allow a mismatch for two samples, then we trigger the disengagement.
+    if not self.active or self.selfdrive.enabled:
+      self.lateral_mismatch_counter = 0
+    elif any(not ps.controlsAllowedLateral for ps in self.selfdrive.sm['pandaStates']
+             if ps.safetyModel not in IGNORED_SAFETY_MODES):
+      self.lateral_mismatch_counter += 1
+
+  def update_events(self, CS: structs.CarState):
+    if not self.selfdrive.enabled and self.enabled:
+      if CS.standstill:
+        if self.events.has(EventName.doorOpen):
+          self.replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen)
+          self.transition_paused_state()
+        if self.events.has(EventName.seatbeltNotLatched):
+          self.replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched)
+          self.transition_paused_state()
+      if self.events.has(EventName.wrongGear) and (CS.vEgo < 2.5 or CS.gearShifter == GearShifter.reverse):
+        self.replace_event(EventName.wrongGear, EventNameSP.silentWrongGear)
+        self.transition_paused_state()
+      if self.events.has(EventName.reverseGear):
+        self.replace_event(EventName.reverseGear, EventNameSP.silentReverseGear)
+        self.transition_paused_state()
+      if self.events.has(EventName.brakeHold):
+        self.replace_event(EventName.brakeHold, EventNameSP.silentBrakeHold)
+        self.transition_paused_state()
+      if self.events.has(EventName.parkBrake):
+        self.replace_event(EventName.parkBrake, EventNameSP.silentParkBrake)
+        self.transition_paused_state()
+
+      if self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE:
+        if self.pedal_pressed_non_gas_pressed(CS):
+          self.transition_paused_state()
+
+      self.events.remove(EventName.preEnableStandstill)
+      self.events.remove(EventName.belowEngageSpeed)
+      self.events.remove(EventName.speedTooLow)
+      self.events.remove(EventName.cruiseDisabled)
+      self.events.remove(EventName.manualRestart)
+      self.events.remove(EventName.espActive)
+
+    selfdrive_enable_events = self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable)
+    set_speed_btns_enable = any(be.type in SET_SPEED_BUTTONS for be in CS.buttonEvents)
+
+    # wrongCarMode alert only or actively block control
+    self.get_wrong_car_mode(selfdrive_enable_events or set_speed_btns_enable)
+
+    if selfdrive_enable_events:
+      if self.pedal_pressed_non_gas_pressed(CS):
+        self.events_sp.add(EventNameSP.pedalPressedAlertOnly)
+
+      if self.block_unified_engagement_mode():
+        self.events.remove(EventName.pcmEnable)
+        self.events.remove(EventName.buttonEnable)
+    else:
+      if self.main_enabled_toggle:
+        if CS.cruiseState.available and not self.selfdrive.CS_prev.cruiseState.available:
+          self.events_sp.add(EventNameSP.lkasEnable)
+
+    for be in CS.buttonEvents:
+      if be.type == ButtonType.cancel:
+        if not self.selfdrive.enabled and self.selfdrive.enabled_prev:
+          self.events_sp.add(EventNameSP.manualLongitudinalRequired)
+      if be.type == ButtonType.lkas and be.pressed and (CS.cruiseState.available or self.allow_always):
+        if self.enabled:
+          if self.selfdrive.enabled:
+            self.events_sp.add(EventNameSP.manualSteeringRequired)
+          else:
+            self.events_sp.add(EventNameSP.lkasDisable)
+        else:
+          self.events_sp.add(EventNameSP.lkasEnable)
+
+    if not CS.cruiseState.available and not self.no_main_cruise:
+      self.events.remove(EventName.buttonEnable)
+      if self.selfdrive.CS_prev.cruiseState.available:
+        self.events_sp.add(EventNameSP.lkasDisable)
+
+    if self.steering_mode_on_brake == MadsSteeringModeOnBrake.DISENGAGE:
+      if self.pedal_pressed_non_gas_pressed(CS):
+        if self.enabled:
+          self.events_sp.add(EventNameSP.lkasDisable)
+        else:
+          # block lkasEnable if being sent, then send pedalPressedAlertOnly event
+          if self.events_sp.contains(EventNameSP.lkasEnable):
+            self.events_sp.remove(EventNameSP.lkasEnable)
+            self.events_sp.add(EventNameSP.pedalPressedAlertOnly)
+
+    if self.should_silent_lkas_enable(CS):
+      if self.state_machine.state == State.paused:
+        self.events_sp.add(EventNameSP.silentLkasEnable)
+
+    if self.lateral_mismatch_counter >= 200:
+      self.events_sp.add(EventNameSP.controlsMismatchLateral)
+
+    self.events.remove(EventName.pcmDisable)
+    self.events.remove(EventName.buttonCancel)
+    self.events.remove(EventName.pedalPressed)
+    self.events.remove(EventName.wrongCruiseMode)
+
+  def update(self, CS: structs.CarState):
+    if not self.enabled_toggle:
+      return
+
+    self.data_sample()
+
+    self.update_events(CS)
+
+    if not self.CP.passive and self.selfdrive.initialized:
+      self.enabled, self.active = self.state_machine.update()
+
+    # Copy of previous SelfdriveD states for MADS events handling
+    self.selfdrive.enabled_prev = self.selfdrive.enabled

openpilot/sunnypilot/mads/state.py

@@ -0,0 +1,135 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from openpilot.cereal import log, custom
+from openpilot.selfdrive.selfdrived.events import ET
+from openpilot.selfdrive.selfdrived.state import SOFT_DISABLE_TIME
+from openpilot.common.realtime import DT_CTRL
+
+State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
+EventName = log.OnroadEvent.EventName
+EventNameSP = custom.OnroadEventSP.EventName
+
+ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding)
+ENABLED_STATES = (State.paused, *ACTIVE_STATES)
+
+GEARS_ALLOW_PAUSED_SILENT = [EventNameSP.silentWrongGear, EventNameSP.silentReverseGear, EventNameSP.silentBrakeHold,
+                             EventNameSP.silentDoorOpen, EventNameSP.silentSeatbeltNotLatched, EventNameSP.silentParkBrake]
+GEARS_ALLOW_PAUSED = [EventName.wrongGear, EventName.reverseGear, EventName.brakeHold,
+                      EventName.doorOpen, EventName.seatbeltNotLatched, EventName.parkBrake]
+
+
+class StateMachine:
+  def __init__(self, mads):
+    self.selfdrive = mads.selfdrive
+    self.ss_state_machine = mads.selfdrive.state_machine
+    self._events = mads.selfdrive.events
+    self._events_sp = mads.selfdrive.events_sp
+
+    self.state = State.disabled
+
+  def add_current_alert_types(self, alert_type):
+    if not self.selfdrive.enabled:
+      self.ss_state_machine.current_alert_types.append(alert_type)
+
+  def check_contains(self, event_type: str) -> bool:
+    return bool(self._events.contains(event_type) or self._events_sp.contains(event_type))
+
+  def check_contains_in_list(self) -> bool:
+    return bool(self._events.contains_in_list(GEARS_ALLOW_PAUSED) or self._events_sp.contains_in_list(GEARS_ALLOW_PAUSED_SILENT))
+
+  def update(self):
+    # soft disable timer and current alert types are from the state machine of openpilot
+    # decrement the soft disable timer at every step, as it's reset on
+    # entrance in SOFT_DISABLING state
+
+    # ENABLED, SOFT DISABLING, PAUSED, OVERRIDING
+    if self.state != State.disabled:
+      # user and immediate disable always have priority in a non-disabled state
+      if self.check_contains(ET.USER_DISABLE):
+        if self._events_sp.has(EventNameSP.silentLkasDisable):
+          self.state = State.paused
+        else:
+          self.state = State.disabled
+        self.ss_state_machine.current_alert_types.append(ET.USER_DISABLE)
+
+      elif self.check_contains(ET.IMMEDIATE_DISABLE):
+        self.state = State.disabled
+        self.add_current_alert_types(ET.IMMEDIATE_DISABLE)
+
+      else:
+        # ENABLED
+        if self.state == State.enabled:
+          if self.check_contains(ET.SOFT_DISABLE):
+            self.state = State.softDisabling
+            if not self.selfdrive.enabled:
+              self.ss_state_machine.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
+              self.ss_state_machine.current_alert_types.append(ET.SOFT_DISABLE)
+
+          elif self.check_contains(ET.OVERRIDE_LATERAL):
+            self.state = State.overriding
+            self.add_current_alert_types(ET.OVERRIDE_LATERAL)
+
+        # SOFT DISABLING
+        elif self.state == State.softDisabling:
+          if not self.check_contains(ET.SOFT_DISABLE):
+            # no more soft disabling condition, so go back to ENABLED
+            self.state = State.enabled
+
+          elif self.ss_state_machine.soft_disable_timer > 0:
+            self.add_current_alert_types(ET.SOFT_DISABLE)
+
+          elif self.ss_state_machine.soft_disable_timer <= 0:
+            self.state = State.disabled
+
+        # PAUSED
+        elif self.state == State.paused:
+          if self.check_contains(ET.ENABLE):
+            if self.check_contains(ET.NO_ENTRY):
+              self.add_current_alert_types(ET.NO_ENTRY)
+
+            else:
+              if self.check_contains(ET.OVERRIDE_LATERAL):
+                self.state = State.overriding
+              else:
+                self.state = State.enabled
+              self.add_current_alert_types(ET.ENABLE)
+
+        # OVERRIDING
+        elif self.state == State.overriding:
+          if self.check_contains(ET.SOFT_DISABLE):
+            self.state = State.softDisabling
+            if not self.selfdrive.enabled:
+              self.ss_state_machine.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
+              self.ss_state_machine.current_alert_types.append(ET.SOFT_DISABLE)
+          elif not self.check_contains(ET.OVERRIDE_LATERAL):
+            self.state = State.enabled
+          else:
+            self.ss_state_machine.current_alert_types += [ET.OVERRIDE_LATERAL]
+
+    # DISABLED
+    elif self.state == State.disabled:
+      if self.check_contains(ET.ENABLE):
+        if self.check_contains(ET.NO_ENTRY):
+          if self.check_contains_in_list():
+            self.state = State.paused
+          self.add_current_alert_types(ET.NO_ENTRY)
+
+        else:
+          if self.check_contains(ET.OVERRIDE_LATERAL):
+            self.state = State.overriding
+          else:
+            self.state = State.enabled
+          self.add_current_alert_types(ET.ENABLE)
+
+    # check if MADS is engaged and actuators are enabled
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+    if active:
+      self.add_current_alert_types(ET.WARNING)
+
+    return enabled, active

openpilot/sunnypilot/mads/tests/test_mads_state_machine.py

@@ -0,0 +1,144 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import pytest
+from pytest_mock import MockerFixture
+
+from openpilot.cereal import custom
+from openpilot.common.realtime import DT_CTRL
+from openpilot.sunnypilot.mads.state import StateMachine, SOFT_DISABLE_TIME
+from openpilot.selfdrive.selfdrived.events import ET, NormalPermanentAlert, Events
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP, EVENTS_SP
+
+State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
+EventNameSP = custom.OnroadEventSP.EventName
+
+# The event types that maintain the current state
+MAINTAIN_STATES = {State.enabled: (None,), State.disabled: (None,), State.softDisabling: (ET.SOFT_DISABLE,),
+                   State.paused: (None,), State.overriding: (ET.OVERRIDE_LATERAL,)}
+ALL_STATES = (State.schema.enumerants.values())
+# The event types checked in DISABLED section of state machine
+ENABLE_EVENT_TYPES = (ET.ENABLE, ET.OVERRIDE_LATERAL)
+
+
+def make_event(event_types):
+  event = {}
+  for ev in event_types:
+    event[ev] = NormalPermanentAlert("alert")
+  EVENTS_SP[0] = event
+  return 0
+
+
+class MockMADS:
+  def __init__(self, mocker: MockerFixture):
+    self.selfdrive = mocker.MagicMock()
+    self.selfdrive.state_machine = mocker.MagicMock()
+    self.selfdrive.events = Events()
+    self.selfdrive.events_sp = EventsSP()
+
+
+class TestMADSStateMachine:
+  @pytest.fixture(autouse=True)
+  def setup_method(self, mocker: MockerFixture):
+    self.mads = MockMADS(mocker)
+    self.state_machine = StateMachine(self.mads)
+    self.events = self.mads.selfdrive.events
+    self.events_sp = self.mads.selfdrive.events_sp
+    self.mads.selfdrive.state_machine.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
+
+  def clear_events(self):
+    self.events.clear()
+    self.events_sp.clear()
+
+  def test_immediate_disable(self):
+    for state in ALL_STATES:
+      for et in MAINTAIN_STATES[state]:
+        self.events_sp.add(make_event([et, ET.IMMEDIATE_DISABLE]))
+        self.state_machine.state = state
+        self.state_machine.update()
+        assert State.disabled == self.state_machine.state
+        self.clear_events()
+
+  def test_user_disable(self):
+    for state in ALL_STATES:
+      for et in MAINTAIN_STATES[state]:
+        self.events_sp.add(make_event([et, ET.USER_DISABLE]))
+        self.state_machine.state = state
+        self.state_machine.update()
+        assert State.disabled == self.state_machine.state
+        self.clear_events()
+
+  def test_user_disable_to_paused(self):
+    paused_events = (EventNameSP.silentLkasDisable, )
+    for state in ALL_STATES:
+      for et in MAINTAIN_STATES[state]:
+        self.events_sp.add(make_event([et, ET.USER_DISABLE]))
+        for en in paused_events:
+          self.events_sp.add(en)
+          self.state_machine.state = state
+          self.state_machine.update()
+          final_state = State.paused if self.events_sp.has(en) and state != State.disabled else State.disabled
+          assert self.state_machine.state == final_state
+          self.clear_events()
+
+  def test_soft_disable(self):
+    for state in ALL_STATES:
+      for et in MAINTAIN_STATES[state]:
+        self.events_sp.add(make_event([et, ET.SOFT_DISABLE]))
+        self.state_machine.state = state
+        self.state_machine.update()
+        assert self.state_machine.state == State.disabled if state == State.disabled else State.softDisabling
+        self.clear_events()
+
+  def test_soft_disable_timer(self):
+    self.state_machine.state = State.enabled
+    self.events_sp.add(make_event([ET.SOFT_DISABLE]))
+    self.state_machine.update()
+    for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)):
+      assert self.state_machine.state == State.softDisabling
+      self.mads.selfdrive.state_machine.soft_disable_timer -= 1
+      self.state_machine.update()
+
+    assert self.state_machine.state == State.disabled
+    self.clear_events()
+
+  def test_no_entry(self):
+    for et in ENABLE_EVENT_TYPES:
+      self.events_sp.add(make_event([ET.NO_ENTRY, et]))
+      self.state_machine.update()
+      assert self.state_machine.state == State.disabled
+      self.clear_events()
+
+  def test_no_entry_paused(self):
+    self.state_machine.state = State.paused
+    self.events_sp.add(make_event([ET.NO_ENTRY]))
+    self.state_machine.update()
+    assert self.state_machine.state == State.paused
+    self.clear_events()
+
+  def test_override_lateral(self):
+    self.state_machine.state = State.enabled
+    self.events_sp.add(make_event([ET.OVERRIDE_LATERAL]))
+    self.state_machine.update()
+    assert self.state_machine.state == State.overriding
+    self.clear_events()
+
+  def test_paused_to_enabled(self):
+    self.state_machine.state = State.paused
+    self.events_sp.add(make_event([ET.ENABLE]))
+    self.state_machine.update()
+    assert self.state_machine.state == State.enabled
+    self.clear_events()
+
+  def test_maintain_states(self):
+    for state in ALL_STATES:
+      for et in MAINTAIN_STATES[state]:
+        self.state_machine.state = state
+        self.events_sp.add(make_event([et]))
+        self.state_machine.update()
+        assert self.state_machine.state == state
+        self.clear_events()

openpilot/sunnypilot/mads/tests/test_mads_steering_mode.py

@@ -0,0 +1,242 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import pytest
+
+from openpilot.cereal import log, custom
+from opendbc.car import structs
+from openpilot.selfdrive.selfdrived.events import Events
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param
+from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem
+from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
+
+State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
+EventName = log.OnroadEvent.EventName
+EventNameSP = custom.OnroadEventSP.EventName
+SafetyModel = structs.CarParams.SafetyModel
+
+
+def make_car_state(brake_pressed=False, regen_braking=False, standstill=False, v_ego=0.0):
+  cs = structs.CarState()
+  cs.brakePressed = brake_pressed
+  cs.regenBraking = regen_braking
+  cs.standstill = standstill
+  cs.vEgo = v_ego
+  cs.cruiseState.available = True
+  return cs
+
+
+def make_panda_state(mocker, controls_allowed_lateral=True):
+  ps = mocker.MagicMock()
+  ps.controlsAllowedLateral = controls_allowed_lateral
+  ps.safetyModel = SafetyModel.hyundai
+  return ps
+
+
+def make_mads(mocker, steering_mode):
+  sd = mocker.MagicMock()
+  sd.CP = structs.CarParams()
+  sd.CP.brand = "hyundai"
+  sd.CP_SP = structs.CarParamsSP()
+  sd.params = mocker.MagicMock()
+  sd.params.get_bool = mocker.MagicMock(side_effect=lambda k: {
+    "Mads": True, "MadsMainCruiseAllowed": False,
+    "DisengageOnAccelerator": True, "MadsUnifiedEngagementMode": False,
+  }.get(k, False))
+  sd.params.get = mocker.MagicMock(return_value=steering_mode)
+  sd.events = Events()
+  sd.events_sp = EventsSP()
+  sd.enabled = False
+  sd.enabled_prev = False
+  sd.initialized = True
+  sd.CS_prev = make_car_state()
+  sd.sm = {'pandaStates': [make_panda_state(mocker)]}
+  sd.state_machine = mocker.MagicMock()
+
+  mads = ModularAssistiveDrivingSystem(sd)
+  mads.enabled_toggle = True
+  mads.steering_mode_on_brake = steering_mode
+  return mads, sd
+
+
+def run_frames(mads, sd, cs, n=1):
+  for _ in range(n):
+    mads.update(cs)
+    sd.CS_prev = cs
+    sd.events.clear()
+    sd.events_sp.clear()
+
+
+# should_silent_lkas_enable across all modes
+
+class TestShouldSilentLkasEnable:
+  @pytest.mark.parametrize("brake,regen", [(True, False), (False, True)])
+  def test_pause_blocks_reenable_on_braking_at_standstill(self, mocker, brake, regen):
+    mads, _ = make_mads(mocker, MadsSteeringModeOnBrake.PAUSE)
+    cs = make_car_state(brake_pressed=brake, regen_braking=regen, standstill=True)
+    assert mads.should_silent_lkas_enable(cs) is False
+
+  def test_pause_allows_reenable_on_brake_release(self, mocker):
+    mads, _ = make_mads(mocker, MadsSteeringModeOnBrake.PAUSE)
+    cs = make_car_state(standstill=True)
+    assert mads.should_silent_lkas_enable(cs) is True
+
+  def test_remain_active_ignores_brake(self, mocker):
+    mads, _ = make_mads(mocker, MadsSteeringModeOnBrake.REMAIN_ACTIVE)
+    cs = make_car_state(brake_pressed=True, standstill=True)
+    assert mads.should_silent_lkas_enable(cs) is True
+
+  def test_disengage_ignores_brake_for_silent_enable(self, mocker):
+    mads, _ = make_mads(mocker, MadsSteeringModeOnBrake.DISENGAGE)
+    cs = make_car_state(brake_pressed=True, standstill=True)
+    assert mads.should_silent_lkas_enable(cs) is True
+
+
+# pause
+
+class TestPauseMode:
+  def test_stays_paused_at_standstill_brake_held(self, mocker):
+    mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.PAUSE)
+    mads.state_machine.state = State.enabled
+    mads.enabled = True
+    mads.active = True
+
+    sd.events.add(EventName.pedalPressed)
+    run_frames(mads, sd, make_car_state(brake_pressed=True, v_ego=15.0))
+    assert mads.state_machine.state == State.paused
+
+    sd.sm['pandaStates'] = [make_panda_state(mocker, False)]
+    run_frames(mads, sd, make_car_state(brake_pressed=True, standstill=True), n=250)
+    assert mads.state_machine.state == State.paused
+
+  def test_resumes_on_brake_release_at_standstill(self, mocker):
+    mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.PAUSE)
+    mads.state_machine.state = State.paused
+    mads.enabled = True
+    mads.active = False
+
+    run_frames(mads, sd, make_car_state(standstill=True))
+    assert mads.state_machine.state == State.enabled
+
+  def test_full_cycle_moving_to_standstill(self, mocker):
+    mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.PAUSE)
+    mads.state_machine.state = State.enabled
+    mads.enabled = True
+    mads.active = True
+
+    sd.events.add(EventName.pedalPressed)
+    run_frames(mads, sd, make_car_state(brake_pressed=True, v_ego=15.0))
+    assert mads.state_machine.state == State.paused
+
+    sd.sm['pandaStates'] = [make_panda_state(mocker, False)]
+    run_frames(mads, sd, make_car_state(brake_pressed=True, standstill=True), n=250)
+    assert mads.state_machine.state == State.paused
+
+    sd.sm['pandaStates'] = [make_panda_state(mocker, True)]
+    run_frames(mads, sd, make_car_state(standstill=True))
+    assert mads.state_machine.state == State.enabled
+
+
+# disengage
+
+class TestDisengageMode:
+  def test_brake_while_enabled_disables(self, mocker):
+    mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.DISENGAGE)
+    mads.state_machine.state = State.enabled
+    mads.enabled = True
+    mads.active = True
+
+    sd.events.add(EventName.pedalPressed)
+    run_frames(mads, sd, make_car_state(brake_pressed=True, v_ego=10.0))
+    assert mads.state_machine.state == State.disabled
+
+  def test_brake_sends_lkas_disable_when_enabled(self, mocker):
+    mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.DISENGAGE)
+    mads.state_machine.state = State.enabled
+    mads.enabled = True
+    mads.active = True
+
+    sd.events.add(EventName.pedalPressed)
+    mads.update_events(make_car_state(brake_pressed=True, v_ego=5.0))
+    assert sd.events_sp.has(EventNameSP.lkasDisable)
+
+
+# remain active
+
+class TestRemainActiveMode:
+  def test_brake_does_not_pause_or_disable(self, mocker):
+    mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.REMAIN_ACTIVE)
+    mads.state_machine.state = State.enabled
+    mads.enabled = True
+    mads.active = True
+
+    sd.events.add(EventName.pedalPressed)
+    run_frames(mads, sd, make_car_state(brake_pressed=True, v_ego=10.0))
+    assert mads.state_machine.state == State.enabled
+
+
+# lateral mismatch counter
+
+class TestLateralMismatchCounter:
+  def test_no_accumulation_while_paused(self, mocker):
+    mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.PAUSE)
+    mads.state_machine.state = State.paused
+    mads.enabled = True
+    mads.active = False
+    sd.sm['pandaStates'] = [make_panda_state(mocker, False)]
+
+    run_frames(mads, sd, make_car_state(brake_pressed=True, standstill=True), n=250)
+    assert mads.lateral_mismatch_counter == 0
+
+  def test_accumulates_when_active_and_panda_disagrees(self, mocker):
+    mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.PAUSE)
+    mads.enabled = True
+    mads.active = True
+    sd.sm['pandaStates'] = [make_panda_state(mocker, False)]
+
+    for _ in range(200):
+      mads.data_sample()
+    assert mads.lateral_mismatch_counter == 200
+
+
+# brand restrictions
+
+class TestBrandSteeringModeRestrictions:
+  def test_rivian_forced_to_disengage(self, mocker):
+    CP = structs.CarParams()
+    CP.brand = "rivian"
+    CP_SP = structs.CarParamsSP()
+    params = mocker.MagicMock()
+    assert read_steering_mode_param(CP, CP_SP, params) == MadsSteeringModeOnBrake.DISENGAGE
+    params.get.assert_not_called()
+
+  def test_tesla_without_vehicle_bus_forced_to_disengage(self, mocker):
+    CP = structs.CarParams()
+    CP.brand = "tesla"
+    CP_SP = structs.CarParamsSP()
+    CP_SP.flags = 0
+    params = mocker.MagicMock()
+    assert read_steering_mode_param(CP, CP_SP, params) == MadsSteeringModeOnBrake.DISENGAGE
+
+  def test_tesla_with_vehicle_bus_uses_param(self, mocker):
+    CP = structs.CarParams()
+    CP.brand = "tesla"
+    CP_SP = structs.CarParamsSP()
+    CP_SP.flags = TeslaFlagsSP.HAS_VEHICLE_BUS
+    params = mocker.MagicMock()
+    params.get = mocker.MagicMock(return_value=MadsSteeringModeOnBrake.REMAIN_ACTIVE)
+    assert read_steering_mode_param(CP, CP_SP, params) == MadsSteeringModeOnBrake.REMAIN_ACTIVE
+
+  @pytest.mark.parametrize("brand", ["hyundai", "toyota", "honda", "gm"])
+  def test_other_brands_use_param(self, mocker, brand):
+    CP = structs.CarParams()
+    CP.brand = brand
+    CP_SP = structs.CarParamsSP()
+    params = mocker.MagicMock()
+    params.get = mocker.MagicMock(return_value=MadsSteeringModeOnBrake.REMAIN_ACTIVE)
+    assert read_steering_mode_param(CP, CP_SP, params) == MadsSteeringModeOnBrake.REMAIN_ACTIVE