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

* reimplement

* bump opendbc

* Apply suggestions from code review

Co-authored-by: DevTekVE <devtekve@gmail.com>

* update toggle positions

* translations from bot later

---------

Co-authored-by: DevTekVE <devtekve@gmail.com>

common/params_keys.h

@@ -132,7 +132,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/sunnypilot/mads_settings.cc

@@ -39,17 +39,19 @@ MadsSettings::MadsSettings(QWidget *parent) : QWidget(parent) {
     "");
   list->addItem(madsUnifiedEngagementModeToggle);
 
-  // Pause Lateral On Brake
+  // Steering Mode On Brake
-  std::vector<QString> lateral_on_brake_texts{tr("Remain Active"), tr("Pause Steering")};
+  std::vector<QString> lateral_on_brake_texts{tr("Remain Active"), tr("Pause Steering"), tr("Disengage")};
-  madsPauseLateralOnBrake = new ButtonParamControl(
+  madsSteeringMode = new ButtonParamControl(
-    "MadsPauseLateralOnBrake",
+    "MadsSteeringMode",
-    tr("Steering Mode After Braking"),
+    tr("Steering Mode on Brake Pedal"),
     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."),
+       "Remain Active: ALC will remain active even after the brake pedal is pressed.\n"
+       "Pause Steering: ALC will be paused when the brake pedal is manually pressed."),
+       "Disengage: ALC will be disengaged after the brake pedal is pressed.\n"
     "",
     lateral_on_brake_texts,
     500);
-  list->addItem(madsPauseLateralOnBrake);
+  list->addItem(madsSteeringMode);
 
   QObject::connect(uiState(), &UIState::offroadTransition, this, &MadsSettings::updateToggles);
 
@@ -61,7 +63,7 @@ void MadsSettings::showEvent(QShowEvent *event) {
 }
 
 void MadsSettings::updateToggles(bool _offroad) {
-  madsPauseLateralOnBrake->setEnabled(_offroad);
+  madsSteeringMode->setEnabled(_offroad);
 
   offroad = _offroad;
 }

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

@@ -32,5 +32,5 @@ private:
 
   ParamControl *madsMainCruiseToggle;
   ParamControl *madsUnifiedEngagementModeToggle;
-  ButtonParamControl *madsPauseLateralOnBrake;
+  ButtonParamControl *madsSteeringMode;
 };

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 f"{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:
+    if steering_mode == MadsSteeringModeOnBrake.DISENGAGE:
-      CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISENGAGE_LATERAL_ON_BRAKE
+      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

@@ -8,6 +8,7 @@ See the LICENSE.md file in the root directory for more details.
 from cereal import car, log, custom
 
 from opendbc.car.hyundai.values import HyundaiFlags
+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
@@ -41,7 +42,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):
@@ -83,11 +84,11 @@ class ModularAssistiveDrivingSystem:
         replace_event(EventName.parkBrake, EventNameSP.silentParkBrake)
         transition_paused_state()
 
-      if self.pause_lateral_on_brake_toggle:
+      if self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE:
         if CS.brakePressed:
           transition_paused_state()
 
-      if not (self.pause_lateral_on_brake_toggle and CS.brakePressed) and \
+      if not (self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE 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)
@@ -123,6 +124,12 @@ class ModularAssistiveDrivingSystem:
       if self.selfdrive.CS_prev.cruiseState.available:
         self.events_sp.add(EventNameSP.lkasDisable)
 
+    if self.steering_mode_on_brake == MadsSteeringModeOnBrake.DISENGAGE:
+      # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0
+      if (CS.brakePressed and (not self.selfdrive.CS_prev.brakePressed or not CS.standstill)) or \
+         (CS.regenBraking and (not self.selfdrive.CS_prev.regenBraking or not CS.standstill)):
+        self.events_sp.add(EventNameSP.lkasDisable)
+
     self.events.remove(EventName.pcmDisable)
     self.events.remove(EventName.buttonCancel)
     self.events.remove(EventName.pedalPressed)

system/manager/manager.py

@@ -46,7 +46,7 @@ def manager_init() -> None:
     ("DynamicExperimentalControl", "0"),
     ("Mads", "1"),
     ("MadsMainCruiseAllowed", "1"),
-    ("MadsPauseLateralOnBrake", "0"),
+    ("MadsSteeringMode", "0"),
     ("MadsUnifiedEngagementMode", "1"),
     ("ModelManager_LastSyncTime", "0"),
     ("ModelManager_ModelsCache", "")