diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 10fc83eb01..eb932b484e 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; @@ -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; diff --git a/opendbc_repo b/opendbc_repo index 7fa1ec698e..0d0d3d4463 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 7fa1ec698e3ea9d47949882cf1d415cfcea91354 +Subproject commit 0d0d3d4463d97adf4a761479bffdab36c30aa3f4 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/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__": 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..be638696a7 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc @@ -40,18 +40,18 @@ 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"), - 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(MadsSteeringMode::REMAIN_ACTIVE)) { + steering_mode = MadsSteeringMode::REMAIN_ACTIVE; + } else if (mads_steering_mode_param == static_cast(MadsSteeringMode::PAUSE)) { + steering_mode = MadsSteeringMode::PAUSE; + } 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..45a65c07c1 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 @@ -33,4 +45,36 @@ private: ParamControl *madsMainCruiseToggle; ParamControl *madsUnifiedEngagementModeToggle; 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 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 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/mads.py b/sunnypilot/mads/mads.py index 0c52e3dc23..69ad30e34c 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.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 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) 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") + } + }