sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
2026-07-21 07:44:02 +00:00
commit de77c89cf6
@@ -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()
@@ -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