refactor

* car: initialize sunnypilot interface immediately

* init

* Revert "car: initialize sunnypilot interface immediately"

This reverts commit 2e0fc4d87eb312a3a74a61bb8cbc780e33efbfc7.

* show int

* show int

* work properly with cereal

* unused

* in its own module

* bump

* not yet

* not yet

* list comp and use structs directly

* allow default key if needed

* lint

* send it with None

* bump

cereal/custom.capnp

@@ -63,7 +63,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
     type @0 :Type;
     artifact @1 :Artifact;  # Main artifact
     metadata @2 :Artifact;  # Metadata artifact
-    
+
     enum Type {
       supercombo @0;
       navigation @1;
@@ -95,6 +95,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
 
 struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   dec @0 :DynamicExperimentalControl;
+  accelPersonality @1 :AccelerationPersonality;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -106,6 +107,13 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
       blended @1;
     }
   }
+
+  enum AccelerationPersonality {
+    sport @0;
+    normal @1;
+    eco @2;
+    stock @3;
+  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {
@@ -144,6 +152,7 @@ struct OnroadEventSP @0xda96579883444c35 {
     hyundaiRadarTracksConfirmed @13;
     experimentalModeSwitched @14;
     wrongCarModeAlertOnly @15;
+    pedalPressedAlertOnly @16;
   }
 }
 
@@ -166,6 +175,12 @@ struct CarParamsSP @0x80ae746ee2596b11 {
 
 struct CarControlSP @0xa5cd762cd951a455 {
   mads @0 :ModularAssistiveDrivingSystem;
+  params @1 :List(Param);
+
+  struct Param {
+    key @0 :Text;
+    value @1 :Text;
+  }
 }
 
 struct BackupManagerSP @0xf98d843bfd7004a3 {
@@ -176,14 +191,14 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
   lastError @4 :Text;
   currentBackup @5 :BackupInfo;
   backupHistory @6 :List(BackupInfo);
-  
+
   enum Status {
     idle @0;
     inProgress @1;
     completed @2;
     failed @3;
   }
-  
+
   struct Version {
     major @0 :UInt16;
     minor @1 :UInt16;
@@ -191,13 +206,13 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
     build @3 :UInt16;
     branch @4 :Text;
   }
-  
+
   struct MetadataEntry {
     key @0 :Text;
     value @1 :Text;
     tags @2 :List(Text);
   }
-  
+
   struct BackupInfo {
     deviceId @0 :Text;
     version @1 :UInt32;

common/params_keys.h

@@ -138,7 +138,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     // MADS params
     {"Mads", PERSISTENT | BACKUP},
     {"MadsMainCruiseAllowed", PERSISTENT | BACKUP},
-    {"MadsPauseLateralOnBrake", PERSISTENT | BACKUP},
+    {"MadsSteeringMode", PERSISTENT | BACKUP},
     {"MadsUnifiedEngagementMode", PERSISTENT | BACKUP},
 
     // Model Manager params
@@ -171,4 +171,5 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"HyundaiRadarTracksToggle", PERSISTENT},
 
     {"DynamicExperimentalControl", PERSISTENT},
+    {"AccelPersonality", PERSISTENT},
 };

selfdrive/car/helpers.py

@@ -55,5 +55,6 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct
   struct_dataclass = structs.CarControlSP(**remove_deprecated({k: v for k, v in struct_dict.items() if not isinstance(k, dict)}))
 
   struct_dataclass.mads = structs.ModularAssistiveDrivingSystem(**remove_deprecated(struct_dict.get('mads', {})))
+  struct_dataclass.params = [structs.CarControlSP.Param(**remove_deprecated(p)) for p in struct_dict.get('params', [])]
 
   return struct_dataclass

selfdrive/controls/controlsd.py

@@ -1,8 +1,10 @@
 #!/usr/bin/env python3
 import math
+import threading
+import time
 from typing import SupportsFloat
 
-from cereal import car, log, custom
+from cereal import car, log
 import cereal.messaging as messaging
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.params import Params
@@ -19,6 +21,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 
+from openpilot.sunnypilot.selfdrive.controls.controlsd_ext import ControlsExt
+
 State = log.SelfdriveState.OpenpilotState
 LaneChangeState = log.LaneChangeState
 LaneChangeDirection = log.LaneChangeDirection
@@ -26,24 +30,23 @@ LaneChangeDirection = log.LaneChangeDirection
 ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
 
 
-class Controls:
+class Controls(ControlsExt):
   def __init__(self) -> None:
     self.params = Params()
     cloudlog.info("controlsd is waiting for CarParams")
     self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
     cloudlog.info("controlsd got CarParams")
 
-    cloudlog.info("controlsd is waiting for CarParamsSP")
-    self.CP_SP = messaging.log_from_bytes(self.params.get("CarParamsSP", block=True), custom.CarParamsSP)
-    cloudlog.info("controlsd got CarParamsSP")
+    # Initialize sunnypilot controlsd extension
+    ControlsExt.__init__(self, self.CP, self.params)
 
     self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP)
 
     self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
                                    'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
-                                   'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + ['selfdriveStateSP'],
+                                   'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + self.sm_services_ext,
                                   poll='selfdriveState')
-    self.pm = messaging.PubMaster(['carControl', 'controlsState'] + ['carControlSP'])
+    self.pm = messaging.PubMaster(['carControl', 'controlsState'] + self.pm_services_ext)
 
     self.steer_limited_by_controls = False
     self.curvature = 0.0
@@ -100,11 +103,8 @@ class Controls:
     # Check which actuators can be enabled
     standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill
 
-    ss_sp = self.sm['selfdriveStateSP']
-    if ss_sp.mads.available:
-      _lat_active = ss_sp.mads.active
-    else:
-      _lat_active = self.sm['selfdriveState'].active
+    # Get which state to use for active lateral control
+    _lat_active = self.get_lat_active(self.sm)
 
     CC.latActive = _lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                    (not standstill or self.CP.steerAtStandstill)
@@ -148,12 +148,9 @@ class Controls:
         cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
         setattr(actuators, p, 0.0)
 
-    CC_SP = custom.CarControlSP.new_message()
-    CC_SP.mads = ss_sp.mads
+    return CC, lac_log
 
-    return CC, CC_SP, lac_log
-
-  def publish(self, CC, CC_SP, lac_log):
+  def publish(self, CC, lac_log):
     CS = self.sm['carState']
 
     # Orientation and angle rates can be useful for carcontroller
@@ -227,19 +224,27 @@ class Controls:
     cc_send.carControl = CC
     self.pm.send('carControl', cc_send)
 
-    # carControlSP
-    cc_sp_send = messaging.new_message('carControlSP')
-    cc_sp_send.valid = CS.canValid
-    cc_sp_send.carControlSP = CC_SP
-    self.pm.send('carControlSP', cc_sp_send)
+  def params_thread(self, evt):
+    while not evt.is_set():
+      self.get_params_sp()
+
+      time.sleep(0.1)
 
   def run(self):
     rk = Ratekeeper(100, print_delay_threshold=None)
