diff --git a/common/libcommon.a b/common/libcommon.a index e11d09e1c..d86f6ee53 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index bf341db8c..feedd84da 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -375,6 +375,7 @@ inline static std::unordered_map keys = { {"KonikDongleId", {PERSISTENT, STRING, "", "", 0}}, {"KonikMinutes", {PERSISTENT, INT, "0", "0", 0}}, {"LaneCentering", {PERSISTENT, BOOL, "0", "0", 2}}, + {"LaneCenteringPauseOnSignal", {PERSISTENT, BOOL, "1", "1", 2}}, {"LaneCenteringE2EAuthority", {PERSISTENT, FLOAT, "1.0", "1.0", 3}}, {"LaneCenterOffset", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, {"LaneChanges", {PERSISTENT, BOOL, "1", "1", 0, SETTINGS_SIMPLE}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index 2e2958d32..27b62573d 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0738d3fbc..2b2a4fc47 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -594,7 +594,9 @@ class Controls: self.starpilot_toggles.lane_center_offset, self.starpilot_toggles.lane_centering_e2e_authority, CC.latActive, - bool(self.sm.all_checks(['modelV2']))) + bool(self.sm.all_checks(['modelV2'])), + self.starpilot_toggles.lane_centering_pause_on_signal, + bool(CS.leftBlinker or CS.rightBlinker)) jerk_factor = 1.0 if self.starpilot_toggles.lane_change_pace < 10: diff --git a/selfdrive/controls/lib/lane_centering.py b/selfdrive/controls/lib/lane_centering.py index c4b7344e2..89c6784c5 100644 --- a/selfdrive/controls/lib/lane_centering.py +++ b/selfdrive/controls/lib/lane_centering.py @@ -15,6 +15,7 @@ _MIN_CENTER_TO_LINE = 1.1 _MAX_RAW_CORRECTION = 0.004 _MAX_GAIN = 0.30 _SMOOTH_TAU = 0.4 +_SIGNAL_RELEASE_TAU = 0.20 _E2E_MAX_PATH_STD = 0.35 _E2E_BREAK_IN_START = 0.25 @@ -28,7 +29,8 @@ class LaneCenteringController: def reset(self) -> None: self._correction = 0.0 - def update(self, model_curvature, model_v2, v_ego, enabled, offset, e2e_authority, lat_active, model_valid) -> float: + def update(self, model_curvature, model_v2, v_ego, enabled, offset, e2e_authority, lat_active, model_valid, + pause_on_signal=False, turn_signal_active=False) -> float: model_curvature = float(model_curvature) try: @@ -47,6 +49,10 @@ class LaneCenteringController: self.reset() return model_curvature + if pause_on_signal and turn_signal_active: + self._correction = float(smooth_value(0.0, self._correction, _SIGNAL_RELEASE_TAU, dt=DT_CTRL)) + return model_curvature + self._correction + try: if model_v2.meta.laneChangeState != log.LaneChangeState.off: self.reset() diff --git a/selfdrive/controls/tests/test_lane_centering.py b/selfdrive/controls/tests/test_lane_centering.py index ecdfd4d31..5ba98b437 100644 --- a/selfdrive/controls/tests/test_lane_centering.py +++ b/selfdrive/controls/tests/test_lane_centering.py @@ -28,8 +28,10 @@ def _model(left=-1.8, right=1.8, model_y=0.0, lane_prob=0.9, lane_std=0.1, path_ ) -def _update(controller, model, *, offset=0.0, authority=1.0, enabled=True, active=True, valid=True, speed=_V_EGO): - return controller.update(0.0, model, speed, enabled, offset, authority, active, valid) +def _update(controller, model, *, offset=0.0, authority=1.0, enabled=True, active=True, valid=True, speed=_V_EGO, + pause_on_signal=False, turn_signal_active=False): + return controller.update(0.0, model, speed, enabled, offset, authority, active, valid, + pause_on_signal, turn_signal_active) def _converge(model, *, offset=0.0, authority=1.0): @@ -57,6 +59,25 @@ def test_lane_change_is_noop(): assert _update(LaneCenteringController(), _model(left=-1.5, right=2.1, lane_change=1)) == 0.0 +def test_turn_signal_fades_lane_centering_correction(): + model = _model(left=-1.5, right=2.1) + controller, centered = _converge(model, authority=0.0) + fading = _update(controller, model, authority=0.0, pause_on_signal=True, turn_signal_active=True) + assert 0.0 < fading < centered + + for _ in range(300): + fading = _update(controller, model, authority=0.0, pause_on_signal=True, turn_signal_active=True) + assert abs(fading) < 1e-6 + + +def test_turn_signal_pause_can_be_disabled(): + model = _model(left=-1.5, right=2.1) + _, output = _converge(model, authority=0.0) + controller, _ = _converge(model, authority=0.0) + signaled = _update(controller, model, authority=0.0, turn_signal_active=True) + assert signaled == pytest.approx(output, abs=1e-7) + + @pytest.mark.parametrize( "field,value", [ diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 119c2768b..78a2563de 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -36,6 +36,7 @@ SAFE_MODE_MANAGED_KEYS = ( "SteerRatio", "CameraOffset", "LaneCentering", + "LaneCenteringPauseOnSignal", "LaneCenteringE2EAuthority", "LaneCenterOffset", "LaneChanges", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 9473a529f..33e12e249 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -714,6 +714,9 @@ class StarPilotVariables: advanced_lateral_tuning = self.get_value("AdvancedLateralTune") toggle.lane_centering = self.get_value("LaneCentering") + toggle.lane_centering_pause_on_signal = self.get_value( + "LaneCenteringPauseOnSignal", condition=toggle.lane_centering, default=True, + ) toggle.force_auto_tune = self.get_value("ForceAutoTune", condition=advanced_lateral_tuning and not has_auto_tune and is_torque_car and not is_angle_car) # Force-off is also meaningful on manually tuned torque cars: it locks the # vehicle-model parameters instead of allowing paramsd to learn over them. diff --git a/starpilot/common/tests/test_lane_centering_galaxy.py b/starpilot/common/tests/test_lane_centering_galaxy.py index 51249d413..ffd1e78f4 100644 --- a/starpilot/common/tests/test_lane_centering_galaxy.py +++ b/starpilot/common/tests/test_lane_centering_galaxy.py @@ -15,7 +15,7 @@ def _sections(): def test_lane_centering_is_only_in_galaxy_developer_section(): sections = _sections() - keys = {"LaneCentering", "LaneCenterOffset", "LaneCenteringE2EAuthority"} + keys = {"LaneCentering", "LaneCenterOffset", "LaneCenteringPauseOnSignal", "LaneCenteringE2EAuthority"} assert keys <= sections["Developer"].keys() for name, params in sections.items(): @@ -30,11 +30,15 @@ def test_lane_centering_galaxy_controls(): developer = _sections()["Developer"] centering = developer["LaneCentering"] offset = developer["LaneCenterOffset"] + pause_on_signal = developer["LaneCenteringPauseOnSignal"] e2e_authority = developer["LaneCenteringE2EAuthority"] assert centering["ui_type"] == "toggle" assert centering["is_parent_toggle"] is True + assert pause_on_signal["ui_type"] == "toggle" + assert pause_on_signal["parent_key"] == "LaneCentering" + assert offset["parent_key"] == "LaneCentering" assert offset["min"] == -0.3 assert offset["max"] == 0.3 diff --git a/starpilot/common/tests/test_safe_mode_lane_centering.py b/starpilot/common/tests/test_safe_mode_lane_centering.py index adf3db6d0..b3f93321b 100644 --- a/starpilot/common/tests/test_safe_mode_lane_centering.py +++ b/starpilot/common/tests/test_safe_mode_lane_centering.py @@ -5,6 +5,7 @@ def test_safe_mode_manages_lane_centering_settings(): assert { "CameraOffset", "LaneCentering", + "LaneCenteringPauseOnSignal", "LaneCenteringE2EAuthority", "LaneCenterOffset", } <= set(SAFE_MODE_MANAGED_KEYS) diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json index b4be6d982..e1276dd34 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json @@ -4087,6 +4087,15 @@ "parent_key": "LaneCentering", "settings_tier": "advanced" }, + { + "key": "LaneCenteringPauseOnSignal", + "label": "Pause Lane Centering On Turn Signal", + "description": "Fade lane-centering correction out when a turn signal is active so it does not fight a lane change or turn.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "LaneCentering", + "settings_tier": "advanced" + }, { "key": "LaneCenteringE2EAuthority", "label": "E2E Override Strength", diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index a36884a2c..1b0cc8ebb 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -966,6 +966,7 @@ _TROUBLESHOOT_ADVANCED_LATERAL_KEYS = [ "ForceTorqueController", "CameraOffset", "LaneCentering", + "LaneCenteringPauseOnSignal", "LaneCenteringE2EAuthority", "LaneCenterOffset", ]