always check

# Conflicts:
#	opendbc_repo
#	panda

cereal/custom.capnp

@@ -8,7 +8,23 @@ $Cxx.namespace("cereal");
 # cereal, so use these if you want custom events in your fork.
 
 # you can rename the struct, but don't change the identifier
-struct CustomReserved0 @0x81c2f05a394cf4af {
+struct SelfdriveStateSP @0x81c2f05a394cf4af {
+  mads @0 :ModularAssistiveDrivingSystem;
+
+  struct ModularAssistiveDrivingSystem {
+    state @0 :ModularAssistiveDrivingSystemState;
+    enabled @1 :Bool;
+    active @2 :Bool;
+    available @3 :Bool;
+
+    enum ModularAssistiveDrivingSystemState {
+      disabled @0;
+      paused @1;
+      enabled @2;
+      softDisabling @3;
+      overriding @4;
+    }
+  }
 }
 
 struct CustomReserved1 @0xaedffd8f31e7b55d {

cereal/log.capnp

@@ -125,6 +125,76 @@ struct OnroadEvent @0xc4fa6047f024e718 {
     espActive @90;
     personalityChanged @91;
     aeb @92;
+    eventReserved93 @93;
+    eventReserved94 @94;
+    eventReserved95 @95;
+    eventReserved96 @96;
+    eventReserved97 @97;
+    eventReserved98 @98;
+    eventReserved99 @99;
+    eventReserved100 @100;
+    eventReserved101 @101;
+    eventReserved102 @102;
+    eventReserved103 @103;
+    eventReserved104 @104;
+    eventReserved105 @105;
+    eventReserved106 @106;
+    eventReserved107 @107;
+    eventReserved108 @108;
+    eventReserved109 @109;
+    eventReserved110 @110;
+    eventReserved111 @111;
+    eventReserved112 @112;
+    eventReserved113 @113;
+    eventReserved114 @114;
+    eventReserved115 @115;
+    eventReserved116 @116;
+    eventReserved117 @117;
+    eventReserved118 @118;
+    eventReserved119 @119;
+    eventReserved120 @120;
+    eventReserved121 @121;
+    eventReserved122 @122;
+    eventReserved123 @123;
+    eventReserved124 @124;
+    eventReserved125 @125;
+    eventReserved126 @126;
+    eventReserved127 @127;
+    eventReserved128 @128;
+    eventReserved129 @129;
+    eventReserved130 @130;
+    eventReserved131 @131;
+    eventReserved132 @132;
+    eventReserved133 @133;
+    eventReserved134 @134;
+    eventReserved135 @135;
+    eventReserved136 @136;
+    eventReserved137 @137;
+    eventReserved138 @138;
+    eventReserved139 @139;
+    eventReserved140 @140;
+    eventReserved141 @141;
+    eventReserved142 @142;
+    eventReserved143 @143;
+    eventReserved144 @144;
+    eventReserved145 @145;
+    eventReserved146 @146;
+    eventReserved147 @147;
+    eventReserved148 @148;
+    eventReserved149 @149;
+    eventReserved150 @150;
+
+    # sunnypilot
+    lkasEnable @151;
+    lkasDisable @152;
+    manualSteeringRequired @153;
+    manualLongitudinalRequired @154;
+    silentPedalPressed @155;
+    silentLkasEnable @156;
+    silentLkasDisable @157;
+    silentBrakeHold @158;
+    silentWrongGear @159;
+    silentReverseGear @160;
 
     soundsUnavailableDEPRECATED @47;
   }
@@ -586,6 +656,7 @@ struct PandaState @0xa7649e2575e4591e {
 
   # safety stuff
   controlsAllowed @3 :Bool;
+  controlsAllowedLat @5 :Bool;
   safetyRxInvalid @19 :UInt32;
   safetyTxBlocked @24 :UInt32;
   safetyModel @14 :Car.CarParams.SafetyModel;
@@ -693,7 +764,6 @@ struct PandaState @0xa7649e2575e4591e {
   }
 
   gasInterceptorDetectedDEPRECATED @4 :Bool;
-  startedSignalDetectedDEPRECATED @5 :Bool;
   hasGpsDEPRECATED @6 :Bool;
   gmlanSendErrsDEPRECATED @9 :UInt32;
   fanSpeedRpmDEPRECATED @11 :UInt16;
@@ -2544,7 +2614,7 @@ struct Event {
     customReservedRawData2 @126 :Data;
 
     # *********** Custom: reserved for forks ***********
-    customReserved0 @107 :Custom.CustomReserved0;
+    selfdriveStateSP @107 :Custom.SelfdriveStateSP;
     customReserved1 @108 :Custom.CustomReserved1;
     customReserved2 @109 :Custom.CustomReserved2;
     customReserved3 @110 :Custom.CustomReserved3;

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),
   "testJoystick": (True, 0.),

common/params.cc

@@ -201,6 +201,12 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START},
     {"UpdaterLastFetchTime", PERSISTENT},
     {"Version", PERSISTENT},
+
+    // sunnypilot params
+    {"Mads", PERSISTENT},
+    {"MadsCruiseMain", PERSISTENT},
+    {"MadsDisengageLateralOnBrake", PERSISTENT},
+    {"MadsUnifiedEngagementMode", PERSISTENT},
 };
 
 } // namespace

