state handling

common/params_keys.h

@@ -200,7 +200,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"LagdToggleDelay", {PERSISTENT | BACKUP, FLOAT, "0.2"}},
     {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}},
-    {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"LaneTurnDesire", {PERSISTENT | BACKUP, INT, "0"}},
     {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
 
     // mapd

selfdrive/controls/lib/desire_helper.py

@@ -109,9 +109,8 @@ class DesireHelper:
     self.prev_one_blinker = one_blinker
 
     # Lane turn controller update
-    lane_change_nudge_mode = self.alc.lane_change_set_timer == AutoLaneChangeMode.NUDGE
     turn_desire = self.lane_turn_controller.update(carstate.leftBlindspot, carstate.rightBlindspot, carstate.leftBlinker, carstate.rightBlinker,
-                                                   carstate.vEgo, lane_change_nudge_mode, carstate.steeringPressed, carstate.steeringTorque)
+                                                   carstate.vEgo, carstate.steeringPressed, carstate.steeringTorque)
     if turn_desire != log.Desire.none:
       self.desire = turn_desire
     else:

selfdrive/ui/sunnypilot/SConscript

@@ -47,6 +47,7 @@ qt_src = [
 lateral_panel_qt_src = [
   "sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/lane_change_settings.cc",
+  "sunnypilot/qt/offroad/settings/lateral/lane_turn_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/mads_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc",
 ]

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_turn_settings.cc

@@ -0,0 +1,86 @@
+/**
+ * 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.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_turn_settings.h"
+
+
+LaneTurnSettings::LaneTurnSettings(QWidget* parent) : QWidget(parent) {
+  QVBoxLayout* main_layout = new QVBoxLayout(this);
+  main_layout->setContentsMargins(50, 20, 50, 20);
+  main_layout->setSpacing(20);
+
+  // Back button
+  PanelBackButton* back = new PanelBackButton(tr("Back"));
+  connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
+  main_layout->addWidget(back, 0, Qt::AlignLeft);
+
+  ListWidgetSP *list = new ListWidgetSP(this, false);
+  // Lane Turn Desire Control
+  laneTurnDesireControl = new LaneTurnDesireControl();
+  laneTurnDesireControl->setUpdateOtherToggles(true);
+  laneTurnDesireControl->showDescription();
+  connect(laneTurnDesireControl, &OptionControlSP::updateLabels, laneTurnDesireControl, &LaneTurnDesireControl::refresh);
+  list->addItem(laneTurnDesireControl);
+
+  // Lane Turn Value control
+  bool is_metric = params.getBool("IsMetric");
+  int per_value_change = is_metric ? 62 : 100; // ~1 km/h or 1 mph
+  laneTurnValueControl = new OptionControlSP("LaneTurnValue", tr("Adjust Lane Turn Speed"),
+    tr("Set the maximum speed for lane turn desires. Default is 19 %1.").arg(is_metric ? "km/h" : "mph"),
+    "", {500, 2000}, per_value_change, false, nullptr, true, true);
+  laneTurnValueControl->showDescription();
+  list->addItem(laneTurnValueControl);
+
+  // show/hide value control based on desire
+  connect(laneTurnDesireControl, &OptionControlSP::updateLabels, this, &LaneTurnSettings::updateValueControlVisibility);
+  connect(laneTurnValueControl, &OptionControlSP::updateLabels, this, &LaneTurnSettings::refreshLaneTurnValueControl);
+  main_layout->addWidget(new ScrollViewSP(list, this));
+}
+
+void LaneTurnSettings::showEvent(QShowEvent *event) {
+  updateValueControlVisibility();
+}
+
+void LaneTurnSettings::updateValueControlVisibility() {
+  QString option = QString::fromStdString(params.get("LaneTurnDesire"));
+  bool visible = (option != "0");
+  laneTurnValueControl->setVisible(visible);
+  if (visible) {
+    refreshLaneTurnValueControl();
+  }
+}
+
+void LaneTurnSettings::refreshLaneTurnValueControl() {
+  if (!laneTurnValueControl) return;
+  float stored_mph = QString::fromStdString(params.get("LaneTurnValue")).toFloat();
+  bool is_metric = params.getBool("IsMetric");
+  QString unit = is_metric ? "km/h" : "mph";
+  float display_value = is_metric ? stored_mph * 1.609344f : stored_mph;
+  laneTurnValueControl->setLabel(QString::number(static_cast<int>(std::round(display_value))) + " " + unit);
+}
+
+// Lane Turn Desire Control
+LaneTurnDesireControl::LaneTurnDesireControl() : OptionControlSP(
+  "LaneTurnDesire",
+  tr("Lane Turn Desires"),
+  tr("If you're driving at 20 mph (32 km/h) or below and have your blinker on, "
+     "the car will plan a turn in that direction at the nearest drivable path. "
+     "This prevents situations (like at red lights) where the car might plan the wrong turn direction."),
+  "../assets/offroad/icon_shell.png",
+  {0, 2}) {
+  refresh();
+}
+
+void LaneTurnDesireControl::refresh() {
+  QString option = QString::fromStdString(params.get("LaneTurnDesire"));
+  static const QMap<QString, QString> options = {
+    {"0", tr("Off")},
+    {"1", tr("Nudge")},
+    {"2", tr("Nudgeless")},
+  };
+  setLabel(options.value(option, tr("Off")));
+}

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_turn_settings.h

@@ -0,0 +1,45 @@
+/**
+ * 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.
+ */
+
+#pragma once
+
+#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
+
+class LaneTurnDesireControl : public OptionControlSP {
+  Q_OBJECT
+
+public:
+  LaneTurnDesireControl();
+
+  void refresh();
+
+private:
+  Params params;
+};
+
+class LaneTurnSettings : public QWidget {
+  Q_OBJECT
+
+public:
+  explicit LaneTurnSettings(QWidget* parent = nullptr);
+  void showEvent(QShowEvent *event) override;
+
+signals:
+  void backPress();
+
+private:
+  void refreshLaneTurnValueControl();
+  void updateValueControlVisibility();
+
+private:
+  Params params;
+  std::map<std::string, ParamControlSP*> toggles;
+
+  LaneTurnDesireControl *laneTurnDesireControl;
+  OptionControlSP *laneTurnValueControl;
+};

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc

@@ -59,7 +59,27 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
     sunnypilotScroller->restoreScrollPosition();
     main_layout->setCurrentWidget(sunnypilotScreen);
   });
