diff --git a/common/params_keys.h b/common/params_keys.h index f2a63ec1b1..dba0742268 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -137,6 +137,7 @@ inline static std::unordered_map 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"}}, diff --git a/selfdrive/ui/sunnypilot/layouts/settings/steering.py b/selfdrive/ui/sunnypilot/layouts/settings/steering.py index f82c4097c3..14d840138e 100644 --- a/selfdrive/ui/sunnypilot/layouts/settings/steering.py +++ b/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() diff --git a/sunnypilot/selfdrive/controls/lib/blinker_pause_lateral.py b/sunnypilot/selfdrive/controls/lib/blinker_pause_lateral.py index 2f27e7e42f..98757cd532 100644 --- a/sunnypilot/selfdrive/controls/lib/blinker_pause_lateral.py +++ b/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) diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_blinker_pause_lateral.py b/sunnypilot/selfdrive/controls/lib/tests/test_blinker_pause_lateral.py index ca8f16fe74..b547ea96b6 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_blinker_pause_lateral.py +++ b/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 diff --git a/sunnypilot/sunnylink/params_metadata.json b/sunnypilot/sunnylink/params_metadata.json index c0c84eac09..ad94ad35ff 100644 --- a/sunnypilot/sunnylink/params_metadata.json +++ b/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": ""