diff --git a/cereal/services.py b/cereal/services.py index e56f8f0c1b..a0f92d79af 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -73,6 +73,9 @@ _services: dict[str, tuple] = { "userFlag": (True, 0., 1), "microphone": (True, 10., 10), + # sunnypilot + "selfdriveStateSP": (True, 100., 10), + # debug "uiDebug": (True, 0., 1), "alertDebug": (True, 20., 5), diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 9ed1b454f0..ef20e8b781 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -105,6 +105,19 @@ class Events: ret.append(event) return ret + def has(self, event_name: int) -> bool: + return event_name in self.events + + def remove(self, event_name: int, static: bool = False) -> None: + if static and event_name in self.static_events: + self.static_events.remove(event_name) + + if event_name in self.events: + self.events.remove(event_name) + + if event_name in self.event_counters: + self.event_counters[event_name] = 0 + class Alert: def __init__(self, diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 5c468ab3c4..fe58bf9f1e 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -449,12 +449,26 @@ class SelfdriveD: self.pm.send('onroadEvents', ce_send) self.events_prev = self.events.names.copy() + def publish_selfdriveStateSP(self): + # selfdriveStateSP + ss_sp_msg = messaging.new_message('selfdriveStateSP') + ss_sp_msg.valid = self.sm.all_checks(['selfdriveState']) + ss_sp = ss_sp_msg.selfdriveStateSP + + # mads + mads = ss_sp.mads + mads.state = self.mads.current_state + mads.enabled = self.mads.enabled + mads.active = self.mads.active + + self.pm.send('selfdriveStateSP', ss_sp_msg) + def step(self): CS = self.data_sample() self.update_events(CS) if not self.CP.passive and self.initialized: self.enabled, self.active = self.state_machine.update(self.events) - self.mads.update(CS) + self.mads.update(CS) self.update_alerts(CS) self.publish_selfdriveState(CS) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 0d5829ec79..7149142655 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -1,7 +1,10 @@ from cereal import car, custom from openpilot.sunnypilot.mads.state import Disabled, Paused, Enabled, SoftDisabling, Overriding +from openpilot.selfdrive.selfdrived.events import Events + State = custom.SelfdriveStateSP.ModifiedAssistDrivingSystem.ModifiedAssistDrivingSystemState +ButtonType = car.CarState.ButtonEvent.Type EventName = car.OnroadEvent.EventName @@ -9,9 +12,9 @@ class ModifiedAssistDrivingSystem: def __init__(self, selfdrive): self.selfdrive = selfdrive - self.CS_prev = car.CarState.new_message() self.current_state = State.disabled - + self.enabled = False + self.active = False self.state_machine = { State.disabled: Disabled(self), State.paused: Paused(self), @@ -20,67 +23,70 @@ class ModifiedAssistDrivingSystem: State.overriding: Overriding(self), } - self.enabled = True # TODO-SP: Apply with toggle - self.main_enabled = True # TODO-SP: Apply with toggle - self.disengage_lateral_on_brake = False # TODO-SP: Apply with toggle + self.enabled_toggle = True # TODO-SP: Apply with toggle + self.main_enabled_toggle = True # TODO-SP: Apply with toggle + self.disengage_on_brake_toggle = False # TODO-SP: Apply with toggle - def update_availability(self, CS: car.CarState): - if not self.enabled: - return False + self.mads_enabled = False + self.mads_alt_button_enabled = False - if self.current_state == State.disabled: - return False + def update_availability(self, CS: car.CarState, available: bool = False) -> bool: + if self.main_enabled_toggle: + available |= CS.cruiseState.available + + available |= self.mads_alt_button_enabled + + return available def update_events(self, CS: car.CarState): - pass + self.selfdrive.events.remove(EventName.pcmEnable) + self.selfdrive.events.remove(EventName.pcmDisable) + + if self.selfdrive.enabled: + if self.selfdrive.events.has(EventName.wrongGear) and CS.vEgo < 5: + self.selfdrive.events.add(EventName.silentWrongGear) + self.selfdrive.events.remove(EventName.wrongGear) + + if not self.selfdrive.enabled: + self.selfdrive.events.remove(EventName.buttonEnable) + self.selfdrive.events.remove(EventName.buttonCancel) + self.selfdrive.events.remove(EventName.wrongCruiseMode) + self.selfdrive.events.remove(EventName.wrongCarMode) + + if self.disengage_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.remove(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.current_state == State.paused and self.mads_enabled: + self.selfdrive.events.add(EventName.silentButtonEnable) + + for be in CS.buttonEvents: + if be.type == ButtonType.cancel: + if self.selfdrive.enabled: + self.selfdrive.events.add(EventName.manualLongitudinalRequired) + if be.type == ButtonType.altButton1 and be.pressed: + if not self.mads_enabled: + if not self.selfdrive.enabled: + self.selfdrive.events.add(EventName.buttonCancel) + else: + self.selfdrive.events.add(EventName.manualSteeringRequired) + else: + if not self.selfdrive.enabled: + self.selfdrive.events.add(EventName.buttonEnable) def update(self, CS: car.CarState): + if not self.enabled_toggle: + return + self.mads_enabled = self.update_availability(CS) + self.update_events(CS) - - - - - - - - - - - - - - self.current_state = self.state_machine[self.current_state]() - - self.CS_prev = CS.as_reader() - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + if not self.selfdrive.CP.passive and self.selfdrive.initialized: + self.current_state, self.enabled, self.active = self.state_machine[self.current_state](self.selfdrive.events) diff --git a/sunnypilot/mads/state.py b/sunnypilot/mads/state.py index 079f04eddc..1d13ee5632 100644 --- a/sunnypilot/mads/state.py +++ b/sunnypilot/mads/state.py @@ -1,61 +1,68 @@ from abc import ABC, abstractmethod from cereal import car, custom -from openpilot.selfdrive.selfdrived.events import ET -from openpilot.selfdrive.selfdrived.state import ACTIVE_STATES, SOFT_DISABLE_TIME +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.ModifiedAssistDrivingSystem.ModifiedAssistDrivingSystemState EventName = car.OnroadEvent.EventName +ENABLED_STATES = (State.enabled, State.softDisabling, State.overriding) +ACTIVE_STATES = (State.paused, *ENABLED_STATES) + class StateMachineBase(ABC): def __init__(self, mads): self.mads = mads self.selfdrive = mads.selfdrive - self.ss_state_machine = mads.ss_state_machine + self.ss_state_machine = mads.selfdrive.ss_state_machine self.state = State.disabled - def __call__(self) -> State: + def __call__(self, events: Events) -> State: if self.state != State.disabled: - if self.selfdrive.events.contains(ET.USER_DISABLE): - if EventName.silentPedalPressed in self.selfdrive.events.events: + if events.contains(ET.USER_DISABLE): + if events.has(EventName.silentPedalPressed) or events.has(EventName.silentBrakeHold): self.state = State.paused else: self.state = State.disabled self.add_current_alert_types(ET.USER_DISABLE) - return self.state - elif self.selfdrive.events.contains(ET.IMMEDIATE_DISABLE): + elif events.contains(ET.IMMEDIATE_DISABLE): self.state = State.disabled self.add_current_alert_types(ET.IMMEDIATE_DISABLE) - return self.state - self.handle() + else: + self.handle(events) - if self.state in ACTIVE_STATES: + elif self.state == State.disabled: + self.handle(events) + + enabled = self.state in ENABLED_STATES and self.mads.mads_enabled + active = self.state in ACTIVE_STATES and self.mads.mads_enabled + if active: self.add_current_alert_types(ET.WARNING) - return self.state + return self.state, enabled, active def add_current_alert_types(self, alert_type): if not self.selfdrive.active: self.add_current_alert_types(alert_type) @abstractmethod - def handle(self): + def handle(self, events: Events): pass class InactiveBase(StateMachineBase): - def handle(self): - if self.selfdrive.events.contain(ET.ENABLE): - if self.selfdrive.events.contain(ET.NO_ENTRY): + def handle(self, events: Events): + if events.contains(ET.ENABLE): + if events.contains(ET.NO_ENTRY): self.add_current_alert_types(ET.NO_ENTRY) else: - if self.selfdrive.events.contain(ET.OVERRIDE_LATERAL): + if events.contains(ET.OVERRIDE_LATERAL): self.state = State.overriding else: self.state = State.enabled @@ -71,21 +78,21 @@ class Paused(InactiveBase): class Enabled(StateMachineBase): - def handle(self): - if self.selfdrive.events.contain(ET.SOFT_DISABLE): + def handle(self, events: Events): + 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.add_current_alert_types(ET.SOFT_DISABLE) - elif self.selfdrive.events.contains(ET.OVERRIDE_LATERAL): + elif events.contains(ET.OVERRIDE_LATERAL): self.state = State.overriding self.add_current_alert_types(ET.OVERRIDE_LATERAL) class SoftDisabling(StateMachineBase): - def handle(self): - if not self.selfdrive.events.contain(ET.SOFT_DISABLE): + def handle(self, events: Events): + if not events.contains(ET.SOFT_DISABLE): # no more soft disabling condition, so go back to ENABLED self.state = State.enabled @@ -97,13 +104,13 @@ class SoftDisabling(StateMachineBase): class Overriding(StateMachineBase): - def handle(self): - if self.selfdrive.events.contain(ET.SOFT_DISABLE): + def handle(self, events: Events): + 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.add_current_alert_types(ET.SOFT_DISABLE) - elif not self.selfdrive.events.contain(ET.OVERRIDE_LATERAL): + elif not events.contains(ET.OVERRIDE_LATERAL): self.state = State.enabled else: self.ss_state_machine.current_alert_types += [ET.OVERRIDE_LATERAL]