openpilot/sunnypilot

@@ -0,0 +1 @@
+../sunnypilot

pyproject.toml

@@ -162,6 +162,7 @@ testpaths = [
   "tools/replay",
   "tools/cabana",
   "cereal/messaging/tests",
+  "sunnypilot",
 ]
 
 [tool.codespell]

selfdrive/car/card.py

@@ -22,6 +22,8 @@ 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
+
 REPLAY = "REPLAY" in os.environ
 
 EventName = log.OnroadEvent.EventName
@@ -113,6 +115,11 @@ class Car:
     if not disengage_on_accelerator:
       self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
 
+    # mads
+    data_services = list(self.sm.data.keys()) + ['selfdriveStateSP']
+    self.sm = messaging.SubMaster(data_services, poll='selfdriveStateSP')
+    MadsParams().set_alternative_experience(self.CP)
+
     openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle")
 
     controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly

selfdrive/controls/controlsd.py

@@ -1,6 +1,8 @@
 #!/usr/bin/env python3
 import math
 from typing import SupportsFloat
+import time
+import threading
 
 from cereal import car, log
 import cereal.messaging as messaging
@@ -19,6 +21,7 @@ from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 
+from opendbc.sunnypilot import SunnypilotParamFlags
 
 State = log.SelfdriveState.OpenpilotState
 LaneChangeState = log.LaneChangeState
@@ -56,6 +59,11 @@ class Controls:
     elif self.CP.lateralTuning.which() == 'torque':
       self.LaC = LatControlTorque(self.CP, self.CI)
 
+    data_services = list(self.sm.data.keys()) + ['selfdriveStateSP']
+    self.sm = messaging.SubMaster(data_services, poll='selfdriveState')
+
+    self.enable_mads = self.params.get_bool("Mads")
+
   def update(self):
     self.sm.update(15)
     if self.sm.updated["liveCalibration"]:
@@ -88,7 +96,12 @@ class Controls:
 
     # Check which actuators can be enabled
     standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
-    CC.latActive = self.sm['selfdriveState'].active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and not standstill
+
+    ss_sp = self.sm['selfdriveStateSP']
+    CC.madsEnabled = ss_sp.mads.enabled
+
+    _lat_active = ss_sp.mads.active if ss_sp.mads.available else self.sm['selfdriveState'].active
+    CC.latActive = _lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and not standstill
     CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl
 
     actuators = CC.actuators
@@ -157,6 +170,9 @@ class Controls:
       hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture
       hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture
 
+    if self.enable_mads:
+      CC.sunnypilotParams |= SunnypilotParamFlags.ENABLE_MADS.value
+
     if self.sm['selfdriveState'].active:
       CO = self.sm['carOutput']
       if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
@@ -203,13 +219,25 @@ class Controls:
     cc_send.carControl = CC
     self.pm.send('carControl', cc_send)
 
+  def params_thread(self, evt):
+    while not evt.is_set():
+      self.enable_mads = self.params.get_bool("Mads")
+      time.sleep(0.1)
+
   def run(self):
