mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-08-05 08:16:03 +08:00
Code Issues Packages Projects Releases Wiki Activity

MADS: Properly handle gear events (#628)

Browse Source 
* MADS: Properly handle gear events

* use referenced object directly

* fix tests

* unused

* rename
This commit is contained in:
Jason Wen
2025-02-01 22:11:10 -05:00
committed by GitHub
parent 17b3092e76
commit d7087d312c
3 changed files with 42 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/mads/mads.py
+2 -2
View File
@@ -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
sunnypilot/mads/state.py
+9 -16
View File
@@ -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)
sunnypilot/mads/tests/test_mads_state_machine.py
+31 -31
View File
@@ -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()