more

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),

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,

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)

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)

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]