From 1e74000f79de4843197b3c9b031707026d33af8a Mon Sep 17 00:00:00 2001 From: Discountchubbs <159560811+Discountchubbs@users.noreply.github.com> Date: Thu, 15 May 2025 20:13:40 -0700 Subject: [PATCH 01/23] Controls: Extension (#858) * Add enhanced HyundaiCAN extension Introduced HyundaiCanEXT for improved object parsing and control integration, leveraging new fields like lead distance and relative speed in CarControlSP. Refactored controlsd to utilize a modular design with ControlsdExt for additional sunnypilot specific functionality. * self.cc_sp * Refactor CarControlSP handling for improved state updates * refactor * bool * Refactor controlsd SP communication logic Update `publish_sp` method to include `CC_SP` parameter and refine SP SubMaster to exclude `radarState`. Remove custom lead vehicle state processing as it is no longer needed. * remove in this pr * bump * start cleanup * inherit instead * even more! * lint * type hint * use the same objects for submaster and pubmaster * more --------- Co-authored-by: Jason Wen --- cereal/custom.capnp | 10 ++-- selfdrive/controls/controlsd.py | 40 ++++++-------- .../selfdrive/controls/controlsd_ext.py | 53 +++++++++++++++++++ 3 files changed, 73 insertions(+), 30 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/controlsd_ext.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 10fc83eb01..2a72a59a10 100644 --- a/cereal/custom.capnp +++ b/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; @@ -176,14 +176,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 +191,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; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 69f468e4cd..a5d97c3db0 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -2,7 +2,7 @@ import math 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 +19,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 +28,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.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 +101,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 +146,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,18 +222,13 @@ 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 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) + CC, lac_log = self.state_control() + self.publish(CC, lac_log) + self.run_ext(self.sm, self.pm) rk.monitor_time() diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py new file mode 100644 index 0000000000..68131001ad --- /dev/null +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -0,0 +1,53 @@ +""" +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 openpilot.common.params import Params +from openpilot.common.swaglog import cloudlog + + +class ControlsExt: + def __init__(self, params: Params): + 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'] + + @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) + + @staticmethod + def state_control_ext(sm: messaging.SubMaster) -> custom.CarControlSP: + CC_SP = custom.CarControlSP.new_message() + + # MADS state + CC_SP.mads = sm['selfdriveStateSP'].mads + + 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) From 60cf2055598ca31d486ead3bb725e87179ef020d Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 16 May 2025 11:41:50 -0400 Subject: [PATCH 02/23] MADS: Steering Mode on Brake Pedal Press --- common/params_keys.h | 2 +- opendbc_repo | 2 +- selfdrive/pandad/pandad.cc | 4 +- .../offroad/settings/lateral/mads_settings.cc | 18 ++++---- .../offroad/settings/lateral/mads_settings.h | 2 +- sunnypilot/mads/helpers.py | 44 ++++++++++++++----- sunnypilot/mads/mads.py | 21 ++++++--- system/manager/manager.py | 2 +- 8 files changed, 65 insertions(+), 30 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index 97f23d5108..d3a9ea7d6d 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 36461557c4..4791abbc77 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 36461557c47176b16a081ea40195bc327a34d97f +Subproject commit 4791abbc77b2a95739ffa7e41d8a66ecdf05c198 diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index d3dbba8182..1b4defd2b9 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 831a75699e..95f39bf43a 100644 --- a/sunnypilot/mads/helpers.py +++ b/sunnypilot/mads/helpers.py @@ -11,15 +11,33 @@ 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(CP: structs.CarParams, params: Params): + if CP.brand in ("rivian", "tesla"): + 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 +49,16 @@ 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 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.put("MadsSteeringMode", "2") + params.put_bool("MadsUnifiedEngagementMode", True) + params.remove("MadsMainCruiseAllowed") - # 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 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.put("MadsSteeringMode", "2") + params.put_bool("MadsUnifiedEngagementMode", True) + params.remove("MadsMainCruiseAllowed") diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 9aeb67972f..0c52e3dc23 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -9,6 +9,7 @@ from cereal import log, custom from opendbc.car import structs 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 @@ -30,6 +31,7 @@ class ModularAssistiveDrivingSystem: 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) @@ -40,14 +42,17 @@ class ModularAssistiveDrivingSystem: if self.selfdrive.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD): self.allow_always = True + if self.selfdrive.CP.brand in ("rivian", "tesla"): + 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.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed") and not self.no_main_cruise + self.steering_mode_on_brake = read_steering_mode_param(self.selfdrive.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.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed") and not self.no_main_cruise self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode") def update_events(self, CS: structs.CarState): @@ -85,7 +90,7 @@ 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() @@ -120,7 +125,13 @@ class ModularAssistiveDrivingSystem: if self.selfdrive.CS_prev.cruiseState.available: self.events_sp.add(EventNameSP.lkasDisable) - if not (self.pause_lateral_on_brake_toggle and CS.brakePressed) and \ + 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) + + 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) diff --git a/system/manager/manager.py b/system/manager/manager.py index 90df415a8c..71f2bc525a 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"), From f06e86a13735d6affbb8ee6933f4e859b9a0d9fb Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 16 May 2025 12:21:44 -0400 Subject: [PATCH 03/23] bump --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 4791abbc77..71fa888643 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 4791abbc77b2a95739ffa7e41d8a66ecdf05c198 +Subproject commit 71fa8886435c63da7d3a9e499ba8c8e7edfe5793 From 19be7afde52fd6ea4cf826fce04a5d0e046fc738 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 16 May 2025 13:28:50 -0400 Subject: [PATCH 04/23] bump --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 71fa888643..bf32e284c6 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 71fa8886435c63da7d3a9e499ba8c8e7edfe5793 +Subproject commit bf32e284c6db099f2b52599f3b80e4f14afc7fa8 From 8242e0f8e120162cbf2697a7bb46c584e1ca3a2c Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 16 May 2025 14:53:30 -0400 Subject: [PATCH 05/23] descriptions --- .../offroad/settings/lateral/mads_settings.cc | 22 +++++++++++++--- .../offroad/settings/lateral/mads_settings.h | 26 +++++++++++++++++++ 2 files changed, 44 insertions(+), 4 deletions(-) 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 fd5d2fd5c8..ae0a477247 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc @@ -44,14 +44,15 @@ MadsSettings::MadsSettings(QWidget *parent) : QWidget(parent) { 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.\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); + QObject::connect(madsSteeringMode, &ButtonParamControl::buttonToggled, [=] { + updateToggles(offroad); + }); list->addItem(madsSteeringMode); + madsSteeringMode->showDescription(); QObject::connect(uiState(), &UIState::offroadTransition, this, &MadsSettings::updateToggles); @@ -63,7 +64,20 @@ void MadsSettings::showEvent(QShowEvent *event) { } void MadsSettings::updateToggles(bool _offroad) { + auto mads_steering_mode_param = std::atoi(params.get("MadsSteeringMode").c_str()); + + MadsSteeringMode steering_mode; + if (mads_steering_mode_param == int(MadsSteeringMode::REMAIN_ACTIVE)) { + steering_mode = MadsSteeringMode::REMAIN_ACTIVE; + } else if (mads_steering_mode_param == int(MadsSteeringMode::PAUSE_STEERING)) { + steering_mode = MadsSteeringMode::PAUSE_STEERING; + } else { + steering_mode = 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 312c4f0db0..022f7eb3bd 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,12 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" +enum class MadsSteeringMode { + REMAIN_ACTIVE, + PAUSE_STEERING, + DISENGAGE, +}; + class MadsSettings : public QWidget { Q_OBJECT @@ -33,4 +39,24 @@ private: ParamControl *madsMainCruiseToggle; ParamControl *madsUnifiedEngagementModeToggle; ButtonParamControl *madsSteeringMode; + + static QString madsSteeringModeDescription(MadsSteeringMode mode) { + QString remain_active_str = tr("Remain Active: ALC will remain active when the brake pedal is pressed."); + QString pause_steering_str = tr("Pause Steering: ALC will pause steering when the brake pedal is pressed."); + QString disengage_str = tr("Disengage: ALC will disengage when the brake pedal is pressed."); + + if (mode == MadsSteeringMode::REMAIN_ACTIVE) { + remain_active_str = "" + remain_active_str + ""; + } else if (mode == MadsSteeringMode::PAUSE_STEERING) { + pause_steering_str = "" + pause_steering_str + ""; + } else if (mode == MadsSteeringMode::DISENGAGE) { + disengage_str = "" + disengage_str + ""; + } + + return QString("%1

%2
%3
%4") + .arg(tr("Choose how Automatic Lane Centering (ALC) behaves after the brake pedal is manually pressed in sunnypilot.")) + .arg(remain_active_str) + .arg(pause_steering_str) + .arg(disengage_str); + } }; From 5a80c8947faf0efa986847d67c045cc08f668416 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 16 May 2025 15:26:43 -0400 Subject: [PATCH 06/23] bump --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index bf32e284c6..08f541826a 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit bf32e284c6db099f2b52599f3b80e4f14afc7fa8 +Subproject commit 08f541826ada95be662bab4b48db44bd35c506ff From f3f6626dc8fa42b27b769cfd01a8c5d41cd1c891 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 16 May 2025 15:36:57 -0400 Subject: [PATCH 07/23] bump --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 08f541826a..a051754777 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 08f541826ada95be662bab4b48db44bd35c506ff +Subproject commit a051754777785cc93eb0be35b081d19f7fbce595 From 89b4ac76abfc544e3d2dba4085081e7c3530d1fe Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 16 May 2025 15:46:21 -0400 Subject: [PATCH 08/23] no tesla or rivian yet --- sunnypilot/mads/helpers.py | 23 ++++++++--------------- sunnypilot/mads/mads.py | 8 ++------ 2 files changed, 10 insertions(+), 21 deletions(-) diff --git a/sunnypilot/mads/helpers.py b/sunnypilot/mads/helpers.py index 95f39bf43a..42a76589c3 100644 --- a/sunnypilot/mads/helpers.py +++ b/sunnypilot/mads/helpers.py @@ -18,9 +18,6 @@ class MadsSteeringModeOnBrake: def read_steering_mode_param(CP: structs.CarParams, params: Params): - if CP.brand in ("rivian", "tesla"): - return MadsSteeringModeOnBrake.DISENGAGE - try: return int(params.get("MadsSteeringMode")) except (ValueError, TypeError): @@ -49,16 +46,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.put("MadsSteeringMode", "2") - params.put_bool("MadsUnifiedEngagementMode", True) - params.remove("MadsMainCruiseAllowed") + # 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.put("MadsSteeringMode", "2") - params.put_bool("MadsUnifiedEngagementMode", True) - params.remove("MadsMainCruiseAllowed") + # 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") diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 0c52e3dc23..0e0dde3225 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -31,7 +31,6 @@ class ModularAssistiveDrivingSystem: 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) @@ -42,17 +41,14 @@ class ModularAssistiveDrivingSystem: if self.selfdrive.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD): self.allow_always = True - if self.selfdrive.CP.brand in ("rivian", "tesla"): - 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") and not self.no_main_cruise + self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed") self.steering_mode_on_brake = read_steering_mode_param(self.selfdrive.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") and not self.no_main_cruise + self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed") self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode") def update_events(self, CS: structs.CarState): From 52dc4141c5e5ec3b4809ad12bb6eb10731a6e5a5 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 17 May 2025 10:09:50 -0400 Subject: [PATCH 09/23] MADS: evaluate brakePressed and regenBraking for Pause Lateral mode (#926) * simplify * MADS: evaluate brakePressed and regenBraking for Pause Lateral mode * nah --- sunnypilot/mads/mads.py | 34 ++++++++++++++++++++++++++-------- 1 file changed, 26 insertions(+), 8 deletions(-) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 9aeb67972f..1132a0df2f 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -9,6 +9,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.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState @@ -24,6 +25,8 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) class ModularAssistiveDrivingSystem: def __init__(self, selfdrive): + self.CP = selfdrive.CP + self.CS_prev = selfdrive.CS_prev self.params = selfdrive.params self.enabled = False @@ -35,9 +38,10 @@ class ModularAssistiveDrivingSystem: 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 # read params on init @@ -50,6 +54,21 @@ class ModularAssistiveDrivingSystem: 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.CS_prev.gasPressed and self.disengage_on_accelerator): + return True + + 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): + return False + + if self.events_sp.contains_in_list(GEARS_ALLOW_PAUSED_SILENT): + return False + + return True + 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) @@ -86,7 +105,7 @@ class ModularAssistiveDrivingSystem: transition_paused_state() if self.pause_lateral_on_brake_toggle: - if CS.brakePressed: + if self.pedal_pressed_non_gas_pressed(CS): transition_paused_state() self.events.remove(EventName.preEnableStandstill) @@ -99,7 +118,7 @@ class ModularAssistiveDrivingSystem: update_unified_engagement_mode() else: if self.main_enabled_toggle: - if CS.cruiseState.available and not self.selfdrive.CS_prev.cruiseState.available: + if CS.cruiseState.available and not self.CS_prev.cruiseState.available: self.events_sp.add(EventNameSP.lkasEnable) for be in CS.buttonEvents: @@ -117,11 +136,10 @@ class ModularAssistiveDrivingSystem: if not CS.cruiseState.available: self.events.remove(EventName.buttonEnable) - if self.selfdrive.CS_prev.cruiseState.available: + if self.CS_prev.cruiseState.available: self.events_sp.add(EventNameSP.lkasDisable) - 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.should_silent_lkas_enable(CS): if self.state_machine.state == State.paused: self.events_sp.add(EventNameSP.silentLkasEnable) @@ -141,7 +159,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 From 7565dd2545488f0b2100c03c753f562da158898a Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 17 May 2025 10:34:38 -0400 Subject: [PATCH 10/23] MADS: Refactor Unified Engagement Mode (#927) --- sunnypilot/mads/mads.py | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 1132a0df2f..dbaec258ab 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -69,13 +69,20 @@ class ModularAssistiveDrivingSystem: return True - 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 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 update_events(self, CS: structs.CarState): def transition_paused_state(): if self.state_machine.state != State.paused: self.events_sp.add(EventNameSP.silentLkasDisable) @@ -115,7 +122,9 @@ class ModularAssistiveDrivingSystem: self.events.remove(EventName.manualRestart) if self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable): - update_unified_engagement_mode() + 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.CS_prev.cruiseState.available: From a347e3fb27c0fbf2510b69fe8148b254fa93b8de Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 17 May 2025 11:22:06 -0400 Subject: [PATCH 11/23] codecov v5 --- .github/workflows/selfdrive_tests.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index f8897258c6..9222f845b7 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -164,7 +164,7 @@ jobs: QT_QPA_PLATFORM=offscreen ./selfdrive/ui/tests/test_translations && \ chmod -R 777 /tmp/comma_download_cache" - name: "Upload coverage to Codecov" - uses: codecov/codecov-action@v4 + uses: codecov/codecov-action@v5 with: name: ${{ github.job }} env: @@ -217,7 +217,7 @@ jobs: ${{ env.RUN }} "ONNXCPU=1 $PYTEST selfdrive/test/process_replay/test_regen.py && \ chmod -R 777 /tmp/comma_download_cache" - name: "Upload coverage to Codecov" - uses: codecov/codecov-action@v4 + uses: codecov/codecov-action@v5 with: name: ${{ github.job }} env: @@ -253,7 +253,7 @@ jobs: NUM_JOBS: 4 JOB_ID: ${{ matrix.job }} - name: "Upload coverage to Codecov" - uses: codecov/codecov-action@v4 + uses: codecov/codecov-action@v5 with: name: ${{ github.job }}-${{ matrix.job }} env: From 26d4ab589f96e7fe3995e15e42e8ea0ad282b559 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 17 May 2025 11:31:04 -0400 Subject: [PATCH 12/23] Revert "codecov v5" This reverts commit a347e3fb27c0fbf2510b69fe8148b254fa93b8de. --- .github/workflows/selfdrive_tests.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 9222f845b7..f8897258c6 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -164,7 +164,7 @@ jobs: QT_QPA_PLATFORM=offscreen ./selfdrive/ui/tests/test_translations && \ chmod -R 777 /tmp/comma_download_cache" - name: "Upload coverage to Codecov" - uses: codecov/codecov-action@v5 + uses: codecov/codecov-action@v4 with: name: ${{ github.job }} env: @@ -217,7 +217,7 @@ jobs: ${{ env.RUN }} "ONNXCPU=1 $PYTEST selfdrive/test/process_replay/test_regen.py && \ chmod -R 777 /tmp/comma_download_cache" - name: "Upload coverage to Codecov" - uses: codecov/codecov-action@v5 + uses: codecov/codecov-action@v4 with: name: ${{ github.job }} env: @@ -253,7 +253,7 @@ jobs: NUM_JOBS: 4 JOB_ID: ${{ matrix.job }} - name: "Upload coverage to Codecov" - uses: codecov/codecov-action@v5 + uses: codecov/codecov-action@v4 with: name: ${{ github.job }}-${{ matrix.job }} env: From b92d717f2fb9114d0153214be487c7db565a056d Mon Sep 17 00:00:00 2001 From: Discountchubbs <159560811+Discountchubbs@users.noreply.github.com> Date: Sat, 17 May 2025 09:33:18 -0700 Subject: [PATCH 13/23] Hyundai: add more FW versions to enable radar tracks debug script (#804) * fw version for my car, and added scc radar module firmware to enable radar points * bump points * bump --------- Co-authored-by: DevTekVE Co-authored-by: Jason Wen --- selfdrive/debug/hyundai_enable_radar_points.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py index 93f5949eac..30e2949378 100755 --- a/selfdrive/debug/hyundai_enable_radar_points.py +++ b/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__": From 80679b74e6256637b35dfe15cfec925e68c83242 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 17 May 2025 23:07:05 -0400 Subject: [PATCH 14/23] MADS: prep for refactor (#930) * MADS: prep for refactor * no longer --- sunnypilot/mads/mads.py | 51 ++++++++++++++++++++--------------------- 1 file changed, 25 insertions(+), 26 deletions(-) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index dbaec258ab..e6665484ae 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -26,7 +26,6 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) class ModularAssistiveDrivingSystem: def __init__(self, selfdrive): self.CP = selfdrive.CP - self.CS_prev = selfdrive.CS_prev self.params = selfdrive.params self.enabled = False @@ -55,7 +54,7 @@ class ModularAssistiveDrivingSystem: 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.CS_prev.gasPressed and self.disengage_on_accelerator): + 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 @@ -82,38 +81,38 @@ class ModularAssistiveDrivingSystem: return False + 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 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 self.pedal_pressed_non_gas_pressed(CS): - transition_paused_state() + self.transition_paused_state() self.events.remove(EventName.preEnableStandstill) self.events.remove(EventName.belowEngageSpeed) @@ -127,7 +126,7 @@ class ModularAssistiveDrivingSystem: self.events.remove(EventName.buttonEnable) else: if self.main_enabled_toggle: - if CS.cruiseState.available and not self.CS_prev.cruiseState.available: + if CS.cruiseState.available and not self.selfdrive.CS_prev.cruiseState.available: self.events_sp.add(EventNameSP.lkasEnable) for be in CS.buttonEvents: @@ -145,7 +144,7 @@ class ModularAssistiveDrivingSystem: if not CS.cruiseState.available: self.events.remove(EventName.buttonEnable) - if self.CS_prev.cruiseState.available: + if self.selfdrive.CS_prev.cruiseState.available: self.events_sp.add(EventNameSP.lkasDisable) if self.should_silent_lkas_enable(CS): @@ -158,7 +157,7 @@ class ModularAssistiveDrivingSystem: 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) + self.replace_event(EventName.wrongCarMode, EventNameSP.wrongCarModeAlertOnly) else: self.events.remove(EventName.wrongCarMode) From 4dbabf4e24a6dddf477858ed149d1a447c55b2de Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 17 May 2025 23:34:55 -0400 Subject: [PATCH 15/23] MADS: retain `pedalPressed` alert only (#928) * MADS: Refactor Unified Engagement Mode * init * bring back pedal pressed event while trying to engage long * MADS: prep for refactor * no longer * this is cleaner? * in another pr --- cereal/custom.capnp | 1 + sunnypilot/mads/mads.py | 7 ++++++- sunnypilot/selfdrive/selfdrived/events.py | 4 ++++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 2a72a59a10..eb932b484e 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -144,6 +144,7 @@ struct OnroadEventSP @0xda96579883444c35 { hyundaiRadarTracksConfirmed @13; experimentalModeSwitched @14; wrongCarModeAlertOnly @15; + pedalPressedAlertOnly @16; } } diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index e6665484ae..456be01882 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -120,7 +120,12 @@ class ModularAssistiveDrivingSystem: self.events.remove(EventName.cruiseDisabled) self.events.remove(EventName.manualRestart) - if self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable): + selfdrive_enable_events = self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable) + + 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) diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index 979de0a80c..67672649c5 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -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") + } + } From 6d516a7704410b69a2707dfcb856eba5be519f81 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 18 May 2025 00:18:41 -0400 Subject: [PATCH 16/23] MADS: `wrongCarMode` alert only with selfdrive enable (#931) * MADS: Refactor Unified Engagement Mode * init * bring back pedal pressed event while trying to engage long * MADS: prep for refactor * no longer * this is cleaner? * in another pr * Revert "in another pr" This reverts commit 31aec8a7aabc798c1b19c365488efe04a47869e9. * less * rename * type hint --- sunnypilot/mads/mads.py | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 456be01882..d5ce9354c1 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -81,6 +81,13 @@ class ModularAssistiveDrivingSystem: 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) @@ -121,11 +128,12 @@ class ModularAssistiveDrivingSystem: self.events.remove(EventName.manualRestart) 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) @@ -160,11 +168,6 @@ class ModularAssistiveDrivingSystem: 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): - self.replace_event(EventName.wrongCarMode, EventNameSP.wrongCarModeAlertOnly) - else: - self.events.remove(EventName.wrongCarMode) def update(self, CS: structs.CarState): if not self.enabled_toggle: From 72a462e82d25fca4edd15cda25b6dc29994d62b7 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 18 May 2025 01:48:19 -0400 Subject: [PATCH 17/23] cleanup --- sunnypilot/mads/mads.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 2eeeafe202..8b7847c990 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -47,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.steering_mode_on_brake = read_steering_mode_param(self.selfdrive.CP, self.params) + 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): From 29d984e42fd9ec4ec449e76d6d5ac7390e4d900e Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 18 May 2025 01:48:27 -0400 Subject: [PATCH 18/23] refactor description --- .../offroad/settings/lateral/mads_settings.cc | 7 ++- .../offroad/settings/lateral/mads_settings.h | 54 ++++++++++++------- 2 files changed, 39 insertions(+), 22 deletions(-) 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 ae0a477247..a653ccbbff 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc @@ -40,13 +40,12 @@ MadsSettings::MadsSettings(QWidget *parent) : QWidget(parent) { list->addItem(madsUnifiedEngagementModeToggle); // 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"), "", "", - lateral_on_brake_texts, + madsSteeringModeTexts(), 500); QObject::connect(madsSteeringMode, &ButtonParamControl::buttonToggled, [=] { updateToggles(offroad); @@ -67,9 +66,9 @@ void MadsSettings::updateToggles(bool _offroad) { auto mads_steering_mode_param = std::atoi(params.get("MadsSteeringMode").c_str()); MadsSteeringMode steering_mode; - if (mads_steering_mode_param == int(MadsSteeringMode::REMAIN_ACTIVE)) { + if (mads_steering_mode_param == static_cast(MadsSteeringMode::REMAIN_ACTIVE)) { steering_mode = MadsSteeringMode::REMAIN_ACTIVE; - } else if (mads_steering_mode_param == int(MadsSteeringMode::PAUSE_STEERING)) { + } else if (mads_steering_mode_param == static_cast(MadsSteeringMode::PAUSE_STEERING)) { steering_mode = MadsSteeringMode::PAUSE_STEERING; } else { steering_mode = MadsSteeringMode::DISENGAGE; 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 022f7eb3bd..73715f998b 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h @@ -13,9 +13,15 @@ #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" enum class MadsSteeringMode { - REMAIN_ACTIVE, - PAUSE_STEERING, - DISENGAGE, + REMAIN_ACTIVE = 0, + PAUSE_STEERING = 1, + DISENGAGE = 2, +}; + +struct MadsSteeringModeOption { + MadsSteeringMode mode; + QString display_text; + QString description; }; class MadsSettings : public QWidget { @@ -40,23 +46,35 @@ private: ParamControl *madsUnifiedEngagementModeToggle; ButtonParamControl *madsSteeringMode; - static QString madsSteeringModeDescription(MadsSteeringMode mode) { - QString remain_active_str = tr("Remain Active: ALC will remain active when the brake pedal is pressed."); - QString pause_steering_str = tr("Pause Steering: ALC will pause steering when the brake pedal is pressed."); - QString disengage_str = tr("Disengage: ALC will disengage when the brake pedal is pressed."); + 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_STEERING, tr("Pause"), tr("Pause: ALC will pause steering when the brake pedal is pressed.")}, + {MadsSteeringMode::DISENGAGE, tr("Disengage"), tr("Disengage: ALC will disengage when the brake pedal is pressed.")}, + }; + return options; + } - if (mode == MadsSteeringMode::REMAIN_ACTIVE) { - remain_active_str = "" + remain_active_str + ""; - } else if (mode == MadsSteeringMode::PAUSE_STEERING) { - pause_steering_str = "" + pause_steering_str + ""; - } else if (mode == MadsSteeringMode::DISENGAGE) { - disengage_str = "" + disengage_str + ""; + 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 QString("%1

%2
%3
%4") - .arg(tr("Choose how Automatic Lane Centering (ALC) behaves after the brake pedal is manually pressed in sunnypilot.")) - .arg(remain_active_str) - .arg(pause_steering_str) - .arg(disengage_str); + return result; } }; From ff3b0d821f9ad09934c43f3e76a32d148f240c28 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 18 May 2025 01:52:14 -0400 Subject: [PATCH 19/23] sync name --- .../sunnypilot/qt/offroad/settings/lateral/mads_settings.cc | 4 ++-- .../ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) 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 a653ccbbff..be638696a7 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc @@ -68,8 +68,8 @@ void MadsSettings::updateToggles(bool _offroad) { MadsSteeringMode steering_mode; if (mads_steering_mode_param == static_cast(MadsSteeringMode::REMAIN_ACTIVE)) { steering_mode = MadsSteeringMode::REMAIN_ACTIVE; - } else if (mads_steering_mode_param == static_cast(MadsSteeringMode::PAUSE_STEERING)) { - steering_mode = MadsSteeringMode::PAUSE_STEERING; + } else if (mads_steering_mode_param == static_cast(MadsSteeringMode::PAUSE)) { + steering_mode = MadsSteeringMode::PAUSE; } else { steering_mode = MadsSteeringMode::DISENGAGE; } 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 73715f998b..45a65c07c1 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h @@ -14,7 +14,7 @@ enum class MadsSteeringMode { REMAIN_ACTIVE = 0, - PAUSE_STEERING = 1, + PAUSE = 1, DISENGAGE = 2, }; @@ -49,7 +49,7 @@ private: 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_STEERING, tr("Pause"), tr("Pause: ALC will pause steering when the brake pedal is pressed.")}, + {MadsSteeringMode::PAUSE, tr("Pause"), tr("Pause: ALC will pause steering when the brake pedal is pressed.")}, {MadsSteeringMode::DISENGAGE, tr("Disengage"), tr("Disengage: ALC will disengage when the brake pedal is pressed.")}, }; return options; From 3aa9ddd3c76406796240494b8638c6bcee6441b7 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 18 May 2025 10:02:50 -0400 Subject: [PATCH 20/23] Revert "MADS: `wrongCarMode` alert only with selfdrive enable (#931)" (#932) This reverts commit 6d516a7704410b69a2707dfcb856eba5be519f81. --- sunnypilot/mads/mads.py | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index d5ce9354c1..456be01882 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -81,13 +81,6 @@ class ModularAssistiveDrivingSystem: 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) @@ -128,12 +121,11 @@ class ModularAssistiveDrivingSystem: self.events.remove(EventName.manualRestart) 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) @@ -168,6 +160,11 @@ class ModularAssistiveDrivingSystem: 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): + self.replace_event(EventName.wrongCarMode, EventNameSP.wrongCarModeAlertOnly) + else: + self.events.remove(EventName.wrongCarMode) def update(self, CS: structs.CarState): if not self.enabled_toggle: From 0a658827769fcfb3c69b7cbaabff455b7d93021f Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 18 May 2025 10:08:20 -0400 Subject: [PATCH 21/23] fix --- sunnypilot/mads/mads.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 8b7847c990..e505fa9a97 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -135,6 +135,9 @@ class ModularAssistiveDrivingSystem: 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) From b14e9e58a14e676b299cc6ce97499041eead49d0 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 18 May 2025 10:13:56 -0400 Subject: [PATCH 22/23] make sure we don't allow if brake was already being pressed --- sunnypilot/mads/mads.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index e505fa9a97..7ec38bd877 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -166,7 +166,13 @@ class ModularAssistiveDrivingSystem: if self.steering_mode_on_brake == MadsSteeringModeOnBrake.DISENGAGE: if self.pedal_pressed_non_gas_pressed(CS): - self.events_sp.add(EventNameSP.lkasDisable) + 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: From a93e788401efd659fc18af848dcf563e5c89ce33 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 18 May 2025 10:14:03 -0400 Subject: [PATCH 23/23] Reapply "MADS: wrongCarMode alert only with selfdrive enable (#931)" (#933) * Revert "MADS: `wrongCarMode` alert only with selfdrive enable (#931)" This reverts commit 6d516a7704410b69a2707dfcb856eba5be519f81. * Reapply "MADS: `wrongCarMode` alert only with selfdrive enable (#931)" This reverts commit c9487597e4ae4aa10a7111d963c5933d472bc88e. * Reapply "MADS: `wrongCarMode` alert only with selfdrive enable (#931)" This reverts commit c9487597e4ae4aa10a7111d963c5933d472bc88e. --- sunnypilot/mads/mads.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 456be01882..e3ee7cd832 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -81,6 +81,13 @@ class ModularAssistiveDrivingSystem: 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) @@ -121,6 +128,10 @@ class ModularAssistiveDrivingSystem: self.events.remove(EventName.manualRestart) 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): @@ -160,11 +171,6 @@ class ModularAssistiveDrivingSystem: 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): - self.replace_event(EventName.wrongCarMode, EventNameSP.wrongCarModeAlertOnly) - else: - self.events.remove(EventName.wrongCarMode) def update(self, CS: structs.CarState): if not self.enabled_toggle: