MADS: Steering Mode on Brake Pedal Press (#924)

* MADS: Steering Mode on Brake Pedal Press

* bump

* bump

* descriptions

* bump

* bump

* no tesla or rivian yet

* codecov v5

* Revert "codecov v5"

This reverts commit a347e3fb27c0fbf2510b69fe8148b254fa93b8de.

* cleanup

* refactor description

* sync name

* fix

* make sure we don't allow if brake was already being pressed

* fix

* diff

* fix description

* in another PR

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

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/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc

@@ -39,17 +39,12 @@ MadsSettings::MadsSettings(QWidget *parent) : QWidget(parent) {
     "");
   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 +56,15 @@ 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))
+  );
+
+  madsSteeringMode->setEnabled(_offroad);
+  madsSteeringMode->setDescription(madsSteeringModeDescription(steering_mode));
+  madsSteeringMode->showDescription();
 
   offroad = _offroad;
 }

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

@@ -12,6 +12,18 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
 
+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 +44,37 @@ private:
 
   ParamControl *madsMainCruiseToggle;
   ParamControl *madsUnifiedEngagementModeToggle;
-  ButtonParamControl *madsPauseLateralOnBrake;
+  ButtonParamControl *madsSteeringMode;
+
+  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<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) {
+    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;
+  }
 };

sunnypilot/mads/helpers.py

@@ -11,15 +11,30 @@ 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 read_steering_mode_param(params: Params):
+  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(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):

sunnypilot/mads/mads.py

@@ -10,6 +10,7 @@ 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
 from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT
 
 State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
@@ -46,7 +47,7 @@ class ModularAssistiveDrivingSystem:
     # 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.params)
     self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode")
 
   def read_params(self):
@@ -60,7 +61,7 @@ class ModularAssistiveDrivingSystem:
     return False
 
   def should_silent_lkas_enable(self, CS: structs.CarState) -> bool:
-    if self.pause_lateral_on_brake_toggle and self.pedal_pressed_non_gas_pressed(CS):
+    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):
@@ -117,7 +118,7 @@ class ModularAssistiveDrivingSystem:
         self.replace_event(EventName.parkBrake, EventNameSP.silentParkBrake)
         self.transition_paused_state()
 
-      if self.pause_lateral_on_brake_toggle:
+      if self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE:
         if self.pedal_pressed_non_gas_pressed(CS):
           self.transition_paused_state()
 
@@ -163,6 +164,16 @@ class ModularAssistiveDrivingSystem:
       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)

system/manager/manager.py

@@ -48,7 +48,7 @@ def manager_init() -> None:
     ("HyundaiLongitudinalTuning", "0"),
     ("Mads", "1"),
     ("MadsMainCruiseAllowed", "1"),
-    ("MadsPauseLateralOnBrake", "0"),
+    ("MadsSteeringMode", "0"),
     ("MadsUnifiedEngagementMode", "1"),
     ("MaxTimeOffroad", "1800"),
     ("ModelManager_LastSyncTime", "0"),