-    while True:
-      self.update()
-      CC, CC_SP, lac_log = self.state_control()
-      self.publish(CC, CC_SP, lac_log)
-      rk.monitor_time()
+    e = threading.Event()
+    t = threading.Thread(target=self.params_thread, args=(e,))
+    try:
+      t.start()
+      while True:
+        self.update()
+        CC, lac_log = self.state_control()
+        self.publish(CC, lac_log)
+        self.run_ext(self.sm, self.pm)
+        rk.monitor_time()
+    finally:
+      e.set()
+      t.join()
 
 
 def main():

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -10,6 +10,9 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.modeld.constants import index_function
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController
+
+
 if __name__ == '__main__':  # generating code
   from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
@@ -228,6 +231,7 @@ class LongitudinalMpc:
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset()
     self.source = SOURCES[2]
+    self.accel_controller = AccelController()
 
   def reset(self):
     # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
@@ -332,6 +336,8 @@ class LongitudinalMpc:
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
+    a_cruise_min = self.accel_controller._get_min_accel_for_speed(v_ego)
+
     lead_xv_0 = self.process_lead(radarstate.leadOne)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)
 
@@ -350,7 +356,7 @@ class LongitudinalMpc:
 
       # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
       # when the leads are no factor.
-      v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
+      v_lower = v_ego + (T_IDXS * a_cruise_min * 1.05)
       # TODO does this make sense when max_a is negative?
       v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
       v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),

selfdrive/controls/lib/longitudinal_planner.py

@@ -126,6 +126,16 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     else:
       accel_clip = [ACCEL_MIN, ACCEL_MAX]
 
+    # Override accel using Accel Controller if enabled
+    if self.accel_controller.is_personality_enabled:
+      max_limit = self.accel_controller._get_max_accel_for_speed(v_ego)
+      if self.mpc.mode == 'acc':
+        # Use the accel controller limits directly
+        accel_clip = [ACCEL_MIN, max_limit]
+        # Recalculate limit turn according to the new max limit
+        steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
+        accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
+
     if reset_state:
       self.v_desired_filter.x = v_ego
       # Clip aEgo to cruise limits to prevent large accelerations when becoming active

selfdrive/debug/hyundai_enable_radar_points.py

@@ -71,6 +71,13 @@ SUPPORTED_FW_VERSIONS = {
   b"DLhe SCC FHCUP      1.00 1.02 99110-L7000 \x01 \x102    ": ConfigValues(
     default_config=b"\x00\x00\x00\x01\x00\x00",
     tracks_enabled=b"\x00\x00\x00\x01\x00\x01"),
+  # 2022 Niro EV
+  b"DEev SCC F-CUP      1.00 1.00 99110-Q4600\x01\x42      ": ConfigValues(
+    default_config=b"\x00\x00\x00\x01\x00\x00",
+    tracks_enabled=b"\x00\x00\x00\x01\x00\x01"),
+  b"DEev SCC F-CUP      1.00 1.00 99110-Q4600 \x07\x03\t%      ": ConfigValues(
+    default_config=b"\x00\x00\x00\x01\x00\x00",
+    tracks_enabled=b"\x00\x00\x00\x01\x00\x01"),
 }
 
 if __name__ == "__main__":

selfdrive/pandad/pandad.cc

@@ -41,7 +41,7 @@
 #define CUTOFF_IL 400
 #define SATURATE_IL 1000
 
-#define ALT_EXP_DISENGAGE_LATERAL_ON_BRAKE 2048
+#define ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE 2048
 
 ExitHandler do_exit;
 
@@ -57,7 +57,7 @@ bool check_all_connected(const std::vector<Panda *> &pandas) {
 
 bool process_mads_heartbeat(SubMaster *sm) {
   const int &alt_exp = (*sm)["carParams"].getCarParams().getAlternativeExperience();
-  const bool disengage_lateral_on_brake = (alt_exp & ALT_EXP_DISENGAGE_LATERAL_ON_BRAKE) != 0;
+  const bool disengage_lateral_on_brake = (alt_exp & ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE) != 0;
 
   const auto &mads = (*sm)["selfdriveStateSP"].getSelfdriveStateSP().getMads();
   const bool heartbeat_type = disengage_lateral_on_brake ? mads.getActive() : mads.getEnabled();

selfdrive/ui/qt/offroad/settings.cc

@@ -78,6 +78,16 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
                                           "../assets/offroad/icon_speed_limit.png",
                                           longi_button_texts);
 
+  // accel controller
+  std::vector<QString> accel_personality_texts{tr("Sport"), tr("Normal"), tr("Eco"), tr("Stock")};
+  accel_personality_setting = new ButtonParamControlSP("AccelPersonality", tr("Acceleration Personality"),
+                                          tr("Normal is recommended. In sport mode, sunnypilot will provide aggressive acceleration for a dynamic driving experience. "
+                                             "In eco mode, sunnypilot will apply smoother and more relaxed acceleration. On supported cars, you can cycle through these "
+                                             "acceleration personality within Onroad Settings on the driving screen."),
+                                          "",
+                                          accel_personality_texts);
+  accel_personality_setting->showDescription();
+
   // set up uiState update for personality setting
   QObject::connect(uiState(), &UIState::uiUpdate, this, &TogglesPanel::updateState);
 
@@ -93,6 +103,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
     // insert longitudinal personality after NDOG toggle
     if (param == "DisengageOnAccelerator") {
       addItem(long_personality_setting);
+      addItem(accel_personality_setting);
     }
   }
 
@@ -113,6 +124,13 @@ void TogglesPanel::updateState(const UIState &s) {
     }
     uiState()->scene.personality = personality;
   }
