Controls: Pause lateral control based on blinker state and vehicle speed (#983)

* Controls: Pause lateral control based on blinker state and vehicle speed

* in its own module

* tests

* cleanup tests

* ui

* always refresh on show panel

* remove default

* change to 20 default

* need to update params live

* shorter

* off by default

* lol lint

* use ExpandableToggleRow

* not needed

* shorter

* ci ui preview delay for all offroad

* more

* Revert "more"

This reverts commit 809cfd99dac4183e318f3f7dff8e7c14f8223d62.

* Revert "ci ui preview delay for all offroad"

This reverts commit ab38292fa84165081e32fcfc7b44f7f7afdfe791.

---------

Co-authored-by: nayan8teen <nayan8teen@gmail.com>

common/params_keys.h

@@ -125,6 +125,8 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"ApiCache_DriveStats", PERSISTENT},
     {"AutoLaneChangeBsmDelay", PERSISTENT},
     {"AutoLaneChangeTimer", PERSISTENT},
+    {"BlinkerMinLateralControlSpeed", PERSISTENT | BACKUP},
+    {"BlinkerPauseLateralControl", PERSISTENT | BACKUP},
     {"CarParamsSP", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"CarParamsSPCache", CLEAR_ON_MANAGER_START},
     {"CarParamsSPPersistent", PERSISTENT},

selfdrive/ui/sunnypilot/SConscript

@@ -40,6 +40,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/mads_settings.cc",
   "sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc",

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

