Update compile_modeld.py

.gitmodules

@@ -4,7 +4,6 @@
 [submodule "opendbc"]
   path = opendbc_repo
   url = https://github.com/sunnypilot/opendbc.git
-  branch = tn
 [submodule "msgq"]
   path = msgq_repo
   url = https://github.com/commaai/msgq.git

cereal/custom.capnp

@@ -194,7 +194,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
-  acceleration @8 :Acceleration;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -297,23 +296,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     greenLightAlert @0 :Bool;
     leadDepartAlert @1 :Bool;
   }
-
-  # Acceleration Personality (Eco / Normal / Sport)
-  struct Acceleration {
-    personality @0 :AccelerationPersonality;
-    enabled @1 :Bool;
-    maxAccel @2 :Float32;     # current speed-indexed accel ceiling
-    brakeNeed @3 :Float32;    # predicted decel demand from the lookahead (m/s^2, positive)
-    decelTarget @4 :Float32;  # early-soft comfort decel target (m/s^2, negative)
-    smoothActive @5 :Bool;    # early-soft braking currently shaping the target
-    bypassed @6 :Bool;        # passthrough to stock plan (hard brake / FCW / should_stop / closing lead / e2e)
-  }
-
-  enum AccelerationPersonality {
-    eco @0;
-    normal @1;
-    sport @2;
-  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {
@@ -360,7 +342,6 @@ struct OnroadEventSP @0xda96579883444c35 {
     speedLimitChanged @21;
     speedLimitPending @22;
     e2eChime @23;
-    laneChangeRoadEdge @24;
   }
 }
 
@@ -467,8 +448,6 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
 
 struct ModelDataV2SP @0xa1680744031fdb2d {
   laneTurnDirection @0 :TurnDirection;
-  leftLaneChangeEdgeBlock @1 :Bool;
-  rightLaneChangeEdgeBlock @2 :Bool;
 
   enum TurnDirection {
     none @0;

common/params_keys.h

@@ -4,7 +4,6 @@
 #include <unordered_map>
 
 #include "cereal/gen/cpp/log.capnp.h"
-#include "cereal/gen/cpp/custom.capnp.h"
 
 inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"AccessToken", {CLEAR_ON_MANAGER_START | DONT_LOG, STRING}},
@@ -180,19 +179,12 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"RoadEdgeLaneChangeEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
 
-    // toyota specific params
-    {"ToyotaAutoHold", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"ToyotaEnhancedBsm", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"ToyotaTSS2Long", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"ToyotaDriveMode", {PERSISTENT | BACKUP, BOOL, "0"}},
-
     // MADS params
     {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"MadsMainCruiseAllowed", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -236,13 +228,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
-    // Acceleration Personality (Eco / Normal / Sport)
-    {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"AccelPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast<int>(cereal::LongitudinalPlanSP::AccelerationPersonality::NORMAL))}},
-
-    // Radar Distance: hold a lead through radar flicker/dropout so the MPC doesn't lose+regain it
-    {"RadarDistance", {PERSISTENT | BACKUP, BOOL, "0"}},
-
     // sunnypilot model params
     {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},

selfdrive/car/card.py

@@ -10,7 +10,7 @@ from cereal import car, log, custom
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
 from openpilot.common.swaglog import cloudlog, ForwardingHandler
-from opendbc.safety import ALTERNATIVE_EXPERIENCE
+
 from opendbc.car import DT_CTRL, structs
 from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
 from opendbc.car.carlog import carlog
@@ -121,13 +121,7 @@ class Car:
       self.CI, self.CP, self.CP_SP = CI, CI.CP, CI.CP_SP
       self.RI = RI
 
-    # set alternative experiences from parameters
-    sp_toyota_auto_brake_hold = self.params.get_bool("ToyotaAutoHold")
     self.CP.alternativeExperience = 0
-    if sp_toyota_auto_brake_hold:
-      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
-
-
     # mads
     set_alternative_experience(self.CP, self.CP_SP, self.params)
     set_car_specific_params(self.CP, self.CP_SP, self.params)

selfdrive/controls/lib/desire_helper.py

@@ -56,7 +56,7 @@ class DesireHelper:
   def get_lane_change_direction(CS):
     return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
 
-  def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected=False, right_edge_detected=False):
+  def update(self, carstate, lateral_active, lane_change_prob):
     self.alc.update_params()
     self.lane_turn_controller.update_params()
     v_ego = carstate.vEgo
@@ -88,8 +88,8 @@ class DesireHelper:
                          ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
                           (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
 
-        blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
-                              ((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
+        blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
+                              (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
 
         self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -110,7 +110,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     # No change cost when user is controlling the speed, or when standstill
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
-    accel_clip = [ACCEL_MIN, self.accel.get_max_accel(v_ego)]
+    accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
     steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
     accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
 
@@ -138,7 +138,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(self.smooth_radarstate(sm['radarState']), v_cruise, personality=sm['selfdriveState'].personality)
+    self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -160,8 +160,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
-    is_e2e = self.is_e2e(sm)
-    if is_e2e:
+    if self.is_e2e(sm):
       output_a_target = min(output_a_target_e2e, output_a_target_mpc)
       self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
       if output_a_target < output_a_target_mpc:
@@ -170,14 +169,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       output_a_target = output_a_target_mpc
       self.output_should_stop = output_should_stop_mpc
 
-    # Acceleration Personality: early soft braking (never weaker than the plan). No-op when disabled.
-    output_a_target = self.accel.smooth_target_accel(output_a_target, self.a_desired_trajectory, CONTROL_N_T_IDX,
-                                                     self.output_should_stop or force_slow_decel, reset=reset_state, stock_brake=is_e2e)
-
-    # Lower (braking) bound and the ceiling's downward slew stay at the stock rate; only the ceiling's
-    # upward slew is tier-dependent (Acceleration Personality).
-    accel_clip[0] = np.clip(accel_clip[0], self.prev_accel_clip[0] - 0.05, self.prev_accel_clip[0] + 0.05)
-    accel_clip[1] = np.clip(accel_clip[1], self.prev_accel_clip[1] - 0.05, self.prev_accel_clip[1] + self.accel.get_rise_rate())
+    for idx in range(2):
+      accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)
     self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1])
     self.prev_accel_clip = accel_clip
 

selfdrive/selfdrived/selfdrived.py

@@ -321,16 +321,9 @@ class SelfdriveD(CruiseHelper):
     # Handle lane change
     if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
       direction = self.sm['modelV2'].meta.laneChangeDirection
-      mdv2sp = self.sm['modelDataV2SP']
-
       if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
-            (CS.rightBlindspot and direction == LaneChangeDirection.right):
+         (CS.rightBlindspot and direction == LaneChangeDirection.right):
         self.events.add(EventName.laneChangeBlocked)
-
-      elif (mdv2sp.leftLaneChangeEdgeBlock and direction == LaneChangeDirection.left) or \
-           (mdv2sp.rightLaneChangeEdgeBlock and direction == LaneChangeDirection.right):
-        self.events_sp.add(custom.OnroadEventSP.EventName.laneChangeRoadEdge)
-
       else:
         if direction == LaneChangeDirection.left:
           self.events.add(EventName.preLaneChangeLeft)

selfdrive/ui/layouts/settings/toggles.py

@@ -31,15 +31,6 @@ DESCRIPTIONS = {
     "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
     "without a turn signal activated while driving over 31 mph (50 km/h)."
   ),
-  "AccelPersonalityEnabled": tr_noop("Enable Eco/Normal/Sport acceleration profiles, including early soft braking."),
-  "AccelPersonality": tr_noop(
-    "Eco accelerates gently and brakes early and soft; Sport accelerates briskly. " +
-    "Hard-braking authority is always preserved."
-  ),
-  "RadarDistance": tr_noop(
-    "Hold a lead through brief radar flicker/dropout so sunnypilot does not lose and re-grab it, " +
-    "smoothing the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock."
-  ),
   "AlwaysOnDM": tr_noop("Enable driver monitoring even when sunnypilot is not engaged."),
   'RecordFront': tr_noop("Upload data from the driver facing camera and help improve the driver monitoring algorithm."),
   "IsMetric": tr_noop("Display speed in km/h instead of mph."),
@@ -73,12 +64,6 @@ class TogglesLayout(Widget):
         "disengage_on_accelerator.png",
         False,
       ),
-      "RadarDistance": (
-        lambda: tr("Radar Distance"),
-        DESCRIPTIONS["RadarDistance"],
-        "speed_limit.png",
-        False,
-      ),
       "IsLdwEnabled": (
         lambda: tr("Enable Lane Departure Warnings"),
         DESCRIPTIONS["IsLdwEnabled"],
@@ -121,24 +106,6 @@ class TogglesLayout(Widget):
       icon="speed_limit.png"
     )
 
-    self._accel_personality_enabled = toggle_item(
-      lambda: tr("Enable Acceleration Profiles"),
-      lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]),
-      self._params.get_bool("AccelPersonalityEnabled"),
-      callback=self._set_accel_personality_enabled,
-      icon="speed_limit.png",
-    )
-
-    self._accel_personality_setting = multiple_button_item(
-      lambda: tr("Acceleration Profile"),
-      lambda: tr(DESCRIPTIONS["AccelPersonality"]),
-      buttons=[lambda: tr("Eco"), lambda: tr("Normal"), lambda: tr("Sport")],
-      button_width=300,
-      callback=self._set_accel_personality,
-      selected_index=self._params.get("AccelPersonality", return_default=True),
-      icon="speed_limit.png"
-    )
-
     self._toggles = {}
     self._locked_toggles = set()
     for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
@@ -168,11 +135,9 @@ class TogglesLayout(Widget):
 
       self._toggles[param] = toggle
 
-      # insert longitudinal + acceleration personality after NDOG toggle
+      # insert longitudinal personality after NDOG toggle
       if param == "DisengageOnAccelerator":
         self._toggles["LongitudinalPersonality"] = self._long_personality_setting
-        self._toggles["AccelPersonalityEnabled"] = self._accel_personality_enabled
-        self._toggles["AccelPersonality"] = self._accel_personality_setting
 
     self._update_experimental_mode_icon()
     self._scroller = Scroller(list(self._toggles.values()), line_separator=True, spacing=0)
@@ -211,15 +176,11 @@ class TogglesLayout(Widget):
         self._toggles["ExperimentalMode"].action_item.set_enabled(True)
         self._toggles["ExperimentalMode"].set_description(e2e_description)
         self._long_personality_setting.action_item.set_enabled(True)
-        self._accel_personality_enabled.action_item.set_enabled(True)
-        self._accel_personality_setting.action_item.set_enabled(True)
       else:
         # no long for now
         self._toggles["ExperimentalMode"].action_item.set_enabled(False)
         self._toggles["ExperimentalMode"].action_item.set_state(False)
         self._long_personality_setting.action_item.set_enabled(False)
-        self._accel_personality_enabled.action_item.set_enabled(False)
-        self._accel_personality_setting.action_item.set_enabled(False)
         self._params.remove("ExperimentalMode")
 
         unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.")
@@ -286,9 +247,3 @@ class TogglesLayout(Widget):
 
   def _set_longitudinal_personality(self, button_index: int):
     self._params.put("LongitudinalPersonality", button_index, block=True)
-
-  def _set_accel_personality(self, button_index: int):
-    self._params.put("AccelPersonality", button_index, block=True)
-
-  def _set_accel_personality_enabled(self, state: bool):
-    self._params.put_bool("AccelPersonalityEnabled", state, block=True)

selfdrive/ui/mici/layouts/main.py

@@ -13,7 +13,6 @@ from openpilot.system.ui.lib.application import gui_app
 
 if gui_app.sunnypilot_ui():
   from openpilot.selfdrive.ui.sunnypilot.mici.layouts.settings import SettingsLayoutSP as SettingsLayout
-  from openpilot.selfdrive.ui.sunnypilot.mici.layouts.onroad import OnroadViewContainerSP as AugmentedRoadView
 
 ONROAD_DELAY = 2.5  # seconds
 
@@ -119,15 +118,13 @@ class MiciMainLayout(Scroller):
 
     # FIXME: these two pops can interrupt user interacting in the settings
     if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY:
-      if not gui_app.sunnypilot_ui() or self._should_auto_scroll_to_onroad():
-        gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
+      gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
       self._onroad_time_delay = None
 
     # When car leaves standstill, pop nav stack and scroll to onroad
     CS = ui_state.sm["carState"]
     if not CS.standstill and self._prev_standstill:
-      if not gui_app.sunnypilot_ui() or self._should_auto_scroll_to_onroad():
-        gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
+      gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
     self._prev_standstill = CS.standstill
 
   def _on_interactive_timeout(self):

selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/lane_change_settings.py

@@ -51,17 +51,11 @@ class LaneChangeSettingsLayout(Widget):
       description=lambda: tr("Toggle to enable a delay timer for seamless lane changes when blind spot monitoring " +
                              "(BSM) detects a obstructing vehicle, ensuring safe maneuvering."),
     )
-    self._road_edge_block = toggle_item_sp(
-      param="RoadEdgeLaneChangeEnabled",
-      title=lambda: tr("Block Lane Change: Road Edge Detection"),
-      description=lambda: tr("Blocks the lane change if the model sees a road edge on your signaled side."),
-    )
 
     items = [
       self._lane_change_timer,
       LineSeparatorSP(40),
       self._bsm_delay,
-      self._road_edge_block,
     ]
 
     return items

selfdrive/ui/sunnypilot/mici/layouts/main.py