+  if (sm.updated("longitudinalPlanSP")) {
+    auto accel_personality = sm["longitudinalPlanSP"].getLongitudinalPlanSP().getAccelPersonality();
+    if (accel_personality != s.scene.accel_personality && s.scene.started && isVisible()) {
+      accel_personality_setting->setCheckedButton(static_cast<int>(accel_personality));
+    }
+    uiState()->scene.accel_personality = accel_personality;
+  }
 }
 
 void TogglesPanel::expandToggleDescription(const QString &param) {
@@ -150,10 +168,12 @@ void TogglesPanel::updateToggles() {
       experimental_mode_toggle->setEnabled(true);
       experimental_mode_toggle->setDescription(e2e_description);
       long_personality_setting->setEnabled(true);
+      accel_personality_setting->setEnabled(true);
     } else {
       // no long for now
       experimental_mode_toggle->setEnabled(false);
       long_personality_setting->setEnabled(false);
+      accel_personality_setting->setEnabled(true);
       params.remove("ExperimentalMode");
 
       const QString unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.");

selfdrive/ui/qt/offroad/settings.h

@@ -85,6 +85,7 @@ protected:
   Params params;
   std::map<std::string, ParamControl*> toggles;
   ButtonParamControl *long_personality_setting;
+  ButtonParamControl *accel_personality_setting;
 
   virtual void updateToggles();
 };

selfdrive/ui/sunnypilot/SConscript

@@ -3,6 +3,7 @@ widgets_src = [
   "sunnypilot/qt/widgets/toggle.cc",
   "sunnypilot/qt/widgets/controls.cc",
   "sunnypilot/qt/widgets/drive_stats.cc",
+  "sunnypilot/qt/widgets/expandable_row.cc",
   "sunnypilot/qt/widgets/prime.cc",
   "sunnypilot/qt/widgets/scrollview.cc",
   "sunnypilot/qt/network/networking.cc",

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc

@@ -21,35 +21,19 @@ MadsSettings::MadsSettings(QWidget *parent) : QWidget(parent) {
 
   ListWidget *list = new ListWidget(this, false);
   // Main cruise
-  madsMainCruiseToggle = new ParamControl(
-    "MadsMainCruiseAllowed",
-    tr("Toggle with Main Cruise"),
-    tr("Note: For vehicles without LFA/LKAS button, disabling this will prevent lateral control engagement."),
-    "");
+  madsMainCruiseToggle = new ParamControl("MadsMainCruiseAllowed", tr("Toggle with Main Cruise"), "", "");
   list->addItem(madsMainCruiseToggle);
 
   // Unified Engagement Mode
-  madsUnifiedEngagementModeToggle = new ParamControl(
-    "MadsUnifiedEngagementMode",
-    tr("Unified Engagement Mode (UEM)"),
-    QString("%1<br>"
-            "<h4>%2</h4>")
-    .arg(tr("Engage lateral and longitudinal control with cruise control engagement."))
-    .arg(tr("Note: Once lateral control is engaged via UEM, it will remain engaged until it is manually disabled via the MADS button or car shut off.")),
-    "");
+  madsUnifiedEngagementModeToggle = new ParamControl("MadsUnifiedEngagementMode", tr("Unified Engagement Mode (UEM)"), "", "");
   list->addItem(madsUnifiedEngagementModeToggle);
 
-  // Pause Lateral On Brake
-  std::vector<QString> lateral_on_brake_texts{tr("Remain Active"), tr("Pause Steering")};
-  madsPauseLateralOnBrake = new ButtonParamControl(
-    "MadsPauseLateralOnBrake",
-    tr("Steering Mode After Braking"),
-    tr("Choose how Automatic Lane Centering (ALC) behaves after the brake pedal is manually pressed in sunnypilot.\n\n"
-       "Remain Active: ALC will remain active even after the brake pedal is pressed.\nPause Steering: ALC will be paused after the brake pedal is manually pressed."),
-    "",
-    lateral_on_brake_texts,
-    500);
-  list->addItem(madsPauseLateralOnBrake);
+  // Steering Mode On Brake
+  madsSteeringMode = new ButtonParamControl("MadsSteeringMode", tr("Steering Mode on Brake Pedal"), "", "", madsSteeringModeTexts(), 500);
+  QObject::connect(madsSteeringMode, &ButtonParamControl::buttonToggled, [=] {
+    updateToggles(offroad);
+  });
+  list->addItem(madsSteeringMode);
 
   QObject::connect(uiState(), &UIState::offroadTransition, this, &MadsSettings::updateToggles);
 
@@ -61,7 +45,58 @@ void MadsSettings::showEvent(QShowEvent *event) {
 }
 
 void MadsSettings::updateToggles(bool _offroad) {
-  madsPauseLateralOnBrake->setEnabled(_offroad);
+  auto mads_steering_mode_param = std::atoi(params.get("MadsSteeringMode").c_str());
+
+  auto steering_mode = static_cast<MadsSteeringMode>(
+    std::clamp(mads_steering_mode_param, static_cast<int>(MadsSteeringMode::REMAIN_ACTIVE), static_cast<int>(MadsSteeringMode::DISENGAGE))
+  );
+
+  auto cp_bytes = params.get("CarParamsPersistent");
+  if (!cp_bytes.empty()) {
+    AlignedBuffer aligned_buf;
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
+    cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
+
+    if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) {
+      params.remove("MadsMainCruiseAllowed");
+      params.putBool("MadsUnifiedEngagementMode", true);
+      params.put("MadsSteeringMode", std::to_string(static_cast<int>(MadsSteeringMode::DISENGAGE)));
+
+      madsMainCruiseToggle->setEnabled(false);
+      madsMainCruiseToggle->setDescription(madsDescriptionBuilder(DEFAULT_TO_OFF, MADS_MAIN_CRUISE_BASE_DESC));
+      madsMainCruiseToggle->showDescription();
+
+      madsUnifiedEngagementModeToggle->setEnabled(false);
+      madsUnifiedEngagementModeToggle->setDescription(madsDescriptionBuilder(DEFAULT_TO_ON, MADS_UNIFIED_ENGAGEMENT_MODE_BASE_DESC));
+      madsUnifiedEngagementModeToggle->showDescription();
+
+      madsSteeringModeValues = convertMadsSteeringModeValues({MadsSteeringMode::DISENGAGE});
+      madsSteeringMode->setDescription(madsDescriptionBuilder(STATUS_DISENGAGE_ONLY, madsSteeringModeDescription(steering_mode)));
+    } else {
+      madsMainCruiseToggle->setEnabled(true);
+      madsMainCruiseToggle->setDescription(MADS_MAIN_CRUISE_BASE_DESC);
+
+      madsUnifiedEngagementModeToggle->setEnabled(true);
+      madsUnifiedEngagementModeToggle->setDescription(MADS_UNIFIED_ENGAGEMENT_MODE_BASE_DESC);
+
+      madsSteeringModeValues = convertMadsSteeringModeValues(getMadsSteeringModeValues());
+      madsSteeringMode->setDescription(madsSteeringModeDescription(steering_mode));
+    }
+  } else {
+    madsMainCruiseToggle->setEnabled(false);
+    madsMainCruiseToggle->setDescription(madsDescriptionBuilder(STATUS_CHECK_COMPATIBILITY, MADS_MAIN_CRUISE_BASE_DESC));
+    madsMainCruiseToggle->showDescription();
+
+    madsUnifiedEngagementModeToggle->setEnabled(false);
+    madsUnifiedEngagementModeToggle->setDescription(madsDescriptionBuilder(STATUS_CHECK_COMPATIBILITY, MADS_UNIFIED_ENGAGEMENT_MODE_BASE_DESC));
+    madsUnifiedEngagementModeToggle->showDescription();
+
+    madsSteeringModeValues = {};
+    madsSteeringMode->setDescription(madsDescriptionBuilder(STATUS_CHECK_COMPATIBILITY, madsSteeringModeDescription(steering_mode)));
+  }
+
+  madsSteeringMode->setEnableSelectedButtons(_offroad, madsSteeringModeValues);
+  madsSteeringMode->showDescription();
 
   offroad = _offroad;
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h

@@ -12,6 +12,20 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
 
+const std::vector<std::string> mads_limited_settings_brands = {"rivian", "tesla"};
+
+enum class MadsSteeringMode {
+  REMAIN_ACTIVE = 0,
+  PAUSE = 1,
+  DISENGAGE = 2,
+};
+
+struct MadsSteeringModeOption {
+  MadsSteeringMode mode;
+  QString display_text;
+  QString description;
+};
+
 class MadsSettings : public QWidget {
   Q_OBJECT
 
@@ -32,5 +46,70 @@ private:
 
   ParamControl *madsMainCruiseToggle;
   ParamControl *madsUnifiedEngagementModeToggle;
-  ButtonParamControl *madsPauseLateralOnBrake;
+  ButtonParamControl *madsSteeringMode;
+
+  std::vector<int> madsSteeringModeValues = {};
+
+  const QString MADS_MAIN_CRUISE_BASE_DESC = tr("Note: For vehicles without LFA/LKAS button, disabling this will prevent lateral control engagement.");
+  const QString MADS_UNIFIED_ENGAGEMENT_MODE_BASE_DESC = QString("%1<br>"
+                                                                 "<h4>%2</h4>")
+                                                         .arg(tr("Engage lateral and longitudinal control with cruise control engagement."))
+                                                         .arg(tr("Note: Once lateral control is engaged via UEM, it will remain engaged until it is manually disabled via the MADS button or car shut off."));
+
+  const QString STATUS_CHECK_COMPATIBILITY = tr("Start the vehicle to check vehicle compatibility.");
+  const QString DEFAULT_TO_OFF = tr("This feature defaults to OFF, and does not allow selection due to vehicle limitations.");
+  const QString DEFAULT_TO_ON = tr("This feature defaults to ON, and does not allow selection due to vehicle limitations.");
+  const QString STATUS_DISENGAGE_ONLY = tr("This platform only supports Disengage mode due to vehicle limitations.");
+
+  static const std::vector<MadsSteeringModeOption> &madsSteeringModeOptions() {
+    static const std::vector<MadsSteeringModeOption> options = {
+      {MadsSteeringMode::REMAIN_ACTIVE,  tr("Remain Active"), tr("Remain Active: ALC will remain active when the brake pedal is pressed.")},
+      {MadsSteeringMode::PAUSE,          tr("Pause"),         tr("Pause: ALC will pause when the brake pedal is pressed.")},
+      {MadsSteeringMode::DISENGAGE,      tr("Disengage"),     tr("Disengage: ALC will disengage when the brake pedal is pressed.")},
+    };
+    return options;
+  }
+
+  static std::vector<MadsSteeringMode> getMadsSteeringModeValues() {
+    std::vector<MadsSteeringMode> values;
+    for (const auto& option : madsSteeringModeOptions()) {
+      values.push_back(option.mode);
+    }
+    return values;
+  }
+
+  static std::vector<int> convertMadsSteeringModeValues(const std::vector<MadsSteeringMode> &modes) {
+    std::vector<int> values;
+    for (const auto& mode : modes) {
+      values.push_back(static_cast<int>(mode));
+    }
+    return values;
+  }
+
+  static std::vector<QString> madsSteeringModeTexts() {
+    std::vector<QString> texts;
+    for (const auto& option : madsSteeringModeOptions()) {
+      texts.push_back(option.display_text);
+    }
+    return texts;
+  }
+
+  static QString madsSteeringModeDescription(const MadsSteeringMode mode = MadsSteeringMode::REMAIN_ACTIVE) {
+    QString base_desc = tr("Choose how Automatic Lane Centering (ALC) behaves after the brake pedal is manually pressed in sunnypilot.");
+    QString result = base_desc + "<br><br>";
+
+    for (const auto& option : madsSteeringModeOptions()) {
+      QString desc = option.description;
+      if (option.mode == mode) {
+        desc = "<font color='white'><b>" + desc + "</b></font>";
+      }
+      result += desc + "<br>";
+    }
+
+    return result;
+  }
+
+  static QString madsDescriptionBuilder(const QString &custom_description, const QString &base_description) {
+    return "<font color='white'><b>" + custom_description + "</b></font><br><br>" + base_description;
+  }
 };

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc

@@ -122,6 +122,21 @@ void LateralPanel::updateToggles(bool _offroad) {
     toggle->setEnabled(_offroad);
   }
 
+  auto cp_bytes = params.get("CarParamsPersistent");
+  if (!cp_bytes.empty()) {
+    AlignedBuffer aligned_buf;
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
+    cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
+
+    if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) {
+      madsToggle->setDescription(descriptionBuilder(STATUS_MADS_SETTINGS_LIMITED_COMPATIBILITY, MADS_BASE_DESC));
+    } else {
+      madsToggle->setDescription(descriptionBuilder(STATUS_MADS_SETTINGS_FULL_COMPATIBILITY, MADS_BASE_DESC));
+    }
+  } else {
+    madsToggle->setDescription(descriptionBuilder(STATUS_MADS_CHECK_COMPATIBILITY, MADS_BASE_DESC));
+  }
+
   madsSettingsButton->setEnabled(madsToggle->isToggled());
 
   offroad = _offroad;

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h

@@ -30,6 +30,7 @@ public slots:
   void updateToggles(bool _offroad);
 
 private:
+  Params params;
   QStackedLayout* main_layout = nullptr;
   QWidget* sunnypilotScreen = nullptr;
   ScrollViewSP *sunnypilotScroller = nullptr;
@@ -42,4 +43,14 @@ private:
   PushButtonSP *laneChangeSettingsButton;
   LaneChangeSettings *laneChangeWidget = nullptr;
   NeuralNetworkLateralControl *nnlcToggle = nullptr;
+
+  const QString MADS_BASE_DESC = tr("Enables independent engagements of Automatic Lane Centering (ALC) and Adaptive Cruise Control (ACC).");
+
+  const QString STATUS_MADS_CHECK_COMPATIBILITY = tr("Start the vehicle to check vehicle compatibility.");
+  const QString STATUS_MADS_SETTINGS_FULL_COMPATIBILITY = tr("This platform supports all MADS settings.");
+  const QString STATUS_MADS_SETTINGS_LIMITED_COMPATIBILITY = tr("This platform supports limited MADS settings.");
+
+  static QString descriptionBuilder(const QString &custom_description, const QString &base_description) {
+    return "<font color='white'><b>" + custom_description + "</b></font><br><br>" + base_description;
+  }
 };

selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h

@@ -12,6 +12,10 @@
 
 #include "selfdrive/ui/qt/offroad/settings.h"
 
+inline bool isBrandInList(const std::string &brand, const std::vector<std::string> &list) {
+  return std::find(list.begin(), list.end(), brand) != list.end();
+}
+
 class SettingsWindowSP : public SettingsWindow {
   Q_OBJECT
 

selfdrive/ui/sunnypilot/qt/widgets/controls.h

@@ -324,10 +324,11 @@ public:
     }
   }
 