+    e = threading.Event()
+    t = threading.Thread(target=self.params_thread, args=(e, ))
     rk = Ratekeeper(100, print_delay_threshold=None)
-    while True:
-      self.update()
-      CC, lac_log = self.state_control()
-      self.publish(CC, lac_log)
-      rk.monitor_time()
+    try:
+      t.start()
+      while True:
+        self.update()
+        CC, lac_log = self.state_control()
+        self.publish(CC, lac_log)
+        rk.monitor_time()
+    finally:
+      e.set()
+      t.join()
 
 def main():
   config_realtime_process(4, Priority.CTRL_HIGH)

selfdrive/pandad/pandad.cc

@@ -144,6 +144,7 @@ void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::Panda
   ps.setIgnitionLine(health.ignition_line_pkt);
   ps.setIgnitionCan(health.ignition_can_pkt);
   ps.setControlsAllowed(health.controls_allowed_pkt);
+  ps.setControlsAllowedLat(health.controls_allowed_lat_pkt);
   ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt);
   ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt);
   ps.setPandaType(hw_type);

selfdrive/selfdrived/events.py

@@ -105,6 +105,24 @@ class Events:
       ret.append(event)
     return ret
 
+  def has(self, event_name: int) -> bool:
+    return event_name in self.events
+
+  def has_list(self, events_list: list[int]) -> bool:
+    return all(event_name in self.events for event_name in events_list)
+
+  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.event_counters[event_name] = self.event_counters[event_name] + 1
+      self.events.remove(event_name)
+
+  def replace(self, prev_event_name: int, cur_event_name: int, static: bool = False) -> None:
+    self.remove(prev_event_name, static)
+    self.add(cur_event_name, static)
+
 
 class Alert:
   def __init__(self,
@@ -951,6 +969,73 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.WARNING: personality_changed_alert,
   },
 
+  # sunnypilot
+  EventName.lkasEnable: {
+    ET.ENABLE: EngagementAlert(AudibleAlert.engage),
+  },
+
+  EventName.lkasDisable: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
+  },
+
+  EventName.manualSteeringRequired: {
+    ET.USER_DISABLE: Alert(
+      "Automatic Lane Centering is OFF",
+      "Manual Steering Required",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.disengage, 1.),
+  },
+
+  EventName.manualLongitudinalRequired: {
+    ET.WARNING: Alert(
+      "Smart/Adaptive Cruise Control: OFF",
+      "Manual Speed Control Required",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
+  },
+
+  # TODO-SP: remove prior merging
+  EventName.silentPedalPressed: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.none),
+    ET.NO_ENTRY: NoEntryAlert("Pedal Pressed During Attempt",
+                              visual_alert=VisualAlert.brakePressed),
+  },
+
+  EventName.silentLkasEnable: {
+    ET.ENABLE: EngagementAlert(AudibleAlert.none),
+  },
+
+  EventName.silentLkasDisable: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.none),
+  },
+
+  EventName.silentBrakeHold: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.none),
+    ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"),
+  },
+
+  EventName.silentWrongGear: {
+    ET.WARNING: Alert(
+      "Gear not D",
+      "openpilot Unavailable",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 0.),
+    ET.NO_ENTRY: Alert(
+      "Gear not D",
+      "openpilot Unavailable",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 0.),
+  },
+
+  EventName.silentReverseGear: {
+    ET.PERMANENT: Alert(
+      "Reverse\nGear",
+      "",
+      AlertStatus.normal, AlertSize.full,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
+    ET.NO_ENTRY: NoEntryAlert("Reverse Gear"),
+  },
+
 }
 
 

selfdrive/selfdrived/selfdrived.py

@@ -23,6 +23,8 @@ 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
+
 REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
 TESTING_CLOSET = "TESTING_CLOSET" in os.environ
@@ -131,6 +133,10 @@ class SelfdriveD:
     elif self.CP.passive:
       self.events.add(EventName.dashcamMode, static=True)
 
+    self.mads = ModularAssistiveDrivingSystem(self)
+    sock_services = list(self.pm.sock.keys()) + ['selfdriveStateSP']
+    self.pm = messaging.PubMaster(sock_services)
+
   def update_events(self, CS):
     """Compute onroadEvents from carState"""
 