-  list->addItem(laneChangeSettingsButton);
+
+  // Lane Turn Settings
+  laneTurnSettingsButton = new PushButtonSP(tr("Customize Lane Turn"));
+  laneTurnSettingsButton->setObjectName("lane_turn_btn");
+  connect(laneTurnSettingsButton, &QPushButton::clicked, [=]() {
+    sunnypilotScroller->setLastScrollPosition();
+    main_layout->setCurrentWidget(laneTurnWidget);
+  });
+
+  laneTurnWidget = new LaneTurnSettings(this);
+  connect(laneTurnWidget, &LaneTurnSettings::backPress, [=]() {
+    sunnypilotScroller->restoreScrollPosition();
+    main_layout->setCurrentWidget(sunnypilotScreen);
+  });
+
+  QWidget *laneButtonsWidget = new QWidget();
+  QHBoxLayout *laneButtonsLayout = new QHBoxLayout(laneButtonsWidget);
+  laneButtonsLayout->setContentsMargins(0, 0, 0, 0);
+  laneChangeSettingsButton->setFixedWidth(750); laneTurnSettingsButton->setFixedWidth(750);
+  laneButtonsLayout->addWidget(laneChangeSettingsButton); laneButtonsLayout->addWidget(laneTurnSettingsButton);
+  list->addItem(laneButtonsWidget);
 
   list->addItem(vertical_space(0));
   list->addItem(horizontal_line());
@@ -100,6 +120,7 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
   main_layout->addWidget(sunnypilotScreen);
   main_layout->addWidget(madsWidget);
   main_layout->addWidget(laneChangeWidget);