-  void setDisabledSelectedButton(std::string val) {
-    int value = atoi(val.c_str());
+  void setEnableSelectedButtons(bool enable, const std::vector<int>& enabled_btns = {}) const {
     for (int i = 0; i < button_group->buttons().size(); i++) {
-      button_group->buttons()[i]->setEnabled(i != value);
+      // Enable the button if its index is in the enabled list
+      bool should_enable = std::find(enabled_btns.begin(), enabled_btns.end(), i) != enabled_btns.end();
+      button_group->buttons()[i]->setEnabled(enable && should_enable);
     }
   }
 
@@ -440,15 +441,16 @@ public:
 class OptionControlSP : public AbstractControlSP_SELECTOR {
   Q_OBJECT
 
-private:
-  bool is_inline_layout;
-  QHBoxLayout *optionSelectorLayout = is_inline_layout ? new QHBoxLayout() : hlayout;
-
+protected:
   struct MinMaxValue {
     int min_value;
     int max_value;
   };
 
+private:
+  bool is_inline_layout;
+  QHBoxLayout *optionSelectorLayout = is_inline_layout ? new QHBoxLayout() : hlayout;
+
   int getParamValue() {
     const auto param_value = QString::fromStdString(params.get(key));
     const auto result = valueMap != nullptr ? valueMap->key(param_value) : param_value;
@@ -550,6 +552,10 @@ public:
     request_update = _update;
   }
 
+  void setFixedWidth(int width) {
+    label.setFixedWidth(width);
+  }
+
   inline void setLabel(const QString &text) { label.setText(text); }
 
   void setEnabled(bool enabled) {

selfdrive/ui/sunnypilot/qt/widgets/expandable_row.cc

@@ -0,0 +1,31 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/widgets/expandable_row.h"
+
+ExpandableToggleRow::ExpandableToggleRow(const QString &param, const QString &title, const QString &desc, const QString &icon, QWidget *parent)
+  : ToggleControlSP(title, desc, icon, false, parent) {
+
+  key = param.toStdString();
+  QObject::connect(this, &ExpandableToggleRow::toggleFlipped, this, &ExpandableToggleRow::toggleClicked);
+
+  collapsibleWidget = new QFrame(this);
+  collapsibleWidget->setContentsMargins(0, 0, 0, 0);
+  collapsibleWidget->setVisible(false);
+  QVBoxLayout *collapsible_layout = new QVBoxLayout();
+  collapsibleWidget->setLayout(collapsible_layout);
+
+  list = new ListWidgetSP(this, false);
+
+  main_layout->addWidget(collapsibleWidget);
+  collapsible_layout->addWidget(list);
+}
+
+void ExpandableToggleRow::toggleClicked(bool state) {
+  params.putBool(key, state);
+  collapsibleWidget->setVisible(state);
+}

selfdrive/ui/sunnypilot/qt/widgets/expandable_row.h

@@ -0,0 +1,50 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#pragma once
+
+#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+
+class ExpandableToggleRow : public ToggleControlSP {
+  Q_OBJECT
+
+public:
+  ExpandableToggleRow(const QString &param, const QString &title, const QString &desc, const QString &icon, QWidget *parent = nullptr);
+
+  void addItem(QWidget *widget) {
+    list->addItem(widget);
+  }
+
+  ListWidgetSP *innerList() {
+    return list;
+  }
+
+  void refresh() {
+    bool state = params.getBool(key);
+    if (state != toggle.on) {
+      toggle.togglePosition();
+    }
+    collapsibleWidget->setVisible(state);
+  }
+
+  bool isToggled() {
+    return params.getBool(key);
+  }
+
+  void showEvent(QShowEvent *event) override {
+    refresh();
+  }
+
+private:
+  void toggleClicked(bool state);
+
+  std::string key;
+  Params params;
+
+  ListWidgetSP *list;
+  QFrame *collapsibleWidget = nullptr;
+};

selfdrive/ui/ui.h

@@ -60,6 +60,7 @@ typedef struct UIScene {
   cereal::PandaState::PandaType pandaType;
 
   cereal::LongitudinalPersonality personality;
+  cereal::LongitudinalPlanSP::AccelerationPersonality accel_personality;
 
   float light_sensor = -1;
   bool started, ignition, is_metric;

sunnypilot/mads/helpers.py

@@ -11,15 +11,37 @@ from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP, HyundaiSafetyFlagsSP
 
 
+class MadsSteeringModeOnBrake:
+  REMAIN_ACTIVE = 0
+  PAUSE = 1
+  DISENGAGE = 2
+
+
+def get_mads_limited_brands(CP: structs.CarParams) -> bool:
+  return CP.brand in ("rivian", "tesla")
+
+
+def read_steering_mode_param(CP: structs.CarParams, params: Params):
+  if get_mads_limited_brands(CP):
+    return MadsSteeringModeOnBrake.DISENGAGE
+
+  try:
+    return int(params.get("MadsSteeringMode"))
+  except (ValueError, TypeError):
+    return MadsSteeringModeOnBrake.REMAIN_ACTIVE
+
+
 def set_alternative_experience(CP: structs.CarParams, params: Params):
   enabled = params.get_bool("Mads")
-  pause_lateral_on_brake = params.get_bool("MadsPauseLateralOnBrake")
+  steering_mode = read_steering_mode_param(CP, params)
 
   if enabled:
     CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS
 
-    if pause_lateral_on_brake:
-      CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISENGAGE_LATERAL_ON_BRAKE
+    if steering_mode == MadsSteeringModeOnBrake.DISENGAGE:
+      CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.MADS_DISENGAGE_LATERAL_ON_BRAKE
+    elif steering_mode == MadsSteeringModeOnBrake.PAUSE:
+      CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.MADS_PAUSE_LATERAL_ON_BRAKE
 
 
 def set_car_specific_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params):
@@ -31,12 +53,12 @@ def set_car_specific_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, p
       CP_SP.flags |= HyundaiFlagsSP.LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE.value
       CP_SP.safetyParam |= HyundaiSafetyFlagsSP.LONG_MAIN_CRUISE_TOGGLEABLE
 
-  # MADS is currently not supported in Tesla due to lack of consistent states to engage controls
-  # TODO-SP: To enable MADS for Tesla, identify consistent signals for MADS toggling
-  if CP.brand == "tesla":
-    params.remove("Mads")
-
-  # MADS is currently not supported in Rivian due to lack of consistent states to engage controls
-  # TODO-SP: To enable MADS for Rivian, identify consistent signals for MADS toggling
-  if CP.brand == "rivian":
-    params.remove("Mads")
+  # MADS Partial Support
+  # MADS is currently partially supported for these platforms due to lack of consistent states to engage controls
+  # Only MadsSteeringModeOnBrake.DISENGAGE is supported for these platforms
+  # TODO-SP: To enable MADS full support for Rivian/Tesla, identify consistent signals for MADS toggling
+  mads_partial_support = get_mads_limited_brands(CP)
+  if mads_partial_support:
+    params.put("MadsSteeringMode", "2")
+    params.put_bool("MadsUnifiedEngagementMode", True)
+    params.remove("MadsMainCruiseAllowed")

sunnypilot/mads/mads.py

@@ -9,6 +9,8 @@ from cereal import log, custom
 
 from opendbc.car import structs
 from opendbc.car.hyundai.values import HyundaiFlags
+from opendbc.safety import ALTERNATIVE_EXPERIENCE
+from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, get_mads_limited_brands
 from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT
 
 State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
@@ -24,75 +26,105 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
 
 class ModularAssistiveDrivingSystem:
   def __init__(self, selfdrive):
+    self.CP = selfdrive.CP
     self.params = selfdrive.params
 
     self.enabled = False
     self.active = False
     self.available = False
     self.allow_always = False
+    self.no_main_cruise = False
     self.selfdrive = selfdrive
     self.selfdrive.enabled_prev = False
     self.state_machine = StateMachine(self)
     self.events = self.selfdrive.events
     self.events_sp = self.selfdrive.events_sp
+    self.disengage_on_accelerator = not self.CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
 
-    if self.selfdrive.CP.brand == "hyundai":
-      if self.selfdrive.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD):
+    if self.CP.brand == "hyundai":
+      if self.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD):
         self.allow_always = True
 
+    if get_mads_limited_brands(self.CP):
+      self.no_main_cruise = True
+
     # read params on init
     self.enabled_toggle = self.params.get_bool("Mads")
     self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed")
-    self.pause_lateral_on_brake_toggle = self.params.get_bool("MadsPauseLateralOnBrake")
+    self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.params)
     self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode")
 
   def read_params(self):
     self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed")
     self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode")
 
+  def pedal_pressed_non_gas_pressed(self, CS: structs.CarState) -> bool:
+    if self.events.has(EventName.pedalPressed) and not (CS.gasPressed and not self.selfdrive.CS_prev.gasPressed and self.disengage_on_accelerator):
+      return True
+
+    return False
+
+  def should_silent_lkas_enable(self, CS: structs.CarState) -> bool:
+    if self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE and self.pedal_pressed_non_gas_pressed(CS):
+      return False
+
+    if self.events_sp.contains_in_list(GEARS_ALLOW_PAUSED_SILENT):
+      return False
+
+    return True
+
+  def block_unified_engagement_mode(self) -> bool:
+    # UEM disabled
+    if not self.unified_engagement_mode:
+      return True
+
+    if self.enabled:
+      return True
+
+    if self.selfdrive.enabled and self.selfdrive.enabled_prev:
+      return True
+
+    return False
+
+  def get_wrong_car_mode(self, alert_only: bool) -> None:
+    if alert_only:
+      if self.events.has(EventName.wrongCarMode):
+        self.replace_event(EventName.wrongCarMode, EventNameSP.wrongCarModeAlertOnly)
+    else:
+      self.events.remove(EventName.wrongCarMode)
+
+  def transition_paused_state(self):
+    if self.state_machine.state != State.paused:
+      self.events_sp.add(EventNameSP.silentLkasDisable)
+
+  def replace_event(self, old_event: int, new_event: int):
+    self.events.remove(old_event)
+    self.events_sp.add(new_event)
+
   def update_events(self, CS: structs.CarState):