@@ -451,11 +457,25 @@ class SelfdriveD:
       self.pm.send('onroadEvents', ce_send)
     self.events_prev = self.events.names.copy()
 
+    # selfdriveStateSP
+    ss_sp_msg = messaging.new_message('selfdriveStateSP')
+    ss_sp_msg.valid = True
+    ss_sp = ss_sp_msg.selfdriveStateSP
+    mads = ss_sp.mads
+    mads.state = self.mads.state_machine.state
+    mads.enabled = self.mads.enabled
+    mads.active = self.mads.active
+    mads.available = self.mads.enabled_toggle
+
+    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)
+    if not self.CP.notCar:
+      self.mads.update(CS)
     self.update_alerts(CS)
 
     self.publish_selfdriveState(CS)

selfdrive/selfdrived/tests/test_alerts.py

@@ -41,7 +41,7 @@ class TestAlerts:
     events = log.OnroadEvent.EventName.schema.enumerants
 
     for name, e in events.items():
-      if not name.endswith("DEPRECATED"):
+      if not name.endswith("DEPRECATED") and not name.startswith("eventReserved"):
         fail_msg = "%s @%d not in EVENTS" % (name, e)
         assert e in EVENTS.keys(), fail_msg
 

selfdrive/ui/qt/offroad/developer_panel.cc

@@ -24,6 +24,20 @@ DeveloperPanel::DeveloperPanel(SettingsWindow *parent) : ListWidget(parent) {
   });
   addItem(longManeuverToggle);
 
+  // FIXME-SP: Move to sunnypilot panels before merging
+  auto madsToggle = new ParamControl("Mads", tr("Modular Assistive Driving System (MADS)"), "", "");
+  addItem(madsToggle);
+
+  // TODO-SP: Rename toggle
+  auto madsCruiseMainToggle = new ParamControl("MadsCruiseMain", tr("MADS: Cruise Main"), "", "");
+  addItem(madsCruiseMainToggle);
+
+  auto madsDisengageLateralOnBrakeToggle = new ParamControl("MadsDisengageLateralOnBrake", tr("MADS: Disengage Lateral on Brake"), "", "");
+  addItem(madsDisengageLateralOnBrakeToggle);
+
+  auto madsUnifiedEngagementModeToggle = new ParamControl("MadsUnifiedEngagementMode", tr("MADS: Unified Engagement Mode"), "", "");
+  addItem(madsUnifiedEngagementModeToggle);
+
   // Joystick and longitudinal maneuvers should be hidden on release branches
   is_release = params.getBool("IsReleaseBranch");
 

selfdrive/ui/translations/main_ar.ts

@@ -123,6 +123,22 @@
         <source>Longitudinal Maneuver Mode</source>
         <translation>وضع المناورة الطولية</translation>
     </message>
+    <message>
+        <source>MADS: Cruise Main</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Disengage Lateral on Brake</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Modular Assistive Driving System (MADS)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Unified Engagement Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DevicePanel</name>

selfdrive/ui/translations/main_de.ts

@@ -123,6 +123,22 @@
         <source>Longitudinal Maneuver Mode</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>MADS: Cruise Main</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Disengage Lateral on Brake</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Modular Assistive Driving System (MADS)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Unified Engagement Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DevicePanel</name>

selfdrive/ui/translations/main_es.ts

@@ -123,6 +123,22 @@
         <source>Longitudinal Maneuver Mode</source>
         <translation>Modo de maniobra longitudinal</translation>
     </message>
+    <message>
+        <source>MADS: Cruise Main</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Disengage Lateral on Brake</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Modular Assistive Driving System (MADS)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Unified Engagement Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DevicePanel</name>

selfdrive/ui/translations/main_fr.ts

@@ -123,6 +123,22 @@
         <source>Longitudinal Maneuver Mode</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>MADS: Cruise Main</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Disengage Lateral on Brake</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Modular Assistive Driving System (MADS)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Unified Engagement Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DevicePanel</name>

selfdrive/ui/translations/main_ja.ts

@@ -123,6 +123,22 @@
         <source>Longitudinal Maneuver Mode</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>MADS: Cruise Main</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Disengage Lateral on Brake</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Modular Assistive Driving System (MADS)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Unified Engagement Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DevicePanel</name>

selfdrive/ui/translations/main_ko.ts

