From 3fe33cbe97087c8ce2ae0bd86e746f7e72aea0c9 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 13 Feb 2026 11:33:52 -0500 Subject: [PATCH 1/4] [TIZI/TICI] ui: fix toggle states and simplify blindspots/turn signals updates (#1692) * [TIZI/TICI] ui: fix toggle states and simplify blindspots/turn signals updates * more --- selfdrive/ui/sunnypilot/onroad/turn_signal.py | 74 ++++--------------- 1 file changed, 15 insertions(+), 59 deletions(-) diff --git a/selfdrive/ui/sunnypilot/onroad/turn_signal.py b/selfdrive/ui/sunnypilot/onroad/turn_signal.py index 04fff7db76..bd1aa7ee10 100644 --- a/selfdrive/ui/sunnypilot/onroad/turn_signal.py +++ b/selfdrive/ui/sunnypilot/onroad/turn_signal.py @@ -72,72 +72,28 @@ class TurnSignalWidget(Widget): class TurnSignalController: - def __init__(self, config: TurnSignalConfig | None = None): - self._config = config or TurnSignalConfig() + def __init__(self): + self._config = TurnSignalConfig() self._left_signal = TurnSignalWidget(direction=IconSide.left) self._right_signal = TurnSignalWidget(direction=IconSide.right) - self._last_icon_side = None + + @staticmethod + def _update_signal(signal, blindspot, blinker): + if ui_state.blindspot and blindspot: + signal.activate('blind_spot') + elif ui_state.turn_signals and blinker: + signal.activate('signal') + else: + signal.deactivate() def update(self): - sm = ui_state.sm - ss = sm['selfdriveState'] + CS = ui_state.sm['carState'] - event_name = ss.alertType.split('/')[0] if ss.alertType else '' - - if event_name == 'preLaneChangeLeft': - self._last_icon_side = IconSide.left - self._left_signal.activate('signal') - self._right_signal.deactivate() - - elif event_name == 'preLaneChangeRight': - self._last_icon_side = IconSide.right - self._right_signal.activate('signal') - self._left_signal.deactivate() - - elif event_name == 'laneChange': - if self._last_icon_side == IconSide.left: - self._left_signal.activate('signal') - self._right_signal.deactivate() - elif self._last_icon_side == IconSide.right: - self._right_signal.activate('signal') - self._left_signal.deactivate() - - elif event_name == 'laneChangeBlocked': - CS = sm['carState'] - if CS.leftBlinker: - icon_side = IconSide.left - elif CS.rightBlinker: - icon_side = IconSide.right - else: - icon_side = self._last_icon_side - - if icon_side == IconSide.left: - self._left_signal.activate('blind_spot') - self._right_signal.deactivate() - elif icon_side == IconSide.right: - self._right_signal.activate('blind_spot') - self._left_signal.deactivate() - - else: - self._last_icon_side = None - CS = sm['carState'] - - if CS.leftBlindspot: - self._left_signal.activate('blind_spot') - elif CS.leftBlinker: - self._left_signal.activate('signal') - else: - self._left_signal.deactivate() - - if CS.rightBlindspot: - self._right_signal.activate('blind_spot') - elif CS.rightBlinker: - self._right_signal.activate('signal') - else: - self._right_signal.deactivate() + self._update_signal(self._left_signal, CS.leftBlindspot, CS.leftBlinker) + self._update_signal(self._right_signal, CS.rightBlindspot, CS.rightBlinker) def render(self, rect: rl.Rectangle): - if not ui_state.turn_signals: + if not ui_state.turn_signals and not ui_state.blindspot: return x = rect.x + rect.width / 2 From 4bd60cd3cd0743c4fac042c8b0461e20823655ea Mon Sep 17 00:00:00 2001 From: royjr Date: Fri, 13 Feb 2026 11:41:30 -0500 Subject: [PATCH 2/4] [MICI] ui: rainbow path (#1630) * allow rainbow road on mici * initialization --------- Co-authored-by: Jason Wen --- selfdrive/ui/mici/onroad/model_renderer.py | 11 ++++++++--- selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py | 6 ++++++ 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index 0908de0bf4..ca051c6d94 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -12,7 +12,7 @@ from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient from openpilot.system.ui.widgets import Widget -from openpilot.selfdrive.ui.sunnypilot.mici.onroad.model_renderer import LANE_LINE_COLORS_SP +from openpilot.selfdrive.ui.sunnypilot.mici.onroad.model_renderer import LANE_LINE_COLORS_SP, ModelRendererSP CLIP_MARGIN = 500 MIN_DRAW_DISTANCE = 10.0 @@ -51,9 +51,10 @@ class LeadVehicle: fill_alpha: int = 0 -class ModelRenderer(Widget): +class ModelRenderer(Widget, ModelRendererSP): def __init__(self): - super().__init__() + Widget.__init__(self) + ModelRendererSP.__init__(self) self._longitudinal_control = False self._experimental_mode = False self._blend_filter = FirstOrderFilter(1.0, 0.25, 1 / gui_app.target_fps) @@ -340,6 +341,10 @@ class ModelRenderer(Widget): allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control self._blend_filter.update(int(allow_throttle)) + if ui_state.rainbow_path: + self.rainbow_path.draw_rainbow_path(self._rect, self._path) + return + if self._experimental_mode: # Draw with acceleration coloring if ui_state.status == UIStatus.DISENGAGED: diff --git a/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py b/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py index 5a718947cf..e3de9401ce 100644 --- a/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py +++ b/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py @@ -6,8 +6,14 @@ See the LICENSE.md file in the root directory for more details. """ import pyray as rl from openpilot.selfdrive.ui.ui_state import UIStatus +from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath LANE_LINE_COLORS_SP = { UIStatus.LAT_ONLY: rl.Color(0, 255, 64, 255), UIStatus.LONG_ONLY: rl.Color(0, 255, 64, 255), } + + +class ModelRendererSP: + def __init__(self): + self.rainbow_path = RainbowPath() From 037e8c18f89ce6264d92d47538227bb21bbf4983 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 13 Feb 2026 16:22:51 -0500 Subject: [PATCH 3/4] sunnylink: update subscribed services states while running (#1694) --- sunnypilot/sunnylink/sunnylink_state.py | 1 + 1 file changed, 1 insertion(+) diff --git a/sunnypilot/sunnylink/sunnylink_state.py b/sunnypilot/sunnylink/sunnylink_state.py index 02b91c3cbc..acd20d23ae 100644 --- a/sunnypilot/sunnylink/sunnylink_state.py +++ b/sunnypilot/sunnylink/sunnylink_state.py @@ -166,6 +166,7 @@ class SunnylinkState: def _worker_thread(self) -> None: while self._running: + self._sm.update() if self.is_connected(): self._fetch_roles() self._fetch_users() From b62014eb12875a4248382c206f6ba31bc398fc42 Mon Sep 17 00:00:00 2001 From: Yasuhiro Ohno <84763339+yasu-oh@users.noreply.github.com> Date: Sat, 14 Feb 2026 07:03:03 +0900 Subject: [PATCH 4/4] SCC-V: Use p97 for predicted lateral accel (#1635) * SCC-V: Use p97 for predicted lateral accel * SCC-V: add param tests for p97 max_pred_lat_acc * fix * typo --------- Co-authored-by: Jason Wen --- .../tests/test_vision_controller.py | 112 +++++++++++++++++- .../smart_cruise_control/vision_controller.py | 2 +- 2 files changed, 112 insertions(+), 2 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py index 9f6efffb55..d35b79a73b 100644 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py +++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py @@ -5,6 +5,7 @@ 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. """ import numpy as np +import pytest import cereal.messaging as messaging from cereal import custom, log @@ -12,11 +13,52 @@ from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.selfdrive.modeld.constants import ModelConstants -from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision, _ENTERING_PRED_LAT_ACC_TH VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState +def _th_above_f32(th: float) -> float: + """ + Return the next representable float32 *above* `th`. + This avoids flaky comparisons around thresholds due to float32 rounding. + """ + th32 = np.float32(th) + above32 = np.nextafter(th32, np.float32(np.inf), dtype=np.float32) + return float(above32) + + +def _build_single_spike_filtered(n: int, base: float = 1.0) -> np.ndarray: + """ + Create an array where max() is >= threshold but p97 is < threshold. + This demonstrates the behavior difference vs np.amax(). + + Note: We intentionally construct using float32-representable values to match + the data path through cereal/capnp. + """ + th = float(_ENTERING_PRED_LAT_ACC_TH) + th32 = float(np.float32(th)) + + # numpy percentile default is linear interpolation: idx=(n-1)*p/100 + idx = (n - 1) * 0.97 + w = float(idx - np.floor(idx)) + + base32 = float(np.float32(base)) + + # Choose spike so that p97 = base + w*(spike-base) < th + # -> spike < base + (th-base)/w. Use a margin (0.9) and ensure spike >= th. + if w == 0.0: + spike = th32 + 1.0 + else: + spike = base32 + (th32 - base32) / w * 0.9 + spike = max(spike, th32 + 0.01) + + arr = np.full(n, base32, dtype=np.float32) + arr[-1] = np.float32(spike) + return arr + + def generate_modelV2(): model = messaging.new_message('modelV2') position = log.XYZTData.new_message() @@ -101,4 +143,72 @@ class TestSmartCruiseControlVision: self.scc_v.update(self.sm, True, False, 0., 0., 0.) assert self.scc_v.state == VisionState.enabled + @pytest.mark.parametrize( + "case, should_enter", + [ + ("p97_just_above_threshold", True), + ("single_spike_filtered", False), + ("persistent_high_values", True), + ], + ids=[ + "p97>threshold_enters", + "single_spike_max_large_but_p97_below_threshold", + "high_values_persist_trigger_entering", + ], + ) + def test_max_pred_lat_acc_uses_p97_and_threshold(self, case, should_enter): + n = len(ModelConstants.T_IDXS) + th = float(_ENTERING_PRED_LAT_ACC_TH) + + if case == "p97_just_above_threshold": + # Use the next representable float32 above threshold to avoid float32 rounding flakiness. + val = _th_above_f32(th) + pred_lat_accels = np.full(n, np.float32(val), dtype=np.float32) + + elif case == "single_spike_filtered": + pred_lat_accels = _build_single_spike_filtered(n, base=1.0) + + elif case == "persistent_high_values": + # Make enough "high" samples so p97 is driven by the persistent trend, not a single outlier. + high_count = max(2, int(np.ceil(n * 0.03)) + 1) + pred_lat_accels = np.full(n, np.float32(1.0), dtype=np.float32) + pred_lat_accels[-high_count:] = np.float32(2.0) + pred_lat_accels[-1] = np.float32(8.0) # keep one big outlier too + + else: + raise AssertionError(f"Unknown case: {case}") + + # Override model predictions so: + # predicted_lat_accels = abs(orientationRate.z) * velocity.x == pred_lat_accels + mdl = generate_modelV2() + mdl.modelV2.velocity.x = [1.0 for _ in range(n)] + mdl.modelV2.orientationRate.z = [float(x) for x in pred_lat_accels] + self.sm["modelV2"] = mdl.modelV2 + + v_ego = float(MIN_V + 5.0) + + # 1st update: disabled -> enabled + self.scc_v.update(self.sm, True, False, v_ego, 0.0, 0.0) + # 2nd update: evaluate entering condition from enabled state + self.scc_v.update(self.sm, True, False, v_ego, 0.0, 0.0) + + # Controller does percentile on numpy float64 arrays (values already quantized by capnp), + # so compute expected in float64 to match behavior and avoid interpolation/rounding deltas. + expected_p97 = float(np.percentile(pred_lat_accels.astype(np.float64), 97)) + + # allow tiny numeric differences due to float conversions/interpolation + assert np.isclose(self.scc_v.max_pred_lat_acc, expected_p97, rtol=1e-6, atol=1e-5) + + if should_enter: + # We assert entering primarily by state (this is the actual intended behavior). + assert self.scc_v.state == VisionState.entering + # Optional sanity: should be >= threshold with some margin (since we used nextafter above threshold). + assert self.scc_v.max_pred_lat_acc > th + + else: + # Difference vs np.amax(): max can be above threshold, but p97 stays below it. + assert float(np.max(pred_lat_accels)) >= th + assert self.scc_v.max_pred_lat_acc < th + assert self.scc_v.state == VisionState.enabled + # TODO-SP: mock modelV2 data to test other states diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py index d144178e93..a9d2a66227 100644 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py +++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py @@ -90,7 +90,7 @@ class SmartCruiseControlVision: # get the maximum lat accel from the model predicted_lat_accels = rate_plan * vel_plan - self.max_pred_lat_acc = np.amax(predicted_lat_accels) + self.max_pred_lat_acc = np.percentile(predicted_lat_accels, 97) # get the maximum curve based on the current velocity v_ego = max(self.v_ego, 0.1) # ensure a value greater than 0 for calculations