diff --git a/common/params_keys.h b/common/params_keys.h index 97f23d510..d3a9ea7d6 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -138,7 +138,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 36461557c..a05175477 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 36461557c47176b16a081ea40195bc327a34d97f +Subproject commit a051754777785cc93eb0be35b081d19f7fbce595 diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index d3dbba818..1b4defd2b 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 fa0f7f8bd..97b78399d 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,12 @@ 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"), - 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( + std::clamp(mads_steering_mode_param, static_cast(MadsSteeringMode::REMAIN_ACTIVE), static_cast(MadsSteeringMode::DISENGAGE)) + ); + + madsSteeringMode->setEnabled(_offroad); + madsSteeringMode->setDescription(madsSteeringModeDescription(steering_mode)); + madsSteeringMode->showDescription(); 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 fccb4cbb8..07ca07834 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h +++ b/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 &madsSteeringModeOptions() { + static const std::vector 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 madsSteeringModeTexts() { + std::vector 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 + "

"; + + for (const auto& option : madsSteeringModeOptions()) { + QString desc = option.description; + if (option.mode == mode) { + desc = "" + desc + ""; + } + result += desc + "
"; + } + + return result; + } }; diff --git a/sunnypilot/mads/helpers.py b/sunnypilot/mads/helpers.py index 831a75699..5f7f9840e 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 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 e3ee7cd83..38d38230a 100644 --- a/sunnypilot/mads/mads.py +++ b/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) diff --git a/system/manager/manager.py b/system/manager/manager.py index 90df415a8..71f2bc525 100755 --- a/system/manager/manager.py +++ b/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"),