@@ -123,6 +123,22 @@
         <source>Longitudinal Maneuver Mode</source>
         <translation>롱컨 기동 모드</translation>
     </message>
+    <message>
+        <source>MADS: Cruise Main</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Disengage Lateral on Brake</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Modular Assistive Driving System (MADS)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Unified Engagement Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DevicePanel</name>

selfdrive/ui/translations/main_pt-BR.ts

@@ -123,6 +123,22 @@
         <source>Longitudinal Maneuver Mode</source>
         <translation>Modo Longitudinal Maneuver</translation>
     </message>
+    <message>
+        <source>MADS: Cruise Main</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Disengage Lateral on Brake</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Modular Assistive Driving System (MADS)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Unified Engagement Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DevicePanel</name>

selfdrive/ui/translations/main_th.ts

@@ -123,6 +123,22 @@
         <source>Longitudinal Maneuver Mode</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>MADS: Cruise Main</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Disengage Lateral on Brake</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Modular Assistive Driving System (MADS)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Unified Engagement Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DevicePanel</name>

selfdrive/ui/translations/main_tr.ts

@@ -123,6 +123,22 @@
         <source>Longitudinal Maneuver Mode</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>MADS: Cruise Main</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Disengage Lateral on Brake</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Modular Assistive Driving System (MADS)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Unified Engagement Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DevicePanel</name>

selfdrive/ui/translations/main_zh-CHS.ts

@@ -123,6 +123,22 @@
         <source>Longitudinal Maneuver Mode</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>MADS: Cruise Main</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Disengage Lateral on Brake</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Modular Assistive Driving System (MADS)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Unified Engagement Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DevicePanel</name>

selfdrive/ui/translations/main_zh-CHT.ts

@@ -123,6 +123,22 @@
         <source>Longitudinal Maneuver Mode</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>MADS: Cruise Main</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Disengage Lateral on Brake</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Modular Assistive Driving System (MADS)</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>MADS: Unified Engagement Mode</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>DevicePanel</name>

selfdrive/ui/ui.cc