-    def update_unified_engagement_mode():
-      uem_blocked = self.enabled or (self.selfdrive.enabled and self.selfdrive.enabled_prev)
-      if (self.unified_engagement_mode and uem_blocked) 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_sp.add(EventNameSP.silentLkasDisable)
-
-    def replace_event(old_event: int, new_event: int):
-      self.events.remove(old_event)
-      self.events_sp.add(new_event)
-
     if not self.selfdrive.enabled and self.enabled:
       if self.events.has(EventName.doorOpen):
-        replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen)
-        transition_paused_state()
+        self.replace_event(EventName.doorOpen, EventNameSP.silentDoorOpen)
+        self.transition_paused_state()
       if self.events.has(EventName.seatbeltNotLatched):
-        replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched)
-        transition_paused_state()
+        self.replace_event(EventName.seatbeltNotLatched, EventNameSP.silentSeatbeltNotLatched)
+        self.transition_paused_state()
       if self.events.has(EventName.wrongGear) and (CS.vEgo < 2.5 or CS.gearShifter == GearShifter.reverse):
-        replace_event(EventName.wrongGear, EventNameSP.silentWrongGear)
-        transition_paused_state()
+        self.replace_event(EventName.wrongGear, EventNameSP.silentWrongGear)
+        self.transition_paused_state()
       if self.events.has(EventName.reverseGear):
-        replace_event(EventName.reverseGear, EventNameSP.silentReverseGear)
-        transition_paused_state()
+        self.replace_event(EventName.reverseGear, EventNameSP.silentReverseGear)
+        self.transition_paused_state()
       if self.events.has(EventName.brakeHold):
-        replace_event(EventName.brakeHold, EventNameSP.silentBrakeHold)
-        transition_paused_state()
+        self.replace_event(EventName.brakeHold, EventNameSP.silentBrakeHold)
+        self.transition_paused_state()
       if self.events.has(EventName.parkBrake):
-        replace_event(EventName.parkBrake, EventNameSP.silentParkBrake)
-        transition_paused_state()
+        self.replace_event(EventName.parkBrake, EventNameSP.silentParkBrake)
+        self.transition_paused_state()
 
-      if self.pause_lateral_on_brake_toggle:
-        if CS.brakePressed:
-          transition_paused_state()
-
-      if not (self.pause_lateral_on_brake_toggle and CS.brakePressed) and \
-         not self.events_sp.contains_in_list(GEARS_ALLOW_PAUSED_SILENT):
-        if self.state_machine.state == State.paused:
-          self.events_sp.add(EventNameSP.silentLkasEnable)
+      if self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE:
+        if self.pedal_pressed_non_gas_pressed(CS):
+          self.transition_paused_state()
 
       self.events.remove(EventName.preEnableStandstill)
       self.events.remove(EventName.belowEngageSpeed)
@@ -100,8 +132,19 @@ class ModularAssistiveDrivingSystem:
       self.events.remove(EventName.cruiseDisabled)
       self.events.remove(EventName.manualRestart)
 
-    if self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable):
-      update_unified_engagement_mode()
+    selfdrive_enable_events = self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable)
+    set_speed_btns_enable = any(be.type in SET_SPEED_BUTTONS for be in CS.buttonEvents)
+
+    # wrongCarMode alert only or actively block control
+    self.get_wrong_car_mode(selfdrive_enable_events or set_speed_btns_enable)
+
+    if selfdrive_enable_events:
+      if self.pedal_pressed_non_gas_pressed(CS):
+        self.events_sp.add(EventNameSP.pedalPressedAlertOnly)
+
+      if self.block_unified_engagement_mode():
+        self.events.remove(EventName.pcmEnable)
+        self.events.remove(EventName.buttonEnable)
     else:
       if self.main_enabled_toggle:
         if CS.cruiseState.available and not self.selfdrive.CS_prev.cruiseState.available:
@@ -120,20 +163,29 @@ class ModularAssistiveDrivingSystem:
         else:
           self.events_sp.add(EventNameSP.lkasEnable)
 
-    if not CS.cruiseState.available:
+    if not CS.cruiseState.available and not self.no_main_cruise:
       self.events.remove(EventName.buttonEnable)
       if self.selfdrive.CS_prev.cruiseState.available:
         self.events_sp.add(EventNameSP.lkasDisable)
 
