mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-09 03:22:05 +08:00
Remove MADS components and migrate to Opendbc repo
The Modular Assistive Driving System (MADS) related files and tests have been removed from the sunnypilot directory. MADS functionality has been migrated to the opendbc repository, streamlining its structure and integration.
This commit is contained in:
DevTekVE
+1
-1
Submodule opendbc_repo updated: efd5f88dff...007a0aab21
@@ -1 +0,0 @@
|
||||
../sunnypilot
|
||||
@@ -22,7 +22,7 @@ from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
|
||||
from openpilot.selfdrive.car.cruise import VCruiseHelper
|
||||
from openpilot.selfdrive.car.car_specific import MockCarState
|
||||
|
||||
from openpilot.sunnypilot.mads.mads import MadsParams
|
||||
from opendbc.sunnypilot.mads.mads import MadsParams
|
||||
|
||||
REPLAY = "REPLAY" in os.environ
|
||||
|
||||
|
||||
@@ -23,7 +23,7 @@ from openpilot.selfdrive.controls.lib.latcontrol import MIN_LATERAL_CONTROL_SPEE
|
||||
|
||||
from openpilot.system.version import get_build_metadata
|
||||
|
||||
from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem
|
||||
from opendbc.sunnypilot.mads.mads import ModularAssistiveDrivingSystem
|
||||
|
||||
REPLAY = "REPLAY" in os.environ
|
||||
SIMULATION = "SIMULATION" in os.environ
|
||||
|
||||
@@ -1,22 +0,0 @@
|
||||
from panda import ALTERNATIVE_EXPERIENCE
|
||||
|
||||
from openpilot.common.params import Params
|
||||
|
||||
|
||||
class MadsParams:
|
||||
@staticmethod
|
||||
def read_param(key: str, params=None):
|
||||
if params is None:
|
||||
params = Params()
|
||||
|
||||
return params.get_bool(key)
|
||||
|
||||
def set_alternative_experience(self, CP, params):
|
||||
enabled = self.read_param("Mads", params)
|
||||
disengage_lateral_on_brake = self.read_param("MadsDisengageLateralOnBrake", params)
|
||||
|
||||
if enabled:
|
||||
CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS
|
||||
|
||||
if not disengage_lateral_on_brake:
|
||||
CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_LATERAL_ON_BRAKE
|
||||
@@ -1,97 +0,0 @@
|
||||
from cereal import car, log, custom
|
||||
|
||||
from openpilot.sunnypilot.mads.helpers import MadsParams
|
||||
from openpilot.sunnypilot.mads.state import StateMachine
|
||||
|
||||
State = custom.SelfdriveStateSP.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
EventName = log.OnroadEvent.EventName
|
||||
|
||||
|
||||
class ModularAssistiveDrivingSystem:
|
||||
def __init__(self, selfdrive):
|
||||
mads_params = MadsParams()
|
||||
|
||||
self.enabled = False
|
||||
self.active = False
|
||||
self.available = False
|
||||
self.allow_always = False
|
||||
self.selfdrive = selfdrive
|
||||
self.selfdrive.enabled_prev = False
|
||||
self.state_machine = StateMachine(self)
|
||||
|
||||
if self.selfdrive.CP.carName == "hyundai":
|
||||
self.allow_always = True
|
||||
|
||||
# TODO-SP: do we need to pass the params object from SelfdriveD?
|
||||
self.enabled_toggle = mads_params.read_param("Mads", self.selfdrive.params)
|
||||
self.main_enabled_toggle = mads_params.read_param("MadsCruiseMain", self.selfdrive.params)
|
||||
self.disengage_lateral_on_brake_toggle = mads_params.read_param("MadsDisengageLateralOnBrake", self.selfdrive.params)
|
||||
self.unified_engagement_mode = mads_params.read_param("MadsUnifiedEngagementMode", self.selfdrive.params)
|
||||
|
||||
def update_events(self, CS: car.CarState):
|
||||
if not self.selfdrive.enabled:
|
||||
self.selfdrive.events.remove(EventName.wrongCruiseMode)
|
||||
self.selfdrive.events.remove(EventName.wrongCarMode)
|
||||
if self.selfdrive.events.has(EventName.wrongGear) and not self.selfdrive.events.has(EventName.reverseGear):
|
||||
self.selfdrive.events.remove(EventName.wrongGear)
|
||||
self.selfdrive.events.add(EventName.silentWrongGear)
|
||||
if self.selfdrive.events.has(EventName.reverseGear) and CS.vEgo < 5:
|
||||
self.selfdrive.events.remove(EventName.reverseGear)
|
||||
self.selfdrive.events.add(EventName.silentReverseGear)
|
||||
|
||||
if self.disengage_lateral_on_brake_toggle:
|
||||
if self.selfdrive.events.has(EventName.brakeHold):
|
||||
self.selfdrive.events.remove(EventName.brakeHold)
|
||||
self.selfdrive.events.add(EventName.silentBrakeHold)
|
||||
|
||||
if self.selfdrive.events.has(EventName.pedalPressed):
|
||||
self.selfdrive.events.add(EventName.silentPedalPressed)
|
||||
|
||||
if not CS.brakePressed and not CS.brakeHoldActive and not CS.parkingBrake and not CS.regenBraking:
|
||||
if self.state_machine.state == State.paused and self.active:
|
||||
self.selfdrive.events.add(EventName.silentLkasEnable)
|
||||
|
||||
if self.selfdrive.events.has(EventName.pcmEnable) or self.selfdrive.events.has(EventName.buttonEnable):
|
||||
if (self.unified_engagement_mode and self.active) or not self.unified_engagement_mode:
|
||||
if self.selfdrive.events.has(EventName.pcmEnable):
|
||||
self.selfdrive.events.remove(EventName.pcmEnable)
|
||||
if self.selfdrive.events.has(EventName.buttonEnable):
|
||||
self.selfdrive.events.remove(EventName.buttonEnable)
|
||||
else:
|
||||
if self.main_enabled_toggle:
|
||||
if CS.cruiseState.available and not self.selfdrive.CS_prev.cruiseState.available:
|
||||
self.selfdrive.events.add(EventName.lkasEnable)
|
||||
|
||||
for be in CS.buttonEvents:
|
||||
if be.type == ButtonType.cancel:
|
||||
if self.selfdrive.enabled_prev:
|
||||
self.selfdrive.events.add(EventName.manualLongitudinalRequired)
|
||||
if be.type == ButtonType.lkas and be.pressed and (CS.cruiseState.available or self.allow_always):
|
||||
if self.active:
|
||||
if self.selfdrive.enabled:
|
||||
self.selfdrive.events.add(EventName.manualSteeringRequired)
|
||||
else:
|
||||
self.selfdrive.events.add(EventName.lkasDisable)
|
||||
else:
|
||||
self.selfdrive.events.add(EventName.lkasEnable)
|
||||
|
||||
if CS.cruiseState.available:
|
||||
self.selfdrive.events.remove(EventName.pcmDisable)
|
||||
self.selfdrive.events.remove(EventName.buttonCancel)
|
||||
elif self.selfdrive.CS_prev.cruiseState.available:
|
||||
self.selfdrive.events.add(EventName.lkasDisable)
|
||||
|
||||
self.selfdrive.events.remove(EventName.pedalPressed)
|
||||
|
||||
def update(self, CS: car.CarState):
|
||||
if not self.enabled_toggle:
|
||||
return
|
||||
|
||||
self.update_events(CS)
|
||||
|
||||
if not self.selfdrive.CP.passive and self.selfdrive.initialized:
|
||||
self.enabled, self.active = self.state_machine.update(self.selfdrive.events)
|
||||
|
||||
# Copy of previous SelfdriveD states for MADS events handling
|
||||
self.selfdrive.enabled_prev = self.selfdrive.enabled
|
||||
@@ -1,114 +0,0 @@
|
||||
from cereal import log, custom
|
||||
from openpilot.selfdrive.selfdrived.events import ET, Events
|
||||
from openpilot.selfdrive.selfdrived.state import SOFT_DISABLE_TIME
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
|
||||
State = custom.SelfdriveStateSP.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
|
||||
EventName = log.OnroadEvent.EventName
|
||||
|
||||
ENABLED_STATES = (State.enabled, State.softDisabling, State.overriding)
|
||||
ACTIVE_STATES = (State.paused, *ENABLED_STATES)
|
||||
|
||||
|
||||
class StateMachine:
|
||||
def __init__(self, mads):
|
||||
self.mads = mads
|
||||
self.selfdrive = mads.selfdrive
|
||||
self.ss_state_machine = mads.selfdrive.state_machine
|
||||
|
||||
self.state = State.disabled
|
||||
|
||||
def add_current_alert_types(self, alert_type):
|
||||
if not self.selfdrive.active:
|
||||
self.ss_state_machine.current_alert_types.append(alert_type)
|
||||
|
||||
def update(self, events: Events):
|
||||
# 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 events.contains(ET.USER_DISABLE):
|
||||
if events.has(EventName.silentPedalPressed) or events.has(EventName.silentBrakeHold) or \
|
||||
events.has(EventName.silentWrongGear) or events.has(EventName.silentReverseGear):
|
||||
self.state = State.paused
|
||||
else:
|
||||
self.state = State.disabled
|
||||
self.add_current_alert_types(ET.USER_DISABLE)
|
||||
|
||||
elif events.contains(ET.IMMEDIATE_DISABLE):
|
||||
self.state = State.disabled
|
||||
self.add_current_alert_types(ET.IMMEDIATE_DISABLE)
|
||||
|
||||
else:
|
||||
# ENABLED
|
||||
if self.state == State.enabled:
|
||||
if events.contains(ET.SOFT_DISABLE):
|
||||
self.state = State.softDisabling
|
||||
if not self.selfdrive.active:
|
||||
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 events.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 events.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 events.contains(ET.ENABLE):
|
||||
if events.contains(ET.NO_ENTRY):
|
||||
self.add_current_alert_types(ET.NO_ENTRY)
|
||||
|
||||
else:
|
||||
if events.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 events.contains(ET.SOFT_DISABLE):
|
||||
self.state = State.softDisabling
|
||||
if not self.selfdrive.active:
|
||||
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 events.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 events.contains(ET.ENABLE):
|
||||
if events.contains(ET.NO_ENTRY):
|
||||
self.add_current_alert_types(ET.NO_ENTRY)
|
||||
|
||||
else:
|
||||
if events.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 & available, 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
|
||||
@@ -1,124 +0,0 @@
|
||||
import pytest
|
||||
from unittest.mock import MagicMock
|
||||
|
||||
from 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 Events, ET, EVENTS, NormalPermanentAlert
|
||||
|
||||
State = custom.SelfdriveStateSP.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
|
||||
|
||||
# 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[0] = event
|
||||
return 0
|
||||
|
||||
|
||||
class MockMADS:
|
||||
def __init__(self):
|
||||
self.selfdrive = MagicMock()
|
||||
self.selfdrive.state_machine = MagicMock()
|
||||
self.selfdrive.active = False
|
||||
self.available = True
|
||||
|
||||
|
||||
class TestMADSStateMachine:
|
||||
@pytest.fixture(autouse=True)
|
||||
def setup_method(self):
|
||||
self.mads = MockMADS()
|
||||
self.events = Events()
|
||||
self.state_machine = StateMachine(self.mads)
|
||||
self.mads.selfdrive.state_machine.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
|
||||
|
||||
def test_immediate_disable(self):
|
||||
for state in ALL_STATES:
|
||||
for et in MAINTAIN_STATES[state]:
|
||||
self.events.add(make_event([et, ET.IMMEDIATE_DISABLE]))
|
||||
self.state_machine.state = state
|
||||
self.state_machine.update(self.events)
|
||||
assert State.disabled == self.state_machine.state
|
||||
self.events.clear()
|
||||
|
||||
def test_user_disable(self):
|
||||
for state in ALL_STATES:
|
||||
for et in MAINTAIN_STATES[state]:
|
||||
self.events.add(make_event([et, ET.USER_DISABLE]))
|
||||
self.state_machine.state = state
|
||||
self.state_machine.update(self.events)
|
||||
assert self.state_machine.state in (State.disabled, State.paused)
|
||||
self.events.clear()
|
||||
|
||||
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.add(make_event([et, ET.SOFT_DISABLE]))
|
||||
self.state_machine.state = state
|
||||
self.state_machine.update(self.events)
|
||||
assert self.state_machine.state == State.disabled if state == State.disabled else State.softDisabling
|
||||
self.events.clear()
|
||||
|
||||
def test_soft_disable_timer(self):
|
||||
self.state_machine.state = State.enabled
|
||||
self.events.add(make_event([ET.SOFT_DISABLE]))
|
||||
self.state_machine.update(self.events)
|
||||
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)
|
||||
|
||||
assert self.state_machine.state == State.disabled
|
||||
|
||||
def test_no_entry(self):
|
||||
for et in ENABLE_EVENT_TYPES:
|
||||
self.events.add(make_event([ET.NO_ENTRY, et]))
|
||||
self.state_machine.update(self.events)
|
||||
assert self.state_machine.state == State.disabled
|
||||
self.events.clear()
|
||||
|
||||
def test_no_entry_paused(self):
|
||||
self.state_machine.state = State.paused
|
||||
self.events.add(make_event([ET.NO_ENTRY]))
|
||||
self.state_machine.update(self.events)
|
||||
assert self.state_machine.state == State.paused
|
||||
|
||||
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.add(make_event([et]))
|
||||
self.state_machine.update(self.events)
|
||||
assert self.state_machine.state == state
|
||||
self.events.clear()
|
||||
|
||||
def test_override_lateral(self):
|
||||
self.state_machine.state = State.enabled
|
||||
self.events.add(make_event([ET.OVERRIDE_LATERAL]))
|
||||
self.state_machine.update(self.events)
|
||||
assert self.state_machine.state == State.overriding
|
||||
|
||||
def test_paused_to_enabled(self):
|
||||
self.state_machine.state = State.paused
|
||||
self.events.add(make_event([ET.ENABLE]))
|
||||
self.state_machine.update(self.events)
|
||||
assert self.state_machine.state == State.enabled
|
||||
|
||||
def test_mads_unavailable(self):
|
||||
self.mads.available = False
|
||||
for state in ALL_STATES:
|
||||
self.state_machine.state = state
|
||||
enabled, active = self.state_machine.update(self.events)
|
||||
assert not enabled
|
||||
assert not active
|
||||
@@ -1,38 +0,0 @@
|
||||
from abc import abstractmethod
|
||||
from collections import namedtuple
|
||||
|
||||
from opendbc.car import DT_CTRL, structs
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.interfaces import CarStateBase, CarControllerBase
|
||||
|
||||
MadsDataSP = namedtuple("MadsDataSP",
|
||||
["enabled_toggle", "lat_active", "disengaging", "paused"])
|
||||
|
||||
|
||||
class CarControllerSP(CarControllerBase):
|
||||
def __init__(self, dbc_name: str, CP: structs.CarParams):
|
||||
super().__init__(dbc_name, CP)
|
||||
self.lat_disengage_blink = 0
|
||||
self.lat_disengage_init = False
|
||||
self.prev_lat_active = False
|
||||
|
||||
# display LFA "white_wheel" and LKAS "White car + lanes" when not CC.latActive
|
||||
def mads_status_update(self, CS: CarStateBase, CC: structs.CarControl) -> MadsDataSP:
|
||||
if CC.latActive:
|
||||
self.lat_disengage_init = False
|
||||
elif self.prev_lat_active:
|
||||
self.lat_disengage_init = True
|
||||
|
||||
if not self.lat_disengage_init:
|
||||
self.lat_disengage_blink = self.frame
|
||||
|
||||
paused = CC.madsActive and not CC.latActive
|
||||
disengaging = (self.frame - self.lat_disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False
|
||||
|
||||
self.prev_lat_active = CC.latActive
|
||||
|
||||
return MadsDataSP(CS.mads_enabled_toggle, CC.latActive, disengaging, paused)
|
||||
|
||||
@abstractmethod
|
||||
def update(self, CC: structs.CarControl, CS: CarStateBase, now_nanos: int) -> tuple[structs.CarControl.Actuators, list[CanData]]:
|
||||
pass
|
||||
@@ -1,13 +0,0 @@
|
||||
from abc import ABC
|
||||
|
||||
from opendbc.car import structs
|
||||
|
||||
from openpilot.sunnypilot.mads.mads import MadsParams
|
||||
|
||||
|
||||
class CarStateBaseSP(ABC):
|
||||
def __init__(self, CP: structs.CarParams):
|
||||
self.alt_button = 0
|
||||
self.mads_enabled_toggle = MadsParams().read_param("Mads")
|
||||
|
||||
self.main_cruise_enabled = False
|
||||
Reference in New Issue
Block a user