@@ -1,13 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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.
-"""
-
-from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
-
-
-class MiciMainLayoutSP(MiciMainLayout):
-  def _should_auto_scroll_to_onroad(self) -> bool:
-    return not self._onroad_layout.is_on_info_panel()

selfdrive/ui/sunnypilot/mici/layouts/onroad.py

@@ -1,63 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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 pyray as rl
-from openpilot.system.ui.lib.application import gui_app
-from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroller_sp import ScrollerSP
-from openpilot.selfdrive.ui.sunnypilot.mici.onroad.augmented_road_view import AugmentedRoadViewSP
-from openpilot.selfdrive.ui.sunnypilot.mici.layouts.onroad_info_panel import OnroadInfoPanel
-
-CONFIDENCE_BALL_VISIBLE_RATIO = 0.4
-HORIZONTAL_SETTLE_PX = 5
-HORIZONTAL_RESET_RATIO = 0.5
-
-
-class OnroadViewContainerSP(ScrollerSP):
-  def __init__(self, bookmark_callback=None):
-    super().__init__(horizontal=False, snap_items=True, spacing=0, pad=0, scroll_indicator=False, edge_shadows=False)
-    self.road_view = AugmentedRoadViewSP(bookmark_callback=bookmark_callback)
-    self.onroad_info_panel = OnroadInfoPanel(bookmark_callback=bookmark_callback)
-
-    self._scroller.add_widgets([
-      self.road_view,
-      self.onroad_info_panel,
-    ])
-    self._scroller.set_reset_scroll_at_show(False)
-    self._scroller.set_scrolling_enabled(lambda: abs(self.rect.x) < HORIZONTAL_SETTLE_PX)
-
-    for child in (self.road_view, self.onroad_info_panel):
-      inner_touch_valid = child._touch_valid_callback
-      child.set_touch_valid_callback(
-        lambda inner=inner_touch_valid: self._touch_valid() and (inner() if inner else True)
-      )
-
-  def set_rect(self, rect: rl.Rectangle):
-    super().set_rect(rect)
-    self.road_view.set_rect(rect)
-    self.onroad_info_panel.set_rect(rect)
-    return self
-
-  def is_swiping_left(self) -> bool:
-    return self.road_view.is_swiping_left() or self.onroad_info_panel.is_swiping_left()
-
-  def set_click_callback(self, callback) -> None:
-    self.road_view.set_click_callback(callback)
-    self.onroad_info_panel.set_click_callback(callback)
-
-  def is_on_info_panel(self) -> bool:
-    """True when scrolled past halfway toward onroad_info_panel (used by main layout
-    to skip auto-pop-back-to-camera while user is reading the info panel)."""
-    return abs(self._scroller.scroll_panel.get_offset()) > self._rect.height / 2
-
-  def _render(self, rect: rl.Rectangle):
-    if abs(self.rect.x) > gui_app.width * HORIZONTAL_RESET_RATIO:
-      self._scroller.scroll_panel.set_offset(0)
-
-    vertical_offset = self._scroller.scroll_panel.get_offset()
-    show_ball = abs(vertical_offset) < rect.height * CONFIDENCE_BALL_VISIBLE_RATIO
-    self.road_view.set_show_confidence_ball(show_ball)
-
-    super()._render(rect)

selfdrive/ui/sunnypilot/mici/layouts/onroad_info_panel.py

@@ -1,324 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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 pyray as rl
-from dataclasses import dataclass
-from openpilot.common.constants import CV
-from openpilot.common.filter_simple import FirstOrderFilter
-from openpilot.selfdrive.ui.ui_state import ui_state
-from openpilot.system.ui.lib.application import gui_app, FontWeight
-from openpilot.system.ui.lib.multilang import tr
-from openpilot.system.ui.lib.text_measure import measure_text_cached
-from openpilot.system.ui.lib.application import MousePos
-from openpilot.system.ui.widgets import Widget
-from openpilot.selfdrive.ui.mici.onroad.alert_renderer import AlertRenderer
-from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import BookmarkIcon
-
-METER_TO_KM = 0.001
-METER_TO_MILE = 0.000621371
-
-
-@dataclass(frozen=True)
-class OnroadInfoPanelColors:
-  white: rl.Color = rl.WHITE
-  black: rl.Color = rl.BLACK
-  red: rl.Color = rl.Color(255, 0, 0, 255)
-  green: rl.Color = rl.Color(0, 255, 0, 255)
-  grey: rl.Color = rl.Color(190, 195, 190, 255)
-  light_grey: rl.Color = rl.Color(200, 200, 200, 255)
-  dark_grey: rl.Color = rl.Color(100, 100, 100, 255)
-  bg_dark: rl.Color = rl.Color(0, 0, 0, 255)
-  card_bg: rl.Color = rl.Color(50, 50, 50, 200)
-  badge_bg: rl.Color = rl.Color(60, 60, 60, 255)
-
-
-COLORS = OnroadInfoPanelColors()
-
-
-class OnroadInfoPanel(Widget):
-  def __init__(self, bookmark_callback=None):
-    super().__init__()
-    self.speed_limit: float = 0.0
-    self.speed_limit_valid: bool = False
-    self.speed_limit_offset: float = 0.0
-    self.next_speed_limit: float = 0.0
-    self.next_speed_limit_distance: float = 0.0
-    self.road_name: str = ""
-    self.current_speed: float = 0.0
-    self.set_speed: float = 0.0
-    self.cruise_enabled: bool = False
-
-    self._sign_slide: float = 0.0
-
-    self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
-    self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
-    self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
-
-    self._marquee_offset: float = 0.0
-    self._marquee_direction: int = 1
-    self._marquee_pause_timer: float = 0.0
-    self._marquee_speed: float = 40.0
-    self._marquee_pause_duration: float = 1.5
-
-    self._alert_renderer = AlertRenderer()
-    self._alert_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps)
-
-    self._bookmark_icon = BookmarkIcon(bookmark_callback)
-
-  def is_swiping_left(self) -> bool:
-    return self._bookmark_icon.is_swiping_left()
-
-  def _handle_mouse_release(self, mouse_pos: MousePos) -> None:
-    # Mirror stock AugmentedRoadView: suppress click while bookmark gesture active
-    if not self._bookmark_icon.interacting():
-      super()._handle_mouse_release(mouse_pos)
-
-  def _update_state(self) -> None:
-    sm = ui_state.sm
-    speed_conv = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
-
-    if sm.valid["longitudinalPlanSP"]:
-      lp_sp = sm["longitudinalPlanSP"]
-      resolver = lp_sp.speedLimit.resolver
-      self.speed_limit = resolver.speedLimit * speed_conv
-      self.speed_limit_valid = resolver.speedLimitValid
-      self.speed_limit_offset = resolver.speedLimitOffset * speed_conv
-
-    if sm.valid["liveMapDataSP"]:
-      lmd = sm["liveMapDataSP"]
-      self.next_speed_limit = lmd.speedLimitAhead * speed_conv
-      self.next_speed_limit_distance = lmd.speedLimitAheadDistance
-      self.road_name = lmd.roadName
-
-    if sm.updated["carState"]:
-      self.current_speed = sm["carState"].vEgo * speed_conv
-
-    if sm.valid["carState"] and sm.valid["controlsState"]:
-      self.cruise_enabled = sm["carState"].cruiseState.enabled
-      v_cruise_cluster = sm["carState"].vCruiseCluster
-      set_speed_kph = sm["controlsState"].vCruiseDEPRECATED if v_cruise_cluster == 0.0 else v_cruise_cluster
-      self.set_speed = set_speed_kph * (METER_TO_MILE / METER_TO_KM) if not ui_state.is_metric else set_speed_kph
-
-  def _render(self, rect: rl.Rectangle) -> None:
-    self._update_state()
-
-    rl.draw_rectangle(int(rect.x), int(rect.y), int(rect.width), int(rect.height), COLORS.bg_dark)
-    margin = 20
-    mid_y = rect.y + rect.height / 2
-
-    left_x = rect.x + margin
-
-    if self.cruise_enabled:
-      unit = tr("MAX")
-      display_speed = self.set_speed
-    else:
-      unit = tr("km/h") if ui_state.is_metric else tr("MPH")
-      display_speed = self.current_speed
-
-    speed_val = str(round(display_speed))
-    if self.speed_limit_valid and display_speed > self.speed_limit:
-      speed_color = COLORS.red
-    else:
-      speed_color = COLORS.white
-
-    rl.draw_text_ex(self._font_semi_bold, unit, rl.Vector2(left_x, mid_y - 95), 38, 0, COLORS.grey)
-    rl.draw_text_ex(self._font_bold, speed_val, rl.Vector2(left_x, mid_y - 60), 110, 0, speed_color)
-
-    sign_width = 135
-    sign_height = 135 if ui_state.is_metric else 175
-
-    has_next = self.next_speed_limit > 0 and self.next_speed_limit != self.speed_limit
-    target_slide = 1.0 if has_next else 0.0
-    slide_speed = 3.0 * rl.get_frame_time()
-    if self._sign_slide < target_slide:
-      self._sign_slide = min(self._sign_slide + slide_speed, target_slide)
-    elif self._sign_slide > target_slide:
-      self._sign_slide = max(self._sign_slide - slide_speed, target_slide)
-
-    next_w = int(sign_width * 0.7)
-    next_h = int(sign_height * 0.7)
-    next_peek = int(next_w * 0.85) + 5
-    centered_x = rect.x + rect.width - sign_width - margin
-    shifted_x = rect.x + rect.width - sign_width - margin - next_peek
-    sign_x = centered_x + (shifted_x - centered_x) * self._sign_slide
-    sign_y = rect.y + (rect.height - sign_height) / 2
-
-    road_y = mid_y + 55
-    road_width = sign_x - left_x - margin
-    self._draw_road_name(left_x, road_y, road_width)
-
-    if has_next and self._sign_slide > 0.01:
-      next_val = str(round(self.next_speed_limit))
-      dist_str = self._format_distance(self.next_speed_limit_distance)
-      next_x = sign_x + sign_width - int(next_w * 0.15)
-      next_y = sign_y + (sign_height - next_h) / 2
-
-      next_speed_color = COLORS.black
-      if ui_state.is_metric:
-        self._draw_vienna_sign(next_x, next_y, next_w, next_h, next_val, next_speed_color, is_upcoming=True)
-      else:
-        self._draw_mutcd_sign(next_x, next_y, next_w, next_h, next_val, next_speed_color, is_upcoming=True)
-
-      dist_size = measure_text_cached(self._font_medium, dist_str, 24)
-      rl.draw_text_ex(self._font_medium, dist_str, rl.Vector2(next_x + next_w / 2 - dist_size.x / 2, next_y + next_h + 4), 24, 0, COLORS.grey)
-
-    self._draw_speed_limit_sign(sign_x, sign_y, sign_width, sign_height)
-
-    if self.speed_limit_offset != 0 and self.speed_limit_valid:
-      offset_val = str(abs(round(self.speed_limit_offset)))
-      badge_sz = 42
-      badge_x = sign_x + sign_width - badge_sz * 0.85
-      badge_y = sign_y - badge_sz * 0.25
-
-      if ui_state.is_metric:
-        badge_r = badge_sz / 2
-        badge_cx = badge_x + badge_r
-        badge_cy = badge_y + badge_r
-        rl.draw_circle(int(badge_cx), int(badge_cy), badge_r + 2, COLORS.dark_grey)
-        rl.draw_circle(int(badge_cx), int(badge_cy), badge_r, COLORS.badge_bg)
-        self._draw_text_centered(self._font_bold, offset_val, 24, rl.Vector2(badge_cx, badge_cy), COLORS.white)
-      else:
-        mutcd_badge_x = sign_x + sign_width - badge_sz * 0.65
-        mutcd_badge_y = sign_y - badge_sz * 0.50
-        badge_rect = rl.Rectangle(mutcd_badge_x, mutcd_badge_y, badge_sz, badge_sz)
-        rl.draw_rectangle_rounded(badge_rect, 0.25, 10, COLORS.badge_bg)
-        rl.draw_rectangle_rounded_lines_ex(badge_rect, 0.25, 10, 2, COLORS.dark_grey)
-        self._draw_text_centered(self._font_bold, offset_val, 24, rl.Vector2(mutcd_badge_x + badge_sz / 2, mutcd_badge_y + badge_sz / 2), COLORS.white)
-
-    # SCC
-    speed_size = measure_text_cached(self._font_bold, speed_val, 110)
-    scc_x = left_x + speed_size.x + 30
-    scc_y = mid_y - 50
-    self._draw_scc_icons(scc_x, scc_y)
-
-    self._bookmark_icon.render(rect)
-
-    if ui_state.started:
-      alert_obj, no_alert = self._alert_renderer.will_render()
-      self._alert_alpha_filter.update(0 if no_alert else 1)
-      alpha = self._alert_alpha_filter.x
-      if alpha > 0.01:
-        rl.draw_rectangle(int(rect.x), int(rect.y), int(rect.width), int(rect.height), rl.Color(0, 0, 0, int(150 * alpha)))
-      self._alert_renderer.render(rect)
-
-  def _draw_scc_icons(self, x: float, y: float) -> None:
-    sm = ui_state.sm
-    if not sm.valid["longitudinalPlanSP"]:
-      return
-    scc = sm["longitudinalPlanSP"].smartCruiseControl
-
-    box_w, box_h = 100, 36
-    gap = 6
-    drawn = 0
-
-    for label, active in [("SCC-V", scc.vision.active), ("SCC-M", scc.map.active)]:
-      if not active:
-        continue
-      bx = x
-      by = y + drawn * (box_h + gap)
-      rl.draw_rectangle_rounded(rl.Rectangle(bx, by, box_w, box_h), 0.3, 10, COLORS.green)
-      self._draw_text_centered(self._font_bold, label, 20, rl.Vector2(bx + box_w / 2, by + box_h / 2), COLORS.black)
-      drawn += 1
-
-  def _draw_speed_limit_sign(self, x: float, y: float, sign_width: float, sign_height: float) -> None:
-    speed_str = str(round(self.speed_limit)) if self.speed_limit_valid and self.speed_limit > 0 else "--"
-    speed_color = COLORS.black if not self.speed_limit_valid or self.current_speed <= self.speed_limit else COLORS.red
-
-    if ui_state.is_metric:
-      self._draw_vienna_sign(x, y, sign_width, sign_height, speed_str, speed_color, is_upcoming=False)
-    else:
-      self._draw_mutcd_sign(x, y, sign_width, sign_height, speed_str, speed_color, is_upcoming=False)
-
-  def _draw_road_name(self, x: float, y: float, width: float) -> None:
-    road_display = self.road_name if self.road_name else "--"
-    font_size = 30
-    road_size = measure_text_cached(self._font_semi_bold, road_display, font_size)
-    text_width = road_size.x
-
-    if text_width <= width:
-      self._marquee_offset = 0.0
-      self._marquee_direction = 1
-      self._marquee_pause_timer = 0.0
-      rl.draw_text_ex(self._font_semi_bold, road_display, rl.Vector2(x, y), font_size, 0, COLORS.white)
-    else:
-      overflow = text_width - width
-      dt = rl.get_frame_time()
-
-      if self._marquee_pause_timer > 0:
-        self._marquee_pause_timer -= dt
-      else:
-        self._marquee_offset += self._marquee_direction * self._marquee_speed * dt
-
-        if self._marquee_offset >= overflow:
-          self._marquee_offset = overflow
-          self._marquee_direction = -1
-          self._marquee_pause_timer = self._marquee_pause_duration
-        elif self._marquee_offset <= 0:
-          self._marquee_offset = 0
-          self._marquee_direction = 1
-          self._marquee_pause_timer = self._marquee_pause_duration
-
-      rl.begin_scissor_mode(int(x), int(y), int(width), int(road_size.y + 4))
-      text_pos = rl.Vector2(x - self._marquee_offset, y)
-      rl.draw_text_ex(self._font_semi_bold, road_display, text_pos, font_size, 0, COLORS.white)
-      rl.end_scissor_mode()
-
-  def _draw_vienna_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color, is_upcoming: bool = False) -> None:
-    center = rl.Vector2(x + width / 2, y + height / 2)
-    outer_radius = min(width, height) / 2
-
-    rl.draw_circle_v(center, outer_radius, COLORS.white)
-    ring_width = outer_radius * 0.18
-    rl.draw_ring(center, outer_radius - ring_width, outer_radius, 0, 360, 36, COLORS.red)
-
-    font_size = outer_radius * (0.7 if len(speed_str) >= 3 else 0.9)
-    text_size = measure_text_cached(self._font_bold, speed_str, int(font_size))
-    text_pos = rl.Vector2(center.x - text_size.x / 2, center.y - text_size.y / 2)
-    rl.draw_text_ex(self._font_bold, speed_str, text_pos, font_size, 0, speed_color)
-
-  def _draw_mutcd_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color, is_upcoming: bool = False) -> None:
-    sign_rect = rl.Rectangle(x, y, width, height)
-    rl.draw_rectangle_rounded(sign_rect, 0.35, 10, COLORS.white)
-
-    inset = max(4, width * 0.05)
-    inner_rect = rl.Rectangle(x + inset, y + inset, width - inset * 2, height - inset * 2)
-    outer_radius = 0.35 * width / 2.0
-    inner_radius = outer_radius - inset
-    inner_roundness = inner_radius / (inner_rect.width / 2.0)
-    rl.draw_rectangle_rounded_lines_ex(inner_rect, inner_roundness, 10, 3, COLORS.black)
-
-    mid_x = x + width / 2
-    label_size = max(18, int(width * 0.26))
-    if is_upcoming:
-      self._draw_text_centered(self._font_bold, tr("AHEAD"), label_size, rl.Vector2(mid_x, y + height * 0.27), COLORS.black)
-    else:
-      self._draw_text_centered(self._font_bold, tr("SPEED"), label_size, rl.Vector2(mid_x, y + height * 0.20), COLORS.black)
-      self._draw_text_centered(self._font_bold, tr("LIMIT"), label_size, rl.Vector2(mid_x, y + height * 0.40), COLORS.black)
-
-    speed_font_size = int(width * 0.52) if len(speed_str) >= 3 else int(width * 0.62)
-    self._draw_text_centered(self._font_bold, speed_str, speed_font_size, rl.Vector2(mid_x, y + height * 0.72), speed_color)
-
-  def _draw_text_centered(self, font, text, size, pos_center, color):
-    sz = measure_text_cached(font, text, size)
-    rl.draw_text_ex(font, text, rl.Vector2(pos_center.x - sz.x / 2, pos_center.y - sz.y / 2), size, 0, color)
-
-  def _format_distance(self, distance: float) -> str:
-    if ui_state.is_metric:
-      if distance < 50:
-        return tr("Near")
-      if distance >= 1000:
-        return f"{distance * METER_TO_KM:.1f}" + tr("km")
-      if distance < 200:
-        rounded = max(10, int(distance / 10) * 10)
-      else:
-        rounded = int(distance / 100) * 100
-      return str(rounded) + tr("m")
-    else:
-      distance_mi = distance * METER_TO_MILE
-      if distance_mi < 0.1:
-        return tr("Near")
-      return f"{distance_mi:.1f}" + tr("mi")

selfdrive/ui/sunnypilot/mici/onroad/augmented_road_view.py

@@ -1,30 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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 pyray as rl
-from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import AugmentedRoadView
-
-
-class _SuppressedConfidenceBall:
-  def render(self, *_):
-    pass
-
-
-class AugmentedRoadViewSP(AugmentedRoadView):
-  def __init__(self, **kwargs):
-    super().__init__(**kwargs)
-    self._show_confidence_ball: bool = True
-    self._real_confidence_ball = self._confidence_ball
-    self._confidence_ball = _SuppressedConfidenceBall()
-
-  def set_show_confidence_ball(self, show: bool) -> None:
-    self._show_confidence_ball = show
-
-  def _render(self, rect: rl.Rectangle) -> None:
-    super()._render(rect)
-    if self._show_confidence_ball:
-      self._real_confidence_ball.render(self.rect)

selfdrive/ui/sunnypilot/mici/widgets/scroll_panel_sp.py

@@ -1,34 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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 pyray as rl
-from openpilot.system.ui.lib.application import MouseEvent
-from openpilot.system.ui.lib.scroll_panel2 import GuiScrollPanel2, ScrollState
-
-
-class GuiScrollPanel2SP(GuiScrollPanel2):
-  """Reject orthogonal-dominant drags so nested scrollers (outer horizontal +
-  inner vertical) don't both engage on a slightly diagonal swipe.
-
-  Implemented as a post-super state rollback rather than reimplementing the
-  PRESSED state machine — keeps stock behaviour authoritative."""
-
-  def _handle_mouse_event(self, mouse_event: MouseEvent, bounds: rl.Rectangle, bounds_size: float,
-                          content_size: float) -> None:
-    pre_state = self._state
-    super()._handle_mouse_event(mouse_event, bounds, bounds_size, content_size)
-
-    if self._state == ScrollState.MANUAL_SCROLL and pre_state == ScrollState.PRESSED and \
-       self._initial_click_event is not None:
-      diff_x = abs(mouse_event.pos.x - self._initial_click_event.pos.x)
-      diff_y = abs(mouse_event.pos.y - self._initial_click_event.pos.y)
-      along = diff_x if self._horizontal else diff_y
-      anti = diff_y if self._horizontal else diff_x
-      if anti > along:
-        self._state = ScrollState.STEADY
-        self._velocity = 0.0
-        self._velocity_buffer.clear()

selfdrive/ui/sunnypilot/mici/widgets/scroller_sp.py

@@ -1,16 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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.
-"""
-
-from openpilot.system.ui.widgets.scroller import Scroller
-from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroll_panel_sp import GuiScrollPanel2SP
-
-
-class ScrollerSP(Scroller):
-  def __init__(self, **kwargs):
-    super().__init__(**kwargs)
-    inner = self._scroller
-    inner.scroll_panel = GuiScrollPanel2SP(inner._horizontal, handle_out_of_bounds=not inner._snap_items)

selfdrive/ui/ui.py

@@ -10,9 +10,6 @@ from openpilot.selfdrive.ui.layouts.main import MainLayout
 from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
 from openpilot.selfdrive.ui.ui_state import ui_state
 
-if gui_app.sunnypilot_ui():
-  from openpilot.selfdrive.ui.sunnypilot.mici.layouts.main import MiciMainLayoutSP as MiciMainLayout
-
 BIG_UI = gui_app.big_ui()
 
 

sunnypilot/modeld_v2/compile_modeld.py

@@ -10,471 +10,291 @@ import argparse
 import os
 import pickle
 import time
-from functools import partial
 from collections import defaultdict
-
+from functools import partial
 import numpy as np
-from tinygrad.tensor import Tensor
+os.environ['GMMU'] = '0'
+
+def _patch_tinygrad_fetch_fw():
+  import hashlib
+  import pathlib
+  import zstandard
+  from tinygrad import helpers
+  _orig_fetch_fw = helpers.fetch_fw
+  def fetch_fw(path, name, sha256):
+    p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
+    if p.is_file():
+      blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read()
+      if hashlib.sha256(blob).hexdigest() == sha256:
+        return blob
+    return _orig_fetch_fw(path, name, sha256)
+  helpers.fetch_fw = fetch_fw
+_patch_tinygrad_fetch_fw()
+
+from openpilot.selfdrive.modeld.compile_modeld import NV12Frame, make_frame_prepare, sample_desire, sample_skip, shift_and_sample
+from tinygrad import dtypes
 from tinygrad.device import Device
 from tinygrad.engine.jit import TinyJit
-
-from openpilot.selfdrive.modeld.compile_modeld import (
-  NV12Frame, make_frame_prepare,
-  shift_and_sample, sample_skip, sample_desire,
-)
+from tinygrad.tensor import Tensor
 
 MODEL_TYPES = ('vision_policy', 'supercombo', 'vision_multi_policy')
 
 
-def _detect_desire_key(policy_input_shapes):
-  for k in policy_input_shapes:
-    if k.startswith('desire'):
-      return k
-  return None
+def _detect_desire_key(shapes: dict) -> str | None:
+  return next((key for key in shapes if key.startswith('desire')), None)
 
 
-def _detect_vision_keys(vision_input_shapes):
-  img_keys = sorted([k for k in vision_input_shapes if 'img' in k])
-  road_key = next((k for k in img_keys if 'big' not in k), None)
-  wide_key = next((k for k in img_keys if 'big' in k), None)
-  if road_key is None or wide_key is None:
-    raise ValueError(f"Cannot determine road/wide image keys from {list(vision_input_shapes.keys())}")
-  return road_key, wide_key
+def _detect_vision_keys(shapes: dict) -> tuple[str | None, str | None]:
+  img_keys = sorted(key for key in shapes if 'img' in key)
+  return (
+    next((key for key in img_keys if 'big' not in key), None),
+    next((key for key in img_keys if 'big' in key), None)
+  )
 
 
-def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device):
-  road_key, _ = _detect_vision_keys(vision_input_shapes)
-  img = vision_input_shapes[road_key]
-  n_frames = img[1] // 6
-  img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
-
-  fb = policy_input_shapes['features_buffer']
-  desire_key = _detect_desire_key(policy_input_shapes)
-  dp = policy_input_shapes[desire_key]
-  tc = policy_input_shapes.get('traffic_convention', (1, 2))
-
-  npy = {
-    'desire': np.zeros(dp[2], dtype=np.float32),
-    'traffic_convention': np.zeros(tc, dtype=np.float32),
-    'tfm': np.zeros((3, 3), dtype=np.float32),
-    'big_tfm': np.zeros((3, 3), dtype=np.float32),
-  }
-
-  handled = {'features_buffer', desire_key, 'traffic_convention'}
-  for key, shape in policy_input_shapes.items():
-    if key in handled:
-      continue
-    npy[key] = np.zeros(shape, dtype=np.float32)
-
-  input_queues = {
-    'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    'feat_q': Tensor(np.zeros((frame_skip * (fb[1] - 1) + 1, fb[0], fb[2]), dtype=np.float32), device=device).contiguous().realize(),
-    'desire_q': Tensor(np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32), device=device).contiguous().realize(),
-    **{k: Tensor(v, device='NPY').realize() for k, v in npy.items()},
-  }
-  return input_queues, npy
+def derive_frame_skip(vision_input_shapes: dict, policy_input_shapes: dict) -> int:
+  features_buffer = policy_input_shapes.get('features_buffer')
+  return 1 if not features_buffer or features_buffer[1] >= 99 else 4
 
 
-def make_run_split_policy(vision_runner, policy_runner, nv12: NV12Frame, model_w, model_h,
-                          vision_features_slice, frame_skip, desire_key, extra_policy_keys,
-                          vision_road_key, vision_wide_key, prepare_only=False):
-  frame_prepare = make_frame_prepare(nv12, model_w, model_h)
-  sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
-  sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
-
-  def run_policy(img_q, big_img_q, feat_q, desire_q, desire, traffic_convention, tfm, big_tfm, frame, big_frame, **extra):
-    npy_tensors = [tfm.to(Device.DEFAULT), big_tfm.to(Device.DEFAULT),
-                   desire.to(Device.DEFAULT), traffic_convention.to(Device.DEFAULT)]
-    extra_device = {k: extra[k].to(Device.DEFAULT) for k in extra_policy_keys}
-    Tensor.realize(*npy_tensors, *extra_device.values())
-    tfm, big_tfm, desire, traffic_convention = npy_tensors
-
-    img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn)
-    big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn)
-
-    if prepare_only:
-      return img, big_img
-
-    vision_out = next(iter(vision_runner({vision_road_key: img, vision_wide_key: big_img}).values())).cast('float32')
-
-    new_feat = vision_out[:, vision_features_slice].reshape(1, -1).unsqueeze(0)
-    feat_buf = shift_and_sample(feat_q, new_feat, sample_skip_fn)
-    desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
-
-    inputs = {'features_buffer': feat_buf, desire_key: desire_buf, 'traffic_convention': traffic_convention, **extra_device}
-    policy_out = next(iter(policy_runner(inputs).values())).cast('float32')
-
-    return vision_out, policy_out
-  return run_policy
-
-
-def compile_split_policy(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip,
-                         vision_runner, policy_runner, vision_metadata, policy_metadata):
-  print(f"Compiling combined policy JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
-
-  vision_features_slice = vision_metadata['output_slices']['hidden_state']
-  vision_input_shapes = vision_metadata['input_shapes']
-  policy_input_shapes = policy_metadata['input_shapes']
-  desire_key = _detect_desire_key(policy_input_shapes)
-  extra_policy_keys = [k for k in policy_input_shapes if k not in ('features_buffer', desire_key, 'traffic_convention')]
-  vision_road_key, vision_wide_key = _detect_vision_keys(vision_input_shapes)
-
-  _run = make_run_split_policy(vision_runner, policy_runner, nv12, model_w, model_h,
-                               vision_features_slice, frame_skip, desire_key, extra_policy_keys,
-                               vision_road_key, vision_wide_key, prepare_only)
-  run_policy_jit = TinyJit(_run, prune=True)
-
-  SEED = 42
-
-  def random_inputs_run_fn(fn, seed, test_val=None, test_buffers=None, expect_match=True):
-    input_queues, npy = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, Device.DEFAULT)
-    np.random.seed(seed)
-    Tensor.manual_seed(seed)
-
-    testing = test_val is not None or test_buffers is not None
-    n_runs = 1 if testing else 3
-
-    for i in range(n_runs):
-      frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
-      big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
-      for v in npy.values():
-        v[:] = np.random.randn(*v.shape).astype(v.dtype)
-      Device.default.synchronize()
-      st = time.perf_counter()
-      outs = fn(**input_queues, frame=frame, big_frame=big_frame)
-      mt = time.perf_counter()
-      Device.default.synchronize()
-      et = time.perf_counter()
-      print(f"  [{i+1}/{n_runs}] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
-
-      if i == 0:
-        val = [np.copy(v.numpy()) for v in outs]
-        buffers = [np.copy(v.numpy().copy()) for v in input_queues.values()]
-
-    if test_val is not None:
-      match = all(np.array_equal(a, b) for a, b in zip(val, test_val, strict=True))
-      assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={seed})"
-    if test_buffers is not None:
-      match = all(np.array_equal(a, b) for a, b in zip(buffers, test_buffers, strict=True))
-      assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={seed})"
-    return fn, val, buffers
-
-  print('capture + replay')
-  run_policy_jit, test_val, test_buffers = random_inputs_run_fn(run_policy_jit, SEED)
-
-  print('pickle round trip')
-  run_policy_jit = pickle.loads(pickle.dumps(run_policy_jit))
-  random_inputs_run_fn(run_policy_jit, SEED, test_val, test_buffers, expect_match=True)
-  random_inputs_run_fn(run_policy_jit, SEED+1, test_val, test_buffers, expect_match=False)
-  return run_policy_jit
-
-
-def derive_frame_skip(vision_input_shapes, policy_input_shapes):
-  fb = policy_input_shapes.get('features_buffer')
-  if fb is None:
-    return 1
-  fb_history = fb[1]
-  if fb_history >= 99:
-    return 1
-  return 4
-
-
-def make_supercombo_input_queues(input_shapes, frame_skip, device):
-  img_shape = input_shapes.get('img', input_shapes.get('input_imgs'))
-  if img_shape is None:
-    raise ValueError("No img input found in model shapes")
+def generate_queues_and_npy(input_shapes: dict, frame_skip: int, device: str = Device.DEFAULT) -> tuple[dict, dict]:
+  road_key, _ = _detect_vision_keys(input_shapes)
+  if not road_key:
+    raise ValueError("Vision road key missing from input shapes.")
 
+  img_shape = input_shapes[road_key]
   n_frames = img_shape[1] // 6
   img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3])
 
-  npy_keys = {}
-  queue_keys = {}
+  desire_key = _detect_desire_key(input_shapes)
+  if not desire_key:
+    raise ValueError("Desire key missing from input shapes.")
+
+  desire_shape = input_shapes[desire_key]
+  features_buffer = input_shapes.get('features_buffer')
+
+  npy_arrays = {
+    'desire': np.zeros(desire_shape[2], dtype=np.float32),
+    'tfm': np.zeros((3, 3), dtype=np.float32),
+    'big_tfm': np.zeros((3, 3), dtype=np.float32)
+  }
 
   for key, shape in input_shapes.items():
-    if 'img' in key:
-      continue
-    if len(shape) == 3 and shape[1] > 1:
-      if key.startswith('desire'):
-        npy_keys[key] = np.zeros(shape[2], dtype=np.float32)
-        queue_keys[f'{key}_q'] = Tensor(
-          np.zeros((frame_skip * shape[1], shape[0], shape[2]), dtype=np.float32),
-          device=device).contiguous().realize()
-      elif key == 'features_buffer':
-        queue_keys['feat_q'] = Tensor(
-          np.zeros((frame_skip * (shape[1] - 1) + 1, shape[0], shape[2]), dtype=np.float32),
-          device=device).contiguous().realize()
-      else:
-        npy_keys[key] = np.zeros(shape, dtype=np.float32)
-    elif len(shape) == 2:
-      npy_keys[key] = np.zeros(shape, dtype=np.float32)
+    if key not in npy_arrays and 'img' not in key and key not in ('features_buffer', desire_key):
+      npy_arrays[key] = np.zeros(shape, dtype=np.float32)
 