@@ -68,13 +68,26 @@ void ui_update_params(UIState *s) {
 }
 
 void UIState::updateStatus() {
-  if (scene.started && sm->updated("selfdriveState")) {
+  if (scene.started && (sm->updated("selfdriveState") || sm->updated("selfdriveStateSP"))) {
     auto ss = (*sm)["selfdriveState"].getSelfdriveState();
+    auto ss_sp = (*sm)["selfdriveStateSP"].getSelfdriveStateSP();
+    auto mads = ss_sp.getMads();
     auto state = ss.getState();
-    if (state == cereal::SelfdriveState::OpenpilotState::PRE_ENABLED || state == cereal::SelfdriveState::OpenpilotState::OVERRIDING) {
+    auto state_mads = mads.getState();
+    if (state == cereal::SelfdriveState::OpenpilotState::PRE_ENABLED || state == cereal::SelfdriveState::OpenpilotState::OVERRIDING ||
+        state_mads == cereal::SelfdriveStateSP::ModularAssistiveDrivingSystem::ModularAssistiveDrivingSystemState::PAUSED ||
+        state_mads == cereal::SelfdriveStateSP::ModularAssistiveDrivingSystem::ModularAssistiveDrivingSystemState::OVERRIDING) {
       status = STATUS_OVERRIDE;
     } else {
-      status = ss.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED;
+      if (mads.getAvailable()) {
+        if (mads.getEnabled()) {
+          status = ss.getEnabled() ? STATUS_ENGAGED : STATUS_LAT_ONLY;
+        } else {
+          status = STATUS_DISENGAGED;
+        }
+      } else {
+        status = ss.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED;
+      }
     }
   }
 
@@ -94,6 +107,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
     "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
     "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
     "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
+    "selfdriveStateSP",
   });
   prime_state = new PrimeState(this);
   language = QString::fromStdString(Params().get("LanguageSetting"));

selfdrive/ui/ui.h

@@ -42,12 +42,16 @@ typedef enum UIStatus {
   STATUS_DISENGAGED,
   STATUS_OVERRIDE,
   STATUS_ENGAGED,
+  STATUS_LAT_ONLY,
+  STATUS_LONG_ONLY,
 } UIStatus;
 
 const QColor bg_colors [] = {
   [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8),
   [STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1),
   [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1),
+  [STATUS_LAT_ONLY] = QColor(0x00, 0xc8, 0xc8, 0xf1),
+  [STATUS_LONG_ONLY] = QColor(0x96, 0x1C, 0xA8, 0xf1),
 };
 
 typedef struct UIScene {

sunnypilot/mads/helpers.py

@@ -0,0 +1,21 @@
+from panda import ALTERNATIVE_EXPERIENCE
+
+from openpilot.common.params import Params
+
+
+class MadsParams:
+  def __init__(self):
+    self.params = Params()
+
+  def read_param(self, key: str):
+    return self.params.get_bool(key)
+
+  def set_alternative_experience(self, CP):
+    enabled = self.read_param("Mads")
+    disengage_lateral_on_brake = self.read_param("MadsDisengageLateralOnBrake")
+
+    if enabled:
+      CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS
+
+      if not disengage_lateral_on_brake:
+        CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_LATERAL_ON_BRAKE

sunnypilot/mads/mads.py

@@ -0,0 +1,108 @@
+from cereal import car, log, custom
+
+from opendbc.car.hyundai.values import HyundaiFlags
+from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
+
+from openpilot.sunnypilot.mads.helpers import MadsParams
+from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT
+
+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)
+    self.events = self.selfdrive.events
+
+    if self.selfdrive.CP.carName == "hyundai":
+      if (self.selfdrive.CP.sunnypilotFlags & HyundaiFlagsSP.HAS_LFA_BUTTON) or \
+            (self.selfdrive.CP.flags & HyundaiFlags.CANFD):
+        self.allow_always = True
+
+    self.enabled_toggle = mads_params.read_param("Mads")
+    self.main_enabled_toggle = mads_params.read_param("MadsCruiseMain")
+    self.disengage_lateral_on_brake_toggle = mads_params.read_param("MadsDisengageLateralOnBrake")
+    self.unified_engagement_mode = mads_params.read_param("MadsUnifiedEngagementMode")
+
+  def update_events(self, CS: car.CarState):
+    def update_unified_engagement_mode():
+      if (self.unified_engagement_mode and self.enabled) or not self.unified_engagement_mode:
+        self.events.remove(EventName.pcmEnable)
+        self.events.remove(EventName.buttonEnable)
+
+    def transition_paused_state():
+      if self.state_machine.state != State.paused:
+        self.events.add(EventName.silentLkasDisable)
+
+    if not self.selfdrive.enabled and self.enabled:
+      if self.events.has(EventName.wrongGear) and not self.events.has(EventName.reverseGear):
+        self.events.replace(EventName.wrongGear, EventName.silentWrongGear)
+        transition_paused_state()
+      if self.events.has(EventName.reverseGear) and CS.vEgo < 5:
+        self.events.replace(EventName.reverseGear, EventName.silentReverseGear)
+        transition_paused_state()
+      if self.events.has(EventName.brakeHold):
+        self.events.replace(EventName.brakeHold, EventName.silentBrakeHold)
+        transition_paused_state()
+
+      if self.disengage_lateral_on_brake_toggle:
+        if self.events.has(EventName.pedalPressed):
+          transition_paused_state()
+
+      if not (self.disengage_lateral_on_brake_toggle and self.events.has(EventName.pedalPressed)) and \
+         not self.events.has_list(GEARS_ALLOW_PAUSED_SILENT):
+        if self.state_machine.state == State.paused:
+          self.events.add(EventName.silentLkasEnable)
+
+    if self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable):
+      update_unified_engagement_mode()
+    else:
+      if self.main_enabled_toggle:
+        if CS.cruiseState.available and not self.selfdrive.CS_prev.cruiseState.available:
+          self.events.add(EventName.lkasEnable)
+
+    for be in CS.buttonEvents:
+      if be.type == ButtonType.cancel:
+        if not self.selfdrive.enabled and self.selfdrive.enabled_prev:
+          self.events.add(EventName.manualLongitudinalRequired)
+      if be.type == ButtonType.lkas and be.pressed and (CS.cruiseState.available or self.allow_always):
+        if self.enabled:
+          if self.selfdrive.enabled:
+            self.events.add(EventName.manualSteeringRequired)
+          else:
+            self.events.add(EventName.lkasDisable)
+        else:
+          self.events.add(EventName.lkasEnable)
+
+    if not CS.cruiseState.available:
+      self.events.remove(EventName.buttonEnable)
+      if self.selfdrive.CS_prev.cruiseState.available:
+        self.events.add(EventName.lkasDisable)
+
+    self.events.remove(EventName.pcmDisable)
+    self.events.remove(EventName.buttonCancel)
+    self.events.remove(EventName.pedalPressed)
+    self.events.remove(EventName.wrongCruiseMode)
+    self.events.remove(EventName.wrongCarMode)
+
+  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.events)
+
+    # Copy of previous SelfdriveD states for MADS events handling
+    self.selfdrive.enabled_prev = self.selfdrive.enabled

