From 9be2f5628db27cd7aad41f86cbee88b6726abc19 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Fri, 15 Nov 2024 19:35:27 +0100 Subject: [PATCH] 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. --- opendbc_repo | 2 +- openpilot/sunnypilot | 1 - selfdrive/car/card.py | 2 +- selfdrive/selfdrived/selfdrived.py | 2 +- sunnypilot/__init__.py | 0 sunnypilot/mads/helpers.py | 22 ---- sunnypilot/mads/mads.py | 97 -------------- sunnypilot/mads/state.py | 114 ---------------- sunnypilot/mads/tests/test_state_machine.py | 124 ------------------ sunnypilot/selfdrive/__init__.py | 0 sunnypilot/selfdrive/car/__init__.py | 0 sunnypilot/selfdrive/car/hyundai/__init__.py | 0 .../selfdrive/car/hyundai/carcontroller.py | 38 ------ sunnypilot/selfdrive/car/interfaces.py | 13 -- 14 files changed, 3 insertions(+), 412 deletions(-) delete mode 120000 openpilot/sunnypilot delete mode 100644 sunnypilot/__init__.py delete mode 100644 sunnypilot/mads/helpers.py delete mode 100644 sunnypilot/mads/mads.py delete mode 100644 sunnypilot/mads/state.py delete mode 100644 sunnypilot/mads/tests/test_state_machine.py delete mode 100644 sunnypilot/selfdrive/__init__.py delete mode 100644 sunnypilot/selfdrive/car/__init__.py delete mode 100644 sunnypilot/selfdrive/car/hyundai/__init__.py delete mode 100644 sunnypilot/selfdrive/car/hyundai/carcontroller.py delete mode 100644 sunnypilot/selfdrive/car/interfaces.py diff --git a/opendbc_repo b/opendbc_repo index efd5f88dff..007a0aab21 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit efd5f88dff02c842e9d9ad869747aafe1e4fa03f +Subproject commit 007a0aab218bbd9772837a8c83af40cb22634a46 diff --git a/openpilot/sunnypilot b/openpilot/sunnypilot deleted file mode 120000 index c4ca692907..0000000000 --- a/openpilot/sunnypilot +++ /dev/null @@ -1 +0,0 @@ -../sunnypilot \ No newline at end of file diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index cfadadef39..04d0236a7a 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -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 diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 3dceee4c41..5cb0d2f4ed 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -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 diff --git a/sunnypilot/__init__.py b/sunnypilot/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/mads/helpers.py b/sunnypilot/mads/helpers.py deleted file mode 100644 index dbbdbaab06..0000000000 --- a/sunnypilot/mads/helpers.py +++ /dev/null @@ -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 diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py deleted file mode 100644 index 69946d7bbc..0000000000 --- a/sunnypilot/mads/mads.py +++ /dev/null @@ -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 diff --git a/sunnypilot/mads/state.py b/sunnypilot/mads/state.py deleted file mode 100644 index d23c888877..0000000000 --- a/sunnypilot/mads/state.py +++ /dev/null @@ -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 diff --git a/sunnypilot/mads/tests/test_state_machine.py b/sunnypilot/mads/tests/test_state_machine.py deleted file mode 100644 index fdce389905..0000000000 --- a/sunnypilot/mads/tests/test_state_machine.py +++ /dev/null @@ -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 diff --git a/sunnypilot/selfdrive/__init__.py b/sunnypilot/selfdrive/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/selfdrive/car/__init__.py b/sunnypilot/selfdrive/car/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/selfdrive/car/hyundai/__init__.py b/sunnypilot/selfdrive/car/hyundai/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/selfdrive/car/hyundai/carcontroller.py b/sunnypilot/selfdrive/car/hyundai/carcontroller.py deleted file mode 100644 index 3191a3ad02..0000000000 --- a/sunnypilot/selfdrive/car/hyundai/carcontroller.py +++ /dev/null @@ -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 diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py deleted file mode 100644 index d143460dcb..0000000000 --- a/sunnypilot/selfdrive/car/interfaces.py +++ /dev/null @@ -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