-  if 'traffic_convention' not in npy_keys:
-    tc_shape = input_shapes.get('traffic_convention', (1, 2))
-    npy_keys['traffic_convention'] = np.zeros(tc_shape, dtype=np.float32)
-
-  npy_keys['tfm'] = np.zeros((3, 3), dtype=np.float32)
-  npy_keys['big_tfm'] = np.zeros((3, 3), dtype=np.float32)
-
-  input_queues = {
+  queues = {
     'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
     'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    **queue_keys,
-    **{k: Tensor(v, device='NPY').realize() for k, v in npy_keys.items()},
+    'desire_q': Tensor(np.zeros((frame_skip * desire_shape[1], desire_shape[0], desire_shape[2]),
+                  dtype=np.float32), device=device).contiguous().realize()
   }
-  return input_queues, npy_keys
+
+  if features_buffer:
+    queues['feat_q'] = Tensor(np.zeros((frame_skip * (features_buffer[1] - 1) + 1, features_buffer[0], features_buffer[2]),
+                       dtype=np.float32), device=device).contiguous().realize()
+
+  queues.update({key: Tensor(value, device='NPY').realize() for key, value in npy_arrays.items()})
+  return queues, npy_arrays
 
 
-def make_run_supercombo(model_runner, nv12: NV12Frame, model_w, model_h,
-                        features_slice, frame_skip, input_shapes, prepare_only=False):
-  frame_prepare = make_frame_prepare(nv12, model_w, model_h)
+def make_split_input_queues(vision_input_shapes: dict, policy_input_shapes: dict, frame_skip: int, device: str = Device.DEFAULT) -> tuple[dict, dict]:
+  return generate_queues_and_npy({**vision_input_shapes, **policy_input_shapes}, frame_skip, device)
+
+
+def make_supercombo_input_queues(input_shapes: dict, frame_skip: int, device: str = Device.DEFAULT) -> tuple[dict, dict]:
+  return generate_queues_and_npy(input_shapes, frame_skip, device)
+
+
+def create_jit_runner(vision_runner, policy_runners: list, nv12: NV12Frame, model_size: tuple[int, int],
+            features_slice: slice, frame_skip: int, input_shapes: dict, prepare_only: bool):
+  frame_prepare = make_frame_prepare(nv12, *model_size)
   sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
   sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
 
   desire_key = _detect_desire_key(input_shapes)
-  if desire_key is None:
-    raise ValueError(f"No desire* key found in input_shapes: {list(input_shapes.keys())}")
-  road_img_key, wide_img_key = _detect_vision_keys(input_shapes)
-  extra_policy_keys = [k for k in input_shapes
-                       if k not in (desire_key, 'features_buffer', 'traffic_convention')
-                       and 'img' not in k]
+  road_key, wide_key = _detect_vision_keys(input_shapes)
 
-  def run_supercombo(img_q, big_img_q, feat_q, desire_q,
-                     frame, big_frame, **kwargs):
-    desire = kwargs.get(desire_key)
+  if not desire_key or not road_key or not wide_key:
+    raise ValueError("Missing required vision or desire keys in input shapes.")
+
+  extra_keys = [key for key in input_shapes if key not in (desire_key, 'features_buffer', 'traffic_convention') and 'img' not in key]
+
+  def runner(img_q, big_img_q, feat_q, frame, big_frame, tfm, big_tfm, **kwargs):
+    desire_q = kwargs['desire_q']
+    desire = kwargs['desire']
     traffic_convention = kwargs.get('traffic_convention')
-    tfm = kwargs['tfm']
-    big_tfm = kwargs['big_tfm']
 
-    tfm = tfm.to(Device.DEFAULT)
-    big_tfm = big_tfm.to(Device.DEFAULT)
-    desire = desire.to(Device.DEFAULT)
-    traffic_convention = traffic_convention.to(Device.DEFAULT)
-    Tensor.realize(tfm, big_tfm, desire, traffic_convention)
+    npys = [tfm.to(Device.DEFAULT), big_tfm.to(Device.DEFAULT), desire.to(Device.DEFAULT)]
+    if traffic_convention is not None:
+      npys.append(traffic_convention.to(Device.DEFAULT))
 
-    img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn)
-    big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn)
+    extra_tensors = {key: kwargs[key].to(Device.DEFAULT) for key in extra_keys if key in kwargs}
+    Tensor.realize(*npys, *extra_tensors.values())
+
+    tfm_dev, big_tfm_dev, desire_dev = npys[:3]
+    traffic_conv_dev = npys[3] if traffic_convention is not None else None
+
+    img = shift_and_sample(img_q, frame_prepare(frame, tfm_dev).unsqueeze(0), sample_skip_fn)
+    big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm_dev).unsqueeze(0), sample_skip_fn)
 
     if prepare_only:
       return img, big_img
 
-    desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
-    feat_buf = sample_skip_fn(feat_q)
+    desire_buf = shift_and_sample(desire_q, desire_dev.reshape(1, 1, -1), sample_desire_fn)
+    inputs = {desire_key: desire_buf, **extra_tensors}
 
-    inputs = {road_img_key: img, wide_img_key: big_img,
-              desire_key: desire_buf, 'features_buffer': feat_buf,
-              'traffic_convention': traffic_convention}
-    for k in extra_policy_keys:
-      if k in kwargs:
-        inputs[k] = kwargs[k].to(Device.DEFAULT)
+    if traffic_conv_dev is not None:
+      inputs['traffic_convention'] = traffic_conv_dev
 
-    model_out = next(iter(model_runner(inputs).values())).cast('float32')
+    if vision_runner:
+      vision_out = next(iter(vision_runner({road_key: img, wide_key: big_img}).values()))
+      vision_out_cast = vision_out.cast('float32')
+      new_feat = vision_out_cast[:, features_slice].reshape(1, -1).unsqueeze(0)
+      inputs['features_buffer'] = shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
+      policy_outs = [next(iter(pol_runner(inputs).values())).cast('float32') for pol_runner in policy_runners]
+      return (vision_out_cast, *policy_outs) if len(policy_outs) > 1 else (vision_out_cast, policy_outs[0])
+    inputs.update({road_key: img, wide_key: big_img, 'features_buffer': sample_skip_fn(feat_q)})
+    policy_out = next(iter(policy_runners[0](inputs).values())).cast('float32')
+    new_feat = policy_out[:, features_slice].reshape(1, -1).unsqueeze(0)
+    shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
+    return policy_out
 
-    new_feat = model_out[:, features_slice].reshape(1, -1).unsqueeze(0)
-    shift_and_sample(feat_q, new_feat, sample_skip_fn)
-
-    return model_out
-
-  return run_supercombo
+  return runner
 
 
-def make_run_vision_multi_policy(vision_runner, policy_runners, nv12: NV12Frame, model_w, model_h,
-                                 vision_features_slice, frame_skip, desire_key, extra_policy_keys,
-                                 vision_road_key, vision_wide_key, prepare_only=False):
-  frame_prepare = make_frame_prepare(nv12, model_w, model_h)
-  sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
-  sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
+def compile_and_warmup(nv12: NV12Frame, model_size: tuple[int, int], prepare_only: bool, frame_skip: int, vision_runner, policy_runners: list, metadata: dict):
+  print(f"Compiling combined JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
 
-  def run_multi_policy(img_q, big_img_q, feat_q, desire_q, desire,
-                       traffic_convention, tfm, big_tfm, frame, big_frame, **extra):
-    npy_tensors = [tfm.to(Device.DEFAULT), big_tfm.to(Device.DEFAULT),
-                   desire.to(Device.DEFAULT), traffic_convention.to(Device.DEFAULT)]
-    extra_device = {k: extra[k].to(Device.DEFAULT) for k in extra_policy_keys}
-    Tensor.realize(*npy_tensors, *extra_device.values())
-    tfm, big_tfm, desire, traffic_convention = npy_tensors
+  all_shapes = {key: value for meta in metadata.values() for key, value in meta['input_shapes'].items()}
 
-    img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn)
-    big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn)
+  feat_meta = metadata.get('vision') or metadata.get('model') or metadata.get('policy')
+  if not feat_meta:
+    raise ValueError("Could not find vision, model, or policy metadata.")
 
-    if prepare_only:
-      return img, big_img
+  features_slice = feat_meta['output_slices']['hidden_state']
+  WARP_DEV = 'CPU' if "USBGPU" in os.environ else Device.DEFAULT
 
-    vision_out = next(iter(vision_runner({vision_road_key: img, vision_wide_key: big_img}).values())).cast('float32')
+  run_func = create_jit_runner(vision_runner, policy_runners, nv12, model_size, features_slice, frame_skip, all_shapes, prepare_only)
+  run_jit = TinyJit(run_func, prune=True)
+  queues, npy_arrays = generate_queues_and_npy(all_shapes, frame_skip, Device.DEFAULT)
 
-    new_feat = vision_out[:, vision_features_slice].reshape(1, -1).unsqueeze(0)
-    feat_buf = shift_and_sample(feat_q, new_feat, sample_skip_fn)
-    desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
-
-    inputs = {'features_buffer': feat_buf, desire_key: desire_buf, 'traffic_convention': traffic_convention, **extra_device}
-
-    policy_outputs = []
-    for runner in policy_runners:
-      policy_out = next(iter(runner(inputs).values())).cast('float32')
-      policy_outputs.append(policy_out)
-
-    return (vision_out, *policy_outputs)
-
-  return run_multi_policy
-
-
-def _warmup_and_serialize(run_jit, input_queues, npy, nv12):
   for i in range(3):
     np.random.seed(42 + i)
-    frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
-    big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
-    for v in npy.values():
-      v[:] = np.random.randn(*v.shape).astype(v.dtype)
+    frame = Tensor.randint(nv12.size, low=0, high=256, dtype=dtypes.uint8, device=WARP_DEV).realize()
+    big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype=dtypes.uint8, device=WARP_DEV).realize()
+    for arr in npy_arrays.values():
+      arr[:] = np.random.randn(*arr.shape).astype(arr.dtype)
+
     Device.default.synchronize()
-    st = time.perf_counter()
-    run_jit(**input_queues, frame=frame, big_frame=big_frame)
-    mt = time.perf_counter()
+    start_time = time.perf_counter()
+    run_jit(**queues, frame=frame, big_frame=big_frame)
+    mid_time = time.perf_counter()
     Device.default.synchronize()
-    et = time.perf_counter()
-    print(f"  [{i + 1}/3] enqueue {(mt - st) * 1e3:6.2f} ms -- total {(et - st) * 1e3:6.2f} ms")
-  return pickle.loads(pickle.dumps(run_jit))
+    print(f"  [{i + 1}/3] enqueue {(mid_time - start_time) * 1e3:6.2f} ms -- total {(time.perf_counter() - start_time) * 1e3:6.2f} ms")
+
+  return pickle.loads(pickle.dumps(run_jit)) if not prepare_only else run_jit
 
 
-def compile_supercombo(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip,
-                       model_runner, metadata):
-  print(f"Compiling combined supercombo JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
-
-  features_slice = metadata['output_slices']['hidden_state']
-  input_shapes = metadata['input_shapes']
-
-  _run = make_run_supercombo(model_runner, nv12, model_w, model_h,
-                             features_slice, frame_skip, input_shapes, prepare_only)
-  run_jit = TinyJit(_run, prune=True)
-
-  input_queues, npy = make_supercombo_input_queues(input_shapes, frame_skip, Device.DEFAULT)
-
-  run_jit = _warmup_and_serialize(run_jit, input_queues, npy, nv12)
-  return run_jit
+def _parse_size(size_str: str) -> tuple[int, int]:
+  width, height = size_str.lower().split('x')
+  return int(width), int(height)
 
 
-def compile_multi_policy(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip,
-                         vision_runner, policy_runners, vision_metadata, policy_metadata):
-  print(f"Compiling combined multi-policy JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
-
-  vision_features_slice = vision_metadata['output_slices']['hidden_state']
-  vision_input_shapes = vision_metadata['input_shapes']
-  policy_input_shapes = policy_metadata['input_shapes']
-  desire_key = _detect_desire_key(policy_input_shapes)
-  extra_policy_keys = [k for k in policy_input_shapes if k not in ('features_buffer', desire_key, 'traffic_convention')]
-  vision_road_key, vision_wide_key = _detect_vision_keys(vision_input_shapes)
-
-  _run = make_run_vision_multi_policy(vision_runner, policy_runners, nv12, model_w, model_h,
-                                      vision_features_slice, frame_skip, desire_key, extra_policy_keys,
-                                      vision_road_key, vision_wide_key, prepare_only)
-  run_jit = TinyJit(_run, prune=True)
-
-  input_queues, npy = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, Device.DEFAULT)
-
-  run_jit = _warmup_and_serialize(run_jit, input_queues, npy, nv12)
-  return run_jit
+def read_file_chunked_to_shm(path):
+  if not path:
+    return None
+  import atexit
+  from openpilot.common.file_chunker import read_file_chunked
+  from openpilot.system.hardware.hw import Paths
+  shm_path = os.path.join(Paths.shm_path(), os.path.basename(path))
+  atexit.register(lambda: os.path.exists(shm_path) and os.remove(shm_path))
+  with open(shm_path, 'wb') as f:
+    f.write(read_file_chunked(path))
+  return shm_path
 
 
-def _parse_size(s):
-  w, h = s.lower().split('x')
-  return int(w), int(h)
+def _compile_for_resolutions(camera_resolutions: list, model_size: tuple[int, int], frame_skip: int,
+               vision_runner, policy_runners: list, metadata: dict) -> dict:
+  from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
+  return {
+    (cam_w, cam_h): {
+      name: compile_and_warmup(NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h)), model_size, prepare_only,
+                   frame_skip, vision_runner, policy_runners, metadata)
+      for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
+    }
+    for cam_w, cam_h in camera_resolutions
+  }
+
+
+def _load_policy_runners(args: argparse.Namespace) -> tuple[list, list]:
+  runners, keys = [], []
+  for name, onnx_arg in [('policy', args.policy_onnx), ('off_policy', args.off_policy_onnx), ('on_policy', args.on_policy_onnx)]:
+    if onnx_arg:
+      runners.append(OnnxRunner(onnx_arg))
+      keys.append(name)
+  return runners, keys
 
 
 if __name__ == "__main__":
-  from tinygrad.nn.onnx import OnnxRunner
-  from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
   from openpilot.selfdrive.modeld.get_model_metadata import make_metadata_dict
+  from tinygrad.nn.onnx import OnnxRunner
 
-  p = argparse.ArgumentParser(description="Compile combined JIT pkl for sunnypilot modeld_v2")
-  p.add_argument('--model-type', choices=MODEL_TYPES, required=True)
-  p.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
-  p.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True)
-  p.add_argument('--frame-skip', type=int, default=None, help='frame skip value (auto-derived if not provided)')
-  p.add_argument('--output', required=True)
+  parser = argparse.ArgumentParser(description="Compile combined JIT pkl for sunnypilot modeld_v2")
+  parser.add_argument('--model-type', choices=MODEL_TYPES, required=True)
+  parser.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
+  parser.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True)
+  parser.add_argument('--frame-skip', type=int, default=None, help='frame skip value (auto-derived if not provided)')
+  parser.add_argument('--output', required=True)
 
-  p.add_argument('--vision-onnx', help='vision ONNX (for split models)')
-  p.add_argument('--policy-onnx', help='policy ONNX (for vision_policy)')
-  p.add_argument('--off-policy-onnx', help='off-policy ONNX (for vision_multi_policy)')
-  p.add_argument('--on-policy-onnx', help='on-policy ONNX (for vision_multi_policy)')
-  p.add_argument('--supercombo-onnx', help='supercombo ONNX (for supercombo)')
+  parser.add_argument('--vision-onnx', help='vision ONNX (for split models)')
+  parser.add_argument('--policy-onnx', help='policy ONNX (for vision_policy)')
+  parser.add_argument('--off-policy-onnx', help='off-policy ONNX (for vision_multi_policy)')
+  parser.add_argument('--on-policy-onnx', help='on-policy ONNX (for vision_multi_policy)')
+  parser.add_argument('--supercombo-onnx', help='supercombo ONNX (for supercombo)')
 
-  args = p.parse_args()
-  out = defaultdict(dict)
+  args = parser.parse_args()
+  output_data = defaultdict(dict)
+
+  args.vision_onnx = read_file_chunked_to_shm(args.vision_onnx)
+  args.policy_onnx = read_file_chunked_to_shm(args.policy_onnx)
+  args.off_policy_onnx = read_file_chunked_to_shm(args.off_policy_onnx)
+  args.on_policy_onnx = read_file_chunked_to_shm(args.on_policy_onnx)
+  args.supercombo_onnx = read_file_chunked_to_shm(args.supercombo_onnx)
+
+  vision_runner = OnnxRunner(args.vision_onnx) if args.vision_onnx else None
 
   if args.model_type == 'vision_policy':
-    assert args.vision_onnx and args.policy_onnx
-    vision_runner = OnnxRunner(args.vision_onnx)
-    policy_runner = OnnxRunner(args.policy_onnx)
-    out['metadata']['vision'] = make_metadata_dict(args.vision_onnx)
-    out['metadata']['policy'] = make_metadata_dict(args.policy_onnx)
-
-    frame_skip = args.frame_skip if args.frame_skip is not None else derive_frame_skip(out['metadata']['vision']['input_shapes'],
-                                                                                       out['metadata']['policy']['input_shapes'])
-
-    for cam_w, cam_h in args.camera_resolutions:
-      nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-      model_w, model_h = args.model_size
-      out[(cam_w, cam_h)] = {
-        name: compile_split_policy(nv12, model_w, model_h, prepare_only, frame_skip,
-                                   vision_runner, policy_runner,
-                                   out['metadata']['vision'], out['metadata']['policy'])
-        for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
-      }
-
+    assert vision_runner and args.policy_onnx
+    policy_runners = [OnnxRunner(args.policy_onnx)]
+    output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx), 'policy': make_metadata_dict(args.policy_onnx)}
   elif args.model_type == 'supercombo':
     assert args.supercombo_onnx
-    model_runner = OnnxRunner(args.supercombo_onnx)
-    out['metadata']['model'] = make_metadata_dict(args.supercombo_onnx)
-
-    frame_skip = args.frame_skip if args.frame_skip is not None else derive_frame_skip({}, out['metadata']['model']['input_shapes'])
-
-    for cam_w, cam_h in args.camera_resolutions:
-      nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-      model_w, model_h = args.model_size
-      out[(cam_w, cam_h)] = {
-        name: compile_supercombo(nv12, model_w, model_h, prepare_only, frame_skip,
-                                 model_runner, out['metadata']['model'])
-        for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
-      }
-
+    policy_runners = [OnnxRunner(args.supercombo_onnx)]
+    output_data['metadata'] = {'model': make_metadata_dict(args.supercombo_onnx)}
   elif args.model_type == 'vision_multi_policy':
-    assert args.vision_onnx
-    vision_runner = OnnxRunner(args.vision_onnx)
-    out['metadata']['vision'] = make_metadata_dict(args.vision_onnx)
+    assert vision_runner
+    policy_runners, policy_names = _load_policy_runners(args)
+    output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx)}
+    for name in policy_names:
+      runner_arg = getattr(args, f"{name}_onnx")
+      output_data['metadata'][name] = make_metadata_dict(runner_arg)
 
-    policy_runners = []
-    policy_onnxes = []
-    if args.policy_onnx:
-      policy_onnxes.append(('policy', args.policy_onnx))
-    if args.off_policy_onnx:
-      policy_onnxes.append(('off_policy', args.off_policy_onnx))
-    if args.on_policy_onnx:
-      policy_onnxes.append(('on_policy', args.on_policy_onnx))
+  policy_keys = [key for key in output_data['metadata'].keys() if key != 'vision']
+  first_policy_meta = output_data['metadata'][policy_keys[0]] if policy_keys else {}
+  vision_meta = output_data['metadata'].get('vision', {})
 
-    for name, onnx_path in policy_onnxes:
-      runner = OnnxRunner(onnx_path)
-      policy_runners.append(runner)
-      out['metadata'][name] = make_metadata_dict(onnx_path)
+  derived_frame_skip = args.frame_skip or derive_frame_skip(vision_meta.get('input_shapes', {}), first_policy_meta.get('input_shapes', {}))
+  output_data.update(_compile_for_resolutions(args.camera_resolutions, args.model_size, derived_frame_skip,
+                        vision_runner, policy_runners, output_data['metadata']))
 
-    first_policy_key = policy_onnxes[0][0]
-    frame_skip = args.frame_skip if args.frame_skip is not None else derive_frame_skip(out['metadata']['vision']['input_shapes'],
-                                                                                       out['metadata'][first_policy_key]['input_shapes'])
+  with open(args.output, "wb") as file:
+    pickle.dump(output_data, file)
 
-    for cam_w, cam_h in args.camera_resolutions:
-      nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-      model_w, model_h = args.model_size
-      out[(cam_w, cam_h)] = {
-        name: compile_multi_policy(nv12, model_w, model_h, prepare_only, frame_skip,
-                                   vision_runner, policy_runners,
-                                   out['metadata']['vision'], out['metadata'][first_policy_key])
-        for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
-      }
-
-  with open(args.output, "wb") as f:
-    pickle.dump(out, f)
   pkl_size = os.path.getsize(args.output)
   print(f"Saved combined JIT to {args.output} ({pkl_size / 1e6:.2f} MB)")
 
   from openpilot.common.file_chunker import chunk_file, get_chunk_targets
   chunk_targets = get_chunk_targets(args.output, pkl_size)
   chunk_file(args.output, chunk_targets)
-  num_chunks = len(chunk_targets) - 1
-  print(f"Chunked into {num_chunks} file(s)")
+  print(f"Chunked into {len(chunk_targets) - 1} file(s)")

sunnypilot/modeld_v2/modeld.py

@@ -40,7 +40,6 @@ from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelp
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.models.helpers import get_active_bundle
-from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
 PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad"
 
@@ -63,11 +62,6 @@ def _find_driving_pkl(bundle):
   if _pkl_exists(pkl_path):
     return pkl_path
 
-  fallback = os.path.join(model_root, 'driving_tinygrad.pkl')
-  if _pkl_exists(fallback):
-    return fallback
-  return None
-
 
 class FrameMeta:
   frame_id: int = 0
@@ -126,7 +120,7 @@ class ModelState(ModelStateBase):
       self._vision_input_names = [k for k in model_metadata['input_shapes'] if 'img' in k]
       from openpilot.sunnypilot.modeld_v2.compile_modeld import make_supercombo_input_queues
       frame_skip = derive_frame_skip({}, model_metadata['input_shapes'])
-      self.input_queues, self.npy = make_supercombo_input_queues(model_metadata['input_shapes'], frame_skip, device=self.DEV)
+      self.input_queues, self.numpy_inputs = make_supercombo_input_queues(model_metadata['input_shapes'], frame_skip, device=self.DEV)
     else:
       vision_metadata = metadata['vision']
       policy_keys = [k for k in metadata if k != 'vision']
@@ -144,7 +138,7 @@ class ModelState(ModelStateBase):
       policy_input_shapes = first_policy_metadata['input_shapes']
       self._vision_input_names = [k for k in vision_input_shapes if 'img' in k]
       frame_skip = derive_frame_skip(vision_input_shapes, policy_input_shapes)
-      self.input_queues, self.npy = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device=self.DEV)
+      self.input_queues, self.numpy_inputs = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device=self.DEV)
 
     from openpilot.sunnypilot.modeld_v2.parse_model_outputs_split import Parser as SplitParser
     from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser
@@ -184,7 +178,7 @@ class ModelState(ModelStateBase):
 
   @property
   def desire_key(self) -> str:
-    return next(k for k in self.npy if k.startswith('desire'))
+    return next(k for k in self.numpy_inputs if k.startswith('desire'))
 
   def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
                 inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
@@ -200,16 +194,16 @@ class ModelState(ModelStateBase):
 
     desire_key = self.desire_key
     inputs[desire_key][0] = 0
-    self.npy[desire_key][:] = np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0)
+    self.numpy_inputs[desire_key][:] = np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0)
     self.prev_desire[:] = inputs[desire_key]
     for key in ('traffic_convention', 'lateral_control_params'):
-      if key in self.npy and key in inputs:
-        self.npy[key][:] = inputs[key]
+      if key in self.numpy_inputs and key in inputs:
+        self.numpy_inputs[key][:] = inputs[key]
 
     road_key = next(n for n in bufs if 'big' not in n)
     wide_key = next(n for n in bufs if 'big' in n)
-    self.npy['tfm'][:, :] = transforms[road_key].reshape(3, 3)
-    self.npy['big_tfm'][:, :] = transforms[wide_key].reshape(3, 3)
+    self.numpy_inputs['tfm'][:, :] = transforms[road_key].reshape(3, 3)
+    self.numpy_inputs['big_tfm'][:, :] = transforms[wide_key].reshape(3, 3)
 
     if prepare_only:
       self._warp_enqueue(**self.input_queues, frame=self.full_frames[road_key], big_frame=self.full_frames[wide_key])
@@ -237,8 +231,8 @@ class ModelState(ModelStateBase):
       if 'planplus' in outputs and 'plan' in outputs:
         outputs['plan'] = outputs['plan'] + outputs['planplus']
 
-    if 'desired_curvature' in outputs and 'prev_desired_curv' in self.npy:
-      buf = self.npy['prev_desired_curv']
+    if 'desired_curvature' in outputs and 'prev_desired_curv' in self.numpy_inputs:
+      buf = self.numpy_inputs['prev_desired_curv']
       buf[0, :-1] = buf[0, 1:]
       buf[0, -1, :] = outputs['desired_curvature'][0, :] if not self.mlsim else 0
 
@@ -330,7 +324,6 @@ def main(demo=False):
   prev_action = log.ModelDataV2.Action()
 
   DH = DesireHelper()
-  RELC = RoadEdgeLaneChangeController(DH)
   meta_constants = load_meta_constants()
 
   while True:
@@ -411,7 +404,7 @@ def main(demo=False):
       'traffic_convention': traffic_convention,
     }
 
-    if 'lateral_control_params' in model.npy:
+    if 'lateral_control_params' in model.numpy_inputs:
       inputs['lateral_control_params'] = np.array([v_ego, lat_delay], dtype=np.float32)
 
     mt1 = time.perf_counter()
@@ -435,10 +428,7 @@ def main(demo=False):
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
       r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
       lane_change_prob = l_lane_change_prob + r_lane_change_prob
-      RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego)
-      mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected
-      mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected
-      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected)
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

sunnypilot/modeld_v2/models/README.md

@@ -1,62 +0,0 @@
-## Neural networks in openpilot
-To view the architecture of the ONNX networks, you can use [netron](https://netron.app/)
-
-## Supercombo
-### Supercombo input format (Full size: 799906 x float32)
-* **image stream**
-  * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256
-    * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256
-      * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
-      * Channel 4 represents the half-res U channel
-      * Channel 5 represents the half-res V channel
-* **wide image stream**
-  * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256
-    * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256
-      * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
-      * Channel 4 represents the half-res U channel
-      * Channel 5 represents the half-res V channel
-* **desire**
-  * one-hot encoded buffer to command model to execute certain actions, bit needs to be sent for the past 5 seconds (at 20FPS) : 100 * 8
-* **traffic convention**
-  * one-hot encoded vector to tell model whether traffic is right-hand or left-hand traffic : 2
-* **feature buffer**
-  * A buffer of intermediate features that gets appended to the current feature to form a 5 seconds temporal context (at 20FPS) : 99 * 512
-
-
-### Supercombo output format (Full size: XXX x float32)
-Read [here](https://github.com/commaai/openpilot/blob/90af436a121164a51da9fa48d093c29f738adf6a/selfdrive/modeld/models/driving.h#L236) for more.
-
-
-## Driver Monitoring Model
-* .onnx model can be run with onnx runtimes
-* .dlc file is a pre-quantized model and only runs on qualcomm DSPs
-
-### input format
-* single image W = 1440 H = 960 luminance channel (Y) from the planar YUV420 format:
-  * full input size is 1440 * 960 = 1382400
-  * normalized ranging from 0.0 to 1.0 in float32 (onnx runner) or ranging from 0 to 255 in uint8 (snpe runner)
-* camera calibration angles (roll, pitch, yaw) from liveCalibration: 3 x float32 inputs
-
-### output format
-* 84 x float32 outputs = 2 + 41 * 2 ([parsing example](https://github.com/commaai/openpilot/blob/22ce4e17ba0d3bfcf37f8255a4dd1dc683fe0c38/selfdrive/modeld/models/dmonitoring.cc#L33))
-  * for each person in the front seats (2 * 41)
-    * face pose: 12 = 6 + 6
-      * face orientation [pitch, yaw, roll] in camera frame: 3
-      * face position [dx, dy] relative to image center: 2
-      * normalized face size: 1
-      * standard deviations for above outputs: 6
-    * face visible probability: 1
-    * eyes: 20 = (8 + 1) + (8 + 1) + 1 + 1
-      * eye position and size, and their standard deviations: 8
-      * eye visible probability: 1
-      * eye closed probability: 1
-    * wearing sunglasses probability: 1
-    * face occluded probability: 1
-    * touching wheel probability: 1
-    * paying attention probability: 1
-    * (deprecated) distracted probabilities: 2
-    * using phone probability: 1
-    * distracted probability: 1
-  * common outputs 2
-    * poor camera vision probability: 1
-    * left hand drive probability: 1

sunnypilot/modeld_v2/tests/dmon_lag/repro.cc

@@ -1,101 +0,0 @@
-// clang++ -O2 repro.cc && ./a.out
-
-#include <sched.h>
-#include <sys/types.h>
-#include <unistd.h>
-
-#include <cstdint>
-#include <cstdio>
-#include <cstdlib>
-#include <cstring>
-#include <ctime>
-
-static inline double millis_since_boot() {
-  struct timespec t;
-  clock_gettime(CLOCK_BOOTTIME, &t);
-  return t.tv_sec * 1000.0 + t.tv_nsec * 1e-6;
-}
-
-#define MODEL_WIDTH 320
-#define MODEL_HEIGHT 640
-
-// null function still breaks it
-#define input_lambda(x) x
-
-// this is copied from models/dmonitoring.cc, and is the code that triggers the issue
-void inner(uint8_t *resized_buf, float *net_input_buf) {
-  int resized_width = MODEL_WIDTH;
-  int resized_height = MODEL_HEIGHT;
-
-  // one shot conversion, O(n) anyway
-  // yuvframe2tensor, normalize
-  for (int r = 0; r < MODEL_HEIGHT/2; r++) {
-    for (int c = 0; c < MODEL_WIDTH/2; c++) {
-      // Y_ul
-      net_input_buf[(c*MODEL_HEIGHT/2) + r] = input_lambda(resized_buf[(2*r*resized_width) + (2*c)]);
-      // Y_ur
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width) + (2*c+1)]);
-      // Y_dl
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c)]);
-      // Y_dr
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c+1)]);
-      // U
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c]);
-      // V
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c]);
-    }
-  }
-}
-
-float trial() {
-  int resized_width = MODEL_WIDTH;
-  int resized_height = MODEL_HEIGHT;
-
-  int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v
-
-  // allocate the buffers
-  uint8_t *resized_buf = (uint8_t*)malloc(resized_width*resized_height*3/2);
-  float *net_input_buf = (float*)malloc(yuv_buf_len*sizeof(float));
-  printf("allocate -- %p 0x%x -- %p 0x%lx\n", resized_buf, resized_width*resized_height*3/2, net_input_buf, yuv_buf_len*sizeof(float));
-
-  // test for bad buffers
-  static int CNT = 20;
-  float avg = 0.0;
-  for (int i = 0; i < CNT; i++) {
-    double s4 = millis_since_boot();
-    inner(resized_buf, net_input_buf);
-    double s5 = millis_since_boot();
-    avg += s5-s4;
-  }
-  avg /= CNT;
-
-  // once it's bad, it's reliably bad
-  if (avg > 10) {
-    printf("HIT %f\n", avg);
-    printf("BAD\n");
-
-    for (int i = 0; i < 200; i++) {
-      double s4 = millis_since_boot();
-      inner(resized_buf, net_input_buf);
-      double s5 = millis_since_boot();
-      printf("%.2f   ", s5-s4);
-    }
-    printf("\n");
-
-    exit(0);
-  }
-
-  // don't free so we get a different buffer each time
-  //free(resized_buf);
-  //free(net_input_buf);
-
-  return avg;
-}
-
-int main() {
-  while (true) {
-    float ret = trial();
-    printf("got %f\n", ret);
-  }
-}
-

sunnypilot/modeld_v2/tests/test_combined_pkl_loader.py

@@ -46,16 +46,6 @@ class TestFindDrivingPkl:
     assert result is not None
     assert 'driving_fof_tinygrad.pkl' in result
 
-  def test_finds_fallback_driving_tinygrad(self, tmp_path, monkeypatch):
-    (tmp_path / 'driving_tinygrad.pkl').write_bytes(b'fake')
-    from openpilot.system.hardware import hw
-    monkeypatch.setattr(hw.Paths, 'model_root', staticmethod(lambda: str(tmp_path)))
-
-    bundle = DummyBundle(models=[DummyModel('vision', 'nonexistent.pkl')])
-    result = _find_driving_pkl(bundle)
-    assert result is not None
-    assert 'driving_tinygrad.pkl' in result
-
 
 # Init — assertion guard
 
@@ -84,8 +74,8 @@ class TestStockEquivalence:
     skip_keys = {'action_t'}
     assert set(state.input_queues.keys()) == set(stock_queues.keys()) - skip_keys, \
       f"Queue keys differ: v2={set(state.input_queues.keys())}, stock={set(stock_queues.keys())}"
-    assert set(state.npy.keys()) == set(stock_npy.keys()) - skip_keys, \
-      f"Npy keys differ: v2={set(state.npy.keys())}, stock={set(stock_npy.keys())}"
+    assert set(state.numpy_inputs.keys()) == set(stock_npy.keys()) - skip_keys, \
+      f"Npy keys differ: v2={set(state.numpy_inputs.keys())}, stock={set(stock_npy.keys())}"
 
   def test_split_queue_keys_work_with_desire_key(self, model_state_factory):
     from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, make_split_input_queues
@@ -188,16 +178,16 @@ class TestInputQueueCreation:
   def test_npy_contains_transforms(self, archetype_name, model_state_factory):
     arch = ARCHETYPES[archetype_name]
     state = model_state_factory(arch)
-    assert 'tfm' in state.npy, f"{arch.name}: 'tfm' missing from npy"
-    assert 'big_tfm' in state.npy, f"{arch.name}: 'big_tfm' missing from npy"
-    assert state.npy['tfm'].shape == (3, 3)
-    assert state.npy['big_tfm'].shape == (3, 3)
+    assert 'tfm' in state.numpy_inputs, f"{arch.name}: 'tfm' missing from npy"
+    assert 'big_tfm' in state.numpy_inputs, f"{arch.name}: 'big_tfm' missing from npy"
+    assert state.numpy_inputs['tfm'].shape == (3, 3)
+    assert state.numpy_inputs['big_tfm'].shape == (3, 3)
 
   @pytest.mark.parametrize("archetype_name", ARCHETYPE_NAMES)
   def test_npy_contains_desire(self, archetype_name, model_state_factory):
     arch = ARCHETYPES[archetype_name]
     state = model_state_factory(arch)
-    assert arch.expected_desire_key in state.npy, \
+    assert arch.expected_desire_key in state.numpy_inputs, \
       f"{arch.name}: '{arch.expected_desire_key}' missing from npy"
 
 

sunnypilot/modeld_v2/tests/test_warp.py

