fix

cereal/car.capnp

@@ -519,7 +519,7 @@ struct CarParams {
     safetyModel @0 :SafetyModel;
     safetyParam @3 :UInt16;
     safetyParamDEPRECATED @1 :Int16;
-    safetyParam2DEPRECATED @2 :UInt32;
+    spFlags @2 :UInt32;  # flags for car specific quirks in sunnypilot
   }
 
   struct LateralParams {

selfdrive/car/card.py

@@ -155,6 +155,10 @@ class Car:
     # card is driven by can recv, expected at 100Hz
     self.rk = Ratekeeper(100, print_delay_threshold=None)
 
+    data_services = list(self.sm.data.keys()) + ['selfdriveStateSP']
+    self.sm = messaging.SubMaster(data_services, poll='selfdriveStateSP')
+    self.mads_enabled_toggle = True  # TODO-SP: Apply with toggle
+
   def state_update(self) -> tuple[car.CarState, structs.RadarData | None]:
     """carState update loop, driven by can"""
 
@@ -251,6 +255,11 @@ class Car:
       # signal pandad to switch to car safety mode
       self.params.put_bool_nonblocking("ControlsReady", True)
 
+    if self.mads_enabled_toggle:
+      self.CI.mads_enabled = self.sm['selfdriveStateSP'].mads.enabled
+    else:
+      self.CI.mads_enabled = self.sm['carControl'].enabled
+
     if self.sm.all_alive(['carControl']):
       # send car controls over can
       now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)

selfdrive/pandad/panda.cc

@@ -137,8 +137,8 @@ void Panda::enable_deepsleep() {
   handle->control_write(0xfb, 0, 0);
 }
 
-void Panda::send_heartbeat(bool engaged) {
-  handle->control_write(0xf3, engaged, 0);
+void Panda::send_heartbeat(bool engaged, bool mads_engaged) {
+  handle->control_write(0xf3, engaged, mads_engaged);
 }
 
 void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) {

selfdrive/pandad/panda.h

@@ -75,7 +75,7 @@ public:
   std::optional<std::string> get_serial();
   void set_power_saving(bool power_saving);
   void enable_deepsleep();
-  void send_heartbeat(bool engaged);
+  void send_heartbeat(bool engaged, bool mads_engaged);
   void set_can_speed_kbps(uint16_t bus, uint16_t speed);
   void set_data_speed_kbps(uint16_t bus, uint16_t speed);
   void set_canfd_non_iso(uint16_t bus, bool non_iso);

selfdrive/pandad/pandad.cc

@@ -313,7 +313,7 @@ void send_peripheral_state(Panda *panda, PubMaster *pm) {
 }
 
 void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool spoofing_started) {
-  static SubMaster sm({"selfdriveState"});
+  static SubMaster sm({"selfdriveState", "selfdriveStateSP"});
 
   std::vector<std::string> connected_serials;
   for (Panda *p : pandas) {
@@ -352,8 +352,9 @@ void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool spoof
 
     sm.update(0);
     const bool engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled();
+    const bool mads_engaged = sm.allAliveAndValid({"selfdriveStateSP"}) && sm["selfdriveStateSP"].getSelfdriveStateSP().getMads().getEnabled();
     for (const auto &panda : pandas) {
-      panda->send_heartbeat(engaged);
+      panda->send_heartbeat(engaged, mads_engaged);
     }
   }
 }

sunnypilot/mads/mads.py

@@ -28,14 +28,13 @@ class ModifiedAssistDrivingSystem:
     self.main_enabled_toggle = True  # TODO-SP: Apply with toggle
     self.disengage_lateral_on_brake_toggle = False  # TODO-SP: Apply with toggle
 
-    self.mads_enabled = False
+    self.available = False
     self.mads_alt_button_enabled = False
 
   def set_alternative_experience(self, alt_experience: int = 0):
     if self.enabled_toggle:
-      if self.disengage_lateral_on_brake_toggle:
-        alt_experience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS
-      else:
+      alt_experience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS
+      if not self.disengage_lateral_on_brake_toggle:
         alt_experience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_LATERAL_ON_BRAKE
 
     return alt_experience
@@ -73,7 +72,7 @@ class ModifiedAssistDrivingSystem:
         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:
+        if self.current_state == State.paused and self.available:
           self.selfdrive.events.add(EventName.silentButtonEnable)
 
     for be in CS.buttonEvents:
@@ -81,7 +80,7 @@ class ModifiedAssistDrivingSystem:
         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.available:
           if not self.selfdrive.enabled:
             self.selfdrive.events.add(EventName.buttonCancel)
           else:
@@ -94,7 +93,7 @@ class ModifiedAssistDrivingSystem:
     if not self.enabled_toggle:
       return
 
-    self.mads_enabled = self.update_availability(CS)
+    self.available = self.update_availability(CS)
 
     self.update_events(CS)
 

sunnypilot/mads/state.py

@@ -39,8 +39,8 @@ class StateMachineBase(ABC):
     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
+    enabled = self.state in ENABLED_STATES and self.mads.available
+    active = self.state in ACTIVE_STATES and self.mads.available
     if active:
       self.add_current_alert_types(ET.WARNING)
 

sunnypilot/selfdrive/car/hyundai/carcontroller.py

@@ -0,0 +1,38 @@
+from collections import namedtuple
+
+from opendbc.car import DT_CTRL, structs
+from opendbc.car.interfaces import CarStateBase
+
+MadsDataSP = namedtuple("MadsDataSP",
+                        ["enabled_toggle", "lat_active", "disengaging", "lfa_icon"])
+
+
+class CarControllerSP:
+  def __init__(self, car_controller):
+    self.CC = car_controller
+
+    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.CC.frame
+
+    paused = CS.mads_enabled and not CC.latActive
+    disengaging = (self.CC.frame - self.lat_disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False
+
+    if CS.mads_enabled_toggle:
+      lfa_icon = 2 if CC.latActive else 3 if disengaging else 1 if paused else 0
+    else:
+      lfa_icon = 2 if CC.enabled else 0
+
+    self.prev_lat_active = CC.latActive
+
+    return MadsDataSP(CS.mads_enabled_toggle, CC.latActive, disengaging, lfa_icon)

sunnypilot/selfdrive/car/interfaces.py

@@ -0,0 +1,64 @@
+from abc import ABC
+
+from opendbc.car import structs
+
+ButtonType = structs.CarState.ButtonEvent.Type
+
+
+class CarInterfaceBaseSP(ABC):
+  def __init__(self, CP: structs.CarParams, CarController, CarState):
+    self.CP = CP
+
+    self.CS: CarStateBaseSP = CarState(CP)
+    self.cp = self.CS.get_can_parser(CP)
+    self.cp_cam = self.CS.get_cam_can_parser(CP)
+    self.cp_adas = self.CS.get_adas_can_parser(CP)
+    self.cp_body = self.CS.get_body_can_parser(CP)
+    self.cp_loopback = self.CS.get_loopback_can_parser(CP)
+    self.can_parsers = (self.cp, self.cp_cam, self.cp_adas, self.cp_body, self.cp_loopback)
+
+    dbc_name = "" if self.cp is None else self.cp.dbc_name
+    self.CC: CarControllerBaseSP = CarController(dbc_name, CP)
+
+    self.mads_enabled_toggle = False
+    self.mads_enabled = False
+
+  def update_prevs(self):
+    self.CS.mads_enabled_toggle = self.mads_enabled_toggle
+    self.CS.mads_enabled = self.mads_enabled
+
+
+class CarStateBaseSP(ABC):
+  def __init__(self, CP: structs.CarParams):
+    self.CP = CP
+
+    self.alt_button = 0
+    self.mads_enabled_toggle = False
+    self.mads_enabled = False
+    self.button_events: list[structs.CarState.ButtonEvent] = []
+
+  @staticmethod
+  def get_can_parser(CP):
+    return None
+
+  @staticmethod
+  def get_cam_can_parser(CP):
+    return None
+
+  @staticmethod
+  def get_adas_can_parser(CP):
+    return None
+
+  @staticmethod
+  def get_body_can_parser(CP):
+    return None
+
+  @staticmethod
+  def get_loopback_can_parser(CP):
+    return None
+
+
+class CarControllerBaseSP(ABC):
+  def __init__(self, dbc_name: str, CP: structs.CarParams):
+    self.CP = CP
+    self.frame = 0

tools/cabana/streams/pandastream.cc

@@ -72,7 +72,7 @@ void PandaStream::streamThread() {
 
     handleEvent(capnp::messageToFlatArray(msg));
 
-    panda->send_heartbeat(false);
+    panda->send_heartbeat(false, false);
   }
 }