@@ -0,0 +1,27 @@
+/**
+ * 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/blinker_pause_lateral_settings.h"
+
+BlinkerPauseLateralSettings::BlinkerPauseLateralSettings(const QString &param, const QString &title, const QString &description, const QString &icon, QWidget *parent)
+    : ExpandableToggleRow(param, title, description, icon, parent) {
+
+  pauseLateralSpeed = new OptionControlSP("BlinkerMinLateralControlSpeed", "", "", "", {0, 255}, 5);
+  connect(pauseLateralSpeed, &OptionControlSP::updateLabels, this, &BlinkerPauseLateralSettings::refresh);
+  addItem(pauseLateralSpeed);
+
+  refresh();
+}
+
+void BlinkerPauseLateralSettings::refresh() {
+  const QString option = QString::fromStdString(params.get("BlinkerMinLateralControlSpeed"));
+  const bool is_metric = params.getBool("IsMetric");
+  const QString unit = is_metric ? "km/h" : "mph";
+
+  pauseLateralSpeed->setLabel(option + " " + unit);
+}

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

@@ -0,0 +1,25 @@
+/**
+ * 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/ui.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/expandable_row.h"
+
+class BlinkerPauseLateralSettings : public ExpandableToggleRow {
+  Q_OBJECT
+
+public:
+  BlinkerPauseLateralSettings(const QString &param, const QString &title, const QString &description, const QString &icon, QWidget *parent = nullptr);
+  void refresh();
+
+private:
+  Params params;
+  OptionControlSP *pauseLateralSpeed;
+};

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

@@ -64,6 +64,17 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
   list->addItem(vertical_space(0));
   list->addItem(horizontal_line());
 
+  // Blinker Pause Lateral Control
+  blinkerPauseLateralSettings = new BlinkerPauseLateralSettings(
+    "BlinkerPauseLateralControl",
+    tr("Pause Lateral Control with Blinker"),
+    tr("Pause lateral control with blinker when traveling below the desired speed selected."),
+    "",
+    this);
+  list->addItem(blinkerPauseLateralSettings);
+
+  list->addItem(horizontal_line());
+
   // Neural Network Lateral Control
   nnlcToggle = new NeuralNetworkLateralControl();
   list->addItem(nnlcToggle);
@@ -139,5 +150,7 @@ void LateralPanel::updateToggles(bool _offroad) {
 
   madsSettingsButton->setEnabled(madsToggle->isToggled());
 
+  blinkerPauseLateralSettings->refresh();
+
   offroad = _offroad;
 }

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

@@ -11,6 +11,7 @@
 #include <string>
 
 #include "selfdrive/ui/sunnypilot/ui.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.h"
 #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"
@@ -43,6 +44,7 @@ private:
   PushButtonSP *laneChangeSettingsButton;
   LaneChangeSettings *laneChangeWidget = nullptr;
   NeuralNetworkLateralControl *nnlcToggle = nullptr;
+  BlinkerPauseLateralSettings *blinkerPauseLateralSettings = nullptr;
 
   const QString MADS_BASE_DESC = tr("Enables independent engagements of Automatic Lane Centering (ALC) and Adaptive Cruise Control (ACC).");
 

sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -11,12 +11,14 @@ from opendbc.car import structs
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.sunnypilot.selfdrive.controls.lib.param_store import ParamStore
+from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral
 
 
 class ControlsExt:
   def __init__(self, CP: structs.CarParams, params: Params):
     self.CP = CP
     self.params = params
+    self.blinker_pause_lateral = BlinkerPauseLateral()
     self.param_store = ParamStore(self.CP)
     self.get_params_sp()
 
@@ -29,11 +31,13 @@ class ControlsExt:
 
   def get_params_sp(self) -> None:
     self.param_store.update(self.params)
+    self.blinker_pause_lateral.get_params()
+
+  def get_lat_active(self, sm: messaging.SubMaster) -> bool:
+    if self.blinker_pause_lateral.update(sm['carState']):
+      return False
 
-  @staticmethod
-  def get_lat_active(sm: messaging.SubMaster) -> bool:
     ss_sp = sm['selfdriveStateSP']
-
     if ss_sp.mads.available:
       return bool(ss_sp.mads.active)
 

sunnypilot/selfdrive/controls/lib/blinker_pause_lateral.py

@@ -0,0 +1,34 @@
+"""
+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.
+"""
+from cereal import car
+
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params
+
+
+class BlinkerPauseLateral:
+  def __init__(self):
+    self.params = Params()
+
+    self.enabled = self.params.get_bool("BlinkerPauseLateralControl")
+    self.is_metric = self.params.get_bool("IsMetric")
+    self.min_speed = 0
+
+  def get_params(self) -> None:
+    self.enabled = self.params.get_bool("BlinkerPauseLateralControl")
+    self.is_metric = self.params.get_bool("IsMetric")
+    self.min_speed = int(self.params.get("BlinkerMinLateralControlSpeed", encoding='utf8'))
+
+  def update(self, CS: car.CarState) -> bool:
+    if not self.enabled:
+      return False
+
+    one_blinker = CS.leftBlinker != CS.rightBlinker
+    speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
+    min_speed_ms = self.min_speed * speed_factor
+
+    return bool(one_blinker and CS.vEgo < min_speed_ms)

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

@@ -0,0 +1,130 @@
+"""
+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.
+"""
+from cereal import car
+
+from openpilot.common.conversions import Conversions as CV
+from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral
+
+
+class TestBlinkerPauseLateral:
+
+  def setup_method(self):
+    self.blinker_pause_lateral = BlinkerPauseLateral()
+    self._reset_states()
+
+  def _reset_states(self):
+    self.blinker_pause_lateral.enabled = True
+    self.blinker_pause_lateral.is_metric = False
+    self.blinker_pause_lateral.min_speed = 20  # MPH
+
+    self.CS = car.CarState.new_message()
+    self.CS.vEgo = 0
+    self.CS.leftBlinker = False
+    self.CS.rightBlinker = False
+
+  def _test_should_blinker_pause_lateral(self, expected_results) -> None:
+    for left in (True, False):
+      for right in (True, False):
+        self.CS.leftBlinker = left
+        self.CS.rightBlinker = right
+
+        result = self.blinker_pause_lateral.update(self.CS)
+        assert result == expected_results[(left, right)]
+
+  def test_below_min_speed_blinker(self):
+    self.CS.vEgo = 4.5  # ~10 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_above_min_speed_blinker(self):
+    self.CS.vEgo = 13.4  # ~30 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): False,
+      (False, True): False,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_just_below_min_speed(self):
+    self.CS.vEgo = (20 * CV.MPH_TO_MS) - 0.01
+
+    expected_results = {
+      (False, False): False,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_disabled(self):
+    self.blinker_pause_lateral.enabled = False
+    self.CS.vEgo = 4.5  # ~10 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): False,
+      (False, True): False,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_metric_units_below_min_speed(self):
+    self.blinker_pause_lateral.is_metric = True
+    self.CS.vEgo = 5.0  # ~18 km/h
+
+    expected_results = {
+      (False, False): False,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_metric_units_above_threshold(self):
+    self.blinker_pause_lateral.is_metric = True
+    self.CS.vEgo = 6.0  # ~21.6 km/h
+
+    expected_results = {
+      (False, False): False,
+      (True, False): False,
+      (False, True): False,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_change_min_speed_threshold(self):
+    self.blinker_pause_lateral.min_speed = 30  # MPH
+
+    # below min speed
+    self.CS.vEgo = 11.2  # ~25 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+    # above min speed
+    self.CS.vEgo = 15.6  # ~35 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): False,
+      (False, True): False,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)

system/manager/manager.py

@@ -44,6 +44,8 @@ def manager_init() -> None:
   sunnypilot_default_params: list[tuple[str, str | bytes]] = [
     ("AutoLaneChangeTimer", "0"),
     ("AutoLaneChangeBsmDelay", "0"),
+    ("BlinkerMinLateralControlSpeed", "20"),  # MPH or km/h
+    ("BlinkerPauseLateralControl", "0"),
     ("DynamicExperimentalControl", "0"),
     ("HyundaiLongitudinalTuning", "0"),
     ("Mads", "1"),