@@ -1,103 +0,0 @@
-import os
-os.environ['DEV'] = 'CPU'
-import pytest
-import numpy as np
-from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
-from openpilot.sunnypilot.modeld_v2.warp import CAMERA_CONFIGS
-from openpilot.sunnypilot.modeld_v2.warp import Warp, MODEL_W, MODEL_H
-
-VISION_NAME_PAIRS = [  # needed to account for supercombos input_imgs
-  ('img', 'big_img'),
-  ('input_imgs', 'big_input_imgs'),
-]
-
-
-class MockVisionBuf:
-  def __init__(self, w, h):
-    self.width = w
-    self.height = h
-    _, _, _, yuv_size = get_nv12_info(w, h)
-    self.data = np.zeros(yuv_size, dtype=np.uint8)
-
-
-@pytest.mark.parametrize("buffer_length", [2, 5])
-def test_warp_initialization(buffer_length):
-  warp = Warp(buffer_length)
-  assert warp.buffer_length == buffer_length
-  assert warp.img_buffer_shape == (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
-
-
-@pytest.mark.parametrize("buffer_length", [2, 5])
-@pytest.mark.parametrize("cam_w, cam_h", CAMERA_CONFIGS)
-@pytest.mark.parametrize("road, wide", VISION_NAME_PAIRS)
-def test_warp_process(buffer_length, cam_w, cam_h, road, wide):
-  warp = Warp(buffer_length)
-  mock_buf = MockVisionBuf(cam_w, cam_h)
-  transform = np.eye(3, dtype=np.float32).flatten()
-  bufs = {road: mock_buf, wide: mock_buf}
-  transforms = {road: transform, wide: transform}
-
-  out = warp.process(bufs, transforms)
-  assert isinstance(out, dict)
-  assert road in out and wide in out
-  assert out[road].shape == (1, 12, MODEL_H // 2, MODEL_W // 2)
-  assert out[wide].shape == (1, 12, MODEL_H // 2, MODEL_W // 2)
-
-  key = (cam_w, cam_h)
-  assert key in warp.jit_cache
-
-  out2 = warp.process(bufs, transforms)
-  assert out2[road].shape == out[road].shape
-
-
-@pytest.mark.parametrize("road, wide", VISION_NAME_PAIRS)
-def test_warp_buffer_shift(road, wide):
-  warp = Warp(2)
-  cam_w, cam_h = CAMERA_CONFIGS[1]
-  transform = np.eye(3, dtype=np.float32).flatten()
-
-  buf1 = MockVisionBuf(cam_w, cam_h)
-  buf1.data[0] = 255
-  bufs1 = {road: buf1, wide: buf1}
-  transforms = {road: transform, wide: transform}
-  out1 = warp.process(bufs1, transforms)
-  road1 = out1[road].numpy().copy()
-
-  buf2 = MockVisionBuf(cam_w, cam_h)
-  buf2.data[0] = 128
-  bufs2 = {road: buf2, wide: buf2}
-  out2 = warp.process(bufs2, transforms)
-  assert not np.array_equal(road1, out2[road].numpy())
-
-
-@pytest.mark.parametrize("buffer_length", [2, 5])
-@pytest.mark.parametrize("road, wide", VISION_NAME_PAIRS)
-def test_warp_buffer_accumulation(buffer_length, road, wide):
-  warp = Warp(buffer_length)
-  cam_w, cam_h = CAMERA_CONFIGS[0]
-  transform = np.eye(3, dtype=np.float32).flatten()
-  transforms = {road: transform, wide: transform}
-  outputs = []
-
-  for i in range(buffer_length + 1):
-    buf = MockVisionBuf(cam_w, cam_h)
-    buf.data[:] = i * 10
-    out = warp.process({road: buf, wide: buf}, transforms)
-    outputs.append(out[road].numpy().copy())
-
-  assert warp.full_buffers['img'].shape == (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
-  for i in range(1, len(outputs)):
-    assert not np.array_equal(outputs[i - 1], outputs[i])
-
-
-def test_warp_different_cameras_same_instance():
-  warp = Warp(2)
-  transform = np.eye(3, dtype=np.float32).flatten()
-
-  buf1 = MockVisionBuf(*CAMERA_CONFIGS[0])
-  warp.process({'img': buf1, 'big_img': buf1}, {'img': transform, 'big_img': transform})
-  assert len(warp.jit_cache) == 1
-
-  buf2 = MockVisionBuf(*CAMERA_CONFIGS[1])
-  warp.process({'img': buf2, 'big_img': buf2}, {'img': transform, 'big_img': transform})
-  assert len(warp.jit_cache) == 2

sunnypilot/modeld_v2/tests/tf_test/build.sh

@@ -1,2 +0,0 @@
-#!/usr/bin/env bash
-clang++ -I /home/batman/one/external/tensorflow/include/ -L /home/batman/one/external/tensorflow/lib -Wl,-rpath=/home/batman/one/external/tensorflow/lib main.cc -ltensorflow

sunnypilot/modeld_v2/tests/tf_test/main.cc

@@ -1,69 +0,0 @@
-#include <cassert>
-#include <cstdio>
-#include <cstdlib>
-#include "tensorflow/c/c_api.h"
-
-void* read_file(const char* path, size_t* out_len) {
-  FILE* f = fopen(path, "r");
-  if (!f) {
-    return NULL;
-  }
-  fseek(f, 0, SEEK_END);
-  long f_len = ftell(f);
-  rewind(f);
-
-  char* buf = (char*)calloc(f_len, 1);
-  assert(buf);
-
-  size_t num_read = fread(buf, f_len, 1, f);
-  fclose(f);
-
-  if (num_read != 1) {
-    free(buf);
-    return NULL;
-  }
-
-  if (out_len) {
-    *out_len = f_len;
-  }
-
-  return buf;
-}
-
-static void DeallocateBuffer(void* data, size_t) {
-  free(data);
-}
-
-int main(int argc, char* argv[]) {
-  TF_Buffer* buf;
-  TF_Graph* graph;
-  TF_Status* status;
-  char *path = argv[1];
-
-  // load model
-  {
-    size_t model_size;
-    char tmp[1024];
-    snprintf(tmp, sizeof(tmp), "%s.pb", path);
-    printf("loading model %s\n", tmp);
-    uint8_t *model_data = (uint8_t *)read_file(tmp, &model_size);
-    buf = TF_NewBuffer();
-    buf->data = model_data;
-    buf->length = model_size;
-    buf->data_deallocator = DeallocateBuffer;
-    printf("loaded model of size %d\n", model_size);
-  }
-
-  // import graph
-  status = TF_NewStatus();
-  graph = TF_NewGraph();
-  TF_ImportGraphDefOptions *opts = TF_NewImportGraphDefOptions();
-  TF_GraphImportGraphDef(graph, buf, opts, status);
-  TF_DeleteImportGraphDefOptions(opts);
-  TF_DeleteBuffer(buf);
-  if (TF_GetCode(status) != TF_OK) {
-    printf("FAIL: %s\n", TF_Message(status));
-  } else {
-    printf("SUCCESS\n");
-  }
-}

sunnypilot/modeld_v2/tests/tf_test/pb_loader.py

@@ -1,8 +0,0 @@
-#!/usr/bin/env python3
-import sys
-import tensorflow as tf
-
-with open(sys.argv[1], "rb") as f:
-  graph_def = tf.compat.v1.GraphDef()
-  graph_def.ParseFromString(f.read())
-  #tf.io.write_graph(graph_def, '', sys.argv[1]+".try")

sunnypilot/modeld_v2/tests/timing/benchmark.py

@@ -1,38 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-import time
-import numpy as np
-
-import cereal.messaging as messaging
-from openpilot.system.manager.process_config import managed_processes
-
-
-N = int(os.getenv("N", "5"))
-TIME = int(os.getenv("TIME", "30"))
-
-if __name__ == "__main__":
-  sock = messaging.sub_sock('modelV2', conflate=False, timeout=1000)
-
-  execution_times = []
-
-  for _ in range(N):
-    os.environ['LOGPRINT'] = 'debug'
-    managed_processes['modeld'].start()
-    time.sleep(5)
-
-    t = []
-    start = time.monotonic()
-    while time.monotonic() - start < TIME:
-      msgs = messaging.drain_sock(sock, wait_for_one=True)
-      for m in msgs:
-        t.append(m.modelV2.modelExecutionTime)
-
-    execution_times.append(np.array(t[10:]) * 1000)
-    managed_processes['modeld'].stop()
-
-  print("\n\n")
-  print(f"ran modeld {N} times for {TIME}s each")
-  for _, t in enumerate(execution_times):
-    print(f"\tavg: {sum(t)/len(t):0.2f}ms, min: {min(t):0.2f}ms, max: {max(t):0.2f}ms")
-  print("\n\n")

sunnypilot/modeld_v2/warp.py

@@ -1,171 +0,0 @@
-import pickle
-import time
-import numpy as np
-from pathlib import Path
-from tinygrad.tensor import Tensor
-from tinygrad.engine.jit import TinyJit
-from tinygrad.device import Device
-
-from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
-from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE
-from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
-from openpilot.selfdrive.modeld.compile_modeld import NV12Frame, make_frame_prepare as _make_frame_prepare
-
-CAMERA_CONFIGS = [
-  (_ar_ox_fisheye.width, _ar_ox_fisheye.height),
-  (_os_fisheye.width, _os_fisheye.height),
-]
-
-
-def make_frame_prepare(cam_w, cam_h, model_w, model_h):
-  nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-  return _make_frame_prepare(nv12, model_w, model_h)
-
-
-def warp_pkl_path(w, h):
-  from openpilot.selfdrive.modeld.helpers import MODELS_DIR
-  return MODELS_DIR / f'warp_{w}x{h}_tinygrad.pkl'
-
-
-def make_update_img_input(frame_prepare, model_w, model_h):
-  def update_img_input_tinygrad(tensor, frame, M_inv):
-    M_inv = M_inv.to(Device.DEFAULT)
-    new_img = frame_prepare(frame, M_inv)
-    tensor.assign(tensor[6:].cat(new_img, dim=0).contiguous())
-    return Tensor.cat(tensor[:6], tensor[-6:], dim=0).contiguous().reshape(1, 12, model_h//2, model_w//2)
-  return update_img_input_tinygrad
-
-
-def make_update_both_imgs(frame_prepare, model_w, model_h):
-  update_img = make_update_img_input(frame_prepare, model_w, model_h)
-  def update_both_imgs_tinygrad(calib_img_buffer, new_img, M_inv,
-                                calib_big_img_buffer, new_big_img, M_inv_big):
-    calib_img_pair = update_img(calib_img_buffer, new_img, M_inv)
-    calib_big_img_pair = update_img(calib_big_img_buffer, new_big_img, M_inv_big)
-    return calib_img_pair, calib_big_img_pair
-  return update_both_imgs_tinygrad
-
-MODELS_DIR = Path(__file__).parent / 'models'
-MODEL_W, MODEL_H = MEDMODEL_INPUT_SIZE
-UPSTREAM_BUFFER_LENGTH = 5
-
-
-def v2_warp_pkl_path(cam_w, cam_h, buffer_length):
-  return MODELS_DIR / f'warp_{cam_w}x{cam_h}_b{buffer_length}_tinygrad.pkl'
-
-
-def compile_v2_warp(cam_w, cam_h, buffer_length):
-  _, _, _, yuv_size = get_nv12_info(cam_w, cam_h)
-  img_buffer_shape = (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
-
-  print(f"Compiling v2 warp for {cam_w}x{cam_h} buffer_length={buffer_length}...")
-
-  frame_prepare = make_frame_prepare(cam_w, cam_h, MODEL_W, MODEL_H)
-  update_both_imgs = make_update_both_imgs(frame_prepare, MODEL_W, MODEL_H)
-  update_img_jit = TinyJit(update_both_imgs, prune=True)
-
-  full_buffer = Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize()
-  big_full_buffer = Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize()
-  new_frame_np = np.random.randint(0, 256, yuv_size, dtype=np.uint8)
-  new_big_frame_np = np.random.randint(0, 256, yuv_size, dtype=np.uint8)
-  for i in range(10):
-    img_inputs = [full_buffer,
-                  Tensor.from_blob(new_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(),
-                  Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')]
-    big_img_inputs = [big_full_buffer,
-                      Tensor.from_blob(new_big_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(),
-                      Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')]
-    inputs = img_inputs + big_img_inputs
-    Device.default.synchronize()
-
-    st = time.perf_counter()
-    _ = update_img_jit(*inputs)
-    mt = time.perf_counter()
-    Device.default.synchronize()
-    et = time.perf_counter()
-    print(f"  [{i+1}/10] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
-
-  pkl_path = v2_warp_pkl_path(cam_w, cam_h, buffer_length)
-  with open(pkl_path, "wb") as f:
-    pickle.dump(update_img_jit, f)
-  print(f"  Saved to {pkl_path}")
-
-  jit = pickle.load(open(pkl_path, "rb"))
-  verify_frame = np.random.randint(0, 256, yuv_size, dtype=np.uint8)
-  verify_big_frame = np.random.randint(0, 256, yuv_size, dtype=np.uint8)
-  fresh_inputs = [
-    Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize(),
-    Tensor.from_blob(verify_frame.ctypes.data, (yuv_size,), dtype='uint8').realize(),
-    Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY'),
-    Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize(),
-    Tensor.from_blob(verify_big_frame.ctypes.data, (yuv_size,), dtype='uint8').realize(),
-    Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY'),
-  ]
-  jit(*fresh_inputs)
-
-
-class Warp:
-  def __init__(self, buffer_length=2):
-    self.buffer_length = buffer_length
-    self.img_buffer_shape = (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
-
-    self.jit_cache = {}
-    self.full_buffers = {k: Tensor.zeros(self.img_buffer_shape, dtype='uint8').contiguous().realize() for k in ['img', 'big_img']}
-    self._blob_cache: dict[int, Tensor] = {}
-    self._nv12_cache: dict[tuple[int, int], int] = {}
-    self.transforms_np = {k: np.zeros((3, 3), dtype=np.float32) for k in ['img', 'big_img']}
-    self.transforms = {k: Tensor(v, device='NPY').realize() for k, v in self.transforms_np.items()}
-
-  def process(self, bufs, transforms):
-    if not bufs:
-      return {}
-    road = next(n for n in bufs if 'big' not in n)
-    wide = next(n for n in bufs if 'big' in n)
-    cam_w, cam_h = bufs[road].width, bufs[road].height
-    key = (cam_w, cam_h)
-
-    if key not in self.jit_cache:
-      v2_pkl = v2_warp_pkl_path(cam_w, cam_h, self.buffer_length)
-      if v2_pkl.exists():
-        with open(v2_pkl, 'rb') as f:
-          self.jit_cache[key] = pickle.load(f)
-      elif self.buffer_length == UPSTREAM_BUFFER_LENGTH:
-        upstream_pkl = warp_pkl_path(cam_w, cam_h)
-        if upstream_pkl.exists():
-          with open(upstream_pkl, 'rb') as f:
-            self.jit_cache[key] = pickle.load(f)
-      if key not in self.jit_cache:
-        frame_prepare = make_frame_prepare(cam_w, cam_h, MODEL_W, MODEL_H)
-        update_both_imgs = make_update_both_imgs(frame_prepare, MODEL_W, MODEL_H)
-        self.jit_cache[key] = TinyJit(update_both_imgs, prune=True)
-
-    if key not in self._nv12_cache:
-      self._nv12_cache[key] = get_nv12_info(cam_w, cam_h)[3]
-    yuv_size = self._nv12_cache[key]
-
-    road_ptr = bufs[road].data.ctypes.data
-    wide_ptr = bufs[wide].data.ctypes.data
-    if road_ptr not in self._blob_cache:
-      self._blob_cache[road_ptr] = Tensor.from_blob(road_ptr, (yuv_size,), dtype='uint8')
-    if wide_ptr not in self._blob_cache:
-      self._blob_cache[wide_ptr] = Tensor.from_blob(wide_ptr, (yuv_size,), dtype='uint8')
-    road_blob = self._blob_cache[road_ptr]
-    wide_blob = self._blob_cache[wide_ptr] if wide_ptr != road_ptr else Tensor.from_blob(wide_ptr, (yuv_size,), dtype='uint8')
-    np.copyto(self.transforms_np['img'], transforms[road].reshape(3, 3))
-    np.copyto(self.transforms_np['big_img'], transforms[wide].reshape(3, 3))
-
-    Device.default.synchronize()
-    res = self.jit_cache[key](
-      self.full_buffers['img'], road_blob, self.transforms['img'],
-      self.full_buffers['big_img'], wide_blob, self.transforms['big_img'],
-    )
-    out_road = res[0].realize()
-    out_wide = res[1].realize()
-
-    return {road: out_road, wide: out_wide}
-
-
-if __name__ == "__main__":
-  for cam_w, cam_h in CAMERA_CONFIGS:
-    for bl in [2, 5]:
-      compile_v2_warp(cam_w, cam_h, bl)

sunnypilot/models/helpers.py

@@ -6,80 +6,138 @@ See the LICENSE.md file in the root directory for more details.
 """
 
 import hashlib
+import os
 import pickle
+from pathlib import Path
 import numpy as np
 
-from openpilot.common.params import Params
 from cereal import custom
-from openpilot.sunnypilot.models.constants import Meta, MetaTombRaider, MetaSimPose
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.sunnypilot.models.constants import Meta, MetaSimPose, MetaTombRaider
 from openpilot.system.hardware.hw import Paths
-from pathlib import Path
-
-# see the README.md for more details on the model selector versioning
-CURRENT_SELECTOR_VERSION = 15
-REQUIRED_MIN_SELECTOR_VERSION = 14
 
+# SET ME TO THE EXACT JSON VERSION WE SET IN SUNNYPILOT_MODELS REPO
+REQUIRED_JSON_VERSION = 15
 
 CUSTOM_MODEL_PATH = Paths.model_root()
 METADATA_PATH = Path(__file__).parent / '../models/supercombo_metadata.pkl'
-
 ModelManager = custom.ModelManagerSP
+_LAST_VALIDATED_RAW = None
+
+
+def _compute_hash(file_path: str) -> str | None:
+  from openpilot.common.file_chunker import read_file_chunked
+  try:
+    return hashlib.sha256(read_file_chunked(file_path)).hexdigest().lower()
+  except FileNotFoundError:
+    return None
 
 
 async def verify_file(file_path: str, expected_hash: str) -> bool:
-  from openpilot.common.file_chunker import read_file_chunked
-  try:
-    data = read_file_chunked(file_path)
-  except FileNotFoundError:
-    return False
-  return hashlib.sha256(data).hexdigest().lower() == expected_hash.lower()
+  file_hash = _compute_hash(file_path)
+  return file_hash == expected_hash.lower() if file_hash else False
+
+
+def _verify_file(file_path: str, expected_hash: str) -> bool:
+  file_hash = _compute_hash(file_path)
+  return file_hash == expected_hash.lower() if file_hash else False
 
 
 def is_bundle_version_compatible(bundle: dict) -> bool:
   """
-  Checks whether the model bundle is compatible with the current selector version constraints.
-
-  The bundle specifies a `minimum_selector_version`, which defines the minimum selector version
+  The bundle parsed from the json specifies a `minimum_selector_version`, which defines the minimum selector version
   required to load the model. This function ensures that:
-
-    1. The model is not too old: the bundle must require at least `REQUIRED_MIN_SELECTOR_VERSION`.
-    2. The model is not too new: it must support the current selector version (`CURRENT_SELECTOR_VERSION`).
-
-  This allows the selector to enforce both a minimum and maximum range of supported models,
-  even if a model would otherwise be compatible.
-
-  :param bundle: Dictionary containing `minimum_selector_version`, as defined by the model bundle.
-  :type bundle: Dict
-  :return: True if the selector version is within the accepted range for the bundle; otherwise False.
-  :rtype: Bool
+    the bundle MUST match the `REQUIRED_JSON_VERSION` set here in helpers.
   """
-  return bool(REQUIRED_MIN_SELECTOR_VERSION <= bundle.get("minimumSelectorVersion", 0) <= CURRENT_SELECTOR_VERSION)
+  return bundle.get("minimumSelectorVersion", 0) == REQUIRED_JSON_VERSION
 
 
-def get_active_bundle(params: Params = None) -> custom.ModelManagerSP.ModelBundle:
-  """Gets the active model bundle from cache"""
-  if params is None:
-    params = Params()
+def _bundle_artifacts(bundle: custom.ModelManagerSP.ModelBundle) -> list[tuple[str, str]]:
+  artifacts = []
+  for model in getattr(bundle, 'models', []) or []:
+    for artifact in (getattr(model, 'artifact', None), getattr(model, 'metadata', None)):
+      if artifact and getattr(artifact, 'fileName', None) and getattr(artifact, 'downloadUri', None):
+        sha256 = getattr(artifact.downloadUri, 'sha256', None)
+        if sha256:
+          artifacts.append((artifact.fileName, sha256))
+  return artifacts
 
+
+def _bundle_is_valid_locally(bundle: custom.ModelManagerSP.ModelBundle) -> bool:
+  model_root = Paths.model_root()
+  return all(_verify_file(os.path.join(model_root, file_name), expected_hash)
+             for file_name, expected_hash in _bundle_artifacts(bundle))
+
+
+def _bundle_needs_reset(active_bundle: custom.ModelManagerSP.ModelBundle, available_bundles: list[custom.ModelManagerSP.ModelBundle] | None) -> bool:
+  if active_bundle is None:
+    return False
+
+  if available_bundles is not None:
+    matching_bundle = None
+    for bundle in available_bundles:
+      if getattr(active_bundle, 'ref', None) and getattr(bundle, 'ref', None):
+        if active_bundle.ref == bundle.ref:
+          matching_bundle = bundle
+          break
+      elif getattr(active_bundle, 'internalName', None) == getattr(bundle, 'internalName', None):
+        matching_bundle = bundle
+        break
+
+    if matching_bundle is None:
+      return True
+    if active_bundle.minimumSelectorVersion != matching_bundle.minimumSelectorVersion:
+      return True
+
+    active_runner = getattr(active_bundle, 'runner', None)
+    matching_runner = getattr(matching_bundle, 'runner', None)
+    if active_runner is not None and matching_runner is not None:
+      if getattr(active_runner, 'raw', active_runner) != getattr(matching_runner, 'raw', matching_runner):
+        return True
+    if set(_bundle_artifacts(active_bundle)) != set(_bundle_artifacts(matching_bundle)):
+      return True
+
+  return not _bundle_is_valid_locally(active_bundle)
+
+
+def validate_active_bundle(params: Params, available_bundles: list[custom.ModelManagerSP.ModelBundle] | None = None) -> None:
+  global _LAST_VALIDATED_RAW
+
+  raw_bundle = params.get("ModelManager_ActiveBundle")
+  if not raw_bundle:
+    return
+
+  if raw_bundle == _LAST_VALIDATED_RAW:
+    return
+
+  active_bundle = get_active_bundle(params, raw_bundle_dict=raw_bundle)
+  if active_bundle is None or _bundle_needs_reset(active_bundle, available_bundles):
+    cloudlog.warning("Active model bundle invalid; resetting to default")
+    params.remove("ModelManager_ActiveBundle")
+    params.put("ModelRunnerTypeCache", int(custom.ModelManagerSP.Runner.stock), block=True)
+    _LAST_VALIDATED_RAW = None
+  else:
+    _LAST_VALIDATED_RAW = raw_bundle
+
+
+def get_active_bundle(params: Params | None = None, raw_bundle_dict: dict | bytes | None = None) -> "custom.ModelManagerSP.ModelBundle | None":
+  params = params or Params()
   try:
-    if (active_bundle := params.get("ModelManager_ActiveBundle") or {}) and is_bundle_version_compatible(active_bundle):
-      return custom.ModelManagerSP.ModelBundle(**active_bundle)
+    active_bundle_dict = raw_bundle_dict if raw_bundle_dict is not None else (params.get("ModelManager_ActiveBundle") or {})
+    if active_bundle_dict and is_bundle_version_compatible(active_bundle_dict):
+      return custom.ModelManagerSP.ModelBundle(**active_bundle_dict)
   except Exception:
     pass
-
   return None
 
 
-def get_active_model_runner(params: Params = None, force_check=False) -> int:
-  if params is None:
-    params = Params()
-
+def get_active_model_runner(params: Params | None = None, force_check: bool = False) -> int:
+  params = params or Params()
   cached_runner_type = params.get("ModelRunnerTypeCache")
   if cached_runner_type is not None and not force_check:
     return cached_runner_type
-
   runner_type = custom.ModelManagerSP.Runner.stock
-
   if active_bundle := get_active_bundle(params):
     runner_type = active_bundle.runner.raw
 
@@ -88,66 +146,40 @@ def get_active_model_runner(params: Params = None, force_check=False) -> int:
 
   return runner_type
 
+
 def _get_model():
   if bundle := get_active_bundle():
     drive_model = next(model for model in bundle.models if model.type == ModelManager.Model.Type.supercombo)
     return drive_model
-
   return None
 
-def load_metadata():
-  metadata_path = METADATA_PATH
 
-  if model := _get_model():
-    metadata_path = f"{CUSTOM_MODEL_PATH}/{model.metadata.fileName}"
+def load_metadata():
+  model = _get_model()
+  metadata_path = f"{CUSTOM_MODEL_PATH}/{model.metadata.fileName}" if model else METADATA_PATH
 
   with open(metadata_path, 'rb') as f:
     return pickle.load(f)
 
 
-def prepare_inputs(model_metadata) -> dict[str, np.ndarray]:
-  # img buffers are managed in openCL transform code so we don't pass them as inputs
-  inputs = {
-    k: np.zeros(v, dtype=np.float32).flatten()
-    for k, v in model_metadata['input_shapes'].items()
-    if 'img' not in k
+def prepare_inputs(model_metadata: dict) -> dict[str, np.ndarray]:
+  return {
+    key: np.zeros(shape, dtype=np.float32).flatten()
+    for key, shape in model_metadata['input_shapes'].items()
+    if 'img' not in key
   }
 
-  return inputs
 
+def load_meta_constants(model_metadata: dict):
+  """ Loads the appropriate meta model class based on key shapes"""
+  if 'sim_pose' in model_metadata['input_shapes']:
+    return MetaSimPose
 
-def load_meta_constants(model_metadata):
-  """
-  Determines and loads the appropriate meta model class based on the metadata provided. The function checks
-  specific keys and conditions within the provided metadata dictionary to identify the corresponding meta
-  model class to return.
+  meta_slice = model_metadata['output_slices']['meta']
+  if (meta_slice.start, meta_slice.stop, meta_slice.step) == (5868, 5921, None):
+    return MetaTombRaider
 
-  :param model_metadata: Dictionary containing metadata about the model. It includes
-      details such as input shapes, output slices, and other configurations for identifying
-      metadata-dependent meta model classes.
-  :type model_metadata: dict
-  :return: The appropriate meta model class (Meta, MetaSimPose, or MetaTombRaider)
-      based on the conditions and metadata provided.
-  :rtype: type
-  """
-  meta = Meta  # Default Meta
-
-  if 'sim_pose' in model_metadata['input_shapes'].keys():
-    # Meta for models with sim_pose input
-    meta = MetaSimPose
-  else:
-    # Meta for Tomb Raider, it does not include sim_pose input but has the same meta slice as previous models
-    meta_slice = model_metadata['output_slices']['meta']
-    meta_tf_slice = slice(5868, 5921, None)
-
-    if (
-            meta_slice.start == meta_tf_slice.start and
-            meta_slice.stop == meta_tf_slice.stop and
-            meta_slice.step == meta_tf_slice.step
-    ):
-      meta = MetaTombRaider
-
-  return meta
+  return Meta
 
 
 # The following method(s) are modeld helper methods

sunnypilot/models/manager.py

@@ -17,7 +17,7 @@ from openpilot.system.hardware.hw import Paths
 
 from cereal import messaging, custom
 from openpilot.sunnypilot.models.fetcher import ModelFetcher
-from openpilot.sunnypilot.models.helpers import verify_file, get_active_bundle
+from openpilot.sunnypilot.models.helpers import get_active_bundle, validate_active_bundle, verify_file
 
 
 class ModelManagerSP:
@@ -239,6 +239,7 @@ class ModelManagerSP:
     while True:
       try:
         self.available_models = self.model_fetcher.get_available_bundles()
+        validate_active_bundle(self.params, self.available_models)
         self.active_bundle = get_active_bundle(self.params)
 
         if (index_to_download := self.params.get("ModelManager_DownloadIndex")) is not None:
@@ -252,8 +253,8 @@ class ModelManagerSP:
               self.selected_bundle = None
 
         if self.params.get("ModelManager_ClearCache"):
-            self.clear_model_cache()
-            self.params.remove("ModelManager_ClearCache")
+          self.clear_model_cache()
+          self.params.remove("ModelManager_ClearCache")
 
         self._report_status()
         rk.keep_time()

sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py

@@ -1,149 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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.
-"""
-
-from collections.abc import Sequence
-
-import numpy as np
-
-from cereal import messaging
-from opendbc.car import structs
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot import get_sanitize_int_param
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \
-  NORMAL, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, A_CRUISE_MAX_V, RISE_RATE, SMOOTH_DECEL_BP, \
-  SMOOTH_DECEL_V, BRAKE_DEEPENING_JERK, BRAKE_RELEASE_JERK, ACCEL_RISE_JERK, SMOOTH_DECEL_LOOKAHEAD_T, \
-  MIN_SMOOTH_BRAKE_NEED, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED
-
-_ZERO_ACCEL_EPS = 1e-6
-
-
-class AccelController:
-  def __init__(self, CP: structs.CarParams, mpc, params=None):
-    self._CP = CP
-    self._mpc = mpc
-    self._params = params or Params()
-    self._frame = 0
-    self._enabled = self._params.get_bool("AccelPersonalityEnabled")
-    self._personality = NORMAL
-    self._v_ego = 0.0
-    self._last_target_accel = 0.0
-    self._brake_need = 0.0
-    self._decel_target = 0.0
-    self._smooth_active = False
-    self._bypassed = False
-    self._read_params()
-
-  def _read_params(self) -> None:
-    self._enabled = self._params.get_bool("AccelPersonalityEnabled")
-    if not self._enabled:
-      self._personality = NORMAL
-      return
-    self._personality = get_sanitize_int_param("AccelPersonality", PERSONALITY_MIN, PERSONALITY_MAX, self._params)
-
-  def update(self, sm: messaging.SubMaster) -> None:
-    if self._frame % int(1. / DT_MDL) == 0:
-      self._read_params()
-    self._v_ego = sm['carState'].vEgo
-    self._frame += 1
-
-  def get_max_accel(self, v_ego: float) -> float:
-    return float(np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_V[self._personality]))
-
-  def get_rise_rate(self) -> float:
-    return RISE_RATE[self._personality]
-
-  def get_decel_target(self, brake_need: float) -> float:
-    return float(np.interp(max(0.0, float(brake_need)), SMOOTH_DECEL_BP, SMOOTH_DECEL_V[self._personality]))
-
-  def smooth_target_accel(self, raw_target_accel: float, accel_trajectory: Sequence[float], t_idxs: Sequence[float],
-                          should_stop: bool, reset: bool = False, stock_brake: bool = False) -> float:
-    raw_target_accel = float(raw_target_accel)
-    self._brake_need = self._compute_brake_need(raw_target_accel, accel_trajectory, t_idxs)
-    self._decel_target = 0.0
-
-    # disabled, reset, or blended/e2e braking: hand straight to the plan
-    if reset or not self._enabled or (stock_brake and (raw_target_accel < 0.0 or self._brake_need >= MIN_SMOOTH_BRAKE_NEED)):
-      self._bypassed = False
-      return self._passthrough(raw_target_accel)
-
-    self._bypassed = self._emergency_bypass(raw_target_accel, should_stop)
-    if self._bypassed:
-      return self._passthrough(raw_target_accel)
-
-    if self._brake_need < MIN_SMOOTH_BRAKE_NEED:
-      self._smooth_active = False
-      slewed = self._slew(raw_target_accel)
-      return self._finalize(min(slewed, raw_target_accel) if raw_target_accel < 0.0 else slewed)
-
-    # front-load a gentle early brake, never weaker than the plan
-    self._smooth_active = True
-    self._decel_target = self.get_decel_target(self._brake_need)
-    slewed = self._slew(min(raw_target_accel, self._decel_target))
-    return self._finalize(min(slewed, raw_target_accel))
-
-  def _compute_brake_need(self, raw_target_accel: float, accel_trajectory: Sequence[float], t_idxs: Sequence[float]) -> float:
-    min_accel = float(raw_target_accel)
-    for accel, t in zip(accel_trajectory, t_idxs, strict=False):
-      if float(t) <= SMOOTH_DECEL_LOOKAHEAD_T:
-        min_accel = min(min_accel, float(accel))
-    return max(0.0, -min_accel)
-
-  def _emergency_bypass(self, raw_target_accel: float, should_stop: bool) -> bool:
-    return (self._mpc.crash_cnt > 0 or should_stop or
-            raw_target_accel <= HARD_BRAKE_TARGET_ACCEL or self._brake_need >= HARD_BRAKE_NEED)
-
-  def _slew(self, target_accel: float) -> float:
-    target_accel = float(target_accel)
-    if target_accel > self._last_target_accel:
-      return self._slew_up(target_accel)
-    step = BRAKE_DEEPENING_JERK[self._personality] * DT_MDL
-    return self._clean_accel(max(target_accel, self._last_target_accel - step))
-
-  def _slew_up(self, target_accel: float) -> float:
-    if self._last_target_accel < 0.0:
-      released = min(target_accel, self._last_target_accel + BRAKE_RELEASE_JERK * DT_MDL)
-      if released <= 0.0:
-        return self._clean_accel(released)
-      return self._clean_accel(min(target_accel, ACCEL_RISE_JERK[self._personality] * DT_MDL))
-    step = ACCEL_RISE_JERK[self._personality] * DT_MDL
-    return self._clean_accel(min(target_accel, self._last_target_accel + step))
-
-  def _passthrough(self, target_accel: float) -> float:
-    self._smooth_active = False
-    return self._finalize(target_accel)
-
-  def _finalize(self, target_accel: float) -> float:
-    target_accel = self._clean_accel(target_accel)
-    self._last_target_accel = target_accel
-    return target_accel
-
-  @staticmethod
-  def _clean_accel(accel: float) -> float:
-    accel = float(accel)
-    return 0.0 if abs(accel) < _ZERO_ACCEL_EPS else accel
-
-  def enabled(self) -> bool:
-    return self._enabled
-
-  def personality(self):
-    return self._personality
-
-  def max_accel(self) -> float:
-    return self.get_max_accel(self._v_ego)
-
-  def brake_need(self) -> float:
-    return self._brake_need
-
-  def decel_target(self) -> float:
-    return self._decel_target
-
-  def smooth_active(self) -> bool:
-    return self._smooth_active
-
-  def bypassed(self) -> bool:
-    return self._bypassed

sunnypilot/selfdrive/controls/lib/accel_personality/constants.py

@@ -1,43 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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.
-"""
-
-from cereal import custom
-
-AccelerationPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
-ECO = AccelerationPersonality.eco
-NORMAL = AccelerationPersonality.normal
-SPORT = AccelerationPersonality.sport
-
-PERSONALITY_MIN = min(AccelerationPersonality.schema.enumerants.values())
-PERSONALITY_MAX = max(AccelerationPersonality.schema.enumerants.values())
-
-# Accel ceiling. NORMAL is stock so a disabled controller (forced to NORMAL) is stock.
-A_CRUISE_MAX_BP = [0., 10., 25., 40.]
-STOCK_A_CRUISE_MAX_V = [1.6, 1.2, 0.8, 0.6]
-STOCK_RISE_RATE = 0.05
-A_CRUISE_MAX_V = {
-  ECO:    [1.20, 0.85, 0.45, 0.30],
-  NORMAL: STOCK_A_CRUISE_MAX_V,
-  SPORT:  [1.75, 1.30, 0.90, 0.65],
-}
-RISE_RATE = {ECO: 0.02, NORMAL: STOCK_RISE_RATE, SPORT: 0.06}
-
-# Early soft braking: predicted brake need (m/s^2) -> early decel target (m/s^2).
-SMOOTH_DECEL_BP = [0.0, 0.4, 0.8, 1.2, 1.6, 2.0, 2.4]
-SMOOTH_DECEL_V = {
-  ECO:    [0.00, -0.08, -0.20, -0.38, -0.60, -0.82, -1.05],
-  NORMAL: [0.00, -0.13, -0.30, -0.55, -0.84, -1.12, -1.40],
-  SPORT:  [0.00, -0.17, -0.40, -0.72, -1.05, -1.35, -1.65],
-}
-BRAKE_DEEPENING_JERK = {ECO: 0.5, NORMAL: 0.8, SPORT: 1.0}
-BRAKE_RELEASE_JERK = 2.0
-ACCEL_RISE_JERK = {ECO: 0.7, NORMAL: 1.2, SPORT: 1.6}
-
-SMOOTH_DECEL_LOOKAHEAD_T = 3.0
-MIN_SMOOTH_BRAKE_NEED = 0.3
-HARD_BRAKE_TARGET_ACCEL = -2.0
-HARD_BRAKE_NEED = 2.6

sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py

@@ -1,158 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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.
-"""
-
-from types import SimpleNamespace
-
-import numpy as np
-import pytest
-
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \
-  ECO, NORMAL, SPORT, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, RISE_RATE, \
-  STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, HARD_BRAKE_TARGET_ACCEL, AccelerationPersonality
-
-T_IDXS = [0.0, 0.2, 0.4, 0.6, 0.8, 1.0, 1.25, 1.5, 1.75, 2.0, 2.5, 3.0, 4.0]
-_EPS = 1e-6
-
-
-class FakeParams:
-  def __init__(self, store=None):
-    self.store = dict(store or {})
-
-  def get_bool(self, key):
-    return bool(self.store.get(key, False))
-
-  def get(self, key, return_default=False):
-    return int(self.store.get(key, 1))
-
-  def put(self, key, val, block=False):
-    self.store[key] = val
-
-
-def make_sm(v_ego=20.0):
-  return {'carState': SimpleNamespace(vEgo=v_ego)}
-
-
-def make_controller(enabled=True, personality=NORMAL, crash_cnt=0):
-  store = {"AccelPersonalityEnabled": enabled, "AccelPersonality": int(personality)}
-  ctrl = AccelController(CP=SimpleNamespace(), mpc=SimpleNamespace(crash_cnt=crash_cnt), params=FakeParams(store))
-  ctrl.update(make_sm())
-  return ctrl
-
-
-def flat_traj(value):
-  return [float(value)] * len(T_IDXS)
-
-
-def test_enum_source_parity():
-  assert (ECO, NORMAL, SPORT) == (AccelerationPersonality.eco, AccelerationPersonality.normal, AccelerationPersonality.sport)
-  assert (PERSONALITY_MIN, PERSONALITY_MAX) == (0, 2)
-
-
-def test_disabled_forces_normal_and_stock_ceiling():
-  ctrl = make_controller(enabled=False, personality=SPORT)
-  assert ctrl.personality() == NORMAL
-  assert not ctrl.enabled()
-  for v in (0.0, 10.0, 25.0, 40.0):
-    assert ctrl.get_max_accel(v) == pytest.approx(np.interp(v, A_CRUISE_MAX_BP, STOCK_A_CRUISE_MAX_V))
-  assert ctrl.get_rise_rate() == STOCK_RISE_RATE
-
-
-def test_disabled_passes_brake_through():
-  ctrl = make_controller(enabled=False)
-  for raw in (-1.5, -0.5, 0.0, 1.0):
-    out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
-    assert out == pytest.approx(raw, abs=_EPS)
-
-
-def test_normal_matches_stock():
-  ctrl = make_controller(personality=NORMAL)
-  for v in (0.0, 5.0, 10.0, 25.0, 40.0):
-    assert ctrl.get_max_accel(v) == pytest.approx(np.interp(v, A_CRUISE_MAX_BP, STOCK_A_CRUISE_MAX_V))
-  assert ctrl.get_rise_rate() == STOCK_RISE_RATE
-
-
-def test_ceiling_ordering_eco_lt_normal_lt_sport():
-  eco, normal, sport = (make_controller(personality=p) for p in (ECO, NORMAL, SPORT))
-  for v in (0.0, 10.0, 25.0, 40.0):
-    assert eco.get_max_accel(v) < normal.get_max_accel(v) < sport.get_max_accel(v)
-
-
-def test_rise_rate_ordering():
-  assert RISE_RATE[ECO] < RISE_RATE[NORMAL] < RISE_RATE[SPORT]
-
-
-def test_early_soft_braking_brakes_before_plan():
-  ctrl = make_controller(personality=NORMAL)
-  out = ctrl.smooth_target_accel(0.0, flat_traj(-1.0), T_IDXS, should_stop=False)
-  assert out < 0.0
-  assert ctrl.smooth_active()
-  assert ctrl.brake_need() == pytest.approx(1.0)
-
-
-@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT])
-def test_never_weaker_than_plan_sustained_closing(personality):
-  # never command less braking than the plan (route 000003da regression guard)
-  ctrl = make_controller(personality=personality)
-  for raw in [0.0, -0.2, -0.5, -0.9, -1.2, -1.5] + [-1.5] * 40:
-    out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
-    assert out <= raw + _EPS
-
-
-@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT])
-def test_never_weaker_random_walk(personality):
-  rng = np.random.default_rng(0)
-  ctrl = make_controller(personality=personality)
-  for _ in range(500):
-    raw = float(rng.uniform(-1.9, 1.5))
-    traj = flat_traj(raw - float(rng.uniform(0.0, 0.6)))
-    out = ctrl.smooth_target_accel(raw, traj, T_IDXS, should_stop=False)
-    if raw < 0.0:
-      assert out <= raw + _EPS
-
-
-def test_hard_brake_bypass():
-  ctrl = make_controller(personality=ECO)
-  raw = HARD_BRAKE_TARGET_ACCEL - 0.5
-  out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
-  assert out == pytest.approx(raw, abs=_EPS)
-  assert ctrl.bypassed()
-
-
-def test_should_stop_bypass():
-  ctrl = make_controller(personality=ECO)
-  out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=True)
-  assert out == pytest.approx(-1.0, abs=_EPS)
-  assert ctrl.bypassed()
-
-
-def test_fcw_crash_cnt_bypass():
-  ctrl = make_controller(personality=ECO, crash_cnt=3)
-  out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False)
-  assert out == pytest.approx(-1.0, abs=_EPS)
-  assert ctrl.bypassed()
-
-
-def test_e2e_brake_passthrough():
-  ctrl = make_controller(personality=ECO)
-  out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False, stock_brake=True)
-  assert out == pytest.approx(-1.0, abs=_EPS)
-  assert not ctrl.smooth_active()
-
-
-def test_out_of_range_personality_clamps():
-  ctrl = AccelController(CP=SimpleNamespace(), mpc=SimpleNamespace(crash_cnt=0),
-                         params=FakeParams({"AccelPersonalityEnabled": True, "AccelPersonality": 99}))
-  ctrl.update(make_sm())
-  assert ctrl.personality() == PERSONALITY_MAX
-
-
-def test_reset_passes_through():
-  ctrl = make_controller(personality=ECO)
-  out = ctrl.smooth_target_accel(0.0, flat_traj(-1.0), T_IDXS, should_stop=False, reset=True)
-  assert out == pytest.approx(0.0, abs=_EPS)
-  assert not ctrl.bypassed()

sunnypilot/selfdrive/controls/lib/dec/constants.py

@@ -1,46 +1,17 @@
-from openpilot.common.realtime import DT_MDL
-
-
 class WMACConstants:
-  TRAJECTORY_SIZE = 33
-  PARAM_READ_FRAMES = max(1, int(round(1.0 / DT_MDL)))
+  # Lead detection parameters
+  LEAD_WINDOW_SIZE = 6  # Stable detection window
+  LEAD_PROB = 0.45  # Balanced threshold for lead detection
 
-  EMERGENCY_HOLD_FRAMES = max(1, int(round(0.75 / DT_MDL)))
-  MIN_MODE_DURATION = {'acc': max(1, int(round(0.6 / DT_MDL))), 'blended': max(1, int(round(0.5 / DT_MDL)))}
-  ENTER_BLENDED_FRAMES = max(1, int(round(0.4 / DT_MDL)))
-  EXIT_BLENDED_FRAMES = max(1, int(round(0.35 / DT_MDL)))
-  STANDSTILL_FRAMES = max(1, int(round(0.2 / DT_MDL)))
+  # Slow down detection parameters
+  SLOW_DOWN_WINDOW_SIZE = 5  # Responsive but stable
+  SLOW_DOWN_PROB = 0.3  # Balanced threshold for slow down scenarios
 
-  LEAD_PROB = 0.45
-  LEAD_EXIT_PROB = 0.25
-  LEAD_RISE_RATE = 1.0
-  LEAD_FALL_RATE = 0.35
-  RADAR_LEAD_ACC_PROB = 0.5
-  RADAR_LEAD_ACC_EXIT_PROB = 0.4
-  RADAR_LEAD_ACC_RISE_RATE = 1.0
-  RADAR_LEAD_ACC_FALL_RATE = 0.25
-  RADAR_LEAD_ACC_MAX_DREL = 80.0
-  RADAR_LEAD_ACC_MAX_TTC = 6.0
-  RADAR_LEAD_ACC_MIN_CLOSING_SPEED = -0.5
-
-  SLOW_DOWN_PROB = 0.5
-  SLOW_DOWN_EXIT_PROB = 0.4
-  SLOW_DOWN_RISE_RATE = 0.65
-  SLOW_DOWN_FALL_RATE = 0.15
+  # Optimized slow down distance curve - smooth and progressive
   SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
   SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.]
-  URGENT_SLOW_DOWN_PROB = 0.85
 
-  MODEL_DECEL_START = -0.5
-  MODEL_DECEL_RANGE = 2.0
-  ENDPOINT_URGENCY_GAIN = 1.3
-  CRITICAL_ENDPOINT_FACTOR = 0.3
-  CRITICAL_URGENCY_GAIN = 1.5
-  SPEED_URGENCY_MIN = 25.0
-  SPEED_URGENCY_RANGE = 80.0
-
-  SLOWNESS_PROB = 0.55
-  SLOWNESS_EXIT_PROB = 0.45
-  SLOWNESS_RISE_RATE = 0.35
-  SLOWNESS_FALL_RATE = 0.5
-  SLOWNESS_CRUISE_OFFSET = 1.025
+  # Slowness detection parameters
+  SLOWNESS_WINDOW_SIZE = 10  # Stable slowness detection
+  SLOWNESS_PROB = 0.55  # Clear threshold for slowness
+  SLOWNESS_CRUISE_OFFSET = 1.025  # Conservative cruise speed offset

sunnypilot/selfdrive/controls/lib/dec/dec.py

@@ -6,116 +6,129 @@ See the LICENSE.md file in the root directory for more details.
 """
 # Version = 2025-6-30
 
+from cereal import messaging
+from opendbc.car import structs
+from numpy import interp
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
 from typing import Literal
 
-from cereal import messaging
-from numpy import interp
-from opendbc.car import structs
-from openpilot.common.params import Params
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
+# d-e2e, from modeldata.h
+TRAJECTORY_SIZE = 33
+SET_MODE_TIMEOUT = 15
 
+# Define the valid mode types
 ModeType = Literal['acc', 'blended']
 
 
-def clip01(value: float) -> float:
-  return max(0.0, min(1.0, float(value)))
+class SmoothKalmanFilter:
+  """Enhanced Kalman filter with smoothing for stable decision making."""
 
+  def __init__(self, initial_value=0, measurement_noise=0.1, process_noise=0.01,
+               alpha=1.0, smoothing_factor=0.85):
+    self.x = initial_value
+    self.P = 1.0
+    self.R = measurement_noise
+    self.Q = process_noise
+    self.alpha = alpha
+    self.smoothing_factor = smoothing_factor
+    self.initialized = False
+    self.history = []
+    self.max_history = 10
+    self.confidence = 0.0
 
-class SmoothedSignal:
-  def __init__(self, rise_rate: float, fall_rate: float, initial_value: float = 0.0):
-    self.rise_rate = clip01(rise_rate)
-    self.fall_rate = clip01(fall_rate)
-    self.value = clip01(initial_value)
+  def add_data(self, measurement):
+    if len(self.history) >= self.max_history:
+      self.history.pop(0)
+    self.history.append(measurement)
 
-  def update(self, measurement: float) -> float:
-    measurement = clip01(measurement)
-    rate = self.rise_rate if measurement > self.value else self.fall_rate
-    self.value += (measurement - self.value) * rate
-    return self.value
+    if not self.initialized:
+      self.x = measurement
+      self.initialized = True
+      self.confidence = 0.1
+      return
 
-  def reset(self, value: float = 0.0) -> None:
-    self.value = clip01(value)
+    self.P = self.alpha * self.P + self.Q
 
+    K = self.P / (self.P + self.R)
+    effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1
 
-class HysteresisSignal:
-  def __init__(self, enter_threshold: float, exit_threshold: float, rise_rate: float, fall_rate: float):
-    self.enter_threshold = clip01(enter_threshold)
-    self.exit_threshold = clip01(exit_threshold)
-    self.filter = SmoothedSignal(rise_rate, fall_rate)
-    self.active = False
+    innovation = measurement - self.x
+    self.x = self.x + effective_K * innovation
+    self.P = (1 - effective_K) * self.P
 
-  def update(self, measurement: float) -> bool:
-    value = self.filter.update(measurement)
-    threshold = self.exit_threshold if self.active else self.enter_threshold
-    self.active = value > threshold
-    return self.active
+    if abs(innovation) < 0.1:
+      self.confidence = min(1.0, self.confidence + 0.05)
+    else:
+      self.confidence = max(0.1, self.confidence - 0.02)
 
-  def reset(self) -> None:
-    self.filter.reset()
-    self.active = False
+  def get_value(self):
+    return self.x if self.initialized else None
 
-  @property
-  def value(self) -> float:
-    return self.filter.value
+  def get_confidence(self):
+    return self.confidence
+
+  def reset_data(self):
+    self.initialized = False
+    self.history = []
+    self.confidence = 0.0
 
 
 class ModeTransitionManager:
+  """Manages smooth transitions between driving modes with hysteresis."""
+
   def __init__(self):
     self.current_mode: ModeType = 'acc'
+    self.mode_confidence = {'acc': 1.0, 'blended': 0.0}
+    self.transition_timeout = 0
+    self.min_mode_duration = 10
     self.mode_duration = 0
-    self._pending_mode: ModeType = 'acc'
-    self._pending_count = 0
-    self._blended_hold_frames = 0
+    self.emergency_override = False
 
-  def request_mode(self, mode: ModeType, immediate: bool = False, hold_frames: int = 0, cancel_hold: bool = False) -> None:
-    if immediate:
-      self._blended_hold_frames = max(self._blended_hold_frames, hold_frames)
-      self._pending_mode = mode
-      self._pending_count = 0
-      self._switch_mode(mode)
+  def request_mode(self, mode: ModeType, confidence: float = 1.0, emergency: bool = False):
+    # Emergency override for critical situations (stops, collisions)
+    if emergency:
+      self.emergency_override = True
+      self.current_mode = mode
+      self.transition_timeout = SET_MODE_TIMEOUT
+      self.mode_duration = 0
       return
 
-    if cancel_hold and mode == 'acc':
-      self._blended_hold_frames = 0
+    self.mode_confidence[mode] = min(1.0, self.mode_confidence[mode] + 0.1 * confidence)
+    for m in self.mode_confidence:
+      if m != mode:
+        self.mode_confidence[m] = max(0.0, self.mode_confidence[m] - 0.05)
 
-    if self._blended_hold_frames > 0:
-      mode = 'blended'
-
-    if mode == self.current_mode:
-      self._pending_mode = mode
-      self._pending_count = 0
+    # Require minimum duration in current mode (unless emergency)
+    if self.mode_duration < self.min_mode_duration and not self.emergency_override:
       return
 
-    if mode != self._pending_mode:
-      self._pending_mode = mode
-      self._pending_count = 1
-    else:
-      self._pending_count += 1
+    # Hysteresis: higher threshold for mode changes
+    confidence_threshold = 0.6 if mode != self.current_mode else 0.3  # Lower threshold for faster response
 
-    if self.mode_duration < WMACConstants.MIN_MODE_DURATION[self.current_mode]:
-      return
+    if self.mode_confidence[mode] > confidence_threshold:
+      if mode != self.current_mode and self.transition_timeout == 0:
+        self.transition_timeout = SET_MODE_TIMEOUT
+        self.current_mode = mode
+        self.mode_duration = 0
 
-    required_count = WMACConstants.ENTER_BLENDED_FRAMES if mode == 'blended' else WMACConstants.EXIT_BLENDED_FRAMES
-    if self._pending_count >= required_count:
-      self._switch_mode(mode)
-
-  def update(self) -> None:
-    if self._blended_hold_frames > 0:
-      self._blended_hold_frames -= 1
+  def update(self):
+    if self.transition_timeout > 0:
+      self.transition_timeout -= 1
     self.mode_duration += 1
 
+    # Reset emergency override after some time
+    if self.emergency_override and self.mode_duration > 20:
+      self.emergency_override = False
+
+    # Gradual confidence decay
+    for mode in self.mode_confidence:
+      self.mode_confidence[mode] *= 0.98
+
   def get_mode(self) -> ModeType:
     return self.current_mode
 
-  def _switch_mode(self, mode: ModeType) -> None:
-    if mode == self.current_mode:
-      return
-
-    self.current_mode = mode
-    self.mode_duration = 0
-    self._pending_mode = mode
-    self._pending_count = 0
-
 
 class DynamicExperimentalController:
   def __init__(self, CP: structs.CarParams, mpc, params=None):
@@ -129,33 +142,35 @@ class DynamicExperimentalController:
 
     self._mode_manager = ModeTransitionManager()
 
-    self._lead_tracker = HysteresisSignal(
-      enter_threshold=WMACConstants.LEAD_PROB,
-      exit_threshold=WMACConstants.LEAD_EXIT_PROB,
-      rise_rate=WMACConstants.LEAD_RISE_RATE,
-      fall_rate=WMACConstants.LEAD_FALL_RATE,
-    )
-    self._radar_acc_lead_tracker = HysteresisSignal(
-      enter_threshold=WMACConstants.RADAR_LEAD_ACC_PROB,
-      exit_threshold=WMACConstants.RADAR_LEAD_ACC_EXIT_PROB,
-      rise_rate=WMACConstants.RADAR_LEAD_ACC_RISE_RATE,
-      fall_rate=WMACConstants.RADAR_LEAD_ACC_FALL_RATE,
-    )
-    self._slow_down_tracker = HysteresisSignal(
-      enter_threshold=WMACConstants.SLOW_DOWN_PROB,
-      exit_threshold=WMACConstants.SLOW_DOWN_EXIT_PROB,
-      rise_rate=WMACConstants.SLOW_DOWN_RISE_RATE,
-      fall_rate=WMACConstants.SLOW_DOWN_FALL_RATE,
-    )
-    self._slowness_tracker = HysteresisSignal(
-      enter_threshold=WMACConstants.SLOWNESS_PROB,
-      exit_threshold=WMACConstants.SLOWNESS_EXIT_PROB,
-      rise_rate=WMACConstants.SLOWNESS_RISE_RATE,
-      fall_rate=WMACConstants.SLOWNESS_FALL_RATE,
+    # Smooth filters for stable decision making with faster response for critical scenarios
+    self._lead_filter = SmoothKalmanFilter(
+      measurement_noise=0.15,
+      process_noise=0.05,
+      alpha=1.02,
+      smoothing_factor=0.8
     )
 
+    self._slow_down_filter = SmoothKalmanFilter(
+      measurement_noise=0.1,
+      process_noise=0.1,
+      alpha=1.05,
+      smoothing_factor=0.7
+    )
+
+    self._slowness_filter = SmoothKalmanFilter(
+      measurement_noise=0.1,
+      process_noise=0.06,
+      alpha=1.015,
+      smoothing_factor=0.92
+    )
+
+    self._mpc_fcw_filter = SmoothKalmanFilter(
+      measurement_noise=0.2,
+      process_noise=0.1,
+      alpha=1.1,
+      smoothing_factor=0.5
+    )
     self._has_lead_filtered = False
-    self._has_radar_acc_lead = False
     self._has_slow_down = False
     self._has_slowness = False
     self._has_mpc_fcw = False
@@ -164,14 +179,13 @@ class DynamicExperimentalController:
     self._has_standstill = False
     self._mpc_fcw_crash_cnt = 0
     self._standstill_count = 0
-
+    # debug
     self._endpoint_x = float('inf')
     self._expected_distance = 0.0
     self._trajectory_valid = False
-    self._raw_urgency = 0.0
 
   def _read_params(self) -> None:
-    if self._frame % WMACConstants.PARAM_READ_FRAMES == 0:
+    if self._frame % int(1. / DT_MDL) == 0:
       self._enabled = self._params.get_bool("DynamicExperimentalControl")
 
   def mode(self) -> str:
@@ -184,6 +198,7 @@ class DynamicExperimentalController:
     return self._active
 
   def set_mpc_fcw_crash_cnt(self) -> None:
+    """Set MPC FCW crash count"""
     self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
 
   def _update_calculations(self, sm: messaging.SubMaster) -> None:
@@ -195,109 +210,179 @@ class DynamicExperimentalController:
     self._v_cruise_kph = car_state.vCruise
     self._has_standstill = car_state.standstill
 
+    # standstill detection
     if self._has_standstill:
-      self._standstill_count = min(WMACConstants.STANDSTILL_FRAMES * 3, self._standstill_count + 1)
+      self._standstill_count = min(20, self._standstill_count + 1)
     else:
       self._standstill_count = max(0, self._standstill_count - 1)
 
-    self._has_lead_filtered = self._lead_tracker.update(float(lead_one.status))
-    self._has_radar_acc_lead = self._radar_acc_lead_tracker.update(self._radar_acc_lead_score(lead_one))
-    self._has_mpc_fcw = self._mpc_fcw_crash_cnt > 0
+    # Lead detection
+    self._lead_filter.add_data(float(lead_one.status))
+    lead_value = self._lead_filter.get_value() or 0.0
+    self._has_lead_filtered = lead_value > WMACConstants.LEAD_PROB
+
+    # MPC FCW detection
+    fcw_filtered_value = self._mpc_fcw_filter.get_value() or 0.0
+    self._mpc_fcw_filter.add_data(float(self._mpc_fcw_crash_cnt > 0))
+    self._has_mpc_fcw = fcw_filtered_value > 0.5
+
+    # Slow down detection
     self._calculate_slow_down(md)
 
-    if self._standstill_count > WMACConstants.STANDSTILL_FRAMES or self._has_slow_down:
-      self._slowness_tracker.reset()
-      self._has_slowness = False
-    else:
+    # Slowness detection
+    if not (self._standstill_count > 5) and not self._has_slow_down:
       current_slowness = float(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
-      self._has_slowness = self._slowness_tracker.update(current_slowness)
+      self._slowness_filter.add_data(current_slowness)
+      slowness_value = self._slowness_filter.get_value() or 0.0
 
-  def _calculate_slow_down(self, md) -> None:
+      # Hysteresis for slowness
+      threshold = WMACConstants.SLOWNESS_PROB * (0.8 if self._has_slowness else 1.1)
+      self._has_slowness = slowness_value > threshold
+
+  def _calculate_slow_down(self, md):
+    """Calculate urgency based on trajectory endpoint vs expected distance."""
+
+    # Reset to safe defaults
+    urgency = 0.0
     self._endpoint_x = float('inf')
-    self._expected_distance = 0.0
     self._trajectory_valid = False
 
-    urgency = self._model_action_urgency(md)
-    position_valid = len(md.position.x) == WMACConstants.TRAJECTORY_SIZE
+    #Require exact trajectory size
+    position_valid = len(md.position.x) == TRAJECTORY_SIZE
+    orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE
 
-    if position_valid:
-      self._trajectory_valid = True
-      self._endpoint_x = md.position.x[WMACConstants.TRAJECTORY_SIZE - 1]
-      self._expected_distance = interp(self._v_ego_kph, WMACConstants.SLOW_DOWN_BP, WMACConstants.SLOW_DOWN_DIST)
-      urgency = max(urgency, self._endpoint_urgency(self._endpoint_x, self._expected_distance))
+    if not (position_valid and orientation_valid):
+      # Invalid trajectory - this itself might indicate a stop scenario
+      # Apply moderate urgency for incomplete trajectories at speed
+      if self._v_ego_kph > 20.0:
+        urgency = 0.3
 
-    self._raw_urgency = clip01(urgency)
-    self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency)
-    self._urgency = self._slow_down_tracker.value
+      self._slow_down_filter.add_data(urgency)
+      urgency_filtered = self._slow_down_filter.get_value() or 0.0
+      self._has_slow_down = urgency_filtered > WMACConstants.SLOW_DOWN_PROB
+      self._urgency = urgency_filtered
+      return
 
-  def _radar_acc_lead_score(self, lead_one) -> float:
-    if not lead_one.status:
-      return 0.0
+    # We have a valid full trajectory
+    self._trajectory_valid = True
 
-    d_rel = float(getattr(lead_one, 'dRel', float('inf')))
-    v_rel = float(getattr(lead_one, 'vRel', 0.0))
-    if d_rel <= WMACConstants.RADAR_LEAD_ACC_MAX_DREL:
-      return 1.0
-    if v_rel <= WMACConstants.RADAR_LEAD_ACC_MIN_CLOSING_SPEED and d_rel / max(-v_rel, 0.1) <= WMACConstants.RADAR_LEAD_ACC_MAX_TTC:
-      return 1.0
-    return 0.0
+    # Use the exact endpoint (33rd point, index 32)
+    endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
+    self._endpoint_x = endpoint_x
 
-  def _model_action_urgency(self, md) -> float:
-    action = getattr(md, 'action', None)
-    if action is None:
-      return 0.0
+    # Get expected distance based on current speed using tuned constants
+    expected_distance = interp(self._v_ego_kph,
+                               WMACConstants.SLOW_DOWN_BP,
+                               WMACConstants.SLOW_DOWN_DIST)
+    self._expected_distance = expected_distance
 
-    urgency = 1.0 if getattr(action, 'shouldStop', False) else 0.0
-    desired_accel = getattr(action, 'desiredAcceleration', 0.0)
-    if desired_accel < WMACConstants.MODEL_DECEL_START:
-      urgency = max(urgency, min(1.0, (WMACConstants.MODEL_DECEL_START - desired_accel) / WMACConstants.MODEL_DECEL_RANGE))
-    return urgency
+    # Calculate urgency based on trajectory shortage
+    if endpoint_x < expected_distance:
+      shortage = expected_distance - endpoint_x
+      shortage_ratio = shortage / expected_distance
 
-  def _endpoint_urgency(self, endpoint_x: float, expected_distance: float) -> float:
-    if endpoint_x >= expected_distance:
-      return 0.0
+      # Base urgency on shortage ratio
+      urgency = min(1.0, shortage_ratio * 2.0)
 
-    shortage_ratio = (expected_distance - endpoint_x) / expected_distance
-    urgency = min(1.0, shortage_ratio * WMACConstants.ENDPOINT_URGENCY_GAIN)
+      # Increase urgency for very short trajectories (imminent stops)
+      critical_distance = expected_distance * 0.3
+      if endpoint_x < critical_distance:
+        urgency = min(1.0, urgency * 2.0)
 
-    if endpoint_x < expected_distance * WMACConstants.CRITICAL_ENDPOINT_FACTOR:
-      urgency = min(1.0, urgency * WMACConstants.CRITICAL_URGENCY_GAIN)
+      # Speed-based urgency adjustment
+      if self._v_ego_kph > 25.0:
+        speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0
+        urgency = min(1.0, urgency * speed_factor)
 
-    if self._v_ego_kph > WMACConstants.SPEED_URGENCY_MIN:
-      speed_factor = 1.0 + (self._v_ego_kph - WMACConstants.SPEED_URGENCY_MIN) / WMACConstants.SPEED_URGENCY_RANGE
-      urgency = min(1.0, urgency * speed_factor)
+    # Apply filtering but with less smoothing for stops
+    self._slow_down_filter.add_data(urgency)
+    urgency_filtered = self._slow_down_filter.get_value() or 0.0
 
-    return urgency
+    # Update state with lower threshold for better stop detection
+    self._has_slow_down = urgency_filtered > (WMACConstants.SLOW_DOWN_PROB * 0.8)
+    self._urgency = urgency_filtered
 
-  def _desired_mode(self) -> tuple[ModeType, bool]:
-    if not self._CP.radarUnavailable and self._has_radar_acc_lead:
-      return 'acc', False
+  def _radarless_mode(self) -> None:
+    """Radarless mode decision logic with emergency handling."""
 
+    # EMERGENCY: MPC FCW - immediate blended mode
     if self._has_mpc_fcw:
-      return 'blended', True
+      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      return
 
-    standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES
-    urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB
+    # Standstill: use blended
+    if self._standstill_count > 3:
+      self._mode_manager.request_mode('blended', confidence=0.9)
+      return
 
-    if self._CP.radarUnavailable:
-      if standstill or self._has_slow_down:
-        return 'blended', urgent_slow_down
-      return 'acc', False
+    # Slow down scenarios: emergency for high urgency, normal for lower urgency
+    if self._has_slow_down:
+      if self._urgency > 0.7:
+        # Emergency: immediate blended mode for high urgency stops
+        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      else:
+        # Normal: blended with urgency-based confidence
+        confidence = min(1.0, self._urgency * 1.5)
+        self._mode_manager.request_mode('blended', confidence=confidence)
+      return
 
-    if standstill or self._has_slow_down:
-      return 'blended', urgent_slow_down
+    # Driving slow: use ACC (but not if actively slowing down)
+    if self._has_slowness and not self._has_slow_down:
+      self._mode_manager.request_mode('acc', confidence=0.8)
+      return
 
-    return 'acc', False
+    # Default: ACC
+    self._mode_manager.request_mode('acc', confidence=0.7)
+
+  def _radar_mode(self) -> None:
+    """Radar mode with emergency handling."""
+
+    # EMERGENCY: MPC FCW - immediate blended mode
+    if self._has_mpc_fcw:
+      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      return
+
+    # If lead detected and not in standstill: always use ACC
+    if self._has_lead_filtered and not (self._standstill_count > 3):
+      self._mode_manager.request_mode('acc', confidence=1.0)
+      return
+
+    # Slow down scenarios: emergency for high urgency, normal for lower urgency
+    if self._has_slow_down:
+      if self._urgency > 0.7:
+        # Emergency: immediate blended mode for high urgency stops
+        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      else:
+        # Normal: blended with urgency-based confidence
+        confidence = min(1.0, self._urgency * 1.3)
+        self._mode_manager.request_mode('blended', confidence=confidence)
+      return
+
+    # Standstill: use blended
+    if self._standstill_count > 3:
+      self._mode_manager.request_mode('blended', confidence=0.9)
+      return
+
+    # Driving slow: use ACC (but not if actively slowing down)
+    if self._has_slowness and not self._has_slow_down:
+      self._mode_manager.request_mode('acc', confidence=0.8)
+      return
+
+    # Default: ACC
+    self._mode_manager.request_mode('acc', confidence=0.7)
 
   def update(self, sm: messaging.SubMaster) -> None:
     self._read_params()
+
     self.set_mpc_fcw_crash_cnt()
+
     self._update_calculations(sm)
 
-    mode, immediate = self._desired_mode()
-    self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES,
-                                    cancel_hold=self._has_radar_acc_lead)
-    self._mode_manager.update()
+    if self._CP.radarUnavailable:
+      self._radarless_mode()
+    else:
+      self._radar_mode()
 
+    self._mode_manager.update()
     self._active = sm['selfdriveState'].experimentalMode and self._enabled
     self._frame += 1

sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py

@@ -0,0 +1,94 @@
+import pytest
+
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+
+class MockLeadOne:
+  def __init__(self, status=0.0):
+    self.status = status
+
+class MockRadarState:
+  def __init__(self, status=0.0):
+    self.leadOne = MockLeadOne(status=status)
+
+class MockCarState:
+  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
+    self.vEgo = vEgo
+    self.vCruise = vCruise
+    self.standstill = standstill
+
+class MockModelData:
+  def __init__(self, valid=True):
+    size = 33 if valid else 10  # incomplete if invalid
+    self.position = type("Pos", (), {"x": [0.0] * size})()
+    self.orientation = type("Ori", (), {"x": [0.0] * size})()
+
+class MockSelfDriveState:
+  def __init__(self, experimentalMode=False):
+    self.experimentalMode = experimentalMode
+
+class MockParams:
+  def get_bool(self, name):
+    return True
+
+@pytest.fixture
+def default_sm():
+  sm = {
+    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
+    'radarState': MockRadarState(status=1.0),
+    'modelV2': MockModelData(valid=True),
+    'selfdriveState': MockSelfDriveState(experimentalMode=True),
+  }
+  return sm
+
+@pytest.fixture
+def mock_cp():
+  class CP:
+    radarUnavailable = False
+  return CP()
+
+@pytest.fixture
+def mock_mpc():
+  class MPC:
+    crash_cnt = 0
+  return MPC()
+
+# Fake Kalman Filter that always returns a given value
+class FakeKalman:
+  def __init__(self, value=1.0):
+    self.value = value
+  def add_data(self, v): pass
+  def get_value(self): return self.value
+  def get_confidence(self): return 1.0
+  def reset_data(self): pass
+
+def test_initial_mode_is_acc(mock_cp, mock_mpc):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  assert controller.mode() == "acc"
+
+def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['carState'].standstill = True
+  for _ in range(10):
+    controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  mock_mpc.crash_cnt = 1  # simulate FCW
+  for _ in range(2):
+    controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+
+  # Force conditions to simulate slowdown
+  controller._slow_down_filter = FakeKalman(value=1.0)  # Ensure urgency triggers slowdown
+  controller._v_ego_kph = 35.0
+  default_sm['modelV2'] = MockModelData(valid=False)  # Incomplete trajectory
+
+  for _ in range(3):
+    controller.update(default_sm)
+
+  assert controller.mode() == "blended"

sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py

@@ -1,235 +0,0 @@
-import pytest
-
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, HysteresisSignal
-
-
-class MockLeadOne:
-  def __init__(self, status=0.0, dRel=30.0, vRel=0.0):
-    self.status = status
-    self.dRel = dRel
-    self.vRel = vRel
-
-
-class MockRadarState:
-  def __init__(self, status=0.0, dRel=30.0, vRel=0.0):
-    self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel)
-
-
-class MockCarState:
-  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
-    self.vEgo = vEgo
-    self.vCruise = vCruise
-    self.standstill = standstill
-
-
-class MockAction:
-  def __init__(self, desiredAcceleration=0.0, shouldStop=False):
-    self.desiredAcceleration = desiredAcceleration
-    self.shouldStop = shouldStop
-
-
-class MockModelData:
-  def __init__(self, valid=True, endpoint_x=200.0, orientation_valid=None, desired_acceleration=0.0, should_stop=False):
-    position_size = 33 if valid else 10
-    orientation_size = position_size if orientation_valid is None else (33 if orientation_valid else 10)
-    position_x = [0.0] * position_size
-    if position_x:
-      position_x[-1] = endpoint_x
-    self.position = type("Pos", (), {"x": position_x})()
-    self.orientation = type("Ori", (), {"x": [0.0] * orientation_size})()
-    self.acceleration = type("Accel", (), {"x": [0.0] * position_size})()
-    self.action = MockAction(desired_acceleration, should_stop)
-
-
-class MockSelfDriveState:
-  def __init__(self, experimentalMode=False):
-    self.experimentalMode = experimentalMode
-
-
-class MockParams:
-  def get_bool(self, name):
-    return True
-
-
-@pytest.fixture
-def default_sm():
-  sm = {
-    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
-    'radarState': MockRadarState(status=1.0),
-    'modelV2': MockModelData(valid=True),
-    'selfdriveState': MockSelfDriveState(experimentalMode=True),
-  }
-  return sm
-
-
-@pytest.fixture
-def mock_cp():
-  class CP:
-    radarUnavailable = False
-  return CP()
-
-
-@pytest.fixture
-def mock_mpc():
-  class MPC:
-    crash_cnt = 0
-  return MPC()
-
-
-def test_initial_mode_is_acc(mock_cp, mock_mpc):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  assert controller.mode() == "acc"
-
-
-def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['carState'].standstill = True
-  for _ in range(20):
-    controller.update(default_sm)
-  assert controller.mode() == "blended"
-
-
-def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  mock_mpc.crash_cnt = 1
-  controller.update(default_sm)
-  assert controller.mode() == "blended"
-
-
-def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm):
-  mock_cp.radarUnavailable = True
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-
-  controller.update(default_sm)
-
-  assert controller.mode() == "blended"
-
-
-def test_valid_position_with_missing_orientation_can_trigger_slowdown(mock_cp, mock_mpc, default_sm):
-  mock_cp.radarUnavailable = True
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, orientation_valid=False)
-
-  controller.update(default_sm)
-
-  assert controller._trajectory_valid
-  assert controller.mode() == "blended"
-
-
-def test_incomplete_position_does_not_trigger_slowdown(mock_cp, mock_mpc, default_sm):
-  mock_cp.radarUnavailable = True
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=False, endpoint_x=0.0)
-
-  for _ in range(3):
-    controller.update(default_sm)
-
-  assert not controller._trajectory_valid
-  assert not controller._has_slow_down
-  assert controller.mode() == "acc"
-
-
-def test_slowdown_hysteresis_prevents_threshold_chatter():
-  signal = HysteresisSignal(enter_threshold=0.5, exit_threshold=0.4, rise_rate=1.0, fall_rate=1.0)
-
-  assert signal.update(0.55)
-  assert signal.update(0.45)
-  assert not signal.update(0.35)
-
-
-def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mock_mpc, default_sm):
-  mock_cp.radarUnavailable = True
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
-
-  controller.update(default_sm)
-
-  assert not controller._trajectory_valid
-  assert controller.mode() == "blended"
-
-
-def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-
-  for _ in range(3):
-    controller.update(default_sm)
-
-  assert controller._has_slow_down
-  assert controller._has_radar_acc_lead
-  assert controller.mode() == "acc"
-
-
-def test_far_radar_lead_allows_blended_until_acc_relevant(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-
-  controller.update(default_sm)
-
-  assert controller._has_lead_filtered
-  assert not controller._has_radar_acc_lead
-  assert controller.mode() == "blended"
-
-
-def test_relevant_radar_lead_smoothly_returns_to_acc(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-  controller.update(default_sm)
-  assert controller.mode() == "blended"
-
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=45.0, vRel=0.0)
-  for _ in range(20):
-    controller.update(default_sm)
-
-  assert controller._has_radar_acc_lead
-  assert controller.mode() == "acc"
-
-
-def test_closing_far_radar_lead_returns_to_acc(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=-25.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-
-  for _ in range(20):
-    controller.update(default_sm)
-
-  assert controller._has_radar_acc_lead
-  assert controller.mode() == "acc"
-
-
-def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0)
-  default_sm['carState'].standstill = True
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, should_stop=True)
-  mock_mpc.crash_cnt = 1
-
-  for _ in range(10):
-    controller.update(default_sm)
-
-  assert controller._has_lead_filtered
-  assert controller._has_mpc_fcw
-  assert controller.mode() == "acc"
-
-
-def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0)
-  controller.update(default_sm)
-
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-  controller.update(default_sm)
-
-  assert controller._has_lead_filtered
-  assert controller.mode() == "acc"

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -9,8 +9,6 @@ from cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.common.constants import CV
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController
-from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import RadarDistanceController
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
 from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
@@ -28,8 +26,6 @@ class LongitudinalPlannerSP:
     self.events_sp = EventsSP()
     self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
-    self.accel = AccelController(CP, mpc)
-    self.radar_distance = RadarDistanceController(CP)
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
     self.sla = SpeedLimitAssist(CP, CP_SP)
@@ -80,13 +76,8 @@ class LongitudinalPlannerSP:
   def update(self, sm: messaging.SubMaster) -> None:
     self.events_sp.clear()
     self.dec.update(sm)
-    self.accel.update(sm)
-    self.radar_distance.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
 
-  def smooth_radarstate(self, radarstate):
-    return self.radar_distance.smooth_radarstate(radarstate)
-
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
     plan_sp_send = messaging.new_message('longitudinalPlanSP')
 
@@ -147,15 +138,4 @@ class LongitudinalPlannerSP:
     e2eAlerts.greenLightAlert = self.e2e_alerts_helper.green_light_alert
     e2eAlerts.leadDepartAlert = self.e2e_alerts_helper.lead_depart_alert
 
-    # Acceleration Personality
-    acceleration = longitudinalPlanSP.acceleration
-    acceleration.personality = self.accel.personality()
-    acceleration.enabled = self.accel.enabled()
-    acceleration.maxAccel = float(self.accel.max_accel())
-    acceleration.brakeNeed = float(self.accel.brake_need())
-    acceleration.decelTarget = float(self.accel.decel_target())
-    acceleration.smoothActive = self.accel.smooth_active()
-    acceleration.bypassed = bool(self.accel.bypassed())
-
-
     pm.send('longitudinalPlanSP', plan_sp_send)

sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py

@@ -1,107 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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.
-
-Radar Distance: hold a just-dropped, recently-sustained lead alive through radar flicker so the MPC does
-not lose+regain it. Obstacle-monotone (held obstacle <= last real <= stock) -> braking is always >= stock,
-never less. Wall-clock bounded, flicker-proof. Default off => stock passthrough.
-"""
-
-from opendbc.car import structs
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-
-HOLD_MAX_FRAMES = 10     # ~0.5s cap, measured since the last SUSTAINED lead (not reset by 1-frame flicker)
-SUSTAIN_FRAMES = 2       # consecutive valid frames to (re)arm and reset the wall-clock
-DROPOUT_DREL = 1.0
-MIN_PROB = 0.2
-FCW_PROB_CAP = 0.9       # held lead can't reach the FCW gate (>0.9) -> no false FCW
-MIN_HELD_DREL = 0.5
-
-
-class _HeldLead:
-  __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb')
-
-  def __init__(self, dRel, vRel, vLead, aLeadK, aLeadTau, modelProb):
-    self.status = True
-    self.dRel = dRel
-    self.vRel = vRel
-    self.vLead = vLead
-    self.vLeadK = vLead
-    self.aLeadK = aLeadK
-    self.aLeadTau = aLeadTau
-    self.modelProb = modelProb
-    self.yRel = 0.0
-
-
-class _RadarStateProxy:
-  __slots__ = ('leadOne', 'leadTwo')
-
-  def __init__(self, lead_one, lead_two):
-    self.leadOne = lead_one
-    self.leadTwo = lead_two
-
-
-class _LeadHold:
-  def __init__(self):
-    self._last = None
-    self._sustained = 0
-    self._since_real = 0
-    self._armed = False
-    self._held_dRel = 0.0
-
-  def reset(self):
-    self.__init__()
-
-  def step(self, raw):
-    if raw.status and raw.dRel > DROPOUT_DREL and raw.modelProb > MIN_PROB:
-      self._last = (raw.dRel, raw.vRel, raw.vLead, raw.aLeadK, raw.aLeadTau, raw.modelProb)
-      self._sustained += 1
-      if self._sustained >= SUSTAIN_FRAMES:
-        self._since_real = 0
-        self._armed = True
-      return raw
-
-    self._sustained = 0
-    self._since_real += 1
-    if self._armed and self._last is not None and self._since_real <= HOLD_MAX_FRAMES:
-      dRel0, vRel0, vLead0, aLeadK0, aLeadTau0, prob0 = self._last
-      if self._since_real == 1:
-        self._held_dRel = dRel0
-      self._held_dRel = max(MIN_HELD_DREL, self._held_dRel - max(-vRel0, 0.0) * DT_MDL)
-      return _HeldLead(self._held_dRel, vRel0, vLead0, min(aLeadK0, 0.0), aLeadTau0, min(prob0, FCW_PROB_CAP))
-
-    self._armed = False
-    return raw
-
-
-class RadarDistanceController:
-  def __init__(self, CP: structs.CarParams, params=None):
-    self._CP = CP
-    self._params = params or Params()
-    self._frame = 0
-    self._enabled = self._params.get_bool("RadarDistance")
-    self._one = _LeadHold()
-    self._two = _LeadHold()
-
-  def _read_params(self) -> None:
-    enabled = self._params.get_bool("RadarDistance")
-    if enabled and not self._enabled:
-      self._one.reset()
-      self._two.reset()
-    self._enabled = enabled
-
-  def update(self, sm) -> None:
-    if self._frame % int(1. / DT_MDL) == 0:
-      self._read_params()
-    self._frame += 1
-
-  def enabled(self) -> bool:
-    return self._enabled
-
-  def smooth_radarstate(self, radarstate):
-    if not self._enabled:
-      return radarstate
-    return _RadarStateProxy(self._one.step(radarstate.leadOne), self._two.step(radarstate.leadTwo))

sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py

@@ -1,117 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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.
-"""
-
-from types import SimpleNamespace
-
-import pytest
-
-from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import \
-  RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP
-
-COMFORT_BRAKE = 2.5
-
-
-class FakeParams:
-  def __init__(self, store=None):
-    self.store = dict(store or {})
-
-  def get_bool(self, key):
-    return bool(self.store.get(key, False))
-
-
-def lead(status=True, dRel=40.0, vRel=-2.0, vLead=18.0, aLeadK=0.0, aLeadTau=1.5, modelProb=0.95):
-  return SimpleNamespace(status=status, dRel=dRel, yRel=0.0, vRel=vRel, vLead=vLead, vLeadK=vLead,
-                         aLeadK=aLeadK, aLeadTau=aLeadTau, modelProb=modelProb)
-
-
-def rs(one, two=None):
-  return SimpleNamespace(leadOne=one, leadTwo=two or lead(status=False, dRel=0.0, modelProb=0.0))
-
-
-def obstacle(ld):
-  return ld.dRel + ld.vLead ** 2 / (2 * COMFORT_BRAKE)
-
-
-def ctrl(enabled=True):
-  return RadarDistanceController(CP=SimpleNamespace(), params=FakeParams({'RadarDistance': enabled}))
-
-
-def test_disabled_is_identity():
-  c = ctrl(enabled=False)
-  r = rs(lead())
-  assert c.smooth_radarstate(r) is r  # byte-stock passthrough
-
-
-def test_valid_lead_passthrough():
-  c = ctrl()
-  one = lead(dRel=40.0)
-  out = c.smooth_radarstate(rs(one))
-  assert out.leadOne is one
-
-
-def test_holds_after_sustained_dropout():
-  c = ctrl()
-  for _ in range(3):  # sustain (>= SUSTAIN_FRAMES)
-    c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-4.0, vLead=16.0)))
-  out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
-  held = out.leadOne
-  assert held.status is True
-  assert held.dRel < 30.0          # dead-reckoned closer
-  assert held.dRel == pytest.approx(30.0 - 4.0 * 0.05, abs=1e-6)
-
-
-def test_obstacle_monotone_during_hold():
-  c = ctrl()
-  for _ in range(3):
-    c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-4.0, vLead=16.0)))
-  last_obs = obstacle(lead(dRel=30.0, vLead=16.0))
-  prev = last_obs
-  for _ in range(HOLD_MAX_FRAMES):
-    held = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne
-    if not held.status:
-      break
-    o = obstacle(held)
-    assert o <= last_obs + 1e-6     # never farther than the last real obstacle (brakes >= last real)
-    assert o <= prev + 1e-6         # monotonically non-increasing -> brakes more over the hold
-    prev = o
-
-
-def test_releases_after_hold_cap():
-  c = ctrl()
-  for _ in range(3):
-    c.smooth_radarstate(rs(lead(dRel=30.0)))
-  statuses = [c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne.status
-              for _ in range(HOLD_MAX_FRAMES + 3)]
-  assert all(statuses[:HOLD_MAX_FRAMES])        # held through the cap
-  assert statuses[HOLD_MAX_FRAMES] is False     # released after
-
-
-def test_no_hold_without_sustained_lead():
-  c = ctrl()
-  c.smooth_radarstate(rs(lead(dRel=30.0)))       # single valid frame (< SUSTAIN_FRAMES)
-  out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
-  assert out.leadOne.status is False             # not armed -> no hold
-
-
-def test_flicker_does_not_reset_wall_clock():
-  c = ctrl()
-  for _ in range(3):
-    c.smooth_radarstate(rs(lead(dRel=30.0)))
-  # 1/0/1/0 flicker: lone valid frames must NOT reset the wall-clock (sustained < SUSTAIN_FRAMES)
-  for _ in range(4):
-    c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))  # dropout
-    c.smooth_radarstate(rs(lead(dRel=31.0)))                              # lone valid
-  assert c._one._since_real > 0                  # wall-clock kept climbing through the flicker
-
-
-def test_fcw_prob_capped_and_aleadk_not_positive():
-  c = ctrl()
-  for _ in range(3):
-    c.smooth_radarstate(rs(lead(dRel=30.0, aLeadK=1.0, modelProb=0.99)))
-  held = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))).leadOne
-  assert held.modelProb <= FCW_PROB_CAP          # no false FCW from a held phantom
-  assert held.aLeadK <= 0.0                       # never project the held lead as accelerating

sunnypilot/selfdrive/controls/lib/relc.py

@@ -1,84 +0,0 @@
-"""
-Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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
-
-from openpilot.common.constants import CV
-from openpilot.common.realtime import DT_MDL
-from openpilot.common.params import Params
-
-NEARSIDE_PROB = 0.2
-EDGE_PROB = 0.35
-EDGE_REACTION_TIME = 1.0
-EDGE_CLEAR_TIME = 0.3
-MIN_SPEED = 20 * CV.MPH_TO_MS
-
-
-class RoadEdgeLaneChangeController:
-  def __init__(self, desire_helper):
-    self.DH = desire_helper
-    self.params = Params()
-    self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
-    self.param_read_counter = 0
-    self.left_edge_detected = False
-    self.right_edge_detected = False
-    self.left_edge_timer = 0.0
-    self.right_edge_timer = 0.0
-    self.left_clear_timer = 0.0
-    self.right_clear_timer = 0.0
-
-  def read_params(self) -> None:
-    self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
-
-  def update_params(self) -> None:
-    if self.param_read_counter % 50 == 0:
-      self.read_params()
-    self.param_read_counter += 1
-
-  def reset(self) -> None:
-    self.left_edge_detected = False
-    self.right_edge_detected = False
-    self.left_edge_timer = 0.0
-    self.right_edge_timer = 0.0
-    self.left_clear_timer = 0.0
-    self.right_clear_timer = 0.0
-
-  def update(self, road_edge_stds, lane_line_probs, v_ego: float) -> None:
-    self.update_params()
-
-    if not self.enabled or v_ego < MIN_SPEED:
-      self.reset()
-      return
-
-    left_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0)
-    right_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0)
-    left_lane_prob = lane_line_probs[0]
-    right_lane_prob = lane_line_probs[3]
-
-    left_cond = left_edge_prob > EDGE_PROB and left_lane_prob < NEARSIDE_PROB and right_lane_prob >= left_lane_prob
-    right_cond = right_edge_prob > EDGE_PROB and right_lane_prob < NEARSIDE_PROB and left_lane_prob >= right_lane_prob
-
-    if left_cond:
-      self.left_edge_timer = min(self.left_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
-      self.left_clear_timer = 0.0
-      if self.left_edge_timer > EDGE_REACTION_TIME:
-        self.left_edge_detected = True
-    else:
-      self.left_clear_timer += DT_MDL
-      if self.left_clear_timer > EDGE_CLEAR_TIME:
-        self.left_edge_timer = 0.0
-        self.left_edge_detected = False
-
-    if right_cond:
-      self.right_edge_timer = min(self.right_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
-      self.right_clear_timer = 0.0
-      if self.right_edge_timer > EDGE_REACTION_TIME:
-        self.right_edge_detected = True
-    else:
-      self.right_clear_timer += DT_MDL
-      if self.right_clear_timer > EDGE_CLEAR_TIME:
-        self.right_edge_timer = 0.0
-        self.right_edge_detected = False

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

@@ -5,8 +5,6 @@ from openpilot.common.params import Params
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
-from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
-
 
 TurnDirection = custom.ModelDataV2SP.TurnDirection
 
@@ -109,11 +107,7 @@ def set_lane_turn_params():
 ])
 def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
   dh = DesireHelper()
-  relc = RoadEdgeLaneChangeController(dh)
-  relc.enabled = True
   dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
   for _ in range(10):
-    dh.update(carstate, lateral_active, lane_change_prob,
-              left_edge_detected=relc.left_edge_detected, right_edge_detected=relc.right_edge_detected)
+    dh.update(carstate, lateral_active, lane_change_prob)
   assert dh.desire == expected_desire  # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
-

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

@@ -1,99 +0,0 @@
-"""
-Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
-
-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 pytest
-
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.sunnypilot.selfdrive.controls.lib.relc import (
-  RoadEdgeLaneChangeController, EDGE_REACTION_TIME, EDGE_CLEAR_TIME, MIN_SPEED,
-)
-
-V_HIGH = MIN_SPEED + 2.0
-V_LOW = MIN_SPEED - 1.0
-
-
-@pytest.fixture
-def relc(mocker):
-  mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params")
-  mock_params.return_value.get_bool.return_value = True
-  controller = RoadEdgeLaneChangeController(DesireHelper())
-  controller.enabled = True
-  return controller
-
-
-def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH):
-  for _ in range(int(seconds / DT_MDL) + 1):
-    controller.update(road_edge_stds, lane_line_probs, v_ego)
-
-
-@pytest.mark.parametrize("road_edge_stds,lane_line_probs,attr", [
-  ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], "left_edge_detected"),
-  ([0.9, 0.0], [0.8, 0.8, 0.8, 0.0], "right_edge_detected"),
-])
-def test_edge_detection(relc, road_edge_stds, lane_line_probs, attr):
-  drive(relc, road_edge_stds, lane_line_probs, EDGE_REACTION_TIME + 0.1)
-  assert getattr(relc, attr)
-
-
-def test_edge_detection_requires_time(relc):
-  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME - 0.05)
-  assert not relc.left_edge_detected
-
-
-def test_both_edges_detected(relc):
-  drive(relc, [0.0, 0.0], [0.0, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1)
-  assert relc.left_edge_detected
-  assert relc.right_edge_detected
-
-
-def test_noise_doesnt_clear(relc):
-  edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
-  clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
-
-  drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
-  assert relc.left_edge_detected
-
-  relc.update(*clear, V_HIGH)
-  relc.update(*edge, V_HIGH)
-  assert relc.left_edge_detected
-
-
-def test_clears_after_window(relc):
-  edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
-  clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
-
-  drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
-  assert relc.left_edge_detected
-
-  drive(relc, *clear, EDGE_CLEAR_TIME + 0.05)
-  assert not relc.left_edge_detected
-  assert relc.left_edge_timer == 0.0
-
-
-def test_low_speed_skips(relc):
-  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, v_ego=V_LOW)
-  assert not relc.left_edge_detected
-  assert relc.left_edge_timer == 0.0
-
-
-def test_speed_drop_resets(relc):
-  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
-  assert relc.left_edge_detected
-
-  relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_LOW)
-  assert not relc.left_edge_detected
-
-
-def test_param_off_resets(relc):
-  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
-  assert relc.left_edge_detected
-
-  relc.params.get_bool.return_value = False
-  relc.read_params()
-  relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_HIGH)
-  assert not relc.left_edge_detected
-  assert not relc.right_edge_detected

sunnypilot/selfdrive/selfdrived/events.py

@@ -243,12 +243,4 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       AlertStatus.normal, AlertSize.none,
       Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
   },
-
-  EventNameSP.laneChangeRoadEdge: {
-    ET.WARNING: Alert(
-      "Lane Change Unavailable: Road Edge",
-      "",
-      AlertStatus.userPrompt, AlertSize.small,
-      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.1),
-  },
 }

sunnypilot/sunnylink/params_metadata.json

@@ -1,12 +1,4 @@
 {
-  "AccelPersonality": {
-    "title": "Acceleration Profile",
-    "description": "Eco accelerates gently and brakes early and soft; Sport accelerates briskly. Hard-braking authority is always preserved."
-  },
-  "AccelPersonalityEnabled": {
-    "title": "Enable Acceleration Profiles",
-    "description": "Enable Eco/Normal/Sport acceleration profiles, including early soft braking."
-  },
   "AccessToken": {
     "title": "AccessTokenIsNice",
     "description": ""
@@ -1106,10 +1098,6 @@
     "title": "Quiet Mode",
     "description": ""
   },
-  "RadarDistance": {
-    "title": "Radar Distance",
-    "description": "Holds a lead through brief radar flicker/dropout so sunnypilot does not lose and re-grab it, smoothing the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock."
-  },
   "RainbowMode": {
     "title": "Rainbow Mode",
     "description": ""
@@ -1130,10 +1118,6 @@
     "title": "Record Front Lock",
     "description": ""
   },
-  "RoadEdgeLaneChangeEnabled": {
-    "title": "Block Lane Change: Road Edge Detection",
-    "description": ""
-  },
   "RoadName": {
     "title": "Road Name",
     "description": ""
@@ -1340,30 +1324,14 @@
     "step": 0.1,
     "unit": "m/s\u00b2"
   },
-  "ToyotaAutoHold": {
-    "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
-    "description": ""
-  },
-  "ToyotaDriveMode": {
-    "title": "Enable drive mode btn link",
-    "description": ""
-  },
   "ToyotaEnforceStockLongitudinal": {
     "title": "Toyota: Enforce Factory Longitudinal Control",
     "description": "When enabled, sunnypilot will not take over control of gas and brakes. Factory Toyota longitudinal control will be used."
   },
-  "ToyotaEnhancedBsm": {
-    "title": "Toyota: Prius TSS2 BSM and some tssp",
-    "description": ""
-  },
   "ToyotaStopAndGoHack": {
     "title": "Toyota: Stop and Go Hack (Alpha)",
     "description": "sunnypilot will allow some Toyota/Lexus cars to auto resume during stop and go traffic. This feature is only applicable to certain models that are able to use longitudinal control. This is an alpha feature. Use at your own risk."
   },
-  "ToyotaTSS2Long": {
-    "title": "Toyota: custom longitudinal for TSS2",
-    "description": ""
-  },
   "TrainingVersion": {
     "title": "Training Version",
     "description": ""

sunnypilot/sunnylink/settings_ui.json

@@ -587,26 +587,6 @@
                 }
               ]
             },
-            {
-              "key": "RadarDistance",
-              "widget": "toggle",
-              "title": "Radar Distance",
-              "description": "Holds a lead through brief radar flicker/dropout so sunnypilot does not lose and re-grab it, smoothing the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock.",
-              "visibility": [
-                {
-                  "type": "capability",
-                  "field": "has_longitudinal_control",
-                  "equals": true
-                }
-              ],
-              "enablement": [
-                {
-                  "type": "capability",
-                  "field": "has_longitudinal_control",
-                  "equals": true
-                }
-              ]
-            },
             {
               "key": "DisengageOnAccelerator",
               "widget": "toggle",
@@ -640,58 +620,6 @@
                 }
               ]
             },
-            {
-              "key": "AccelPersonalityEnabled",
-              "widget": "toggle",
-              "title": "Enable Acceleration Profiles",
-              "description": "Enables Eco/Normal/Sport acceleration profiles for longitudinal control, including early soft braking.",
-              "visibility": [
-                {
-                  "type": "capability",
-                  "field": "has_longitudinal_control",
-                  "equals": true
-                }
-              ],
-              "enablement": [
-                {
-                  "type": "capability",
-                  "field": "has_longitudinal_control",
-                  "equals": true
-                }
-              ]
-            },
-            {
-              "key": "AccelPersonality",
-              "widget": "multiple_button",
-              "title": "Acceleration Profile",
-              "description": "Eco accelerates gently and brakes early and soft; Sport accelerates briskly. Hard-braking authority is always preserved.",
-              "options": [
-                {
-                  "value": 0,
-                  "label": "Eco"
-                },
-                {
-                  "value": 1,
-                  "label": "Normal"
-                },
-                {
-                  "value": 2,
-                  "label": "Sport"
-                }
-              ],
-              "enablement": [
-                {
-                  "type": "capability",
-                  "field": "has_longitudinal_control",
-                  "equals": true
-                },
-                {
-                  "type": "param",
-                  "key": "AccelPersonalityEnabled",
-                  "equals": true
-                }
-              ]
-            },
             {
               "key": "IntelligentCruiseButtonManagement",
               "widget": "toggle",
@@ -2073,22 +2001,6 @@
                   "equals": true
                 }
               ]
-            },
-            {
-              "key": "PlanplusControl",
-              "widget": "option",
-              "title": "Plan Plus Controls",
-              "description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lane center; too high and it will ping-pong.",
-              "min": 0.0,
-              "max": 2.0,
-              "step": 0.1,
-              "enablement": [
-                {
-                  "type": "param",
-                  "key": "ShowAdvancedControls",
-                  "equals": true
-                }
-              ]
             }
           ]
         },
@@ -2256,50 +2168,6 @@
       "title": "Toyota / Lexus Settings",
       "description": "",
       "items": [
-        {
-          "key": "ToyotaAutoHold",
-          "widget": "toggle",
-          "needs_onroad_cycle": true,
-          "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
-          "enablement": [
-            {
-              "type": "not_engaged"
-            }
-          ]
-        },
-        {
-          "key": "ToyotaEnhancedBsm",
-          "widget": "toggle",
-          "needs_onroad_cycle": true,
-          "title": "Toyota: Prius TSS2 BSM and some tssp",
-          "enablement": [
-            {
-              "type": "not_engaged"
-            }
-          ]
-        },
-        {
-          "key": "ToyotaTSS2Long",
-          "widget": "toggle",
-          "needs_onroad_cycle": true,
-          "title": "Toyota: custom longitudinal for TSS2",
-          "enablement": [
-            {
-              "type": "not_engaged"
-            }
-          ]
-        },
-        {
-          "key": "ToyotaDriveMode",
-          "widget": "toggle",
-          "needs_onroad_cycle": true,
-          "title": "Enable drive mode btn link",
-          "enablement": [
-            {
-              "type": "not_engaged"
-            }
-          ]
-        },
         {
           "key": "ToyotaEnforceStockLongitudinal",
           "widget": "toggle",

sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -24,15 +24,6 @@ sections:
     - $ref: '#/macros/longitudinal'
     enablement:
     - $ref: '#/macros/longitudinal'
-  - key: RadarDistance
-    widget: toggle
-    title: Radar Distance
-    description: Holds a lead through brief radar flicker/dropout so sunnypilot does not lose and re-grab
-      it, smoothing the hard/late brakes that radar drop-outs cause. Braking is never reduced below stock.
-    visibility:
-    - $ref: '#/macros/longitudinal'
-    enablement:
-    - $ref: '#/macros/longitudinal'
   - key: DisengageOnAccelerator
     widget: toggle
     title: Disengage Cruise on Accelerator Pedal
@@ -52,31 +43,6 @@ sections:
       label: Relaxed
     enablement:
     - $ref: '#/macros/longitudinal'
-  - key: AccelPersonalityEnabled
-    widget: toggle
-    title: Enable Acceleration Profiles
-    description: Enables Eco/Normal/Sport acceleration profiles for longitudinal control, including early soft braking.
-    visibility:
-    - $ref: '#/macros/longitudinal'
-    enablement:
-    - $ref: '#/macros/longitudinal'
-  - key: AccelPersonality
-    widget: multiple_button
-    title: Acceleration Profile
-    description: Eco accelerates gently and brakes early and soft; Sport accelerates briskly. Hard-braking
-      authority is always preserved.
-    options:
-    - value: 0
-      label: Eco
-    - value: 1
-      label: Normal
-    - value: 2
-      label: Sport
-    enablement:
-    - $ref: '#/macros/longitudinal'
-    - type: param
-      key: AccelPersonalityEnabled
-      equals: true
   - key: IntelligentCruiseButtonManagement
     widget: toggle
     title: Intelligent Cruise Button Management (ICBM) (Alpha)

sunnypilot/sunnylink/settings_ui_src/pages/models.yaml

@@ -51,16 +51,6 @@ sections:
         key: LagdToggle
         equals: true
     - $ref: '#/macros/advanced_only'
-  - key: PlanplusControl
-    widget: option
-    title: Plan Plus Controls
-    description: Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover
-      to lane center; too high and it will ping-pong.
-    min: 0.0
-    max: 2.0
-    step: 0.1
-    enablement:
-    - $ref: '#/macros/advanced_only'
 - id: lateral_control
   title: Lateral Control
   description: Neural network lateral control for supported models

sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml

@@ -60,30 +60,6 @@ sections:
   title: Toyota / Lexus Settings
   description: ''
   items:
-  - key: ToyotaAutoHold
-    widget: toggle
-    needs_onroad_cycle: true
-    title: 'Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS'
-    enablement:
-    - $ref: '#/macros/not_engaged'
-  - key: ToyotaEnhancedBsm
-    widget: toggle
-    needs_onroad_cycle: true
-    title: 'Toyota: Prius TSS2 BSM and some tssp'
-    enablement:
-    - $ref: '#/macros/not_engaged'
-  - key: ToyotaTSS2Long
-    widget: toggle
-    needs_onroad_cycle: true
-    title: 'Toyota: custom longitudinal for TSS2'
-    enablement:
-    - $ref: '#/macros/not_engaged'
-  - key: ToyotaDriveMode
-    widget: toggle
-    needs_onroad_cycle: true
-    title: Enable drive mode btn link
-    enablement:
-    - $ref: '#/macros/not_engaged'
   - key: ToyotaEnforceStockLongitudinal
     widget: toggle
     needs_onroad_cycle: true

system/ui/lib/scroll_panel2.py

@@ -45,9 +45,8 @@ class ScrollState(Enum):
 
 
 class GuiScrollPanel2:
-  def __init__(self, horizontal: bool = True, handle_out_of_bounds: bool = True) -> None:
+  def __init__(self, horizontal: bool = True) -> None:
     self._horizontal = horizontal
-    self._handle_out_of_bounds = handle_out_of_bounds
     self._state = ScrollState.STEADY
     self._offset: rl.Vector2 = rl.Vector2(0, 0)
     self._initial_click_event: MouseEvent | None = None
@@ -99,7 +98,7 @@ class GuiScrollPanel2:
       # simple exponential return if out of bounds
       # out of bounds is handled by snapping, so skip if set
       out_of_bounds = self.get_offset() > max_offset or self.get_offset() < min_offset
-      if out_of_bounds and snap_target is None and self._handle_out_of_bounds:
+      if out_of_bounds and snap_target is None:
         target = max_offset if self.get_offset() > max_offset else min_offset
 
         dt = rl.get_frame_time() or 1e-6

system/ui/widgets/scroller.py

@@ -75,6 +75,7 @@ class _Scroller(Widget):
     self._items: list[Widget] = []
     self._horizontal = horizontal
     self._snap_items = snap_items
+    assert not self._snap_items or self._horizontal, "Snapping is only supported for horizontal scrolling"
     self._spacing = spacing
     self._pad = pad
 
@@ -190,12 +191,8 @@ class _Scroller(Widget):
     snap_target: float | None = None
     if self._snap_items and visible_items and self._scrolling_to[0] is None:
       # TODO: this doesn't handle two small buttons at the edges well
-      if self._horizontal:
-        center_pos = self._rect.x + self._rect.width / 2
-        closest_delta_pos = min((((item.rect.x + item.rect.width / 2) - center_pos) for item in visible_items), key=abs)
-      else:
-        center_pos = self._rect.y + self._rect.height / 2
-        closest_delta_pos = min((((item.rect.y + item.rect.height / 2) - center_pos) for item in visible_items), key=abs)
+      center_pos = self._rect.x + self._rect.width / 2
+      closest_delta_pos = min((((item.rect.x + item.rect.width / 2) - center_pos) for item in visible_items), key=abs)
       snap_target = self.scroll_panel.get_offset() - closest_delta_pos
 
     return self.scroll_panel.update(self._rect, content_size, snap_target=snap_target)