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