+  main_layout->addWidget(laneTurnWidget);
 
   setStyleSheet(R"(
     #back_btn {

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h

@@ -15,6 +15,7 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_turn_settings.h"
 #include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
@@ -43,6 +44,8 @@ private:
   MadsSettings *madsWidget = nullptr;
   PushButtonSP *laneChangeSettingsButton;
   LaneChangeSettings *laneChangeWidget = nullptr;
+  PushButtonSP *laneTurnSettingsButton;
+  LaneTurnSettings *laneTurnWidget = nullptr;
   NeuralNetworkLateralControl *nnlcToggle = nullptr;
   BlinkerPauseLateralSettings *blinkerPauseLateralSettings = nullptr;
 

selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc

@@ -103,30 +103,6 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) {
   list->addItem(policyFrame);
   list->addItem(horizontal_line());
 
-  // Lane Turn Desire toggle
-  lane_turn_desire_toggle = new ParamControlSP("LaneTurnDesire", tr("Use Lane Turn Desires"),
-                            "If you’re driving at 20 mph (32 km/h) or below and have your blinker on, "
-                            "the car will plan a turn in that direction at the nearest drivable path. "
-                            "This prevents situations (like at red lights) where the car might plan the wrong turn direction.",
-                             "../assets/offroad/icon_shell.png");
-  list->addItem(lane_turn_desire_toggle);
-
-  // Lane Turn Value control
-  int max_value_mph = 20;
-  bool is_metric_initial = params.getBool("IsMetric");
-  const float K = 1.609344f;
-  int per_value_change_scaled = is_metric_initial ? static_cast<int>(std::round((1.0f / K) * 100.0f)) : 100; // 100 -> 1 mph
-  lane_turn_value_control = new OptionControlSP("LaneTurnValue", tr("Adjust Lane Turn Speed"),
-    tr("Set the maximum speed for lane turn desires. Default is 19 %1.").arg(is_metric_initial ? "km/h" : "mph"),
-    "", {5 * 100, max_value_mph * 100}, per_value_change_scaled, false, nullptr, true, true);
-  lane_turn_value_control->showDescription();
-  list->addItem(lane_turn_value_control);
-
-  // Show based on toggle
-  refreshLaneTurnValueControl();
-  connect(lane_turn_desire_toggle, &ParamControlSP::toggleFlipped, this, &ModelsPanel::refreshLaneTurnValueControl);
-  connect(lane_turn_value_control, &OptionControlSP::updateLabels, this, &ModelsPanel::refreshLaneTurnValueControl);
-
   // LiveDelay toggle
   lagd_toggle_control = new ParamControlSP("LagdToggle", tr("Live Learning Steer Delay"), "", "../assets/offroad/icon_shell.png");
   lagd_toggle_control->showDescription();
@@ -169,23 +145,6 @@ QFrame* ModelsPanel::createModelDetailFrame(QWidget *parent, QString &typeName,
   return frame;
 }
 
-void ModelsPanel::refreshLaneTurnValueControl() {
-  if (!lane_turn_value_control) return;
-  float stored_mph = QString::fromStdString(params.get("LaneTurnValue")).toFloat();
-  bool is_metric = params.getBool("IsMetric");
-  QString unit = is_metric ? "km/h" : "mph";
-  float display_value = stored_mph;
-  if (is_metric) {
-    display_value = stored_mph * 1.609344f;
-  }
-  lane_turn_value_control->setLabel(QString::number(static_cast<int>(std::round(display_value))) + " " + unit);
-  lane_turn_value_control->setVisible(params.getBool("LaneTurnDesire"));
-}
-
-/**
- * @brief Updates the UI with bundle download progress information
- * Reads status from modelManagerSP cereal message and displays status for all models
- */
 void ModelsPanel::handleBundleDownloadProgress() {
   supercomboFrame->setVisible(false);
   visionFrame->setVisible(false);
@@ -456,9 +415,6 @@ void ModelsPanel::updateLabels() {
     delay_control->setLabel(QString::number(value, 'f', 2) + "s");
   }
 
-  // Update lane turn desire label and visibility
-  refreshLaneTurnValueControl();
-
   clearModelCacheBtn->setValue(QString::number(calculateCacheSize(), 'f', 2) + " MB");
 }
 

selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h

@@ -38,7 +38,6 @@ private:
   void updateLabels();
   void handleCurrentModelLblBtnClicked();
   void handleBundleDownloadProgress();
-  void refreshLaneTurnValueControl();
   void showResetParamsDialog();
   QProgressBar* createProgressBar(QWidget *parent);
   QFrame* createModelDetailFrame(QWidget *parent, QString &typeName, QProgressBar *progressBar);
@@ -83,6 +82,4 @@ private:
   Params params;
   ButtonControlSP *clearModelCacheBtn;
   ButtonControlSP *refreshAvailableModelsBtn;
-  ParamControlSP *lane_turn_desire_toggle;
-  OptionControlSP *lane_turn_value_control;
 };

sunnypilot/selfdrive/controls/lib/lane_turn_desire.py

@@ -11,6 +11,12 @@ from openpilot.common.params import Params
 
 LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
 
+TURN_STATE = {
+  'OFF': 0,
+  'NUDGE': 1,
+  'NUDGELESS': 2
+}
+
 TURN_DESIRES = {
   custom.TurnDirection.none: log.Desire.none,
   custom.TurnDirection.turnLeft: log.Desire.turnLeft,
@@ -24,10 +30,10 @@ class LaneTurnController:
     self.params = Params()
     self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
     self.param_read_counter = 0
-    self.enabled = self.params.get_bool("LaneTurnDesire")
+    self.lane_turn_type = int(self.params.get("LaneTurnDesire", return_default=True))
 
   def read_params(self):
-    self.enabled = self.params.get_bool("LaneTurnDesire")
+    self.lane_turn_type = int(self.params.get("LaneTurnDesire", return_default=True))
     value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
     self.lane_turn_value = min(float(LANE_CHANGE_SPEED_MIN), value)
 
@@ -37,21 +43,20 @@ class LaneTurnController:
     self.param_read_counter += 1
 
   def update_lane_turn(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float) -> None:
-    if self.enabled and left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left:
+    if self.lane_turn_type != TURN_STATE['OFF'] and left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left:
       self.turn_direction = custom.TurnDirection.turnLeft
-    elif self.enabled and right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right:
+    elif self.lane_turn_type != TURN_STATE['OFF'] and right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right:
       self.turn_direction = custom.TurnDirection.turnRight
     else:
       self.turn_direction = custom.TurnDirection.none
 
-  def return_desire(self, nudge_mode: bool, steering_pressed: bool, steering_torque: float) -> log.Desire:
-    # Return the current Desire for lane-turns.
+  def return_desire(self, steering_pressed: bool, steering_torque: float) -> log.Desire:
     # disabled or no turn -> return none
-    if not self.enabled or self.turn_direction == custom.TurnDirection.none:
+    if self.lane_turn_type == TURN_STATE['OFF'] or self.turn_direction == custom.TurnDirection.none:
       return log.Desire.none
 
-    if nudge_mode:
-      # In nudge mode, require steering torque for lane turn
+    if self.lane_turn_type == TURN_STATE['NUDGE']:
+      # In nudge mode, require steering press for turn desire
       turn_torque_applied = steering_pressed and ((steering_torque > 0 and self.turn_direction == custom.TurnDirection.turnLeft) or
                              (steering_torque < 0 and self.turn_direction == custom.TurnDirection.turnRight))
       if not turn_torque_applied:
@@ -60,7 +65,7 @@ class LaneTurnController:
     return TURN_DESIRES[self.turn_direction]
 
   def update(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float,
-             lane_change_nudge_mode: bool, steering_pressed: bool, steering_torque: float) -> log.Desire:
+             steering_pressed: bool, steering_torque: float) -> log.Desire:
     self.update_params()
     self.update_lane_turn(blindspot_left, blindspot_right, left_blinker, right_blinker, v_ego)
-    return self.return_desire(lane_change_nudge_mode, steering_pressed, steering_torque)
+    return self.return_desire(steering_pressed, steering_torque)

sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py

@@ -4,7 +4,7 @@ from cereal import custom, log
 from openpilot.common.params import Params
 
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
+from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN, TURN_STATE
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
 
 
@@ -20,7 +20,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLan
         "no blinkers", "both blinkers"])
 def test_lane_turn_desire_conditions(left_blinker, right_blinker, v_ego, blindspot_left, blindspot_right, expected):
     controller = LaneTurnController()
-    controller.enabled = True
+    controller.lane_turn_type = TURN_STATE['NUDGELESS']
     controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
     controller.turn_direction = custom.TurnDirection.none
     controller.update_lane_turn(blindspot_left, blindspot_right, left_blinker, right_blinker, v_ego)
@@ -29,7 +29,7 @@ def test_lane_turn_desire_conditions(left_blinker, right_blinker, v_ego, blindsp
 
 def test_lane_turn_desire_disabled():
     controller = LaneTurnController()
-    controller.enabled = False
+    controller.lane_turn_type = TURN_STATE['OFF']
     controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
     controller.turn_direction = custom.TurnDirection.none
     controller.update_lane_turn(False, False, True, False, 6)
@@ -38,7 +38,7 @@ def test_lane_turn_desire_disabled():
 
 def test_lane_turn_overrides_lane_change():
     controller = LaneTurnController()
-    controller.enabled = True
+    controller.lane_turn_type = TURN_STATE['NUDGELESS']
     controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
     controller.turn_direction = custom.TurnDirection.none
     controller.update_lane_turn(False, False, True, False, 5)
@@ -58,7 +58,7 @@ def test_lane_turn_overrides_lane_change():
 ], ids=["below threshold", "above threshold", "just above threshold"])
 def test_lane_turn_desire_speed_boundary(v_ego, expected):
     controller = LaneTurnController()
-    controller.enabled = True
+    controller.lane_turn_type = TURN_STATE['NUDGELESS']
     controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
     controller.turn_direction = custom.TurnDirection.none
     controller.update_lane_turn(False, True, True, False, v_ego)
@@ -80,13 +80,13 @@ class DummyCarState:
 
 def nudgeless_params():
     params = Params()
-    params.put("LaneTurnDesire", True)
+    params.put("LaneTurnDesire", TURN_STATE['NUDGELESS'])
     params.put("LaneTurnValue", 20.0)
     params.put("AutoLaneChangeTimer", AutoLaneChangeMode.NUDGELESS)
 
 def nudge_params():
     params = Params()
-    params.put("LaneTurnDesire", True)
+    params.put("LaneTurnDesire", TURN_STATE['NUDGE'])
     params.put("LaneTurnValue", 20.0)
     params.put("AutoLaneChangeTimer", AutoLaneChangeMode.NUDGE)