From d683da24ea39c1a6eef2f38ac876a8c2004972a2 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 26 Aug 2026 16:28:11 -0500 Subject: [PATCH] lines --- selfdrive/controls/lib/lane_centering.py | 9 +++++++-- selfdrive/controls/tests/test_lane_centering.py | 5 +++++ selfdrive/ui/mici/onroad/model_renderer.py | 7 +++++++ selfdrive/ui/onroad/model_renderer.py | 7 +++++++ 4 files changed, 26 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/lane_centering.py b/selfdrive/controls/lib/lane_centering.py index 10efbdfdc..6dd4fac05 100644 --- a/selfdrive/controls/lib/lane_centering.py +++ b/selfdrive/controls/lib/lane_centering.py @@ -158,7 +158,8 @@ def get_raw_lane_centering_correction(model_v2, v_ego: float, offset: float, def get_lane_centering_visual_direction(model_v2, v_ego: float, offset: float, e2e_authority: float, enabled: bool, lat_active: bool, pause_on_signal: bool = False, - turn_signal_active: bool = False) -> int: + turn_signal_active: bool = False, + applied_correction: float | None = None) -> int: """Return 1 for a right correction, -1 for left, and 0 when no correction is active.""" if not enabled or not lat_active or (pause_on_signal and turn_signal_active): return 0 @@ -180,6 +181,10 @@ def get_lane_centering_visual_direction(model_v2, v_ego: float, offset: float, e float(np.clip(offset, -_MAX_OFFSET, _MAX_OFFSET)), float(np.clip(e2e_authority, 0.0, 1.0)), ) - if not valid or not np.isfinite(correction) or correction == 0.0: + if not valid or not np.isfinite(correction): return 0 + if correction == 0.0: + if applied_correction is None or not np.isfinite(applied_correction) or applied_correction == 0.0: + return 0 + correction = applied_correction return 1 if correction > 0.0 else -1 diff --git a/selfdrive/controls/tests/test_lane_centering.py b/selfdrive/controls/tests/test_lane_centering.py index 6a8e88867..beb89646b 100644 --- a/selfdrive/controls/tests/test_lane_centering.py +++ b/selfdrive/controls/tests/test_lane_centering.py @@ -186,3 +186,8 @@ def test_visual_direction_requires_both_primary_lane_lines(): model = _model(left=-1.5, right=2.1) model.laneLineProbs[2] = 0.2 assert get_lane_centering_visual_direction(model, _V_EGO, 0.0, 0.0, True, True) == 0 + + +def test_visual_direction_uses_filtered_correction_in_deadband(): + model = _model() + assert get_lane_centering_visual_direction(model, _V_EGO, 0.0, 0.0, True, True, applied_correction=0.001) == 1 diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index 3ad5d1aee..9c6d657a5 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -369,6 +369,12 @@ class ModelRenderer(Widget): return 0 car_state = sm["carState"] + applied_correction = None + if sm.valid.get("controlsState", False): + try: + applied_correction = sm["controlsState"].desiredCurvature - sm["modelV2"].action.desiredCurvature + except (AttributeError, TypeError, ValueError): + pass return get_lane_centering_visual_direction( sm["modelV2"], car_state.vEgo, toggles.get("lane_center_offset", 0.0), @@ -377,6 +383,7 @@ class ModelRenderer(Widget): ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active, bool(toggles.get("lane_centering_pause_on_signal", True)), bool(car_state.leftBlinker or car_state.rightBlinker), + applied_correction, ) def _lane_line_palette(self) -> tuple[bool, rl.Color, rl.Color, int]: diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index 3a49f40d1..9c8169a93 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -377,6 +377,12 @@ class ModelRenderer(Widget): return 0 car_state = sm["carState"] + applied_correction = None + if sm.valid.get("controlsState", False): + try: + applied_correction = sm["controlsState"].desiredCurvature - sm["modelV2"].action.desiredCurvature + except (AttributeError, TypeError, ValueError): + pass return get_lane_centering_visual_direction( sm["modelV2"], car_state.vEgo, toggles.get("lane_center_offset", 0.0), @@ -385,6 +391,7 @@ class ModelRenderer(Widget): ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active, bool(toggles.get("lane_centering_pause_on_signal", True)), bool(car_state.leftBlinker or car_state.rightBlinker), + applied_correction, ) def _draw_lane_lines(self):