Merge remote-tracking branch 'sunnypilot/sunnypilot/mads-disengage-on-brake' into mads-tesla

# Conflicts:
#	opendbc_repo
#	selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc
#	selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h
#	sunnypilot/mads/helpers.py
#	sunnypilot/mads/mads.py
This commit is contained in:
Jason Wen
2025-05-18 11:15:18 -04:00
9 changed files with 235 additions and 82 deletions
+6 -5
View File
@@ -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;
@@ -144,6 +144,7 @@ struct OnroadEventSP @0xda96579883444c35 {
hyundaiRadarTracksConfirmed @13;
experimentalModeSwitched @14;
wrongCarModeAlertOnly @15;
pedalPressedAlertOnly @16;
}
}
@@ -176,14 +177,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 +192,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;
+15 -25
View File
@@ -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()
@@ -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__":
@@ -40,18 +40,18 @@ MadsSettings::MadsSettings(QWidget *parent) : QWidget(parent) {
list->addItem(madsUnifiedEngagementModeToggle);
// Steering Mode On Brake
std::vector<QString> 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.\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,
"",
madsSteeringModeTexts(),
500);
QObject::connect(madsSteeringMode, &ButtonParamControl::buttonToggled, [=] {
updateToggles(offroad);
});
list->addItem(madsSteeringMode);
madsSteeringMode->showDescription();
QObject::connect(uiState(), &UIState::offroadTransition, this, &MadsSettings::updateToggles);
@@ -63,7 +63,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 == static_cast<int>(MadsSteeringMode::REMAIN_ACTIVE)) {
steering_mode = MadsSteeringMode::REMAIN_ACTIVE;
} else if (mads_steering_mode_param == static_cast<int>(MadsSteeringMode::PAUSE)) {
steering_mode = MadsSteeringMode::PAUSE;
} else {
steering_mode = MadsSteeringMode::DISENGAGE;
}
madsSteeringMode->setEnabled(_offroad);
madsSteeringMode->setDescription(madsSteeringModeDescription(steering_mode));
madsSteeringMode->showDescription();
offroad = _offroad;
}
@@ -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
@@ -33,4 +45,36 @@ private:
ParamControl *madsMainCruiseToggle;
ParamControl *madsUnifiedEngagementModeToggle;
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 steering 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;
}
};
+86 -45
View File
@@ -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.helpers import MadsSteeringModeOnBrake, read_steering_mode_param
from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT
@@ -25,6 +26,7 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
class ModularAssistiveDrivingSystem:
def __init__(self, selfdrive):
self.CP = selfdrive.CP
self.params = selfdrive.params
self.enabled = False
@@ -37,9 +39,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
if self.selfdrive.CP.brand in ("rivian", "tesla"):
@@ -48,51 +51,80 @@ 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.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):
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 pedal_pressed_non_gas_pressed(self, CS: structs.CarState) -> bool:
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
def should_silent_lkas_enable(self, CS: structs.CarState) -> bool:
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):
return False
return True
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 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)
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 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 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.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE:
if CS.brakePressed:
transition_paused_state()
if self.pedal_pressed_non_gas_pressed(CS):
self.transition_paused_state()
self.events.remove(EventName.preEnableStandstill)
self.events.remove(EventName.belowEngageSpeed)
@@ -100,8 +132,19 @@ class ModularAssistiveDrivingSystem:
self.events.remove(EventName.cruiseDisabled)
self.events.remove(EventName.manualRestart)
if self.events.has(EventName.pcmEnable) or self.events.has(EventName.buttonEnable):
update_unified_engagement_mode()
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)
else:
if self.main_enabled_toggle:
if CS.cruiseState.available and not self.selfdrive.CS_prev.cruiseState.available:
@@ -126,13 +169,16 @@ class ModularAssistiveDrivingSystem:
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)
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 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.should_silent_lkas_enable(CS):
if self.state_machine.state == State.paused:
self.events_sp.add(EventNameSP.silentLkasEnable)
@@ -140,11 +186,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):
replace_event(EventName.wrongCarMode, EventNameSP.wrongCarModeAlertOnly)
else:
self.events.remove(EventName.wrongCarMode)
def update(self, CS: structs.CarState):
if not self.enabled_toggle:
@@ -152,7 +193,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
@@ -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)
@@ -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")
}
}