diff --git a/common/params_keys.h b/common/params_keys.h index cd8208bbb1..be038f5a2a 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -135,7 +135,7 @@ inline static std::unordered_map keys = { // MADS params {"Mads", PERSISTENT | BACKUP}, {"MadsMainCruiseAllowed", PERSISTENT | BACKUP}, - {"MadsPauseLateralOnBrake", PERSISTENT | BACKUP}, + {"MadsSteeringMode", PERSISTENT | BACKUP}, {"MadsUnifiedEngagementMode", PERSISTENT | BACKUP}, // Model Manager params diff --git a/opendbc_repo b/opendbc_repo index b46741a413..748d962355 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit b46741a41301bf63bbf42c4217acbc3f42f615b9 +Subproject commit 748d96235571fa51b8fd28fb14f217bc5fd6cfb1 diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index 3025d0091b..c0ff2db399 100644 --- a/selfdrive/pandad/pandad.cc +++ b/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 &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(); diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc index fa0f7f8bd4..fd5d2fd5c8 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc @@ -39,17 +39,19 @@ MadsSettings::MadsSettings(QWidget *parent) : QWidget(parent) { ""); list->addItem(madsUnifiedEngagementModeToggle); - // Pause Lateral On Brake - std::vector lateral_on_brake_texts{tr("Remain Active"), tr("Pause Steering")}; - madsPauseLateralOnBrake = new ButtonParamControl( - "MadsPauseLateralOnBrake", - tr("Steering Mode After Braking"), + // Steering Mode On Brake + std::vector lateral_on_brake_texts{tr("Remain Active"), tr("Pause Steering"), tr("Disengage")}; + madsSteeringMode = new ButtonParamControl( + "MadsSteeringMode", + 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; } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h index fccb4cbb84..312c4f0db0 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h @@ -32,5 +32,5 @@ private: ParamControl *madsMainCruiseToggle; ParamControl *madsUnifiedEngagementModeToggle; - ButtonParamControl *madsPauseLateralOnBrake; + ButtonParamControl *madsSteeringMode; }; diff --git a/sunnypilot/mads/helpers.py b/sunnypilot/mads/helpers.py index 547ea0adfd..5f33a08cdb 100644 --- a/sunnypilot/mads/helpers.py +++ b/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: - 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): diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 46edd7804e..7651e87327 100644 --- a/sunnypilot/mads/mads.py +++ b/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 @@ -45,7 +46,7 @@ class ModularAssistiveDrivingSystem: # read params on init self.enabled_toggle = self.params.get_bool("Mads") self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed") and not self.no_main_cruise - 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): @@ -87,11 +88,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) @@ -127,6 +128,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) diff --git a/system/manager/manager.py b/system/manager/manager.py index 6bd0a557c8..eb6c60c9ee 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -48,7 +48,7 @@ def manager_init() -> None: ("DynamicExperimentalControl", "0"), ("Mads", "1"), ("MadsMainCruiseAllowed", "1"), - ("MadsPauseLateralOnBrake", "0"), + ("MadsSteeringMode", "0"), ("MadsUnifiedEngagementMode", "1"), ("ModelManager_LastSyncTime", "0"), ("ModelManager_ModelsCache", ""),