diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 5846dc4fd..6335517a4 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -111,7 +111,7 @@ class ModularAssistiveDrivingSystem: transition_paused_state() if not (self.pause_lateral_on_brake_toggle and CS.brakePressed) and \ - not self.events.contains_in_list(GEARS_ALLOW_PAUSED_SILENT): + not self.events_sp.contains_in_list(GEARS_ALLOW_PAUSED_SILENT): if self.state_machine.state == State.paused: self.events_sp.add(EventNameSP.silentLkasEnable) @@ -160,7 +160,7 @@ class ModularAssistiveDrivingSystem: self.update_events(CS) if not self.selfdrive.CP.passive and self.selfdrive.initialized: - self.enabled, self.active = self.state_machine.update(self.events, self.events_sp) + self.enabled, self.active = self.state_machine.update() # Copy of previous SelfdriveD states for MADS events handling self.selfdrive.enabled_prev = self.selfdrive.enabled diff --git a/sunnypilot/mads/state.py b/sunnypilot/mads/state.py index 342b481dd..e3acf3a0e 100644 --- a/sunnypilot/mads/state.py +++ b/sunnypilot/mads/state.py @@ -25,12 +25,10 @@ Last updated: July 29, 2024 """ from cereal import log, custom -from openpilot.selfdrive.selfdrived.events import ET, Events +from openpilot.selfdrive.selfdrived.events import ET from openpilot.selfdrive.selfdrived.state import SOFT_DISABLE_TIME from openpilot.common.realtime import DT_CTRL -from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP - State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState EventName = log.OnroadEvent.EventName EventNameSP = custom.OnroadEventSP.EventName @@ -41,20 +39,18 @@ 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, - *GEARS_ALLOW_PAUSED_SILENT] + 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 - self._events = Events() - self._events_sp = EventsSP() - def add_current_alert_types(self, alert_type): if not self.selfdrive.enabled: self.ss_state_machine.current_alert_types.append(alert_type) @@ -62,22 +58,19 @@ class StateMachine: 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, events_list: list[int]) -> bool: - return bool(self._events.contains_in_list(events_list) or self._events_sp.contains_in_list(events_list)) + 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, events: Events, events_sp: EventsSP): + 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 - self._events = events - self._events_sp = events_sp - # 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 events_sp.has(EventNameSP.silentLkasDisable) or events_sp.has(EventNameSP.silentBrakeHold): + if self._events_sp.has(EventNameSP.silentLkasDisable) or self._events_sp.has(EventNameSP.silentBrakeHold): self.state = State.paused else: self.state = State.disabled @@ -141,7 +134,7 @@ class StateMachine: elif self.state == State.disabled: if self.check_contains(ET.ENABLE): if self.check_contains(ET.NO_ENTRY): - if self.check_contains_in_list(GEARS_ALLOW_PAUSED): + if self.check_contains_in_list(): self.state = State.paused self.add_current_alert_types(ET.NO_ENTRY) diff --git a/sunnypilot/mads/tests/test_mads_state_machine.py b/sunnypilot/mads/tests/test_mads_state_machine.py index 694d62362..546893d28 100644 --- a/sunnypilot/mads/tests/test_mads_state_machine.py +++ b/sunnypilot/mads/tests/test_mads_state_machine.py @@ -29,9 +29,9 @@ from pytest_mock import MockerFixture from cereal import custom from openpilot.common.realtime import DT_CTRL -from openpilot.sunnypilot.mads.state import StateMachine, SOFT_DISABLE_TIME, GEARS_ALLOW_PAUSED -from openpilot.selfdrive.selfdrived.events import ET, NormalPermanentAlert -from openpilot.sunnypilot.selfdrive.selfdrived.events import EVENTS_SP +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 @@ -56,7 +56,8 @@ class MockMADS: def __init__(self, mocker: MockerFixture): self.selfdrive = mocker.MagicMock() self.selfdrive.state_machine = mocker.MagicMock() - self.selfdrive.active = False + self.selfdrive.events = Events() + self.selfdrive.events_sp = EventsSP() class TestMADSStateMachine: @@ -64,32 +65,31 @@ class TestMADSStateMachine: def setup_method(self, mocker: MockerFixture): self.mads = MockMADS(mocker) self.state_machine = StateMachine(self.mads) - self.events = self.state_machine._events - self.events_sp = self.state_machine._events_sp + 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 reset(self): + def clear_events(self): self.events.clear() self.events_sp.clear() - self.state_machine.state = State.disabled 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(self.events, self.events_sp) + self.state_machine.update() assert State.disabled == self.state_machine.state - self.reset() + 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(self.events, self.events_sp) + self.state_machine.update() assert State.disabled == self.state_machine.state - self.reset() + self.clear_events() def test_user_disable_to_paused(self): paused_events = (EventNameSP.silentLkasDisable, EventNameSP.silentBrakeHold) @@ -99,65 +99,65 @@ class TestMADSStateMachine: for en in paused_events: self.events_sp.add(en) self.state_machine.state = state - self.state_machine.update(self.events, self.events_sp) + 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.reset() + self.clear_events() def test_soft_disable(self): for state in ALL_STATES: - if state == State.paused: # paused considers USER_DISABLE instead - continue 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(self.events, self.events_sp) + self.state_machine.update() assert self.state_machine.state == State.disabled if state == State.disabled else State.softDisabling - self.reset() + 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(self.events, self.events_sp) + 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(self.events, self.events_sp) + 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])) - if not self.state_machine.check_contains_in_list(GEARS_ALLOW_PAUSED): - self.state_machine.update(self.events, self.events_sp) - assert self.state_machine.state == State.disabled - self.reset() + 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(self.events, self.events_sp) + 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(self.events, self.events_sp) + 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(self.events, self.events_sp) + 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 - if et is not None: - self.events_sp.add(make_event([et])) - self.state_machine.update(self.events, self.events_sp) + self.events_sp.add(make_event([et])) + self.state_machine.update() assert self.state_machine.state == state - self.reset() + self.clear_events()