From 2c0ab40c826ddbacfd8e9b87ecfddc3dd8961856 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 31 May 2024 23:01:31 +0000 Subject: [PATCH] Controls: Customizable Pause Lateral Speed --- CHANGELOGS.md | 3 ++ common/params.cc | 1 + selfdrive/car/interfaces.py | 12 +++++- selfdrive/controls/lib/desire_helper.py | 8 +++- selfdrive/manager/manager.py | 1 + .../sunnypilot/lane_change_settings.cc | 43 +++++++++++++++++-- .../offroad/sunnypilot/lane_change_settings.h | 17 ++++++++ 7 files changed, 78 insertions(+), 7 deletions(-) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 7fb80c26b6..4eeeb94121 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -29,6 +29,9 @@ sunnypilot - 0.9.7.0 (2024-05-xx) * When Forced Offroad mode is on, allows changing offroad-only settings even when the car is turned on * UPDATED: Auto Lane Change Timer -> Auto Lane Change by Blinker * NEW❗: New "Off" option to disable lane change by blinker +* UPDATED: Pause Lateral Below Speed with Blinker + * NEW❗: Customizable Pause Lateral Speed + * Pause lateral actuation with blinker when traveling below the desired speed selected. Default is 20 MPH or 32 km/h. * UPDATED: Hyundai CAN Longitudinal * Auto-enable radar tracks on platforms with applicable Mando radar * UPDATED: Hyundai CAN-FD Camera-based SCC diff --git a/common/params.cc b/common/params.cc index 920dd564da..7762c5cfec 100644 --- a/common/params.cc +++ b/common/params.cc @@ -289,6 +289,7 @@ std::unordered_map keys = { {"OsmWayTest", PERSISTENT}, {"OsmDownloadedDate", PERSISTENT}, {"PathOffset", PERSISTENT | BACKUP}, + {"PauseLateralSpeed", PERSISTENT | BACKUP}, {"QuietDrive", PERSISTENT | BACKUP}, {"RoadEdge", PERSISTENT | BACKUP}, {"ReverseAccChange", PERSISTENT | BACKUP}, diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index fc9715cb82..23eeb1c848 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -18,7 +18,7 @@ from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG from openpilot.selfdrive.car.values import PLATFORMS -from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN +from openpilot.selfdrive.controls.lib.desire_helper import get_min_lateral_speed from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, V_CRUISE_UNSET, get_friction from openpilot.selfdrive.controls.lib.events import Events from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel @@ -220,7 +220,9 @@ class CarInterfaceBase(ABC): self.gear_warning = 0 self.cruise_cancelled_btn = True self.acc_mads_combo = self.param_s.get_bool("AccMadsCombo") + self.is_metric = self.param_s.get_bool("IsMetric") self.below_speed_pause = self.param_s.get_bool("BelowSpeedPause") + self.pause_lateral_speed = int(self.param_s.get("PauseLateralSpeed", encoding="utf8")) self.prev_acc_mads_combo = False self.mads_event_lock = True self.gap_button_counter = 0 @@ -606,7 +608,9 @@ class CarInterfaceBase(ABC): if self.CP.openpilotLongitudinalControl: self.toggle_exp_mode(gap_button) - cs_out.belowLaneChangeSpeed = cs_out.vEgo < LANE_CHANGE_SPEED_MIN and self.below_speed_pause + lane_change_speed_min = get_min_lateral_speed(self.pause_lateral_speed, self.is_metric) + + cs_out.belowLaneChangeSpeed = cs_out.vEgo < lane_change_speed_min and self.below_speed_pause if cs_out.gearShifter in [GearShifter.park, GearShifter.reverse] or cs_out.doorOpen or \ (cs_out.seatbeltUnlatched and cs_out.gearShifter != GearShifter.park): @@ -708,6 +712,10 @@ class CarInterfaceBase(ABC): def sp_update_params(self): self.experimental_mode = self.param_s.get_bool("ExperimentalMode") self._frame += 1 + if self._frame % 100 == 0: + self.is_metric = self.param_s.get_bool("IsMetric") + self.below_speed_pause = self.param_s.get_bool("BelowSpeedPause") + self.pause_lateral_speed = int(self.param_s.get("PauseLateralSpeed", encoding="utf8")) if self._frame % 300 == 0: self._frame = 0 self.reverse_dm_cam = self.param_s.get_bool("ReverseDmCam") diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index d6eec9b603..bf2e2cba86 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -43,6 +43,11 @@ AUTO_LANE_CHANGE_TIMER = { } +def get_min_lateral_speed(value: int, is_metric: bool, default: float = LANE_CHANGE_SPEED_MIN): + speed: float = default if value == 0 else value * CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS + return speed + + class DesireHelper: def __init__(self): self.lane_change_state = LaneChangeState.off @@ -80,12 +85,13 @@ class DesireHelper: lane_change_auto_timer = AUTO_LANE_CHANGE_TIMER.get(self.lane_change_set_timer, 2.0) v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker + # TODO: SP: !659: User-defined minimum lane change speed below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN if self.model_use_lateral_planner: self.road_edge = get_road_edge(carstate, model_data, self.edge_toggle) - if not carstate.madsEnabled or self.lane_change_timer > LANE_CHANGE_TIME_MAX or self.lane_change_set_timer == -1: + if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX or self.lane_change_set_timer == -1: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.prev_lane_change = False diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index a527d6d975..88681da870 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -78,6 +78,7 @@ def manager_init() -> None: ("OnroadScreenOffEvent", "1"), ("OnroadSettings", "1"), ("PathOffset", "0"), + ("PauseLateralSpeed", "0"), ("ReverseAccChange", "0"), ("ScreenRecorder", "1"), ("ShowDebugUI", "1"), diff --git a/selfdrive/ui/qt/offroad/sunnypilot/lane_change_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot/lane_change_settings.cc index d2cf106bd3..8f16091460 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/lane_change_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/lane_change_settings.cc @@ -15,8 +15,8 @@ LaneChangeSettings::LaneChangeSettings(QWidget* parent) : QWidget(parent) { std::vector> toggle_defs{ { "BelowSpeedPause", - tr("Pause Lateral Below Speed w/ Blinker"), - tr("Enable this toggle to pause lateral actuation with blinker when traveling below 20 MPH or 32 km/h."), + tr("Pause Lateral Below Speed with Blinker"), + tr("Enable this toggle to pause lateral actuation with blinker when traveling below the desired speed selected below."), "../assets/offroad/icon_blank.png", }, { @@ -41,16 +41,29 @@ LaneChangeSettings::LaneChangeSettings(QWidget* parent) : QWidget(parent) { connect(auto_lane_change_timer, &AutoLaneChangeTimer::updateOtherToggles, this, &LaneChangeSettings::updateToggles); list->addItem(auto_lane_change_timer); + // Pause Lateral Below Speed w/ Blinker + pause_lateral_speed = new PauseLateralSpeed(); + pause_lateral_speed->showDescription(); + connect(pause_lateral_speed, &SPOptionControl::updateLabels, pause_lateral_speed, &PauseLateralSpeed::refresh); + for (auto &[param, title, desc, icon] : toggle_defs) { auto toggle = new ParamControl(param, title, desc, icon, this); list->addItem(toggle); toggles[param.toStdString()] = toggle; - // trigger offroadTransition when going onroad/offroad - connect(uiState(), &UIState::offroadTransition, toggle, &ParamControl::setEnabled); + if (param == "BelowSpeedPause") { + list->addItem(pause_lateral_speed); + } } + connect(toggles["BelowSpeedPause"], &ToggleControl::toggleFlipped, [=](bool state) { + pause_lateral_speed->setEnabled(state); + pause_lateral_speed->setVisible(state); + }); + pause_lateral_speed->setEnabled(toggles["BelowSpeedPause"]->isToggled()); + pause_lateral_speed->setVisible(toggles["BelowSpeedPause"]->isToggled()); + main_layout->addWidget(new ScrollView(list, this)); } @@ -113,3 +126,25 @@ void AutoLaneChangeTimer::refresh() { setLabel("2 " + second); } } + +PauseLateralSpeed::PauseLateralSpeed() : SPOptionControl ( + "PauseLateralSpeed", + "", + tr("Pause lateral actuation with blinker when traveling below the desired speed selected. Default is 20 MPH or 32 km/h."), + "../assets/offroad/icon_blank.png", + {0, 255}, + 5) { + + refresh(); +} + +void PauseLateralSpeed::refresh() { + QString option = QString:: fromStdString(params.get("PauseLateralSpeed")); + bool is_metric = params.getBool("IsMetric"); + + if (option == "0") { + setLabel(tr("Default")); + } else { + setLabel(option + " " + (is_metric ? tr("km/h") : tr("mph"))); + } +} diff --git a/selfdrive/ui/qt/offroad/sunnypilot/lane_change_settings.h b/selfdrive/ui/qt/offroad/sunnypilot/lane_change_settings.h index 86bdd3ede3..dd8e1748bd 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/lane_change_settings.h +++ b/selfdrive/ui/qt/offroad/sunnypilot/lane_change_settings.h @@ -19,6 +19,22 @@ private: Params params; }; +class PauseLateralSpeed : public SPOptionControl { + Q_OBJECT + +public: + PauseLateralSpeed(); + + void refresh(); + + signals: + void ToggleUpdated(); + +private: + Params params; +}; + + class LaneChangeSettings : public QWidget { Q_OBJECT @@ -37,4 +53,5 @@ private: std::map toggles; AutoLaneChangeTimer *auto_lane_change_timer; + PauseLateralSpeed *pause_lateral_speed; };