Pause Lateral Control with Blinker: Post-Blinker Delay (#1567)

* Delay lateral reengagement

* UI elements

* Add tests

* Update title and description

* Update params_metadata

* Didn't mean to pass this to int()

* Keep sentry happy

* Title and description update

* always 100 hz

---------

Co-authored-by: Jason Wen <haibin.wen3@gmail.com>

common/params_keys.h

@@ -137,6 +137,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"ApiCache_DriveStats", {PERSISTENT, JSON}},
     {"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
+    {"BlinkerLateralReengageDelay", {PERSISTENT | BACKUP, INT, "0"}},  // seconds
     {"BlinkerMinLateralControlSpeed", {PERSISTENT | BACKUP, INT, "20"}},  // MPH or km/h
     {"BlinkerPauseLateralControl", {PERSISTENT | BACKUP, INT, "0"}},
     {"Brightness", {PERSISTENT | BACKUP, INT, "0"}},

selfdrive/ui/sunnypilot/layouts/settings/steering.py

@@ -72,6 +72,15 @@ class SteeringLayout(Widget):
       description="",
       label_callback=lambda speed: f'{speed} {"km/h" if ui_state.is_metric else "mph"}',
     )
+    self._blinker_reengage_delay = option_item_sp(
+      param="BlinkerLateralReengageDelay",
+      title=lambda: tr("Post-Blinker Delay"),
+      min_value=0,
+      max_value=10,
+      value_change_step=1,
+      description=lambda: tr("Delay before lateral control resumes after the turn signal ends."),
+      label_callback=lambda delay: f'{delay} {"s"}'
+    )
     self._torque_control_toggle = toggle_item_sp(
       param="EnforceTorqueControl",
       title=lambda: tr("Enforce Torque Lateral Control"),
@@ -96,6 +105,7 @@ class SteeringLayout(Widget):
       LineSeparatorSP(40),
       self._blinker_control_toggle,
       self._blinker_control_options,
+      self._blinker_reengage_delay,
       LineSeparatorSP(40),
       self._torque_control_toggle,
       self._torque_customization_button,
@@ -128,6 +138,7 @@ class SteeringLayout(Widget):
     self._mads_toggle.action_item.set_enabled(ui_state.is_offroad())
     self._mads_settings_button.action_item.set_enabled(ui_state.is_offroad() and self._mads_toggle.action_item.get_state())
     self._blinker_control_options.set_visible(self._blinker_control_toggle.action_item.get_state())
+    self._blinker_reengage_delay.set_visible(self._blinker_control_toggle.action_item.get_state())
 
     enforce_torque_enabled = self._torque_control_toggle.action_item.get_state()
     nnlc_enabled = self._nnlc_toggle.action_item.get_state()

sunnypilot/selfdrive/controls/lib/blinker_pause_lateral.py

@@ -17,13 +17,16 @@ class BlinkerPauseLateral:
     self.enabled = self.params.get_bool("BlinkerPauseLateralControl")
     self.is_metric = self.params.get_bool("IsMetric")
     self.min_speed = 0
+    self.reengage_delay = 0
+    self.blinker_off_timer = 0.0
 
   def get_params(self) -> None:
     self.enabled = self.params.get_bool("BlinkerPauseLateralControl")
     self.is_metric = self.params.get_bool("IsMetric")
-    self.min_speed = self.params.get("BlinkerMinLateralControlSpeed")
+    self.min_speed = self.params.get("BlinkerMinLateralControlSpeed", return_default=True)
+    self.reengage_delay = self.params.get("BlinkerLateralReengageDelay", return_default=True)
 
-  def update(self, CS: car.CarState) -> bool:
+  def update(self, CS: car.CarState, DT_CTRL: float = 0.01) -> bool:
     if not self.enabled:
       return False
 
@@ -31,4 +34,11 @@ class BlinkerPauseLateral:
     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)
+    below_speed = CS.vEgo < min_speed_ms
+
+    if one_blinker and below_speed:
+      self.blinker_off_timer = self.reengage_delay
+    elif self.blinker_off_timer > 0:
+      self.blinker_off_timer -= DT_CTRL
+
+    return bool((one_blinker and below_speed) or self.blinker_off_timer > 0)

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

@@ -20,6 +20,8 @@ class TestBlinkerPauseLateral:
     self.blinker_pause_lateral.enabled = True
     self.blinker_pause_lateral.is_metric = False
     self.blinker_pause_lateral.min_speed = 20  # MPH
+    self.blinker_pause_lateral.reengage_delay = 0
+    self.blinker_pause_lateral.blinker_off_timer = 0.0
 
     self.CS = car.CarState.new_message()
     self.CS.vEgo = 0
@@ -46,6 +48,18 @@ class TestBlinkerPauseLateral:
     }
     self._test_should_blinker_pause_lateral(expected_results)
 
+  def test_reengage_delay(self):
+    self.blinker_pause_lateral.reengage_delay = 2 # seconds
+    self.CS.vEgo = 4.5 # ~10 MPH
+
+    expected_results = {
+      (False, False): True,
+      (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
 

sunnypilot/sunnylink/params_metadata.json

@@ -93,6 +93,10 @@
     "title": "[TIZI/TICI only] Blind Spot Detection",
     "description": "Enabling this will display warnings when a vehicle is detected in your blind spot as long as your car has BSM supported."
   },
+  "BlinkerLateralReengageDelay": {
+    "title": "Post-Blinker Delay",
+    "description": "Delay before lateral control resumes after the turn signal ends."
+  },
   "BlinkerMinLateralControlSpeed": {
     "title": "Blinker Min Lateral Control Speed",
     "description": ""