+    if self.steering_mode_on_brake == MadsSteeringModeOnBrake.DISENGAGE:
+      if self.pedal_pressed_non_gas_pressed(CS):
+        if self.enabled:
+          self.events_sp.add(EventNameSP.lkasDisable)
+        else:
+          # block lkasEnable if being sent, then send pedalPressedAlertOnly event
+          if self.events_sp.contains(EventNameSP.lkasEnable):
+            self.events_sp.remove(EventNameSP.lkasEnable)
+            self.events_sp.add(EventNameSP.pedalPressedAlertOnly)
+
+    if self.should_silent_lkas_enable(CS):
+      if self.state_machine.state == State.paused:
+        self.events_sp.add(EventNameSP.silentLkasEnable)
+
     self.events.remove(EventName.pcmDisable)
     self.events.remove(EventName.buttonCancel)
     self.events.remove(EventName.pedalPressed)
     self.events.remove(EventName.wrongCruiseMode)
-    if any(be.type in SET_SPEED_BUTTONS for be in CS.buttonEvents):
-      if self.events.has(EventName.wrongCarMode):
-        replace_event(EventName.wrongCarMode, EventNameSP.wrongCarModeAlertOnly)
-    else:
-      self.events.remove(EventName.wrongCarMode)
 
   def update(self, CS: structs.CarState):
     if not self.enabled_toggle:
@@ -141,7 +193,7 @@ class ModularAssistiveDrivingSystem:
 
     self.update_events(CS)
 
-    if not self.selfdrive.CP.passive and self.selfdrive.initialized:
+    if not self.CP.passive and self.selfdrive.initialized:
       self.enabled, self.active = self.state_machine.update()
 
     # Copy of previous SelfdriveD states for MADS events handling

sunnypilot/mads/state.py

@@ -51,7 +51,7 @@ class StateMachine:
     if self.state != State.disabled:
       # user and immediate disable always have priority in a non-disabled state
       if self.check_contains(ET.USER_DISABLE):
-        if self._events_sp.has(EventNameSP.silentLkasDisable) or self._events_sp.has(EventNameSP.silentBrakeHold):
+        if self._events_sp.has(EventNameSP.silentLkasDisable):
           self.state = State.paused
         else:
           self.state = State.disabled

sunnypilot/mads/tests/test_mads_state_machine.py

@@ -73,7 +73,7 @@ class TestMADSStateMachine:
         self.clear_events()
 
   def test_user_disable_to_paused(self):
-    paused_events = (EventNameSP.silentLkasDisable, EventNameSP.silentBrakeHold)
+    paused_events = (EventNameSP.silentLkasDisable, )
     for state in ALL_STATES:
       for et in MAINTAIN_STATES[state]:
         self.events_sp.add(make_event([et, ET.USER_DISABLE]))

sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -0,0 +1,63 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import cereal.messaging as messaging
+from cereal import custom
+
+from opendbc.car import structs
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.sunnypilot.selfdrive.controls.lib.param_store import ParamStore
+
+
+class ControlsExt:
+  def __init__(self, CP: structs.CarParams, params: Params):
+    self.CP = CP
+    self.params = params
+    self.param_store = ParamStore(self.CP)
+    self.get_params_sp()
+
+    cloudlog.info("controlsd_ext is waiting for CarParamsSP")
+    self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP)
+    cloudlog.info("controlsd_ext got CarParamsSP")
+
+    self.sm_services_ext = ['selfdriveStateSP']
+    self.pm_services_ext = ['carControlSP']
+
+  def get_params_sp(self) -> None:
+    self.param_store.update(self.params)
+
+  @staticmethod
+  def get_lat_active(sm: messaging.SubMaster) -> bool:
+    ss_sp = sm['selfdriveStateSP']
+
+    if ss_sp.mads.available:
+      return bool(ss_sp.mads.active)
+
+    # MADS not available, use stock state to engage
+    return bool(sm['selfdriveState'].active)
+
+  def state_control_ext(self, sm: messaging.SubMaster) -> custom.CarControlSP:
+    CC_SP = custom.CarControlSP.new_message()
+
+    # MADS state
+    CC_SP.mads = sm['selfdriveStateSP'].mads
+
+    CC_SP.params = self.param_store.publish()
+
+    return CC_SP
+
+  @staticmethod
+  def publish_ext(CC_SP: custom.CarControlSP, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
+    cc_sp_send = messaging.new_message('carControlSP')
+    cc_sp_send.valid = sm['carState'].canValid
+    cc_sp_send.carControlSP = CC_SP
+
+    pm.send('carControlSP', cc_sp_send)
+
+  def run_ext(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
+    CC_SP = self.state_control_ext(sm)
+    self.publish_ext(CC_SP, sm, pm)

sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py

@@ -0,0 +1,75 @@
+"""
+Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from cereal import custom
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_profiles import (
+  get_max_accel_hermite,
+  get_min_accel_hermite
+)
+
+
+AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+
+class AccelController:
+  def __init__(self):
+    self.params = Params()
+    self.personality = AccelPersonality.stock
+    self.frame = 0
+
+  def _update_personality_from_param(self):
+    if self.frame % int(1. / DT_MDL) == 0:
+      personality_str = self.params.get("AccelPersonality", encoding='utf-8')
+      if personality_str is not None:
+        personality_int = int(personality_str)
+        if personality_int in [AccelPersonality.stock, AccelPersonality.normal, AccelPersonality.eco, AccelPersonality.sport]:
+          self.personality = personality_int
+
+  def _get_max_accel_for_speed(self, v_ego: float) -> float:
+    self._update_personality_from_param()
+
+    if self.personality == AccelPersonality.eco:
+      mode = "eco"
+    elif self.personality == AccelPersonality.sport:
+      mode = "sport"
+    else:
+      mode = "normal"
+
+    return get_max_accel_hermite(v_ego, mode)
+
+  def _get_min_accel_for_speed(self, v_ego: float) -> float:
+    self._update_personality_from_param()
+
+    if self.personality == AccelPersonality.eco:
+      mode = "eco"
+    elif self.personality == AccelPersonality.sport:
+      mode = "sport"
+    elif self.personality == AccelPersonality.normal:
+      mode = "normal"
+    else:
+      mode = "stock"
+
+    return get_min_accel_hermite(v_ego, mode)
+
+  def get_accel_limits(self, v_ego: float, accel_limits: list[float]) -> tuple[float, float]:
+    self._update_personality_from_param()
+
+    if self.personality == AccelPersonality.stock:
+      return (accel_limits[0], accel_limits[1])
+    else:
+      max_accel = self._get_max_accel_for_speed(v_ego)
+      min_accel = self._get_min_accel_for_speed(v_ego)
+      return (min_accel, max_accel)
+
+  def is_personality_enabled(self, accel_personality: int = AccelPersonality.stock) -> bool:
+    self.personality = accel_personality
+    self._update_personality_from_param()
+    return bool(self.personality != AccelPersonality.stock)
+
+  def update(self):
+    self.frame += 1

sunnypilot/selfdrive/controls/lib/accel_personality/accel_profiles.py

@@ -0,0 +1,79 @@
+"""
+Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import numpy as np
+
+# Profiles
+MAX_ACCEL_PROFILES = {
+  "eco":                [2.00, 2.00, 1.98, 1.54, 0.83, .572, .455, .365, .32, .10],
+  "normal":             [2.00, 2.00, 1.99, 1.66, 1.06, .66,  .58, .49, .37, .15],
+  "sport":              [2.00, 2.00, 2.00, 1.95, 1.25, .88,  .70, .60, .45, .20],
+}
+MAX_ACCEL_BREAKPOINTS = [0.,   1.,   6.,   8.,   11.,  16.,  20., 25., 30., 55.]
+
+MIN_ACCEL_PROFILES = {
+  "eco":                [-.06,  -.06,  -.11,  -.11, -.071, -.071, -.072, -.65, -.65],
+  "normal":             [-.07,  -.07,  -.12,  -.12, -.072, -.072, -.073, -.70, -.70],
+  "sport":              [-1.2,  -1.2,  -1.2,  -1.2, -1.2,  -1.2,  -1.2,  -1.2, -1.2],
+  "stock":              [-1.2,  -1.2,  -1.2,  -1.2, -1.2,  -1.2,  -1.2,  -1.2, -1.2],
+}
+MIN_ACCEL_BREAKPOINTS = [0.,    0.5,    0.6,    1.2,    8.,    12.,   14.,   25.,  40.]
+
+# Precompute slopes for Cubic Bézier curves
+def compute_symmetric_slopes(x, y):
+  n = len(x)
+  if n < 2:
+    raise ValueError("At least two points are required to compute slopes.")
+  m = np.zeros(n)
+  for i in range(n):
+    if i == 0:
+      m[i] = (y[i+1] - y[i]) / (x[i+1] - x[i])
+    elif i == n-1:
+      m[i] = (y[i] - y[i-1]) / (x[i] - x[i-1])
+    else:
+      m[i] = ((y[i+1] - y[i]) / (x[i+1] - x[i]) + (y[i] - y[i-1]) / (x[i] - x[i-1])) / 2
+  return m
+
+
+MAX_ACCEL_SLOPES = {
+  mode: compute_symmetric_slopes(MAX_ACCEL_BREAKPOINTS, values)
+  for mode, values in MAX_ACCEL_PROFILES.items()
+}
+
+MIN_ACCEL_SLOPES = {
+  mode: compute_symmetric_slopes(MIN_ACCEL_BREAKPOINTS, values)
+  for mode, values in MIN_ACCEL_PROFILES.items()
+}
+
+# Hermite interpolation function
+def hermite_interpolate(x, xp, yp, slopes, mode):
+  # Clip x inside the domain
+  x = np.clip(x, xp[0], xp[-1])
+
+  # Find segment
+  idx = np.searchsorted(xp, x) - 1
+  idx = np.clip(idx, 0, len(slopes[mode]) - 1)
+
+  x0, x1 = xp[idx], xp[idx+1]
+  y0, y1 = yp[idx], yp[idx+1]
+  m0, m1 = slopes[mode][idx], slopes[mode][idx+1]
+
+  t = (x - x0) / (x1 - x0)
+  h00 = 2*t**3 - 3*t**2 + 1
+  h10 = t**3 - 2*t**2 + t
+  h01 = -2*t**3 + 3*t**2
+  h11 = t**3 - t**2
+
+  interpolated = (h00 * y0) + (h10 * (x1 - x0) * m0) + (h01 * y1) + (h11 * (x1 - x0) * m1)
+  return interpolated
+
+# Final functions to call
+def get_max_accel_hermite(v_ego: float, mode: str = "normal") -> float:
+  return float(hermite_interpolate(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[mode], MAX_ACCEL_SLOPES, mode))
+
+def get_min_accel_hermite(v_ego: float, mode: str = "normal") -> float:
+  return float(hermite_interpolate(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[mode], MIN_ACCEL_SLOPES, mode))

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -9,13 +9,14 @@ from cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.sunnypilot.models.helpers import get_active_model_runner
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
-
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 
 
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, mpc):
     self.dec = DynamicExperimentalController(CP, mpc)
+    self.accel_controller = AccelController()
     self.is_stock = get_active_model_runner() == custom.ModelManagerSP.Runner.stock
 
   def get_mpc_mode(self) -> str | None:
@@ -26,6 +27,7 @@ class LongitudinalPlannerSP:
 
   def update(self, sm: messaging.SubMaster) -> None:
     self.dec.update(sm)
+    self.accel_controller.update()
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
     plan_sp_send = messaging.new_message('longitudinalPlanSP')

sunnypilot/selfdrive/controls/lib/param_store.py

@@ -0,0 +1,35 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import capnp
+
+from cereal import custom
+
+from opendbc.car import structs
+from openpilot.common.params import Params
+
+
+class ParamStore:
+  keys: list[str]
+  values: dict[str, str]
+
+  def __init__(self, CP: structs.CarParams):
+    universal_params: list[str] = []
+    brand_params: list[str] = []
+
+    self.keys = universal_params + brand_params
+    self.values = {}
+
+  def update(self, params: Params) -> None:
+    self.values = {k: params.get(k, encoding='utf8') or "0" for k in self.keys}
+
+  def publish(self) -> list[capnp.lib.capnp._DynamicStructBuilder]:
+    params_list: list[capnp.lib.capnp._DynamicStructBuilder] = []
+
+    for k in self.keys:
+      params_list.append(custom.CarControlSP.Param(key=k, value=self.values[k]))
+
+    return params_list

sunnypilot/selfdrive/selfdrived/events.py

@@ -64,7 +64,7 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
   },
 
   EventNameSP.silentBrakeHold: {
-    ET.USER_DISABLE: EngagementAlert(AudibleAlert.none),
+    ET.WARNING: EngagementAlert(AudibleAlert.none),
     ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"),
   },
 
@@ -134,4 +134,8 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.WARNING: wrong_car_mode_alert,
   },
 
+  EventNameSP.pedalPressedAlertOnly: {
+    ET.WARNING: NoEntryAlert("Pedal Pressed")
+  }
+
 }

system/manager/manager.py

@@ -5,7 +5,7 @@ import signal
 import sys
 import traceback
 
-from cereal import log
+from cereal import log, custom
 import cereal.messaging as messaging
 import openpilot.system.sentry as sentry
 from openpilot.common.params import Params, ParamKeyType
@@ -42,13 +42,14 @@ def manager_init() -> None:
   ]
 
   sunnypilot_default_params: list[tuple[str, str | bytes]] = [
+    ("AccelPersonality", str(custom.LongitudinalPlanSP.AccelerationPersonality.stock)),
     ("AutoLaneChangeTimer", "0"),
     ("AutoLaneChangeBsmDelay", "0"),
     ("DynamicExperimentalControl", "0"),
     ("HyundaiLongitudinalTuning", "0"),
     ("Mads", "1"),
     ("MadsMainCruiseAllowed", "1"),
-    ("MadsPauseLateralOnBrake", "0"),
+    ("MadsSteeringMode", "0"),
     ("MadsUnifiedEngagementMode", "1"),
     ("MaxTimeOffroad", "1800"),
     ("ModelManager_LastSyncTime", "0"),