Controls: Customizable Pause Lateral Speed

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

common/params.cc

@@ -289,6 +289,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"OsmWayTest", PERSISTENT},
     {"OsmDownloadedDate", PERSISTENT},
     {"PathOffset", PERSISTENT | BACKUP},
+    {"PauseLateralSpeed", PERSISTENT | BACKUP},
     {"QuietDrive", PERSISTENT | BACKUP},
     {"RoadEdge", PERSISTENT | BACKUP},
     {"ReverseAccChange", PERSISTENT | BACKUP},

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")

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

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"),

selfdrive/ui/qt/offroad/sunnypilot/lane_change_settings.cc

@@ -15,8 +15,8 @@ LaneChangeSettings::LaneChangeSettings(QWidget* parent) : QWidget(parent) {
   std::vector<std::tuple<QString, QString, QString, QString>> 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")));
+  }
+}

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<std::string, ParamControl*> toggles;
 
   AutoLaneChangeTimer *auto_lane_change_timer;
+  PauseLateralSpeed *pause_lateral_speed;
 };