sunnypilot/mads/state.py

@@ -0,0 +1,117 @@
+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
+
+ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding)
+ENABLED_STATES = (State.paused, *ACTIVE_STATES)
+
+GEARS_ALLOW_PAUSED_SILENT = [EventName.silentWrongGear, EventName.silentReverseGear, EventName.silentBrakeHold]
+GEARS_ALLOW_PAUSED = [EventName.wrongGear, EventName.reverseGear, EventName.brakeHold, *GEARS_ALLOW_PAUSED_SILENT]
+
+
+class StateMachine:
+  def __init__(self, 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.enabled:
+      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.silentLkasDisable) or events.has(EventName.silentBrakeHold):
+          self.state = State.paused
+        else:
+          self.state = State.disabled
+        self.ss_state_machine.current_alert_types.append(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.enabled:
+              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.enabled:
+              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):
+          if events.has_list(GEARS_ALLOW_PAUSED):
+            self.state = State.paused
+          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 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

sunnypilot/mads/tests/test_mads_state_machine.py

@@ -0,0 +1,130 @@
+import pytest
+from pytest_mock import MockerFixture
+
+from cereal import log, custom
+from openpilot.common.realtime import DT_CTRL
+from openpilot.sunnypilot.mads.state import StateMachine, SOFT_DISABLE_TIME, GEARS_ALLOW_PAUSED
+from openpilot.selfdrive.selfdrived.events import Events, ET, EVENTS, NormalPermanentAlert
+
+State = custom.SelfdriveStateSP.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
+EventName = log.OnroadEvent.EventName
+
+# 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, mocker: MockerFixture):
+    self.selfdrive = mocker.MagicMock()
+    self.selfdrive.state_machine = mocker.MagicMock()
+    self.selfdrive.active = False
+
+
+class TestMADSStateMachine:
+  @pytest.fixture(autouse=True)
+  def setup_method(self, mocker: MockerFixture):
+    self.mads = MockMADS(mocker)
+    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 State.disabled == self.state_machine.state
+        self.events.clear()
+
+  def test_user_disable_to_paused(self):
+    paused_events = (EventName.silentLkasDisable, EventName.silentBrakeHold)
+    for state in ALL_STATES:
+      for et in MAINTAIN_STATES[state]:
+        self.events.add(make_event([et, ET.USER_DISABLE]))
+        for en in paused_events:
+          self.events.add(en)
+          self.state_machine.state = state
+          self.state_machine.update(self.events)
+          final_state = State.paused if self.events.has(en) and state != State.disabled else State.disabled
+          assert self.state_machine.state == final_state
+          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]))
+      if not self.events.has_list(GEARS_ALLOW_PAUSED):
+        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_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_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()

system/manager/manager.py

@@ -42,11 +42,18 @@ def manager_init() -> None:
     ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)),
   ]
 
+  sunnypilot_default_params: list[tuple[str, str | bytes]] = [
+    ("Mads", "1"),
+    ("MadsCruiseMain", "1"),
+    ("MadsDisengageLateralOnBrake", "0"),
+    ("MadsUnifiedEngagementMode", "1"),
+  ]
+
   if params.get_bool("RecordFrontLock"):
     params.put_bool("RecordFront", True)
 
   # set unset params
-  for k, v in default_params:
+  for k, v in (default_params + sunnypilot_default_params):
     if params.get(k) is None:
       params.put(k, v)