minimize standstill duration display for mici

README.md

@@ -1,7 +1,7 @@
 # StarPilot
 
 [![Ask DeepWiki](https://deepwiki.com/badge.svg)](https://deepwiki.com/firestar5683/StarPilot)
-[![Discord](https://img.shields.io/discord/1137853399715549214?label=Discord)](https://firestar.link/discord)
+[![Discord](https://img.shields.io/discord/1387432184121393333?label=Discord)](https://firestar.link/discord)
 [![Last Updated](https://img.shields.io/github/last-commit/firestar5683/StarPilot/StarPilot)](https://github.com/firestar5683/StarPilot)
 [![Wiki](https://img.shields.io/badge/Wiki-StarPilot-blue?logo=wiki)](https://wiki.firestar.link)
 
@@ -15,11 +15,11 @@ Openpilot provides
 * Lane Change Assist
 * Driver Monitoring *without wheel nags*
 
-StarPilot adds support for many GM vehicles along with improved tuning,
+StarPilot was formerly a GM targeted fork,
-especially for radar-less (camera only) vehicles.
+but [has expanded to offer Quality-Of-Life improvements for all](#features)!
 
-StarPilot is built off of [StarPilot](https://github.com/FrogAi/StarPilot)
+StarPilot is built off of [FrogPilot](https://github.com/FrogAi/FrogPilot)
-and supports the major features StarPilot offers.
+and supports the major features FrogPilot offers.
 
 StarPilot has a vibrant, welcoming community [discord](https://firestar.link/discord).
 Stop by to chat or ask questions!
@@ -32,9 +32,9 @@ installation guides, and software configuration.
 ## Features
 
 * Full support for Comma C3, C3X, and C4
-  * C4 is currently in release testing. Join our fleet of C4 testers!
 * Model switcher with all of comma's tinygrad driving models
 * Special longitudinal planner tuning for VoACC (visual only, radar-less) vehicles
+* Custom-tuned torque controllers for an expanding list of cars.
 * Galaxy: StarPilot's portal to configure your comma device using your phone from anywhere.
 Download models, change settings, update software, visualize live model outputs for tuning.
 * Always On Lateral (full time steering assist)*
@@ -49,8 +49,9 @@ Download models, change settings, update software, visualize live model outputs
 * ZSS support*
 * High quality dashcam recordings*
 * Enhanced tuning for CEM (dynamic experimental mode switching)
+* And more!
 
-\* [Inherited from StarPilot](https://github.com/FrogAi/StarPilot#openpilot-vs-starpilot)
+\* [Inherited from FrogPilot](https://github.com/FrogAi/FrogPilot#openpilot-vs-frogpilot)
 
 ## GM-only Features
 

common/params_keys.h

@@ -447,6 +447,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"QOLLateral", {PERSISTENT, BOOL, "1", "0", 1}},
     {"QOLLongitudinal", {PERSISTENT, BOOL, "1", "0", 1}},
     {"QOLVisuals", {PERSISTENT, BOOL, "1", "0", 0}},
+    {"RadarTakeoffs", {PERSISTENT, BOOL, "1", "0", 2}},
     {"RadarTracksUI", {PERSISTENT, BOOL, "0", "0", 3}},
     {"RainbowPath", {PERSISTENT, BOOL, "0", "0", 1}},
     {"RandomEvents", {PERSISTENT, BOOL, "0", "0", 1}},

opendbc_repo/opendbc/car/gm/interface.py

@@ -609,6 +609,12 @@ class CarInterface(CarInterfaceBase):
       ret.startAccel = 1.15
       ret.vEgoStarting = max(ret.vEgoStarting, 0.35)
 
+    if ret.openpilotLongitudinalControl and candidate in (CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_SILVERADO_CC) and not ret.enableGasInterceptorDEPRECATED:
+      ret.longitudinalTuning.kpBP = [0.0, 5.0, 15.0, 35.0]
+      ret.longitudinalTuning.kpV = [0.02, 0.03, 0.028, 0.022]
+      ret.longitudinalTuning.kiBP = [0.0, 5.0, 15.0, 35.0]
+      ret.longitudinalTuning.kiV = [0.28, 0.26, 0.20, 0.16]
+
     elif candidate in CC_ONLY_CAR and not ret.enableGasInterceptorDEPRECATED:
       ret.flags |= GMFlags.CC_LONG.value
       ret.alphaLongitudinalAvailable = False
@@ -659,7 +665,6 @@ class CarInterface(CarInterfaceBase):
 
     volt_stock_auto_hold_safety = (
       gm_auto_hold and
-      not ret.openpilotLongitudinalControl and
       candidate in {
         CAR.CHEVROLET_VOLT,
         CAR.CHEVROLET_VOLT_2019,
@@ -668,8 +673,10 @@ class CarInterface(CarInterfaceBase):
       }
     )
     if volt_stock_auto_hold_safety:
-      # Reuse the paddle-scheduler safety bit as a stock-Volt auto-hold marker on
+      # Reuse the paddle-scheduler safety bit as a Volt auto-hold marker on
-      # non-pedal paths. The scheduler logic remains inactive without pedal-long.
+      # non-pedal paths. Hold can run while OP longitudinal is configured but
+      # not currently active, so the bit must be present regardless of the
+      # current long-control mode.
       ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value
 
     use_panda_3d1_sched = (

opendbc_repo/opendbc/car/gm/tests/test_gm.py

@@ -117,6 +117,21 @@ class TestGMInterface:
     assert car_params.flags & GMFlags.NO_CAMERA.value
     assert car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_NO_CAMERA.value
 
+  def test_silverado_alpha_long_uses_trimmed_longitudinal_tune(self):
+    CarInterface = interfaces[CAR.CHEVROLET_SILVERADO]
+    fingerprint = _empty_fingerprint()
+    fingerprint[0] = FINGERPRINTS[CAR.CHEVROLET_SILVERADO][0].copy()
+
+    car_params = CarInterface.get_params(CAR.CHEVROLET_SILVERADO, fingerprint, [], alpha_long=True, is_release=False,
+                                         docs=False, starpilot_toggles=_test_starpilot_toggles())
+
+    assert car_params.openpilotLongitudinalControl
+    assert not car_params.enableGasInterceptorDEPRECATED
+    assert list(car_params.longitudinalTuning.kpBP) == pytest.approx([0.0, 5.0, 15.0, 35.0])
+    assert list(car_params.longitudinalTuning.kpV) == pytest.approx([0.02, 0.03, 0.028, 0.022])
+    assert list(car_params.longitudinalTuning.kiBP) == pytest.approx([0.0, 5.0, 15.0, 35.0])
+    assert list(car_params.longitudinalTuning.kiV) == pytest.approx([0.28, 0.26, 0.20, 0.16])
+
   def test_volt_gateway_without_accel_pos_uses_brake_pedal_message(self):
     CarInterface = interfaces[CAR.CHEVROLET_VOLT]
     fingerprint = _empty_fingerprint()
@@ -132,6 +147,22 @@ class TestGMInterface:
     assert "ECMAcceleratorPos" not in pt_parser.vl
     assert "EBCMBrakePedalPosition" in pt_parser.vl
 
+  def test_volt_auto_hold_sets_stock_hold_safety_bit_with_op_long_enabled(self):
+    CarInterface = interfaces[CAR.CHEVROLET_VOLT_ASCM]
+    fingerprint = _empty_fingerprint()
+    fingerprint[0][0x2FF] = 8
+
+    params = Params()
+    try:
+      params.put_bool("GMAutoHold", True)
+      car_params = CarInterface.get_params(CAR.CHEVROLET_VOLT_ASCM, fingerprint, [], alpha_long=True, is_release=False,
+                                           docs=False, starpilot_toggles=_test_starpilot_toggles())
+    finally:
+      params.remove("GMAutoHold")
+
+    assert car_params.openpilotLongitudinalControl
+    assert car_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value
+
   @parameterized.expand(VOLT_CARS)
   def test_volt_bsm_is_enabled_without_fingerprint_match(self, car_model):
     CarInterface = interfaces[car_model]

opendbc_repo/opendbc/car/hyundai/carcontroller.py

@@ -2,7 +2,8 @@ from dataclasses import dataclass
 
 import numpy as np
 from opendbc.can import CANPacker
-from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, structs
+from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, rate_limit, structs
+from opendbc.car.common.filter_simple import FirstOrderFilter
 from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.hyundai import hyundaicanfd, hyundaican
@@ -61,6 +62,7 @@ REDNECK_BUTTON_COPIES = 2
 REDNECK_BUTTON_COPIES_TIME = 7
 REDNECK_BUTTON_COPIES_TIME_IMPERIAL = [REDNECK_BUTTON_COPIES_TIME + 3, 70]
 REDNECK_BUTTON_COPIES_TIME_METRIC = [REDNECK_BUTTON_COPIES_TIME, 40]
+ANGLE_SAFETY_BASELINE_MODEL = str(CAR.KIA_SPORTAGE_HEV_2026)
 
 
 @dataclass
@@ -195,6 +197,28 @@ def update_genesis_g90_longitudinal_tuning(state: GenesisG90LongitudinalTuningSt
   return state
 
 
+def get_baseline_safety_cp():
+  from opendbc.car.hyundai.interface import CarInterface
+  return CarInterface.get_non_essential_params(ANGLE_SAFETY_BASELINE_MODEL)
+
+
+def compute_torque_reduction_gain(steering_torque, v_ego, lat_active, last_gain):
+  if lat_active:
+    ceiling = np.interp(v_ego, [0.5, 1.5], [1.0, 0.85])
+    shelf = np.interp(v_ego, [2.0, 11.0], [0.45, 0.6])
+    floor = np.interp(v_ego, [2.0, 22.0], [0.1, 0.3])
+    bp1 = np.interp(v_ego, [2.0, 11.0], [75.0, 125.0])
+    bp2 = np.interp(v_ego, [2.0, 11.0], [125.0, 150.0])
+    bp3 = np.interp(v_ego, [2.0, 11.0], [175.0, 275.0])
+    bp4 = np.interp(v_ego, [2.0, 22.0], [400.0, 700.0])
+    target = np.interp(abs(steering_torque), [bp1, bp2, bp3, bp4], [ceiling, shelf, shelf, floor])
+  else:
+    target = 0.0
+
+  gain = rate_limit(target, last_gain, -0.014, 0.004)
+  return round(gain / 0.004) * 0.004
+
+
 def process_hud_alert(enabled, fingerprint, hud_control):
   sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
 
@@ -227,6 +251,8 @@ class CarController(CarControllerBase):
     self.packer = CANPacker(dbc_names[Bus.pt])
     self.angle_limit_counter = 0
     self.VM = VehicleModel(CP)
+    self.BASELINE_VM = VehicleModel(get_baseline_safety_cp()) if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING else self.VM
+    self.angle_filter = FirstOrderFilter(0.0, 0.2, DT_CTRL)
 
     self.accel_last = 0
     self.apply_torque_last = 0
@@ -328,32 +354,41 @@ class CarController(CarControllerBase):
     hud_control = CC.hudControl
     lka_icon, lfa_icon = self._update_dash_icon_state(CC)
 
-    self.params = CarControllerParams(self.CP, CS.out.vEgoRaw)
+    if not self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
+      self.params = CarControllerParams(self.CP, CS.out.vEgoRaw)
     apply_angle = CS.out.steeringAngleDeg
 
     if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
+      v_ego_raw = CS.out.vEgoRaw
       desired_angle = float(np.clip(actuators.steeringAngleDeg,
                                     -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
                                     self.params.ANGLE_LIMITS.STEER_ANGLE_MAX))
-      apply_angle = apply_steer_angle_limits_vm(desired_angle, self.apply_angle_last, CS.out.vEgoRaw,
+
+      self.angle_filter.update_alpha(float(np.interp(CS.out.vEgo, [5.0, 10.0, 20.0], [0.2, 0.1, 0.0])))
+      desired_angle = self.angle_filter.update(desired_angle)
+
+      apply_angle = apply_steer_angle_limits_vm(desired_angle, self.apply_angle_last, v_ego_raw,
                                                 CS.out.steeringAngleDeg, CC.latActive, self.params, self.VM)
 
-      if CS.out.steeringPressed and abs(CS.out.steeringTorque) > self.params.STEER_THRESHOLD:
+      if str(self.CP.carFingerprint) != ANGLE_SAFETY_BASELINE_MODEL:
-        apply_torque = self.params.ANGLE_MIN_TORQUE_REDUCTION_GAIN
+        apply_angle = apply_steer_angle_limits_vm(apply_angle or desired_angle, self.apply_angle_last, v_ego_raw,
-      elif CC.latActive and CS.out.vEgoRaw < 0.3:
+                                                  CS.out.steeringAngleDeg, CC.latActive, self.params, self.BASELINE_VM)
-        apply_torque = self.params.ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN
-      else:
-        apply_torque = self.params.ANGLE_MAX_TORQUE_REDUCTION_GAIN if CC.latActive else 0.0
 
-      apply_steer_req = CC.latActive and apply_torque > 0.0
+      apply_torque = compute_torque_reduction_gain(CS.out.steeringTorque, v_ego_raw, CC.latActive, self.apply_torque_last)
+      apply_steer_req = CC.latActive and apply_torque != 0.0
       torque_fault = False
 
       if apply_angle is None:
-        apply_torque = 0.0
+        apply_torque = 0
         apply_angle = CS.out.steeringAngleDeg
         apply_steer_req = False
 
       self.apply_angle_last = apply_angle
+      if not CC.latActive:
+        self.apply_angle_last = float(np.clip(CS.out.steeringAngleDeg,
+                                              -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
+                                              self.params.ANGLE_LIMITS.STEER_ANGLE_MAX))
+        self.angle_filter.x = self.apply_angle_last
     else:
       # steering torque
       new_torque = int(round(actuators.torque * self.params.STEER_MAX))

opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py

@@ -1167,11 +1167,11 @@ class TestHyundaiFingerprint:
     assert parser.vl["FCA12"]["FCA_DrvSetState"] == 2
     assert parser.vl["FCA12"]["FCA_USM"] == 2
 
-  def test_sportage_angle_steering_uses_lfa_and_adas_cmd_with_send_lfa(self):
+  def test_angle_steering_uses_lfa_and_adas_cmd_with_send_lfa(self):
     fingerprint = gen_empty_fingerprint()
     cam_can = CanBus(None, fingerprint).CAM
     fingerprint[cam_can][0xCB] = 24
-    CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, None)
+    CP = CarInterface.get_params(CAR.HYUNDAI_SANTA_FE_HEV_5TH_GEN, fingerprint, [], False, False, False, None)
 
     assert CP.flags & HyundaiFlags.SEND_LFA
     assert CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING
@@ -1474,34 +1474,6 @@ class TestHyundaiFingerprint:
     ]
     assert hyundaicanfd.create_ioniq_6_cluster_lane_change_messages(can_bus, 5, "none") == []
 
-  def test_sportage_angle_jerk_override_is_scoped(self):
-    sportage = CarParams.new_message()
-    sportage.carFingerprint = CAR.KIA_SPORTAGE_HEV_2026
-    sportage.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_ANGLE_STEERING)
-
-    comparison_angle = CarParams.new_message()
-    comparison_angle.carFingerprint = CAR.KIA_EV6
-    comparison_angle.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_ANGLE_STEERING)
-
-    ioniq6 = CarParams.new_message()
-    ioniq6.carFingerprint = CAR.HYUNDAI_IONIQ_6
-    ioniq6.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT)
-
-    sportage_params = CarControllerParams(sportage)
-    sportage_low_speed_params = CarControllerParams(sportage, vEgoRaw=5.0)
-    sportage_high_speed_params = CarControllerParams(sportage, vEgoRaw=20.0)
-    comparison_params = CarControllerParams(comparison_angle)
-    ioniq6_params = CarControllerParams(ioniq6)
-
-    assert sportage_params.ANGLE_LIMITS.MAX_LATERAL_JERK < comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK
-    assert sportage_high_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK == sportage_params.ANGLE_LIMITS.MAX_LATERAL_JERK
-    assert sportage_low_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK > sportage_high_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK
-    assert sportage_low_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK < comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK
-    assert sportage_params.ANGLE_LIMITS.STEER_ANGLE_MAX > comparison_params.ANGLE_LIMITS.STEER_ANGLE_MAX
-    assert sportage_params.ANGLE_LIMITS.MAX_LATERAL_ACCEL > comparison_params.ANGLE_LIMITS.MAX_LATERAL_ACCEL
-    assert sportage_params.ANGLE_LIMITS.MAX_ANGLE_RATE > comparison_params.ANGLE_LIMITS.MAX_ANGLE_RATE
-    assert comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK == ioniq6_params.ANGLE_LIMITS.MAX_LATERAL_JERK
-
   def test_ioniq_5_canfd_aux_messages_are_optional(self):
     toggles = get_test_toggles()
     fingerprint = gen_empty_fingerprint()

opendbc_repo/opendbc/car/hyundai/values.py

@@ -1,9 +1,9 @@
 import re
-from dataclasses import dataclass, field, replace
+from dataclasses import dataclass, field
 from enum import IntFlag
 
 from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
-from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL, ISO_LATERAL_JERK
+from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.structs import CarParams
 from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts, SupportType
@@ -11,13 +11,6 @@ from opendbc.car.fw_query_definitions import FwQueryConfig, Request, p16
 
 Ecu = CarParams.Ecu
 AVERAGE_ROAD_ROLL = 0.06  # conservative roll margin used by Hyundai CAN-FD angle steering safety
-SPORTAGE_HEV_2026_MAX_LATERAL_ACCEL = 3.6
-SPORTAGE_HEV_2026_BASE_LATERAL_JERK = 3.25
-SPORTAGE_HEV_2026_LOW_SPEED_JERK_BOOST = 0.55
-SPORTAGE_HEV_2026_LOW_SPEED_JERK_SPEED = 11.0
-SPORTAGE_HEV_2026_LOW_SPEED_JERK_WIDTH = 5.0
-SPORTAGE_HEV_2026_MAX_ANGLE_RATE = 6.5
-SPORTAGE_HEV_2026_STEER_ANGLE_MAX = 220.0
 HYUNDAI_MANDO_FRONT_RADAR_DBC = "hyundai_kia_mando_front_radar_generated"
 HYUNDAI_MRREVO14F_RADAR_DBC = "hyundai_mrrevo14f_radar_generated"
 HYUNDAI_MRR30_RADAR_DBC = "hyundai_mrr30_radar_generated"
@@ -28,16 +21,13 @@ class CarControllerParams:
   ACCEL_MIN = -3.5 # m/s
   ACCEL_MAX = 3.5 # m/s
   ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
-    180,
+    360,
     ([], []),
     ([], []),
     MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
-    MAX_LATERAL_JERK=ISO_LATERAL_JERK + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
+    MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
     MAX_ANGLE_RATE=5,
   )
-  ANGLE_MAX_TORQUE_REDUCTION_GAIN = 1.0
-  ANGLE_MIN_TORQUE_REDUCTION_GAIN = 0.6
-  ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN = 0.6
 
   def __init__(self, CP, vEgoRaw=100.):
     self.ANGLE_LIMITS = self.ANGLE_LIMITS
@@ -64,21 +54,6 @@ class CarControllerParams:
     if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
       self.STEER_THRESHOLD = 175
 
-      # The Sportage angle port still needs more authority in real turns than the
-      # fully calmed branch-wide ceiling allows, but the old low-speed jerk boost
-      # made the 3-20 degree band angry and ping-pongy as the car slowed down.
-      # Split the difference:
-      # - keep a calmer low-speed boost that fades out earlier
-      # - give the car a little more true turn headroom through accel/rate limits
-      if CP.carFingerprint == CAR.KIA_SPORTAGE_HEV_2026:
-        sportage_low_speed_weight = min(max((SPORTAGE_HEV_2026_LOW_SPEED_JERK_SPEED - vEgoRaw) / SPORTAGE_HEV_2026_LOW_SPEED_JERK_WIDTH, 0.0), 1.0)
-        sportage_lateral_jerk = SPORTAGE_HEV_2026_BASE_LATERAL_JERK + (SPORTAGE_HEV_2026_LOW_SPEED_JERK_BOOST * sportage_low_speed_weight)
-        self.ANGLE_LIMITS = replace(self.ANGLE_LIMITS,
-                                    STEER_ANGLE_MAX=SPORTAGE_HEV_2026_STEER_ANGLE_MAX,
-                                    MAX_LATERAL_ACCEL=SPORTAGE_HEV_2026_MAX_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
-                                    MAX_LATERAL_JERK=sportage_lateral_jerk + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
-                                    MAX_ANGLE_RATE=SPORTAGE_HEV_2026_MAX_ANGLE_RATE)
-
     # To determine the limit for your car, find the maximum value that the stock LKAS will request.
     # If the max stock LKAS request is <384, add your car to this list.
     elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.HYUNDAI_ELANTRA, CAR.HYUNDAI_ELANTRA_GT_I30, CAR.HYUNDAI_IONIQ,

opendbc_repo/opendbc/car/toyota/carcontroller.py

@@ -25,7 +25,7 @@ ACCEL_WINDUP_LIMIT = 4.0 * DT_CTRL * 3  # m/s^2 / frame
 ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3  # m/s^2 / frame
 ACCEL_PID_UNWIND = 0.03 * DT_CTRL * 3  # m/s^2 / frame
 PRIUS_INTEGRAL_MISMATCH_UNWIND = 8.0
-PRIUS_POSITIVE_FEEDFORWARD_SCALE = 0.5
+PRIUS_POSITIVE_FEEDFORWARD_SCALE = 0.7
 
 MAX_PITCH_COMPENSATION = 1.5  # m/s^2
 TOYOTA_COAST_BRAKE_MIN_SPEED = 15.0  # m/s
@@ -142,6 +142,25 @@ def limit_interceptor_stopping_accel(pcm_accel_cmd: float, target_accel: float,
   return max(pcm_accel_cmd, max(stop_floor, planner_floor))
 
 
+def limit_prius_stopping_accel(pcm_accel_cmd: float, target_accel: float, stopping: bool, v_ego: float, lead_visible: bool) -> float:
+  if not stopping or pcm_accel_cmd >= 0.0 or v_ego >= 1.5:
+    return pcm_accel_cmd
+
+  # Prius can hold onto a stale full negative stop command at standstill even after the
+  # planner has already softened. Keep enough brake to hold the stop, but let the command
+  # unwind toward the live planner target so launches are not delayed and stop transitions
+  # are less abrupt.
+  if target_accel <= -1.8:
+    return pcm_accel_cmd
+
+  stop_floor = float(np.interp(v_ego,
+                               [0.0, 0.2, 0.5, 0.9, 1.5],
+                               [-0.96, -1.00, -1.08, -1.18, -1.35] if lead_visible else [-0.84, -0.88, -0.96, -1.08, -1.24]))
+  target_buffer = float(np.interp(v_ego, [0.0, 0.5, 1.5], [0.06, 0.10, 0.16]))
+  planner_floor = float(target_accel) - target_buffer
+  return max(pcm_accel_cmd, max(stop_floor, planner_floor))
+
+
 class CarController(CarControllerBase):
   def __init__(self, dbc_names, CP):
     super().__init__(dbc_names, CP)
@@ -410,6 +429,8 @@ class CarController(CarControllerBase):
         if self.CP.enableGasInterceptorDEPRECATED:
           pcm_accel_cmd = limit_interceptor_pcm_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo)
           pcm_accel_cmd = limit_interceptor_stopping_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo, bool(hud_control.leadVisible))
+        elif self.CP.carFingerprint == CAR.TOYOTA_PRIUS:
+          pcm_accel_cmd = limit_prius_stopping_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo, lead)
 
         pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
 

opendbc_repo/opendbc/car/toyota/tests/test_toyota.py

@@ -7,7 +7,8 @@ from opendbc.can import CANPacker, CANParser
 from opendbc.car.structs import CarParams
 from opendbc.car.fw_versions import build_fw_dict
 from opendbc.car.toyota import toyotacan
-from opendbc.car.toyota.carcontroller import CarController, limit_interceptor_pcm_accel, limit_interceptor_stopping_accel, update_permit_braking
+from opendbc.car.toyota.carcontroller import CarController, limit_interceptor_pcm_accel, limit_interceptor_stopping_accel, \
+                                             limit_prius_stopping_accel, update_permit_braking
 from opendbc.car.toyota.carstate import calculate_interceptor_gas_pressed
 from opendbc.car.toyota.fingerprints import FW_VERSIONS
 from opendbc.car.toyota.interface import CarInterface
@@ -252,6 +253,14 @@ class TestToyotaCarController:
     assert update_permit_braking(False, 0.10, True, True, 25.0, False) is True
     assert update_permit_braking(False, 0.10, False, False, 25.0, False) is True
 
+  def test_prius_stopping_accel_unwinds_stale_stop_hold(self):
+    limited = limit_prius_stopping_accel(-3.28, -0.05, True, 0.0, True)
+    assert -1.5 < limited < 0.0
+
+  def test_prius_stopping_accel_keeps_hard_stop_commands(self):
+    limited = limit_prius_stopping_accel(-3.28, -2.0, True, 0.0, True)
+    assert limited == -3.28
+
   def test_sng_hack_clears_existing_standstill_latch(self):
     controller = self._make_controller(standstill_req=True, last_standstill=True)
 

opendbc_repo/opendbc/safety/modes/hyundai_canfd.h

@@ -190,7 +190,7 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) {
     .has_steer_req_tolerance = true,
   };
   const AngleSteeringLimits HYUNDAI_CANFD_ANGLE_STEERING_LIMITS = {
-    .max_angle = 1800,
+    .max_angle = 3600,
     .angle_deg_to_can = 10,
     .frequency = 100U,
   };
@@ -230,9 +230,16 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) {
       int desired_angle = (msg->data[11] << 6U) | (msg->data[10] >> 2U);
       desired_angle = to_signed(desired_angle, 14);
 
+      // ADAS_ACIAnglTqRedcGainVal: bit 96, 8 bits, unsigned. Raw 0-250 valid, 251-255 reserved.
+      const uint8_t gain_raw = msg->data[12];
+      bool gain_violation = gain_raw > 250U;
+      if (!steer_angle_req && (gain_raw != 0U)) {
+        gain_violation = true;
+      }
+
       if (steer_angle_cmd_checks_vm(desired_angle, steer_angle_req,
                                     HYUNDAI_CANFD_ANGLE_STEERING_LIMITS,
-                                    HYUNDAI_CANFD_ANGLE_STEERING_PARAMS)) {
+                                    HYUNDAI_CANFD_ANGLE_STEERING_PARAMS) || gain_violation) {
         tx = false;
       }
     } else {

opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py

@@ -174,7 +174,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest):
   BUTTONS_TX_BUS = 2
   LATERAL_FREQUENCY = 100
   STANDSTILL_THRESHOLD = 12
-  STEER_ANGLE_MAX = 180
+  STEER_ANGLE_MAX = 360
   DEG_TO_CAN = 10
   GAS_MSG = ("ACCELERATOR_ALT", "ACCELERATOR_PEDAL")
   SAFETY_PARAM = HyundaiSafetyFlags.CANFD_ANGLE_STEERING | HyundaiSafetyFlags.CAMERA_SCC | HyundaiSafetyFlags.HYBRID_GAS
@@ -248,7 +248,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest):
     checksum = sig_checksum.calc_checksum(addr, sig_checksum, dat)
     _set_value(dat, sig_checksum, checksum)
 
-  def _angle_cmd_msg(self, angle, enabled, increment_timer=True):
+  def _angle_cmd_msg(self, angle, enabled, increment_timer=True, gain_raw=250):
     if increment_timer:
       self.safety.set_timer(self.angle_cmd_cnt * int(1e6 / self.LATERAL_FREQUENCY))
       self.angle_cmd_cnt += 1
@@ -272,7 +272,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest):
     dat[9] = (dat[9] & ~0x30) | (((2 if enabled else 1) & 0x3) << 4)
     dat[10] = (dat[10] & 0x03) | ((desired_angle & 0x3F) << 2)
     dat[11] = (desired_angle >> 6) & 0xFF
-    dat[12] = 250 if enabled else 0
+    dat[12] = gain_raw if enabled or gain_raw != 250 else 0
     self._update_checksum(addr, dat)
     return libsafety_py.make_CANPacket(addr, 0, bytes(dat))
 
@@ -335,6 +335,19 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest):
     self.assertFalse(self._tx(self._angle_cmd_msg(0, True, increment_timer=False)))
     self.assertTrue(self._tx(self._angle_cmd_msg(0, True, increment_timer=False)))
 
+  def test_angle_torque_reduction_gain_limits(self):
+    if self.__class__.__name__ != "TestHyundaiCanfdAngleSteering":
+      return
+
+    self.safety.set_controls_allowed(True)
+    self._reset_speed_measurement(1)
+    self._set_prev_desired_angle(0)
+    self.assertTrue(self._tx(self._angle_cmd_msg(0, True, gain_raw=250)))
+    self._set_prev_desired_angle(0)
+    self.assertFalse(self._tx(self._angle_cmd_msg(0, True, gain_raw=251)))
+    self._set_prev_desired_angle(0)
+    self.assertFalse(self._tx(self._angle_cmd_msg(0, False, gain_raw=1)))
+
 
 class TestHyundaiCanfdAngleSteeringLfaAlt(TestHyundaiCanfdAngleSteering):
 

panda/board/obj/gitversion.h

@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-78e9b513-DEBUG";
+const uint8_t gitversion[19] = "DEV-e5cc7460-DEBUG";

panda/board/obj/version

@@ -1 +1 @@
-DEV-78e9b513-DEBUG
+DEV-e5cc7460-DEBUG

scripts/diagnose_slc_mapd.py

@@ -28,6 +28,7 @@ import time
 from dataclasses import dataclass
 from datetime import datetime, timedelta, timezone
 from pathlib import Path
+import types
 from typing import Any, Optional
 from urllib.parse import parse_qs, urlparse
 
@@ -35,6 +36,16 @@ import requests
 
 # ── StarPilot / openpilot imports ──────────────────────────────────────────
 
+sys.path.insert(0, str(Path(__file__).resolve().parent.parent))
+
+# smbus2 is a hardware I2C library only present on comma's tici device.
+# On PC it's never installed, and SMBus is never called (the TICI flag is
+# False). Stub it so the eager import chain in openpilot.system.hardware
+# succeeds without installing tici-only platform dependencies.
+_smbus2 = types.ModuleType('smbus2')
+_smbus2.SMBus = None
+sys.modules['smbus2'] = _smbus2
+
 from openpilot.common.constants import CV
 from openpilot.tools.lib.auth import login as oauth_login
 from openpilot.tools.lib.auth_config import get_token, set_token
@@ -126,6 +137,16 @@ class ScsSample:
     decel_pressed: bool
 
 
+@dataclass
+class LeadSample:
+    """One radarState lead-vehicle event from the log."""
+
+    log_mono_time: int
+    has_lead: bool
+    d_rel: float   # metres ahead
+    v_lead: float  # m/s absolute speed of lead
+
+
 @dataclass
 class GpsSample:
     """One gpsLocationExternal event from the log."""
@@ -240,6 +261,12 @@ def parse_args(argv: list[str] | None = None) -> argparse.Namespace:
         formatter_class=argparse.RawDescriptionHelpFormatter,
         epilog=JWT_HELP,
     )
+    p.add_argument(
+        "route_pos",
+        nargs="?",
+        default=None,
+        help="Route name (positional format alternative)."
+    )
     p.add_argument(
         "--route",
         default=None,
@@ -257,7 +284,10 @@ def parse_args(argv: list[str] | None = None) -> argparse.Namespace:
         default=False,
         help="Display speeds in km/h (default: mph).",
     )
-    return p.parse_args(argv)
+    args = p.parse_args(argv)
+    if args.route_pos:
+        args.route = args.route_pos
+    return args
 
 
 def resolve_route_identifier(raw: str) -> str:
@@ -297,6 +327,8 @@ def resolve_route_identifier(raw: str) -> str:
             "Expected: dongle_id|log_id  (16 hex chars | identifier)"
         )
     dongle, log_id, suffix = m.group(1), m.group(2), m.group(3) or ""
+    if suffix:
+        suffix = re.sub(r"^/(\d+)/(\d+)$", r"/\1:\2", suffix)
     return f"{dongle}/{log_id}{suffix}"
 
 
@@ -354,18 +386,19 @@ def parse_route_logs(
     list[CarSample],
     list[GpsSample],
     list[ScsSample],
+    list[LeadSample],
     list[int],
 ]:
     """Use LogReader to parse qlog and extract all speed-related messages.
 
-    Returns (mapd_events, splan_events, car_events, gps_events, scs_events, segments).
+    Returns (mapd_events, splan_events, car_events, gps_events, scs_events, lead_events, segments).
     """
     mapd_events: list[MapdSample] = []
     splan_events: list[SplanSample] = []
     car_events: list[CarSample] = []
     gps_events: list[GpsSample] = []
     scs_events: list[ScsSample] = []
-    segments_found: set[int] = set()
+    lead_events: list[LeadSample] = []
     segments_found: set[int] = set()
     seg_of_msg: dict[int, int] = {}
 
@@ -448,6 +481,17 @@ def parse_route_logs(
                     decel_pressed=bool(s.decelPressed),
                 )
             )
+        elif which == "radarState":
+            r = msg.radarState
+            lead = r.leadOne
+            lead_events.append(
+                LeadSample(
+                    log_mono_time=t,
+                    has_lead=bool(lead.status),
+                    d_rel=float(lead.dRel or 0),
+                    v_lead=float(lead.vLead or 0),
+                )
+            )
 
     if not all_msgs:
         print("Warning: no messages found in route logs.", file=sys.stderr)
@@ -472,7 +516,7 @@ def parse_route_logs(
 
     segments = sorted(segments_found) if segments_found else [0]
 
-    return mapd_events, splan_events, car_events, gps_events, scs_events, segments
+    return mapd_events, splan_events, car_events, gps_events, scs_events, lead_events, segments
 
 
 # ============================================================================
@@ -874,9 +918,10 @@ def detect_changes(
     osm_ways: dict[int, OsmWay],
     gps_timeline: list[tuple[float, float, float]],
     base_time_ns: int,
+    lead_events: list[LeadSample] | None = None,
 ) -> list[ChangeRow]:
     """Walk starpilotPlan events to detect every SLC state transition,
-    correlating with mapd, carState, and starpilotCarState for full context.
+    correlating with mapd, carState, starpilotCarState, and radarState for full context.
 
     Produces a timeline of SLC decisions: limits, overrides, prompts, lookaheads.
     """
@@ -888,6 +933,9 @@ def detect_changes(
     mapd_sorted = (
         sorted(mapd_events, key=lambda e: e.log_mono_time) if mapd_events else []
     )
+    lead_sorted = (
+        sorted(lead_events, key=lambda e: e.log_mono_time) if lead_events else []
+    )
 
     prev_slc = -1.0
     prev_source_key = ""
@@ -910,6 +958,7 @@ def detect_changes(
         m = _nearest(mapd_sorted, t_ns)
         c = _nearest(car_events, t_ns)
         s = _nearest(scs_events, t_ns)
+        lv = _nearest(lead_sorted, t_ns) if lead_sorted else None
 
         mapd_sl = m.speed_limit if m else 0
         mapd_next = m.next_speed_limit if m else 0
@@ -920,6 +969,9 @@ def detect_changes(
         gas = bool(c.gas_pressed) if c else False
         accel = bool(s.accel_pressed) if s else False
         decel = bool(s.decel_pressed) if s else False
+        has_lead = bool(lv.has_lead) if lv else False
+        lead_d_rel = lv.d_rel if lv and lv.has_lead else 0.0
+        lead_v = lv.v_lead if lv and lv.has_lead else 0.0
 
         lat, lon = gps_at_time(t_ns, gps_timeline) if gps_timeline else (0, 0)
         osm_sl, osm_name = (
@@ -1001,11 +1053,17 @@ def detect_changes(
             detail = f"gas pressed: {v_ego * KPH_TO_MPH * MS_TO_KPH:.0f} > {slc * KPH_TO_MPH * MS_TO_KPH:.0f}"
             if accel:
                 detail += " (accel)"
+            if has_lead:
+                detail += f" [lead {lead_d_rel:.0f}m ahead]"
+
+        elif ov_end and not limit_changed and has_lead and not gas:
+            event_type = "OVERRIDE CLEAR"
+            detail = f"ACC decel behind lead ({lead_d_rel:.0f}m, {lead_v * KPH_TO_MPH * MS_TO_KPH:.0f} mph) — not driver brake"
 
         elif ov_end and not limit_changed:
             if ov_phase:
                 event_type = "OVERRIDE CLEAR"
-                detail = "override ended"
+                detail = "override ended" + (f" [lead {lead_d_rel:.0f}m, {lead_v * KPH_TO_MPH * MS_TO_KPH:.0f} mph]" if has_lead else "")
 
         elif active_ov and ov_phase != "active":
             event_type = "OVERRIDE ACTIVE"
@@ -1014,6 +1072,8 @@ def detect_changes(
         elif stale_ov and ov_phase != "stale":
             event_type = "STALE OVERRIDE"
             detail = f"overridden > {slc * KPH_TO_MPH * MS_TO_KPH:.0f}, v_ego={v_ego * KPH_TO_MPH * MS_TO_KPH:.0f}"
+            if has_lead:
+                detail += f" [ACC: lead {lead_d_rel:.0f}m, {lead_v * KPH_TO_MPH * MS_TO_KPH:.0f} mph]"
 
         elif source_changed:
             event_type = "SOURCE"
@@ -1091,14 +1151,14 @@ def fmt_speed(mps: float, use_mph: bool, width: int = 5) -> str:
 
 
 def print_header(
-    route_name: str, mapd_n: int, splan_n: int, gps_n: int, scs_n: int
+    route_name: str, mapd_n: int, splan_n: int, gps_n: int, scs_n: int, lead_n: int = 0
 ) -> None:
     sep = "=" * 82
     print(f"\n{sep}")
     print(f"  SLC / mapd Diagnostic Timeline")
     print(f"  Route: {route_name}")
     print(
-        f"  Events: mapdOut={mapd_n} | starpilotPlan={splan_n} | starpilotCarState={scs_n} | GPS={gps_n}"
+        f"  Events: mapdOut={mapd_n} | starpilotPlan={splan_n} | starpilotCarState={scs_n} | GPS={gps_n} | radarState={lead_n}"
     )
     print(f"{sep}")
 
@@ -1197,7 +1257,7 @@ def main(argv: list[str] | None = None) -> int:
         print(f"\nProcessing route: {canonical}", file=sys.stderr)
 
         # ── Parse qlog ──────────────────────────────────────────────────
-        mapd_ev, splan_ev, car_ev, gps_ev, scs_ev, segments = parse_route_logs(
+        mapd_ev, splan_ev, car_ev, gps_ev, scs_ev, lead_ev, segments = parse_route_logs(
             canonical
         )
 
@@ -1226,11 +1286,12 @@ def main(argv: list[str] | None = None) -> int:
 
         # ── Detect changes ──────────────────────────────────────────
         rows = detect_changes(
-            mapd_ev, splan_ev, car_ev, scs_ev, osm_ways, gps_timeline, base_time
+            mapd_ev, splan_ev, car_ev, scs_ev, osm_ways, gps_timeline, base_time,
+            lead_events=lead_ev,
         )
 
         # ── Output ─────────────────────────────────────────────────
-        print_header(canonical, len(mapd_ev), len(splan_ev), len(gps_ev), len(scs_ev))
+        print_header(canonical, len(mapd_ev), len(splan_ev), len(gps_ev), len(scs_ev), len(lead_ev))
         print_table(rows, not args.kmh)
 
         stale = sum(1 for r in rows if r.stale)

selfdrive/car/card.py

@@ -392,26 +392,38 @@ class Car:
       return
 
     filtered_CS = self._get_button_event_filtered_state(CS)
-    send_button, v_target = self.redneck_cruise.run(filtered_CS, CC, self._get_redneck_target_speed(CS), self.is_metric)
+    v_target_ms, lead_present = self._get_redneck_target_speed(CS)
+    send_button, v_target = self.redneck_cruise.run(filtered_CS, CC, v_target_ms, self.is_metric, lead_present=lead_present)
     self.CI.CS.redneck_send_button = send_button
     self.CI.CS.redneck_v_target = v_target
 
-  def _get_redneck_target_speed(self, CS: car.CarState) -> float:
+  def _get_redneck_target_speed(self, CS: car.CarState) -> tuple[float, bool]:
     starpilot_target_speed = 0.0
+    allow_plan_decrease = False
+    lead_present = False
+    lookahead_points = REDNECK_DECREASE_LOOKAHEAD_POINTS
     if self.sm.seen['starpilotPlan'] and self.sm.valid['starpilotPlan']:
       starpilot_target_speed = float(self.sm['starpilotPlan'].vCruise)
 
     plan_speeds = []
     if self.sm.seen['longitudinalPlan'] and self.sm.valid['longitudinalPlan']:
-      plan_speeds = [float(speed) for speed in self.sm['longitudinalPlan'].speeds if math.isfinite(float(speed))]
+      longitudinal_plan = self.sm['longitudinalPlan']
+      plan_speeds = [float(speed) for speed in longitudinal_plan.speeds if math.isfinite(float(speed))]
+      lead_present = bool(longitudinal_plan.hasLead)
+      allow_plan_decrease = bool(lead_present or longitudinal_plan.shouldStop or
+                                 str(longitudinal_plan.longitudinalPlanSource) != "cruise")
+      if lead_present and len(plan_speeds) > 0:
+        lookahead_points = len(plan_speeds)
 
     return select_redneck_target_speed(
       float(getattr(CS, "vCruise", 0.0)),
       float(CS.cruiseState.speedCluster),
       starpilot_target_speed,
       plan_speeds,
-      REDNECK_DECREASE_LOOKAHEAD_POINTS,
+      lookahead_points,
-    )
+      allow_plan_decrease=allow_plan_decrease,
+      lead_present=lead_present,
+    ), lead_present
 
   def _advance_redneck_button_feedback_filter(self) -> None:
     if self.redneck_cruise is None:

selfdrive/car/cruise.py

@@ -161,9 +161,10 @@ class VCruiseHelper:
       return
 
     engage_floor_kph = max(V_CRUISE_MIN, 7.0 * CV.MPH_TO_KPH)
+    resume_pressed = any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents)
+    remembered_resume = resume_prev_button and (self.gm_cc_only or self.redneck_non_pcm)
 
-    if (any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents)
+    if self.v_cruise_initialized and (resume_pressed or remembered_resume):
-      and self.v_cruise_initialized or (self.gm_cc_only and resume_prev_button)):
       self.v_cruise_kph = self.v_cruise_kph_last
     elif desired_speed_limit > 0 and getattr(starpilot_toggles, "set_speed_limit", False):
       # Respect the exact SLC limit+offset on engage instead of snapping upward to

selfdrive/car/cruise_state.py

@@ -0,0 +1,22 @@
+from opendbc.car import structs
+
+
+def hyundai_openpilot_longitudinal_acc_req_feedback(CP: structs.CarParams) -> bool:
+  return CP.brand == 'hyundai' and CP.openpilotLongitudinalControl and not CP.pcmCruise
+
+
+def should_cancel_stock_cruise(CP: structs.CarParams, cruise_enabled: bool, controls_enabled: bool) -> bool:
+  if not cruise_enabled:
+    return False
+  if not controls_enabled:
+    return True
+  return not CP.pcmCruise and not hyundai_openpilot_longitudinal_acc_req_feedback(CP)
+
+
+def should_flag_cruise_mismatch(CP: structs.CarParams, cruise_enabled: bool, controls_enabled: bool,
+                                effective_pcm_cruise: bool) -> bool:
+  if not cruise_enabled:
+    return False
+  if not controls_enabled:
+    return True
+  return not effective_pcm_cruise and not hyundai_openpilot_longitudinal_acc_req_feedback(CP)

selfdrive/car/redneck_cruise.py

@@ -12,6 +12,9 @@ SEND_BUTTON_DECREASE = 2
 HYST_GAP = 0.0
 INCREASE_INACTIVE_TIMER = 0.4
 DECREASE_INACTIVE_TIMER = 0.1
+LEAD_INCREASE_INACTIVE_TIMER = 0.1
+LEAD_RECOVERY_LOOKAHEAD_POINTS = 4
+LEAD_COAST_BUFFER_MS = 1.0 * CV.MPH_TO_MS
 
 CRUISE_BUTTON_TIMERS = {
   int(ButtonType.decelCruise): 0,
@@ -25,16 +28,25 @@ CRUISE_BUTTON_TIMERS = {
 
 def select_redneck_target_speed(v_cruise_kph: float, speed_cluster_ms: float,
                                 starpilot_target_speed_ms: float, plan_speeds_ms: list[float],
-                                lookahead_points: int) -> float:
+                                lookahead_points: int, allow_plan_decrease: bool = True,
+                                lead_present: bool = False) -> float:
   target_speed_ms = float(speed_cluster_ms)
   if v_cruise_kph > 0:
     target_speed_ms = float(v_cruise_kph) * CV.KPH_TO_MS
   elif starpilot_target_speed_ms > 0:
     target_speed_ms = float(starpilot_target_speed_ms)
 
-  if len(plan_speeds_ms) > 0:
+  if allow_plan_decrease and len(plan_speeds_ms) > 0:
+    if lead_present and plan_speeds_ms[0] > speed_cluster_ms:
+      recovery_lookahead_points = min(len(plan_speeds_ms), LEAD_RECOVERY_LOOKAHEAD_POINTS)
+      recovery_target_speed_ms = max(speed_cluster_ms, min(plan_speeds_ms[:recovery_lookahead_points]))
+      return min(target_speed_ms, recovery_target_speed_ms)
+
     decrease_target_speed_ms = min(plan_speeds_ms[:lookahead_points])
-    if decrease_target_speed_ms < min(target_speed_ms, float(speed_cluster_ms)):
+    if lead_present and decrease_target_speed_ms < speed_cluster_ms:
+      decrease_target_speed_ms = max(0.0, decrease_target_speed_ms - LEAD_COAST_BUFFER_MS)
+
+    if decrease_target_speed_ms < target_speed_ms:
       return decrease_target_speed_ms
 
   return target_speed_ms
@@ -106,20 +118,25 @@ class RedneckCruise:
     return "holding"
 
   @staticmethod
-  def _get_pre_active_frames(state: str) -> int:
+  def _get_pre_active_frames(state: str, lead_present: bool) -> int:
-    timer = DECREASE_INACTIVE_TIMER if state == "decreasing" else INCREASE_INACTIVE_TIMER
+    if state == "decreasing":
+      timer = DECREASE_INACTIVE_TIMER
+    elif lead_present:
+      timer = LEAD_INCREASE_INACTIVE_TIMER
+    else:
+      timer = INCREASE_INACTIVE_TIMER
     return int(timer / DT_CTRL)
 
-  def _arm_pre_active(self, desired_state: str) -> None:
+  def _arm_pre_active(self, desired_state: str, lead_present: bool) -> None:
     if desired_state == "holding":
       self.state = "holding"
       self.pre_active_timer = 0
       return
 
     self.state = "preActive"
-    self.pre_active_timer = self._get_pre_active_frames(desired_state)
+    self.pre_active_timer = self._get_pre_active_frames(desired_state, lead_present)
 
-  def _update_state_machine(self) -> int:
+  def _update_state_machine(self, lead_present: bool) -> int:
     desired_state = self._desired_state()
 
     if not self.is_ready:
@@ -127,35 +144,36 @@ class RedneckCruise:
       self.pre_active_timer = 0
     elif self.state == "inactive":
       if not self.is_ready_prev:
-        self._arm_pre_active(desired_state)
+        self._arm_pre_active(desired_state, lead_present)
     elif self.state == "preActive":
       if desired_state == "holding":
         self.state = "holding"
         self.pre_active_timer = 0
       else:
-        desired_frames = self._get_pre_active_frames(desired_state)
+        desired_frames = self._get_pre_active_frames(desired_state, lead_present)
         self.pre_active_timer = max(0, min(self.pre_active_timer, desired_frames) - 1)
         if self.pre_active_timer <= 0:
           self.state = desired_state
     elif self.state == "holding":
       if desired_state != "holding":
-        self._arm_pre_active(desired_state)
+        self._arm_pre_active(desired_state, lead_present)
     elif self.state != desired_state:
       if desired_state == "holding":
         self.state = "holding"
       else:
-        self._arm_pre_active(desired_state)
+        self._arm_pre_active(desired_state, lead_present)
 
     return self._send_button_for_state(self.state)
 
-  def run(self, CS: car.CarState, CC: car.CarControl, v_target_ms: float, is_metric: bool) -> tuple[int, int]:
+  def run(self, CS: car.CarState, CC: car.CarControl, v_target_ms: float, is_metric: bool,
+          lead_present: bool = False) -> tuple[int, int]:
     if self.FPCP.pcmCruiseSpeed or not self.FPCP.redneckCruiseAvailable:
       self._reset()
       return SEND_BUTTON_NONE, 0
 
     self._update_calculations(CS, v_target_ms, is_metric)
     self._update_readiness(CS, CC)
-    send_button = self._update_state_machine()
+    send_button = self._update_state_machine(lead_present)
 
     self.is_ready_prev = self.is_ready
     return send_button, self.v_target

selfdrive/car/tests/test_car_specific.py

@@ -0,0 +1,41 @@
+from cereal import car
+
+from openpilot.selfdrive.car.cruise_state import should_cancel_stock_cruise, should_flag_cruise_mismatch
+
+
+def make_cp(brand="hyundai", op_long=True, pcm_cruise=False):
+  cp = car.CarParams.new_message()
+  cp.brand = brand
+  cp.openpilotLongitudinalControl = op_long
+  cp.pcmCruise = pcm_cruise
+  return cp
+
+
+def test_hyundai_openpilot_long_does_not_cancel_active_acc_req_feedback():
+  cp = make_cp()
+
+  assert not should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=True)
+  assert not should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=True, effective_pcm_cruise=False)
+
+
+def test_hyundai_openpilot_long_still_flags_cruise_when_controls_disabled():
+  cp = make_cp()
+
+  assert should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=False)
+  assert should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=False, effective_pcm_cruise=False)
+
+
+def test_non_hyundai_openpilot_long_behavior_is_unchanged():
+  cp = make_cp(brand="toyota")
+
+  assert should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=True)
+  assert should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=True, effective_pcm_cruise=False)
+
+
+def test_pcm_cruise_behavior_is_unchanged():
+  cp = make_cp(op_long=False, pcm_cruise=True)
+
+  assert not should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=True)
+  assert should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=False)
+  assert not should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=True, effective_pcm_cruise=True)
+  assert should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=False, effective_pcm_cruise=True)

selfdrive/car/tests/test_cruise_speed.py

@@ -347,6 +347,30 @@ class TestVCruiseHelperRedneck:
     assert self.v_cruise_helper.v_cruise_kph == pytest.approx(62 * CV.MPH_TO_KPH)
     assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(62 * CV.MPH_TO_KPH)
 
+  def test_resume_keeps_previous_internal_max_speed(self):
+    engage_cs = car.CarState(
+      vEgo=75 * CV.MPH_TO_MS,
+      cruiseState={"speedCluster": 75 * CV.MPH_TO_MS},
+    )
+    self.v_cruise_helper.initialize_v_cruise(engage_cs, experimental_mode=False, resume_prev_button=False,
+                                             starpilot_toggles=self.starpilot_toggles)
+
+    disabled_cs = car.CarState(
+      cruiseState={"available": True, "speedCluster": 38 * CV.MPH_TO_MS},
+    )
+    self.v_cruise_helper.update_v_cruise(disabled_cs, enabled=False, is_metric=False,
+                                         speed_limit_changed=False, starpilot_toggles=self.starpilot_toggles)
+
+    resume_cs = car.CarState(
+      vEgo=38 * CV.MPH_TO_MS,
+      cruiseState={"speedCluster": 38 * CV.MPH_TO_MS},
+    )
+    self.v_cruise_helper.initialize_v_cruise(resume_cs, experimental_mode=False, resume_prev_button=True,
+                                             starpilot_toggles=self.starpilot_toggles)
+
+    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(75 * CV.MPH_TO_KPH)
+    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(75 * CV.MPH_TO_KPH)
+
   def test_reverse_cruise_increase_swaps_short_and_long_press_intervals(self):
     self.enable(55 * CV.MPH_TO_MS, experimental_mode=False)
     initial_v_cruise_kph = self.v_cruise_helper.v_cruise_kph

selfdrive/car/tests/test_redneck_cruise.py

@@ -7,6 +7,7 @@ from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.car.redneck_cruise import (
   DECREASE_INACTIVE_TIMER,
   INCREASE_INACTIVE_TIMER,
+  LEAD_INCREASE_INACTIVE_TIMER,
   RedneckCruise,
   SEND_BUTTON_DECREASE,
   SEND_BUTTON_INCREASE,
@@ -41,7 +42,8 @@ class TestRedneckCruise(unittest.TestCase):
   def _button_event(button_type, pressed):
     return SimpleNamespace(type=button_type, pressed=pressed)
 
-  def _run_until_active(self, target_mph, speed_cluster_mph=20.0, button_events=None, override=False, cancel=False, resume=False):
+  def _run_until_active(self, target_mph, speed_cluster_mph=20.0, button_events=None,
+                        override=False, cancel=False, resume=False, lead_present=False):
     frames = int(max(INCREASE_INACTIVE_TIMER, DECREASE_INACTIVE_TIMER) / DT_CTRL) + 2
     send_button = SEND_BUTTON_NONE
     v_target = 0
@@ -51,11 +53,12 @@ class TestRedneckCruise(unittest.TestCase):
         self._new_control(override=override, cancel=cancel, resume=resume),
         target_mph * CV.MPH_TO_MS,
         is_metric=False,
+        lead_present=lead_present,
       )
       button_events = None
     return send_button, v_target
 
-  def _frames_until_button(self, target_mph, speed_cluster_mph):
+  def _frames_until_button(self, target_mph, speed_cluster_mph, lead_present=False):
     frames = int(INCREASE_INACTIVE_TIMER / DT_CTRL) + 4
     for frame in range(frames):
       send_button, _ = self.redneck.run(
@@ -63,6 +66,7 @@ class TestRedneckCruise(unittest.TestCase):
         self._new_control(),
         target_mph * CV.MPH_TO_MS,
         is_metric=False,
+        lead_present=lead_present,
       )
       if send_button != SEND_BUTTON_NONE:
         return frame
@@ -87,6 +91,16 @@ class TestRedneckCruise(unittest.TestCase):
     self.assertIsNotNone(increase_frame)
     self.assertLess(decrease_frame, increase_frame)
 
+  def test_lead_increase_activates_faster_than_free_cruise_increase(self):
+    free_cruise_frame = self._frames_until_button(target_mph=25.0, speed_cluster_mph=20.0, lead_present=False)
+    self.redneck = RedneckCruise(self.CP, self.FPCP)
+    lead_frame = self._frames_until_button(target_mph=25.0, speed_cluster_mph=20.0, lead_present=True)
+
+    self.assertIsNotNone(free_cruise_frame)
+    self.assertIsNotNone(lead_frame)
+    self.assertLess(lead_frame, free_cruise_frame)
+    self.assertLessEqual(lead_frame, int(LEAD_INCREASE_INACTIVE_TIMER / DT_CTRL))
+
   def test_suppresses_output_during_manual_cruise_button_use(self):
     button_event = self._button_event(ButtonType.accelCruise, True)
     send_button, _ = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0, button_events=[button_event])
@@ -119,6 +133,17 @@ class TestRedneckCruise(unittest.TestCase):
     )
     self.assertAlmostEqual(120.0 * CV.KPH_TO_MS, target_speed)
 
+  def test_target_speed_ignores_plan_drift_during_free_cruise(self):
+    target_speed = select_redneck_target_speed(
+      104.4,
+      63.0 * CV.MPH_TO_MS,
+      0.0,
+      [62.55 * CV.MPH_TO_MS, 62.44 * CV.MPH_TO_MS, 62.36 * CV.MPH_TO_MS],
+      10,
+      allow_plan_decrease=False,
+    )
+    self.assertAlmostEqual(104.4 * CV.KPH_TO_MS, target_speed)
+
   def test_target_speed_returns_plan_minimum_when_slowing_down(self):
     target_speed = select_redneck_target_speed(
       120.0,
@@ -126,9 +151,49 @@ class TestRedneckCruise(unittest.TestCase):
       0.0,
       [74.0 * CV.MPH_TO_MS, 72.0 * CV.MPH_TO_MS, 71.0 * CV.MPH_TO_MS],
       10,
+      allow_plan_decrease=True,
     )
     self.assertAlmostEqual(71.0 * CV.MPH_TO_MS, target_speed)
 
+  def test_target_speed_uses_longer_horizon_and_buffer_for_lead_slowdown(self):
+    target_speed = select_redneck_target_speed(
+      120.0,
+      75.0 * CV.MPH_TO_MS,
+      0.0,
+      [74.9 * CV.MPH_TO_MS, 74.6 * CV.MPH_TO_MS, 74.2 * CV.MPH_TO_MS, 73.8 * CV.MPH_TO_MS,
+       73.4 * CV.MPH_TO_MS, 73.0 * CV.MPH_TO_MS, 72.6 * CV.MPH_TO_MS, 72.2 * CV.MPH_TO_MS,
+       71.8 * CV.MPH_TO_MS, 71.4 * CV.MPH_TO_MS, 71.0 * CV.MPH_TO_MS],
+      11,
+      allow_plan_decrease=True,
+      lead_present=True,
+    )
+    self.assertLess(target_speed, 71.4 * CV.MPH_TO_MS)
+
+  def test_target_speed_uses_near_term_recovery_for_lead_speedup(self):
+    target_speed = select_redneck_target_speed(
+      120.0,
+      55.0 * CV.MPH_TO_MS,
+      0.0,
+      [57.15 * CV.MPH_TO_MS, 56.9 * CV.MPH_TO_MS, 56.4 * CV.MPH_TO_MS, 55.8 * CV.MPH_TO_MS,
+       54.88 * CV.MPH_TO_MS, 52.0 * CV.MPH_TO_MS, 50.15 * CV.MPH_TO_MS],
+      10,
+      allow_plan_decrease=True,
+      lead_present=True,
+    )
+    self.assertAlmostEqual(55.8 * CV.MPH_TO_MS, target_speed)
+
+  def test_target_speed_stays_on_lead_target_when_cluster_drops_below_it(self):
+    target_speed = select_redneck_target_speed(
+      76.9,
+      32.9 * CV.MPH_TO_MS,
+      47.8 * CV.MPH_TO_MS,
+      [37.3 * CV.MPH_TO_MS, 37.2 * CV.MPH_TO_MS, 37.1 * CV.MPH_TO_MS],
+      10,
+      allow_plan_decrease=True,
+      lead_present=True,
+    )
+    self.assertAlmostEqual(37.1 * CV.MPH_TO_MS, target_speed)
+
 
 if __name__ == "__main__":
   unittest.main()

selfdrive/controls/controlsd.py

@@ -23,6 +23,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
   get_bolt_2017_steer_ratio_scale,
 )
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
+from openpilot.selfdrive.car.cruise_state import should_cancel_stock_cruise
 from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 
@@ -46,6 +47,30 @@ def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float:
   return spoofed_speed
 
 
+def get_torque_control_params(CP, torque_params, starpilot_toggles, use_live_params: bool) -> tuple[float, float, float]:
+  torque_tune = CP.lateralTuning.torque
+  lat_accel_factor = torque_tune.latAccelFactor
+  lat_accel_offset = torque_tune.latAccelOffset
+  friction = torque_tune.friction
+
+  use_custom_lat_accel = getattr(starpilot_toggles, "use_custom_latAccelFactor", False)
+  use_custom_friction = getattr(starpilot_toggles, "use_custom_friction", False)
+
+  if use_live_params:
+    if not use_custom_lat_accel:
+      lat_accel_factor = torque_params.latAccelFactorFiltered
+      lat_accel_offset = torque_params.latAccelOffsetFiltered
+    if not use_custom_friction:
+      friction = torque_params.frictionCoefficientFiltered
+
+  if use_custom_lat_accel:
+    lat_accel_factor = starpilot_toggles.latAccelFactor
+  if use_custom_friction:
+    friction = starpilot_toggles.friction
+
+  return lat_accel_factor, lat_accel_offset, friction
+
+
 class Controls:
   def __init__(self) -> None:
     self.params = Params()
@@ -82,6 +107,8 @@ class Controls:
     self.sm = self.sm.extend(['liveDelay', 'starpilotCarState', 'starpilotPlan'])
 
     self.starpilot_toggles = get_starpilot_toggles()
+    self.ecu_disable_failed = False
+    self.ecu_disable_failed_checked = not self.CP.openpilotLongitudinalControl
 
     if self.CP.lateralTuning.which() == "torque" and (self.starpilot_toggles.nnff or self.starpilot_toggles.nnff_lite):
       self.LaC = LatControlNNFF(self.CP, self.CI, DT_CTRL)
@@ -102,6 +129,16 @@ class Controls:
 
     self.starpilot_toggles = get_starpilot_toggles(self.sm)
 
+  def update_ecu_disable_failed(self):
+    if self.ecu_disable_failed_checked:
+      return
+
+    # ControlsReady is set after CarInterface.init(), where Hyundai ECU disable
+    # writes EcuDisableFailed. Once init has completed, the value is stable.
+    if self.params.get_bool("ControlsReady"):
+      self.ecu_disable_failed = self.params.get_bool("EcuDisableFailed")
+      self.ecu_disable_failed_checked = True
+
   def state_control(self):
     CS = self.sm['carState']
 
@@ -121,9 +158,15 @@ class Controls:
     # Update Torque Params
     if self.CP.lateralTuning.which() == 'torque':
       torque_params = self.sm['liveTorqueParameters']
-      if self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or self.starpilot_toggles.force_auto_tune):
+      force_auto_tune = getattr(self.starpilot_toggles, "force_auto_tune", False)
-        self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
+      use_live_params = self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or force_auto_tune)
-                                           torque_params.frictionCoefficientFiltered)
+      use_custom_torque_params = (
+        getattr(self.starpilot_toggles, "use_custom_latAccelFactor", False) or
+        getattr(self.starpilot_toggles, "use_custom_friction", False)
+      )
+      if use_live_params or use_custom_torque_params:
+        lat_accel_factor, lat_accel_offset, friction = get_torque_control_params(self.CP, torque_params, self.starpilot_toggles, use_live_params)
+        self.LaC.update_live_torque_params(lat_accel_factor, lat_accel_offset, friction)
 
     long_plan = self.sm['longitudinalPlan']
     model_v2 = self.sm['modelV2']
@@ -140,8 +183,8 @@ class Controls:
                                       self.sm['starpilotPlan'].lateralCheck)
     # EcuDisableFailed is set when car started in READY mode (ECU disable was rejected)
     # Disable longitudinal so stock ACC works instead
-    ecu_disable_failed = self.params.get_bool("EcuDisableFailed")
+    self.update_ecu_disable_failed()
-    CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and not ecu_disable_failed
+    CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state
@@ -233,7 +276,7 @@ class Controls:
       CC.enabled,
       self.sm['starpilotCarState'].alwaysOnLateralEnabled,
     )
-    cancel_requested = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise)
+    cancel_requested = should_cancel_stock_cruise(self.CP, CS.cruiseState.enabled, CC.enabled)
     CC.cruiseControl.cancel = cancel_requested and not pacifica_hybrid_aol
 
     legacy_resume_hack = False

selfdrive/controls/lib/latcontrol_torque.py

@@ -6,6 +6,7 @@ from cereal import log
 from opendbc.car.gm.values import CAR as GM_CAR
 from opendbc.car.honda.values import CAR as HONDA_CAR, HondaFlags
 from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR
+from opendbc.car.toyota.values import CAR as TOYOTA_CAR
 from opendbc.car.lateral import get_friction
 from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY, CV
 from openpilot.common.filter_simple import FirstOrderFilter
@@ -143,6 +144,9 @@ KIA_EV6_CARS = (
 KIA_FORTE_CARS = (
   HYUNDAI_CAR.KIA_FORTE,
 )
+PRIUS_CARS = (
+  TOYOTA_CAR.TOYOTA_PRIUS,
+)
 
 BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3
 BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045
@@ -324,6 +328,12 @@ KIA_FORTE_TURN_IN_BOOST_LEFT = 0.10
 KIA_FORTE_TURN_IN_BOOST_RIGHT = 0.00
 KIA_FORTE_UNWIND_TAPER_LEFT = 0.26
 KIA_FORTE_UNWIND_TAPER_RIGHT = 0.04
+KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.10
+KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.14
+KIA_FORTE_CRAWL_TURN_IN_FF_SPEED = 4.5
+KIA_FORTE_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.8
+KIA_FORTE_CRAWL_TURN_IN_FF_LAT = 0.10
+KIA_FORTE_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.05
 KIA_FORTE_CENTER_TAPER_MAX = 0.14
 KIA_FORTE_CENTER_TAPER_LAT = 0.16
 KIA_FORTE_CENTER_TAPER_LAT_WIDTH = 0.03
@@ -480,11 +490,17 @@ IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.10
 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04
 IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 0.60
 IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.14
-IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.62
+IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.98
-IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 17.0
+IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.2
-IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 2.0
+IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.5
-IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.45
+IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.10
-IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.14
+IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.06
+IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.12
+IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.16
+IONIQ_6_CRAWL_TURN_IN_FF_SPEED = 4.5
+IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.8
+IONIQ_6_CRAWL_TURN_IN_FF_LAT = 0.10
+IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.05
 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.82
 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.12
 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.10
@@ -551,6 +567,28 @@ VOLT_PLEXY_TURN_IN_FRICTION_BOOST_LEFT = 0.08
 VOLT_PLEXY_TURN_IN_FRICTION_BOOST_RIGHT = 0.06
 VOLT_PLEXY_UNWIND_FRICTION_REDUCTION_LEFT = 0.16
 VOLT_PLEXY_UNWIND_FRICTION_REDUCTION_RIGHT = 0.40
+PRIUS_TRANSITION_SPEED = 10.0
+PRIUS_PHASE_SCALE = 0.09
+PRIUS_FF_GAIN_LEFT = 0.10
+PRIUS_FF_GAIN_RIGHT = 0.14
+PRIUS_FF_ONSET = 0.16
+PRIUS_FF_ONSET_WIDTH = 0.08
+PRIUS_FF_CUTOFF = 1.25
+PRIUS_FF_CUTOFF_WIDTH = 0.30
+PRIUS_FRICTION_LAT_RISE = 0.18
+PRIUS_FRICTION_JERK_RISE = 0.22
+PRIUS_TURN_IN_BOOST_LEFT = 0.48
+PRIUS_TURN_IN_BOOST_RIGHT = 0.62
+PRIUS_UNWIND_TAPER_LEFT = 0.44
+PRIUS_UNWIND_TAPER_RIGHT = 0.72
+PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.18
+PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.24
+PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT = 0.28
+PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.44
+PRIUS_TURN_IN_FRICTION_BOOST_LEFT = 0.08
+PRIUS_TURN_IN_FRICTION_BOOST_RIGHT = 0.12
+PRIUS_UNWIND_FRICTION_REDUCTION_LEFT = 0.14
+PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT = 0.24
 
 
 def _sigmoid(x: float) -> float:
@@ -567,6 +605,74 @@ def get_friction_threshold(v_ego: float) -> float:
   return float(np.interp(v_ego, [1 * CV.MPH_TO_MS, 20 * CV.MPH_TO_MS, 75 * CV.MPH_TO_MS], [0.16, 0.19, 0.27]))
 
 
+def _prius_sigmoid(x: float) -> float:
+  return _sigmoid(x)
+
+
+def _prius_low_speed_factor(v_ego: float) -> float:
+  return 1.0 / (1.0 + (max(v_ego, 0.0) / PRIUS_TRANSITION_SPEED) ** 2)
+
+
+def _prius_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
+  return math.tanh((desired_lateral_accel * desired_lateral_jerk) / PRIUS_PHASE_SCALE)
+
+
+def _prius_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
+  return left_value if desired_lateral_accel >= 0.0 else right_value
+
+
+def _prius_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
+  lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / PRIUS_FRICTION_LAT_RISE)
+  jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / PRIUS_FRICTION_JERK_RISE)
+  return _prius_low_speed_factor(v_ego) * lat_factor * jerk_factor
+
+
+def get_prius_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
+  if desired_lateral_accel == 0.0:
+    return 1.0
+
+  gain = _prius_side_value(desired_lateral_accel, PRIUS_FF_GAIN_LEFT, PRIUS_FF_GAIN_RIGHT)
+  abs_lateral_accel = abs(desired_lateral_accel)
+  onset = _prius_sigmoid((abs_lateral_accel - PRIUS_FF_ONSET) / PRIUS_FF_ONSET_WIDTH)
+  cutoff = _prius_sigmoid((PRIUS_FF_CUTOFF - abs_lateral_accel) / PRIUS_FF_CUTOFF_WIDTH)
+  extra_scale = gain * onset * cutoff
+  phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk)
+  turn_in_weight = max(phase, 0.0)
+  unwind_weight = max(-phase, 0.0)
+  low_speed_factor = _prius_low_speed_factor(v_ego)
+  turn_in_boost = 1.0 + (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_BOOST_LEFT, PRIUS_TURN_IN_BOOST_RIGHT) *
+                          turn_in_weight * (0.35 + 0.65 * low_speed_factor))
+  unwind_taper = 1.0 - (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_TAPER_LEFT, PRIUS_UNWIND_TAPER_RIGHT) *
+                         unwind_weight * (0.35 + 0.65 * low_speed_factor))
+  return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))
+
+
+def get_prius_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
+  base_threshold = get_friction_threshold(v_ego)
+  transition_envelope = _prius_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
+  phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk)
+  turn_in_weight = max(phase, 0.0)
+  unwind_weight = max(-phase, 0.0)
+  threshold_scale = 1.0 - (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_THRESHOLD_REDUCTION_LEFT, PRIUS_TURN_IN_THRESHOLD_REDUCTION_RIGHT) *
+                           transition_envelope * turn_in_weight)
+  threshold_scale += (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_THRESHOLD_INCREASE_LEFT, PRIUS_UNWIND_THRESHOLD_INCREASE_RIGHT) *
+                      transition_envelope * unwind_weight)
+  return base_threshold * min(max(threshold_scale, 0.86), 1.16)
+
+
+def get_prius_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
+  transition_envelope = _prius_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
+  phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk)
+  turn_in_weight = max(phase, 0.0)
+  unwind_weight = max(-phase, 0.0)
+  friction_scale = 1.0
+  friction_scale += (_prius_side_value(desired_lateral_accel, PRIUS_TURN_IN_FRICTION_BOOST_LEFT, PRIUS_TURN_IN_FRICTION_BOOST_RIGHT) *
+                     transition_envelope * turn_in_weight)
+  friction_scale -= (_prius_side_value(desired_lateral_accel, PRIUS_UNWIND_FRICTION_REDUCTION_LEFT, PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT) *
+                     transition_envelope * unwind_weight)
+  return min(max(friction_scale, 0.90), 1.14)
+
+
 def civic_bosch_modified_lateral_testing_ground_active() -> bool:
   return testing_ground.use("8", "B")
 
@@ -1169,7 +1275,15 @@ def get_kia_forte_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: f
                          turn_in_weight * (0.35 + 0.65 * low_speed_factor))
   unwind_taper = 1.0 - (_kia_forte_side_value(desired_lateral_accel, KIA_FORTE_UNWIND_TAPER_LEFT, KIA_FORTE_UNWIND_TAPER_RIGHT) *
                         unwind_weight * (0.35 + 0.65 * low_speed_factor))
-  return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)
+  crawl_turn_in_scale = 0.0
+  if desired_lateral_accel * desired_lateral_jerk > 0.0:
+    crawl_speed_weight = _kia_forte_sigmoid((KIA_FORTE_CRAWL_TURN_IN_FF_SPEED - max(v_ego, 0.0)) /
+                                            KIA_FORTE_CRAWL_TURN_IN_FF_SPEED_WIDTH)
+    crawl_lat_weight = _kia_forte_sigmoid((abs_lateral_accel - KIA_FORTE_CRAWL_TURN_IN_FF_LAT) /
+                                          KIA_FORTE_CRAWL_TURN_IN_FF_LAT_WIDTH)
+    crawl_turn_in_scale = _kia_forte_side_value(desired_lateral_accel, KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_LEFT,
+                                                KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_RIGHT) * crawl_speed_weight * crawl_lat_weight
+  return ((1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)) + crawl_turn_in_scale
 
 
 def get_kia_forte_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
@@ -1479,7 +1593,15 @@ def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo
                           turn_in_weight * low_speed_factor)
   unwind_taper = 1.0 - (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_TAPER_LEFT, IONIQ_6_UNWIND_TAPER_RIGHT) *
                          unwind_weight * (0.30 + 0.70 * low_speed_factor))
-  return (1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego)
+  crawl_turn_in_scale = 0.0
+  if desired_lateral_accel * desired_lateral_jerk > 0.0:
+    crawl_speed_weight = _ioniq_6_sigmoid((IONIQ_6_CRAWL_TURN_IN_FF_SPEED - max(v_ego, 0.0)) /
+                                          IONIQ_6_CRAWL_TURN_IN_FF_SPEED_WIDTH)
+    crawl_lat_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CRAWL_TURN_IN_FF_LAT) /
+                                        IONIQ_6_CRAWL_TURN_IN_FF_LAT_WIDTH)
+    crawl_turn_in_scale = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_CRAWL_TURN_IN_FF_BOOST_LEFT,
+                                              IONIQ_6_CRAWL_TURN_IN_FF_BOOST_RIGHT) * crawl_speed_weight * crawl_lat_weight
+  return (1.0 + crawl_turn_in_scale + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego)
 
 
 def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
@@ -1748,6 +1870,7 @@ class LatControlTorque(LatControl):
     self.is_volt_standard = CP.carFingerprint in VOLT_STANDARD_CARS
     self.is_genesis_g90 = CP.carFingerprint in GENESIS_G90_CARS
     self.is_palisade = CP.carFingerprint in PALISADE_CARS
+    self.is_prius = CP.carFingerprint in PRIUS_CARS
     self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS
     self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS
     self.is_ioniq_6 = CP.carFingerprint in IONIQ_6_CARS
@@ -1880,6 +2003,7 @@ class LatControlTorque(LatControl):
       volt_standard_test_active = self.is_volt_standard and volt_standard_lateral_testing_ground_active()
       genesis_g90_test_active = self.is_genesis_g90 and genesis_g90_lateral_testing_ground_active()
       palisade_active = self.is_palisade
+      prius_active = self.is_prius
       ioniq_5_active = self.is_ioniq_5
       ioniq_ev_old_active = self.is_ioniq_ev_old
       ioniq_6_active = self.is_ioniq_6
@@ -1922,6 +2046,10 @@ class LatControlTorque(LatControl):
         ff *= get_palisade_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
         friction_threshold = get_palisade_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
         friction_scale = get_palisade_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
+      elif prius_active:
+        ff *= get_prius_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
+        friction_threshold = get_prius_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
+        friction_scale = get_prius_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
       elif ioniq_5_active:
         ff *= get_ioniq_5_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_5_center_taper
         friction_threshold = get_ioniq_5_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)

selfdrive/controls/lib/longitudinal_planner.py

@@ -45,6 +45,14 @@ LEAD_DEPART_CONFIDENT_MAX_GAP = 5.25
 LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.3
 LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA = 0.25
 LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL = 0.2
+RADAR_DEPART_CONFLICT_MAX_EGO_SPEED = 1.6
+RADAR_DEPART_CONFLICT_MIN_RADAR_LATERAL = 1.5
+RADAR_DEPART_CONFLICT_MAX_RADAR_DISTANCE = 18.0
+RADAR_DEPART_CONFLICT_MIN_MODEL_PROB = 0.95
+RADAR_DEPART_CONFLICT_MAX_MODEL_DISTANCE = 18.0
+RADAR_DEPART_CONFLICT_MAX_MODEL_LATERAL = 0.9
+RADAR_DEPART_CONFLICT_MAX_MODEL_LEAD_SPEED = 2.0
+RADAR_DEPART_CONFLICT_MAX_DISTANCE_MISMATCH = 4.0
 LEAD_DEPART_ACCEL_HOLD_TIME = 1.2
 LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED = 1.5
 LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED = 0.6
@@ -186,6 +194,17 @@ LOW_SPEED_FOLLOW_TRANSITION_PREV_ACCEL_MIN = 0.18
 LOW_SPEED_FOLLOW_TRANSITION_TARGET_BRAKE_MIN = -0.18
 LOW_SPEED_FOLLOW_TRANSITION_MAX_BRAKE = 0.14
 LOW_SPEED_FOLLOW_TRANSITION_MIN_BRAKE = 0.08
+CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED = 10.0
+CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_SPEED = 20.0
+CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_MODEL_PROB = 0.85
+CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LEAD_BRAKE = 0.25
+CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED = 1.0
+CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_MIN = 12.0
+CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_GAIN = 0.9
+CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET = 1.15
+CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MIN_CLOSING_SPEED = 1.5
+CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MAX_LEAD_DELTA = 0.25
+CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_ACCEL = 0.18
 
 # Uncertainty-based filter disable thresholds
 UNCERT_SLOPE_TRIG = 0.12  # per second
@@ -237,6 +256,21 @@ MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP = 0.18
 MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP = 0.08
 MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP = 0.16
 MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP = 0.10
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED = 10.0
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_SPEED = MATCHED_FOLLOW_TRANSITION_MIN_SPEED
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN = 0.45
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN = 1.00
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB = 0.98
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE = 0.08
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED = 1.25
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TTC = 18.0
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP = 0.06
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP = 0.10
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP = 0.05
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP = 0.08
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP = 0.06
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET = -0.12
+LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_DELTA_A = 0.12
 NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED = 20.0
 NEAR_DUPLICATE_LEAD_TRANSITION_MIN_MODEL_PROB = 0.95
 NEAR_DUPLICATE_LEAD_TRANSITION_MAX_LEAD_BRAKE = 0.35
@@ -960,6 +994,63 @@ class LongitudinalPlanner:
       lead_accel >= LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL
     )
 
+  @staticmethod
+  def get_centered_model_lead(model_data):
+    try:
+      leads = model_data.leadsV3
+    except Exception:
+      return None
+
+    best_candidate = None
+    for i in range(3):
+      try:
+        lead = leads[i]
+        prob = float(lead.prob)
+        x = float(lead.x[0])
+        y = float(lead.y[0])
+        v = float(lead.v[0])
+      except Exception:
+        continue
+
+      if (
+        prob < RADAR_DEPART_CONFLICT_MIN_MODEL_PROB or
+        x <= 0.0 or
+        x > RADAR_DEPART_CONFLICT_MAX_MODEL_DISTANCE or
+        abs(y) > RADAR_DEPART_CONFLICT_MAX_MODEL_LATERAL or
+        max(v, 0.0) > RADAR_DEPART_CONFLICT_MAX_MODEL_LEAD_SPEED
+      ):
+        continue
+
+      if best_candidate is None or x < best_candidate[0]:
+        best_candidate = (x, y, v, prob)
+
+    return best_candidate
+
+  def has_offcenter_radar_depart_conflict(self, sm):
+    if float(getattr(sm["carState"], "vEgo", 0.0)) > RADAR_DEPART_CONFLICT_MAX_EGO_SPEED:
+      return False
+
+    centered_model_lead = self.get_centered_model_lead(sm["modelV2"])
+    if centered_model_lead is None:
+      return False
+
+    centered_model_dist = float(centered_model_lead[0])
+    for lead in (self.lead_one, self.lead_two):
+      if not lead.status or not bool(getattr(lead, "radar", False)):
+        continue
+
+      lead_dist = float(getattr(lead, "dRel", 0.0))
+      if lead_dist <= 0.0 or lead_dist > RADAR_DEPART_CONFLICT_MAX_RADAR_DISTANCE:
+        continue
+      if abs(float(getattr(lead, "yRel", 0.0))) < RADAR_DEPART_CONFLICT_MIN_RADAR_LATERAL:
+        continue
+      if abs(lead_dist - centered_model_dist) > RADAR_DEPART_CONFLICT_MAX_DISTANCE_MISMATCH:
+        continue
+
+      return True
+
+    return False
+
   def get_lead_depart_accel_floor(self, lead, v_ego, model_desired_accel):
     if lead is None or not lead.status:
       return None
@@ -1074,6 +1165,61 @@ class LongitudinalPlanner:
     ))
     return -cap_decel
 
+  def get_cruise_tracking_lead_accel_cap(self, lead, v_ego, t_follow, current_source, tracking_lead_active):
+    if lead is None or not lead.status or current_source != "cruise":
+      return None
+    if not (CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED <= float(v_ego) <= CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_SPEED):
+      return None
+
+    lead_prob = float(getattr(lead, "modelProb", 1.0 if bool(getattr(lead, "radar", False)) else 0.0))
+    if not bool(getattr(lead, "radar", False)) and lead_prob < CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_MODEL_PROB:
+      return None
+
+    lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
+    if lead_brake > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LEAD_BRAKE:
+      return None
+
+    if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET:
+      return None
+
+    lead_delta = float(lead.vLead) - float(v_ego)
+    if lead_delta > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED:
+      return None
+
+    closing_speed = max(float(v_ego) - float(lead.vLead), 0.0)
+    raw_close_lead = self.raw_close_lead_needs_control(lead, v_ego)
+    unresolved_slow_lead = (
+      closing_speed >= CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MIN_CLOSING_SPEED and
+      lead_delta <= CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MAX_LEAD_DELTA
+    )
+    if not tracking_lead_active and not raw_close_lead and not unresolved_slow_lead:
+      return None
+
+    desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow))
+    gap_error = float(lead.dRel) - desired_gap
+    gap_buffer = max(CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_MIN,
+                     CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_GAIN * float(v_ego))
+    if gap_error > gap_buffer:
+      return None
+
+    base_cap = float(np.interp(
+      lead_delta,
+      [-1.5, -0.5, 0.0, 0.5, CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED],
+      [0.0, 0.04, 0.08, 0.12, 0.16],
+    ))
+
+    if raw_close_lead:
+      base_cap = min(base_cap, float(np.interp(closing_speed, [0.5, 1.5, 3.5], [0.10, 0.05, 0.0])))
+    else:
+      base_cap = min(base_cap, float(np.interp(closing_speed, [0.0, 1.0, 2.0], [0.18, 0.12, 0.06])))
+
+    if gap_error <= 0.0:
+      return max(0.0, base_cap)
+
+    gap_factor = float(np.clip(gap_error / max(gap_buffer, 0.1), 0.0, 1.0))
+    cap = min(CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_ACCEL, base_cap + 0.06 * gap_factor)
+    return max(0.0, cap)
+
   def lead_is_matched_follow_window(self, lead, v_ego, base_t_follow):
     if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED:
       return False
@@ -1215,18 +1361,26 @@ class LongitudinalPlanner:
     ))
     return -max(0.0, cap_decel - relax_decel)
 
-  def get_matched_follow_transition_target(self, lead, v_ego, base_t_follow, prev_output_a_target, output_a_target):
+  def get_matched_follow_transition_target(self, lead, v_ego, base_t_follow, prev_output_a_target, output_a_target,
+                                           current_source, tracking_lead_active):
     if lead is None or not lead.status:
       return None
-    if float(v_ego) < MATCHED_FOLLOW_TRANSITION_MIN_SPEED:
+    low_speed_extension_active = (
+      bool(tracking_lead_active) and
+      current_source == "cruise" and
+      LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED <= float(v_ego) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_SPEED
+    )
+    if float(v_ego) < MATCHED_FOLLOW_TRANSITION_MIN_SPEED and not low_speed_extension_active:
       return None
 
     lead_prob = float(getattr(lead, "modelProb", 0.0))
-    if lead_prob < MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB:
+    min_model_prob = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB
+    if lead_prob < min_model_prob:
       return None
 
     lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
-    if lead_brake > MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE:
+    max_lead_brake = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE
+    if lead_brake > max_lead_brake:
       return None
 
     relative_speed = float(v_ego) - float(lead.vLead)
@@ -1234,49 +1388,65 @@ class LongitudinalPlanner:
       return None
 
     closing_speed = max(0.0, relative_speed)
-    if closing_speed > MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED:
+    max_closing_speed = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED
+    if closing_speed > max_closing_speed:
       return None
 
     ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf")
-    if ttc < MATCHED_FOLLOW_TRANSITION_MIN_TTC:
+    min_ttc = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TTC if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_TTC
+    if ttc < min_ttc:
       return None
 
     actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
     headway_margin = actual_headway - float(base_t_follow)
-    if headway_margin < MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN:
+    min_headway_margin = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN
+    full_headway_margin = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN
+    if headway_margin < min_headway_margin:
       return None
     if actual_headway > float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET:
       return None
 
     target_delta = float(output_a_target) - float(prev_output_a_target)
-    if abs(target_delta) < 1e-3:
+    if low_speed_extension_active:
+      if float(prev_output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET:
+        return None
+      if float(output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET:
+        return None
+      if abs(target_delta) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_DELTA_A:
+        return None
+    elif abs(target_delta) < 1e-3:
       return None
 
     headway_factor = float(np.clip(
-      (headway_margin - MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN) /
+      (headway_margin - min_headway_margin) /
-      max(MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN - MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN, 1e-3),
+      max(full_headway_margin - min_headway_margin, 1e-3),
       0.0,
       1.0,
     ))
 
+    min_positive_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP
+    max_positive_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP
     positive_step = float(np.interp(
       max(float(lead.vLead) - float(v_ego), 0.0),
       [0.0, 1.0],
-      [MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP, MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP],
+      [min_positive_step, max_positive_step],
     ))
+    min_negative_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP
+    max_negative_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP
     negative_step = float(np.interp(
       closing_speed,
-      [0.0, MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED],
+      [0.0, max_closing_speed],
-      [MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP, MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP],
+      [min_negative_step, max_negative_step],
     ))
 
     # The more space we still have, the less abrupt the comfort path should be.
-    positive_step = float(np.interp(headway_factor, [0.0, 1.0], [positive_step, MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP]))
+    positive_step = float(np.interp(headway_factor, [0.0, 1.0], [positive_step, min_positive_step]))
-    negative_step = float(np.interp(headway_factor, [0.0, 1.0], [negative_step, MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP]))
+    negative_step = float(np.interp(headway_factor, [0.0, 1.0], [negative_step, min_negative_step]))
 
     if float(prev_output_a_target) * float(output_a_target) < 0.0:
-      positive_step = min(positive_step, MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP)
+      sign_cross_step = LOW_SPEED_MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP if low_speed_extension_active else MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP
-      negative_step = min(negative_step, MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP)
+      positive_step = min(positive_step, sign_cross_step)
+      negative_step = min(negative_step, sign_cross_step)
 
     lower = float(prev_output_a_target) - negative_step
     upper = float(prev_output_a_target) + positive_step
@@ -1864,7 +2034,8 @@ class LongitudinalPlanner:
 
     standstill_nudge_gap = max(float(getattr(starpilot_toggles, "stop_distance", STOP_DISTANCE)), STOP_DISTANCE) - 0.5
     moving_leads = [lead for lead in (self.lead_one, self.lead_two)
-                    if lead.status and lead.vLead > STANDSTILL_LEAD_NUDGE_MIN_SPEED and lead.dRel >= standstill_nudge_gap]
+                    if lead.status and
+                    lead.vLead > STANDSTILL_LEAD_NUDGE_MIN_SPEED and lead.dRel >= standstill_nudge_gap]
     confident_depart_ready = any(self.is_confident_lead_depart(lead, float(sm['carState'].vEgo))
                                  for lead in (self.lead_one, self.lead_two))
     lead_depart_ready = any(
@@ -1873,14 +2044,17 @@ class LongitudinalPlanner:
       lead.dRel >= standstill_nudge_gap + STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN
       for lead in (self.lead_one, self.lead_two)
     )
+    depart_safety_veto = (not bool(getattr(starpilot_toggles, "radar_takeoffs", False))
+                          and self.has_offcenter_radar_depart_conflict(sm))
 
-    if lead_control_active and sm['carState'].standstill and moving_leads:
+    if lead_control_active and sm['carState'].standstill and moving_leads and not depart_safety_veto:
       output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL)
 
     if (
       lead_control_active and
       sm['carState'].standstill and
       (confident_depart_ready or lead_depart_ready) and
+      not depart_safety_veto and
       not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and
       not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and
       (confident_depart_ready or model_desired_accel >= STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL)
@@ -1889,14 +2063,14 @@ class LongitudinalPlanner:
       output_should_stop = False
       output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL)
 
-    if lead_control_active and lead_depart_ready and not output_should_stop and float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED:
+    if lead_control_active and lead_depart_ready and not depart_safety_veto and not output_should_stop and float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED:
       output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL)
 
-    if output_should_stop or bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) or bool(getattr(sm['starpilotPlan'], 'redLight', False)):
+    if depart_safety_veto or output_should_stop or bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) or bool(getattr(sm['starpilotPlan'], 'redLight', False)):
       self.lead_depart_accel_hold_until = 0.0
 
     lead_depart_accel_floor = None
-    if lead_control_active and not output_should_stop:
+    if lead_control_active and not output_should_stop and not depart_safety_veto:
       lead_depart_accel_floors = [
         floor for floor in (
           self.get_lead_depart_accel_floor(self.lead_one, scene_v_ego, model_desired_accel),
@@ -2042,6 +2216,8 @@ class LongitudinalPlanner:
         effective_t_follow,
         prev_output_a_target,
         output_a_target,
+        self.mpc.source,
+        bool(getattr(sm["starpilotPlan"], "trackingLead", False)),
       )
       if matched_follow_transition_target is not None:
         if matched_follow_transition_target < output_a_target:
@@ -2067,6 +2243,18 @@ class LongitudinalPlanner:
           self.a_desired = max(self.a_desired, near_duplicate_transition_target)
         output_a_target = near_duplicate_transition_target
 
+    if follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active:
+      cruise_tracking_lead_accel_cap = self.get_cruise_tracking_lead_accel_cap(
+        follow_control_lead,
+        scene_v_ego,
+        effective_t_follow,
+        self.mpc.source,
+        tracking_lead,
+      )
+      if cruise_tracking_lead_accel_cap is not None:
+        self.a_desired = min(self.a_desired, cruise_tracking_lead_accel_cap)
+        output_a_target = min(output_a_target, cruise_tracking_lead_accel_cap)
+
     output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1]
     output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max))
 
@@ -2086,6 +2274,12 @@ class LongitudinalPlanner:
       self.a_desired = min(self.a_desired, close_release_hold_cap)
       output_a_target = min(output_a_target, close_release_hold_cap)
 
+    if depart_safety_veto:
+      self.a_desired = min(self.a_desired, 0.0)
+      output_a_target = min(output_a_target, 0.0)
+      if sm['carState'].standstill:
+        output_should_stop = True
+
     if lead_depart_accel_hold_active:
       output_a_target = max(output_a_target, lead_depart_accel_floor)
 

selfdrive/controls/tests/test_latcontrol.py

@@ -43,6 +43,9 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
   get_palisade_ff_scale,
   get_palisade_friction_scale,
   get_palisade_friction_threshold,
+  get_prius_ff_scale,
+  get_prius_friction_scale,
+  get_prius_friction_threshold,
   get_ioniq_5_ff_scale,
   get_ioniq_5_friction_scale,
   get_ioniq_5_friction_threshold,
@@ -293,10 +296,13 @@ class TestLatControl:
     assert steady_left < 1.0
     assert steady_right < steady_left
     assert turn_in_left > steady_left
-    assert turn_in_right == pytest.approx(steady_right)
+    assert turn_in_right > steady_right
     assert unwind_left < steady_left
     assert unwind_right < steady_right
     assert unwind_right > unwind_left
+    assert get_kia_forte_ff_scale(0.30, 0.60, 3.0) > get_kia_forte_ff_scale(0.30, 0.60, 6.0)
+    assert get_kia_forte_ff_scale(0.30, 0.60, 6.0) > get_kia_forte_ff_scale(0.30, 0.60, 12.0)
+    assert get_kia_forte_ff_scale(0.30, -0.60, 3.0) < get_kia_forte_ff_scale(0.30, 0.60, 3.0)
 
   def test_kia_forte_center_taper_curve(self):
     assert get_kia_forte_center_taper_scale(0.0, 30.0) < get_kia_forte_center_taper_scale(0.0, 15.0)
@@ -363,6 +369,41 @@ class TestLatControl:
     assert left_turn_in > right_turn_in > base
     assert base > left_unwind > right_unwind
 
+  def test_prius_ff_scale_curve(self):
+    assert get_prius_ff_scale(0.0, 0.0, 20.0) == 1.0
+    steady_left = get_prius_ff_scale(0.7, 0.0, 8.0)
+    steady_right = get_prius_ff_scale(-0.7, 0.0, 8.0)
+    turn_in_left = get_prius_ff_scale(0.7, 0.8, 8.0)
+    turn_in_right = get_prius_ff_scale(-0.7, -0.8, 8.0)
+    unwind_left = get_prius_ff_scale(0.7, -0.8, 8.0)
+    unwind_right = get_prius_ff_scale(-0.7, 0.8, 8.0)
+    assert steady_left > 1.0
+    assert steady_right > steady_left
+    assert turn_in_left > steady_left
+    assert turn_in_right > steady_right
+    assert unwind_left < steady_left
+    assert unwind_right < steady_right
+    assert unwind_right < unwind_left
+
+  def test_prius_friction_curves(self):
+    base_threshold = get_friction_threshold(12.0)
+    left_turn_in_threshold = get_prius_friction_threshold(6.0, 0.7, 0.8)
+    right_turn_in_threshold = get_prius_friction_threshold(6.0, -0.7, -0.8)
+    left_unwind_threshold = get_prius_friction_threshold(6.0, 0.7, -0.8)
+    right_unwind_threshold = get_prius_friction_threshold(6.0, -0.7, 0.8)
+    assert left_turn_in_threshold < base_threshold
+    assert right_turn_in_threshold < left_turn_in_threshold
+    assert left_unwind_threshold > base_threshold
+    assert right_unwind_threshold >= left_unwind_threshold
+
+    base_scale = get_prius_friction_scale(25.0, 0.7, 0.8)
+    left_turn_in_scale = get_prius_friction_scale(6.0, 0.7, 0.8)
+    right_turn_in_scale = get_prius_friction_scale(6.0, -0.7, -0.8)
+    left_unwind_scale = get_prius_friction_scale(6.0, 0.7, -0.8)
+    right_unwind_scale = get_prius_friction_scale(6.0, -0.7, 0.8)
+    assert right_turn_in_scale > left_turn_in_scale > base_scale
+    assert base_scale > left_unwind_scale > right_unwind_scale
+
   def test_ioniq_5_ff_scale_curve(self):
     assert get_ioniq_5_ff_scale(0.0, 0.0, 20.0) == 1.0
     steady_left = get_ioniq_5_ff_scale(0.7, 0.0, 12.0)
@@ -419,6 +460,9 @@ class TestLatControl:
     assert get_ioniq_6_ff_scale(-0.4, -0.7, 8.0) >= get_ioniq_6_ff_scale(-0.4, 0.0, 8.0) >= get_ioniq_6_ff_scale(-0.4, 0.7, 8.0)
     assert get_ioniq_6_ff_scale(-1.2, 0.0, 20.0) < get_ioniq_6_ff_scale(1.2, 0.0, 20.0) < 1.0
     assert get_ioniq_6_ff_scale(-1.2, 0.7, 20.0) <= get_ioniq_6_ff_scale(-1.2, 0.0, 20.0)
+    assert get_ioniq_6_ff_scale(0.30, 0.60, 3.0) > get_ioniq_6_ff_scale(0.30, 0.60, 6.0)
+    assert get_ioniq_6_ff_scale(0.30, 0.60, 6.0) > get_ioniq_6_ff_scale(0.30, 0.60, 12.0)
+    assert get_ioniq_6_ff_scale(0.30, -0.60, 3.0) < get_ioniq_6_ff_scale(0.30, 0.60, 3.0)
 
   def test_ioniq_6_directional_taper_curve(self):
     assert get_ioniq_6_directional_taper_scale(0.0, 0.0) == 1.0
@@ -433,6 +477,14 @@ class TestLatControl:
     assert get_ioniq_6_directional_taper_scale(-1.2, -0.40, 8.0) > get_ioniq_6_directional_taper_scale(-1.2, -0.40, 25.0)
     assert get_ioniq_6_directional_taper_scale(1.2, 0.40, 8.0) > get_ioniq_6_directional_taper_scale(1.2, 0.40, 25.0)
     assert get_ioniq_6_directional_taper_scale(-1.2, 0.7, 8.0) == pytest.approx(get_ioniq_6_directional_taper_scale(-1.2, 0.7, 25.0), abs=0.02)
+    assert get_ioniq_6_directional_taper_scale(-0.18, -0.40, 3.0) > get_ioniq_6_directional_taper_scale(-0.18, -0.40, 9.0)
+    assert get_ioniq_6_directional_taper_scale(-0.18, -0.40, 9.0) > get_ioniq_6_directional_taper_scale(-0.18, -0.40, 20.0)
+    assert get_ioniq_6_directional_taper_scale(-0.50, -0.40, 3.0) > get_ioniq_6_directional_taper_scale(-0.50, -0.40, 6.0)
+    assert get_ioniq_6_directional_taper_scale(-0.50, -0.40, 6.0) > get_ioniq_6_directional_taper_scale(-0.50, -0.40, 9.0)
+    assert get_ioniq_6_directional_taper_scale(-0.50, -0.40, 9.0) > get_ioniq_6_directional_taper_scale(-0.50, -0.40, 20.0)
+    assert get_ioniq_6_directional_taper_scale(-0.70, -0.70, 6.0) > get_ioniq_6_directional_taper_scale(-0.70, -0.70, 12.0)
+    assert get_ioniq_6_directional_taper_scale(-0.70, -0.70, 12.0) > get_ioniq_6_directional_taper_scale(-0.70, -0.70, 20.0)
+    assert get_ioniq_6_directional_taper_scale(0.30, 0.60, 5.0) > get_ioniq_6_directional_taper_scale(0.30, 0.60, 12.0)
 
   def test_ioniq_6_output_taper_curve(self):
     assert get_ioniq_6_output_taper_scale(0.0, 0.0, 25.0) < get_ioniq_6_output_taper_scale(0.0, 0.0, 8.0) <= 1.0

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -17,7 +17,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 
 
 def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead: float = 0.0,
-              radar: bool = False, model_prob: float = 0.0):
+              radar: bool = False, model_prob: float = 0.0, y_rel: float = 0.0):
   lead = log.RadarState.LeadData.new_message()
   lead.status = status
   lead.dRel = d_rel
@@ -26,6 +26,7 @@ def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead
   lead.aLeadK = a_lead
   lead.vRel = 0.0
   lead.aRel = 0.0
+  lead.yRel = y_rel
   lead.modelProb = model_prob
   lead.radar = radar
   return lead
@@ -33,6 +34,7 @@ def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead
 
 def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0, brake_press_prob: float = 0.0):
   model = log.ModelDataV2.new_message()
+  model.init('leadsV3', 3)
   t_idxs = ModelConstants.T_IDXS
 
   model.position.x = [float(v_ego * t) for t in t_idxs]
@@ -57,6 +59,15 @@ def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0,
   return model
 
 
+def set_model_lead(model, idx: int, *, prob: float, x0: float, y0: float, v0: float, a0: float = 0.0):
+  lead = model.leadsV3[idx]
+  lead.prob = float(prob)
+  lead.x = [float(x0)]
+  lead.y = [float(y0)]
+  lead.v = [float(v0)]
+  lead.a = [float(a0)]
+
+
 def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimental_mode: bool = True,
             tracking_lead: bool = False, lead_one=None, lead_two=None,
             gas_press_prob: float = 1.0, brake_press_prob: float = 0.0, disable_throttle: bool = False):
@@ -100,7 +111,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta
   }
 
 
-def make_toggles(model_version: str = "v11"):
+def make_toggles(model_version: str = "v11", radar_takeoffs: bool = False):
   return SimpleNamespace(
     taco_tune=False,
     classic_model=False,
@@ -108,6 +119,7 @@ def make_toggles(model_version: str = "v11"):
     model_version=model_version,
     stop_distance=6.0,
     vEgoStopping=0.5,
+    radar_takeoffs=radar_takeoffs,
   )
 
 
@@ -1232,6 +1244,126 @@ def test_standstill_moving_lead_depart_accel_hold_cancels_if_lead_brakes(model_v
   assert planner.output_a_target < 0.1
 
 
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
+def test_standstill_radar_depart_kept_when_radar_lead_is_centered(model_version):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+
+  sm = make_sm(
+    0.0,
+    desired_accel=0.45,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=False,
+    lead_one=make_lead(status=True, d_rel=11.2, v_lead=0.63, a_lead=0.36, radar=True, model_prob=0.998, y_rel=0.2),
+  )
+  sm["carState"].standstill = True
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  sm["starpilotPlan"].vCruise = 10.0
+  sm["modelV2"].action.shouldStop = False
+  set_model_lead(sm["modelV2"], 0, prob=0.999, x0=12.2, y0=0.03, v0=0.4)
+
+  planner.update(sm, make_toggles(model_version))
+
+  assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL
+
+
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
+def test_standstill_radar_depart_blocks_offcenter_radar_conflict(model_version):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+
+  sm = make_sm(
+    0.0,
+    desired_accel=0.45,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=False,
+    lead_one=make_lead(status=True, d_rel=11.2, v_lead=0.63, a_lead=0.36, radar=True, model_prob=0.998, y_rel=2.3),
+  )
+  sm["carState"].standstill = True
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  sm["starpilotPlan"].vCruise = 10.0
+  sm["modelV2"].action.shouldStop = False
+  set_model_lead(sm["modelV2"], 0, prob=0.999, x0=12.2, y0=0.03, v0=0.4)
+
+  planner.update(sm, make_toggles(model_version))
+
+  assert planner.output_a_target < longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL
+
+
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
+def test_low_speed_radar_depart_hold_blocks_offcenter_radar_conflict(model_version):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=1.25)
+
+  sm = make_sm(
+    1.25,
+    desired_accel=0.20,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=False,
+    lead_one=make_lead(status=True, d_rel=9.95, v_lead=0.43, a_lead=0.44, radar=True, model_prob=0.999, y_rel=2.2),
+  )
+  sm["carState"].standstill = False
+  sm["controlsState"].longControlState = LongCtrlState.pid
+  sm["starpilotPlan"].vCruise = 10.0
+  sm["modelV2"].action.shouldStop = False
+  set_model_lead(sm["modelV2"], 0, prob=0.999, x0=11.4, y0=0.0, v0=0.2)
+
+  planner.update(sm, make_toggles(model_version))
+
+  assert planner.output_a_target < longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL
+
+
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
+def test_standstill_radar_takeoffs_toggle_bypasses_offcenter_veto(model_version):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+
+  sm = make_sm(
+    0.0,
+    desired_accel=0.45,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=False,
+    lead_one=make_lead(status=True, d_rel=11.2, v_lead=0.63, a_lead=0.36, radar=True, model_prob=0.998, y_rel=2.3),
+  )
+  sm["carState"].standstill = True
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  sm["starpilotPlan"].vCruise = 10.0
+  sm["modelV2"].action.shouldStop = False
+  set_model_lead(sm["modelV2"], 0, prob=0.999, x0=12.2, y0=0.03, v0=0.4)
+
+  planner.update(sm, make_toggles(model_version, radar_takeoffs=True))
+
+  assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL
+
+
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
+def test_low_speed_radar_takeoffs_toggle_bypasses_offcenter_veto(model_version):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=1.25)
+
+  sm = make_sm(
+    1.25,
+    desired_accel=0.20,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=False,
+    lead_one=make_lead(status=True, d_rel=9.95, v_lead=0.43, a_lead=0.44, radar=True, model_prob=0.999, y_rel=2.2),
+  )
+  sm["carState"].standstill = False
+  sm["controlsState"].longControlState = LongCtrlState.pid
+  sm["starpilotPlan"].vCruise = 10.0
+  sm["modelV2"].action.shouldStop = False
+  set_model_lead(sm["modelV2"], 0, prob=0.999, x0=11.4, y0=0.0, v0=0.2)
+
+  planner.update(sm, make_toggles(model_version, radar_takeoffs=True))
+
+  assert planner.output_a_target >= 0.0
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
 def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version):
   far_v_ego = 29.26
@@ -1778,6 +1910,8 @@ def test_matched_follow_transition_target_damps_large_comfort_sign_flip():
     1.45,
     prev_output_a_target=0.12,
     output_a_target=-0.40,
+    current_source="cruise",
+    tracking_lead_active=True,
   )
 
   assert smoothed is not None
@@ -1797,11 +1931,124 @@ def test_matched_follow_transition_target_skips_urgent_closure():
     1.45,
     prev_output_a_target=0.10,
     output_a_target=-0.60,
+    current_source="cruise",
+    tracking_lead_active=True,
   )
 
   assert smoothed is None
 
 
+def test_matched_follow_transition_target_damps_low_speed_tracking_cruise_throttle_jitter():
+  v_ego = 14.5
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=31.1, v_lead=14.5, a_lead=0.0, radar=False, model_prob=0.999)
+
+  smoothed = planner.get_matched_follow_transition_target(
+    lead,
+    v_ego,
+    1.45,
+    prev_output_a_target=0.08,
+    output_a_target=0.46,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  assert smoothed is not None
+  assert smoothed == pytest.approx(0.14, abs=1e-6)
+
+
+def test_matched_follow_transition_target_skips_low_speed_without_tracking():
+  v_ego = 14.5
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=31.1, v_lead=14.5, a_lead=0.0, radar=False, model_prob=0.999)
+
+  smoothed = planner.get_matched_follow_transition_target(
+    lead,
+    v_ego,
+    1.45,
+    prev_output_a_target=0.08,
+    output_a_target=0.46,
+    current_source="cruise",
+    tracking_lead_active=False,
+  )
+
+  assert smoothed is None
+
+
+def test_matched_follow_transition_target_skips_low_speed_real_braking():
+  v_ego = 14.5
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=29.0, v_lead=13.6, a_lead=0.0, radar=False, model_prob=0.999)
+
+  smoothed = planner.get_matched_follow_transition_target(
+    lead,
+    v_ego,
+    1.45,
+    prev_output_a_target=0.08,
+    output_a_target=-0.30,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  assert smoothed is None
+
+
+def test_cruise_tracking_lead_accel_cap_limits_mid_speed_follow_nibble():
+  v_ego = 16.2
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=33.4, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.12)
+
+  cap = planner.get_cruise_tracking_lead_accel_cap(
+    lead,
+    v_ego,
+    1.45,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  assert cap is not None
+  assert 0.05 <= cap <= 0.10
+
+
+def test_cruise_tracking_lead_accel_cap_blocks_unresolved_raw_close_lead_burst():
+  v_ego = 17.6
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=41.9, v_lead=14.2, a_lead=0.0, radar=True, model_prob=0.99, y_rel=-0.97)
+
+  cap = planner.get_cruise_tracking_lead_accel_cap(
+    lead,
+    v_ego,
+    1.45,
+    current_source="cruise",
+    tracking_lead_active=False,
+  )
+
+  assert cap is not None
+  assert 0.0 <= cap <= 0.05
+
+
+def test_cruise_tracking_lead_accel_cap_skips_when_lead_clearly_pulls_away():
+  v_ego = 14.5
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=35.0, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.1)
+
+  cap = planner.get_cruise_tracking_lead_accel_cap(
+    lead,
+    v_ego,
+    1.45,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  assert cap is None
+
+
 def test_near_duplicate_lead_source_hysteresis_prefers_previous_source():
   v_ego = 27.0
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)

selfdrive/controls/tests/test_speed_limit_controller.py

@@ -114,11 +114,129 @@ def test_new_source_limit_clears_override_until_gas_release():
 
     assert controller.overridden_speed == pytest.approx(mph(65))
     assert controller.override_slc
+
+    # --- Dropout / Fallback Test Condition ---
+    controller.starpilot_toggles = make_toggles(slc_fallback_set_speed=True)
+    # No limit available → falls back to v_cruise (75 mph) with source "None".
+    # Override persists because target_to_use resolves to last_valid_limit (45 mph) which is
+    # below overridden_speed (65 mph) — the sticky override_slc chain stays True.
+    controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(65), sm)
+    controller.update_override(mph(75), 0.0, mph(65), 0.0, sm)
+
+    assert controller.target == pytest.approx(mph(75))
+    assert controller.source == "None"
+    assert controller.overridden_speed == pytest.approx(mph(65))
+    assert controller.override_slc
+
+    # Recovery to a confirmed limit (55 mph) clears the override: this is a genuinely new
+    # speed zone (55 != last_valid 45), so clear_override_for_source_limit fires correctly.
+    controller.update_limits(mph(55), datetime.now(timezone.utc), False, mph(75), mph(65), sm)
+    controller.update_override(mph(75), 0.0, mph(65), 0.0, sm)
+
+    assert controller.target == pytest.approx(mph(55))
+    assert controller.source == "Dashboard"
+    assert controller.overridden_speed == 0
+    assert not controller.override_slc
+
+      # --- Override Clipping Check (set-speed fallback) ---
+      # Separate controller: active override, then fallback to v_cruise that is BELOW the override.
+      # overridden_speed clips to v_cruise (override_slc stays True via sticky chain, but
+      # np.clip clamps overridden_speed to the new target+offset).
+    clip_controller = make_controller(slc_fallback_set_speed=True)
+    try:
+      clip_controller.source = "Dashboard"
+      clip_controller.target = mph(55)
+      clip_controller.previous_source = "Dashboard"
+      clip_controller.previous_target = mph(55)
+      clip_controller.last_valid_limit = mph(55)
+      clip_controller.override_slc = True
+      clip_controller.overridden_speed = mph(65)
+
+      sm_no_gas = make_sm(gas_pressed=False)
+      # v_cruise = 30 mph (below last_valid 55), so target_to_use returns mph(30).
+      # override_slc sticky: overridden_speed=65 > 30+0=30 > 0 — still True from chain.
+      # np.clip(65, 30, 30) = 30, so overridden_speed clips to mph(30).
+      clip_controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(30), mph(30), sm_no_gas)
+      clip_controller.update_override(mph(30), 0.0, mph(30), 0.0, sm_no_gas)
+
+      assert clip_controller.target == pytest.approx(mph(30))
+      # Clipped to v_cruise — not locked at mph(55) or mph(65)
+      assert clip_controller.overridden_speed == pytest.approx(mph(30))
+      assert clip_controller.override_slc
+    finally:
+      clip_controller.shutdown()
+
+    # --- Lost Speed Limit (no fallback) clears target to 0 ---
+    # When all limit sources drop to 0 with no fallback, target becomes 0
+    # and override_slc is False (target_to_use=0, chain evaluates False).
+    lost_controller = make_controller(slc_fallback_set_speed=False, slc_fallback_previous_speed_limit=False)
+    try:
+      lost_controller.source = "Dashboard"
+      lost_controller.target = mph(45)
+      lost_controller.previous_source = "Dashboard"
+      lost_controller.previous_target = mph(45)
+
+      sm_on = make_sm(gas_pressed=False)
+      lost_controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(65), sm_on)
+      lost_controller.update_override(mph(75), 0.0, mph(65), 0.0, sm_on)
+
+      assert lost_controller.target == 0
+      assert lost_controller.overridden_speed == 0
+      assert not lost_controller.override_slc
+    finally:
+      lost_controller.shutdown()
   finally:
     controller.shutdown()
 
 
 def test_unconfirmed_lower_limit_keeps_existing_override():
+  # First, verify startup behavior where target is 0 and priority limit is detected
+  startup_controller = make_controller(
+    speed_limit_priority1="Dashboard",
+    slc_fallback_previous_speed_limit=True,
+  )
+  try:
+    startup_controller.previous_target = mph(55)
+    startup_controller.previous_source = "Dashboard"
+    startup_controller.target = 0
+
+    sm = make_sm(gas_pressed=False)
+    startup_controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(65), sm)
+
+    assert startup_controller.target == pytest.approx(mph(45))
+    assert startup_controller.source == "Dashboard"
+  finally:
+    startup_controller.shutdown()
+
+  # Verify Bug 3: Fallback transitions should bypass confirmation checks
+  fallback_confirm_controller = make_controller(
+    slc_fallback_set_speed=True,
+    speed_limit_confirmation_higher=True
+  )
+  try:
+    fallback_confirm_controller.source = "Dashboard"
+    fallback_confirm_controller.target = mph(35)
+    fallback_confirm_controller.previous_target = mph(35)
+
+    sm = make_sm(gas_pressed=False)
+    fallback_confirm_controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(60), mph(35), sm)
+
+    assert fallback_confirm_controller.target == pytest.approx(mph(60))
+    assert fallback_confirm_controller.source == "None"
+    assert fallback_confirm_controller.unconfirmed_speed_limit == 0
+  finally:
+    fallback_confirm_controller.shutdown()
+
+  # Verify Bug 1: Boundaries are correctly mapped and not falling back to 0
+  boundary_controller = make_controller()
+  boundary_controller.starpilot_toggles.speed_limit_offset1 = 1.0
+  boundary_controller.starpilot_toggles.speed_limit_offset2 = 2.0
+
+  # Exact boundary speed: 11.2 m/s is the *start* of band 2 (25–34 mph range).
+  # With low <= target < high: 11.2 <= 11.2 < 15.2 → True → maps to offset2 (not 0).
+  offset = boundary_controller.get_offset(11.2)
+  assert offset != 0.0
+
   controller = make_controller(speed_limit_confirmation_lower=True)
   try:
     controller.source = "Dashboard"

selfdrive/locationd/models/generated/car.cpp

@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
  *                                                                            *
  *                         This file is part of 'ekf'                         *
  ******************************************************************************/
-void err_fun(double *nom_x, double *delta_x, double *out_6333242559483084265) {
+void err_fun(double *nom_x, double *delta_x, double *out_2765937094321424100) {
-   out_6333242559483084265[0] = delta_x[0] + nom_x[0];
+   out_2765937094321424100[0] = delta_x[0] + nom_x[0];
-   out_6333242559483084265[1] = delta_x[1] + nom_x[1];
+   out_2765937094321424100[1] = delta_x[1] + nom_x[1];
-   out_6333242559483084265[2] = delta_x[2] + nom_x[2];
+   out_2765937094321424100[2] = delta_x[2] + nom_x[2];
-   out_6333242559483084265[3] = delta_x[3] + nom_x[3];
+   out_2765937094321424100[3] = delta_x[3] + nom_x[3];
-   out_6333242559483084265[4] = delta_x[4] + nom_x[4];
+   out_2765937094321424100[4] = delta_x[4] + nom_x[4];
-   out_6333242559483084265[5] = delta_x[5] + nom_x[5];
+   out_2765937094321424100[5] = delta_x[5] + nom_x[5];
-   out_6333242559483084265[6] = delta_x[6] + nom_x[6];
+   out_2765937094321424100[6] = delta_x[6] + nom_x[6];
-   out_6333242559483084265[7] = delta_x[7] + nom_x[7];
+   out_2765937094321424100[7] = delta_x[7] + nom_x[7];
-   out_6333242559483084265[8] = delta_x[8] + nom_x[8];
+   out_2765937094321424100[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_829248915283082072) {
+void inv_err_fun(double *nom_x, double *true_x, double *out_1382255689110899984) {
-   out_829248915283082072[0] = -nom_x[0] + true_x[0];
+   out_1382255689110899984[0] = -nom_x[0] + true_x[0];
-   out_829248915283082072[1] = -nom_x[1] + true_x[1];
+   out_1382255689110899984[1] = -nom_x[1] + true_x[1];
-   out_829248915283082072[2] = -nom_x[2] + true_x[2];
+   out_1382255689110899984[2] = -nom_x[2] + true_x[2];
-   out_829248915283082072[3] = -nom_x[3] + true_x[3];
+   out_1382255689110899984[3] = -nom_x[3] + true_x[3];
-   out_829248915283082072[4] = -nom_x[4] + true_x[4];
+   out_1382255689110899984[4] = -nom_x[4] + true_x[4];
-   out_829248915283082072[5] = -nom_x[5] + true_x[5];
+   out_1382255689110899984[5] = -nom_x[5] + true_x[5];
-   out_829248915283082072[6] = -nom_x[6] + true_x[6];
+   out_1382255689110899984[6] = -nom_x[6] + true_x[6];
-   out_829248915283082072[7] = -nom_x[7] + true_x[7];
+   out_1382255689110899984[7] = -nom_x[7] + true_x[7];
-   out_829248915283082072[8] = -nom_x[8] + true_x[8];
+   out_1382255689110899984[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_8283696179185346057) {
+void H_mod_fun(double *state, double *out_6175643942212596402) {
-   out_8283696179185346057[0] = 1.0;
+   out_6175643942212596402[0] = 1.0;
-   out_8283696179185346057[1] = 0.0;
+   out_6175643942212596402[1] = 0.0;
-   out_8283696179185346057[2] = 0.0;
+   out_6175643942212596402[2] = 0.0;
-   out_8283696179185346057[3] = 0.0;
+   out_6175643942212596402[3] = 0.0;
-   out_8283696179185346057[4] = 0.0;
+   out_6175643942212596402[4] = 0.0;
-   out_8283696179185346057[5] = 0.0;
+   out_6175643942212596402[5] = 0.0;
-   out_8283696179185346057[6] = 0.0;
+   out_6175643942212596402[6] = 0.0;
-   out_8283696179185346057[7] = 0.0;
+   out_6175643942212596402[7] = 0.0;
-   out_8283696179185346057[8] = 0.0;
+   out_6175643942212596402[8] = 0.0;
-   out_8283696179185346057[9] = 0.0;
+   out_6175643942212596402[9] = 0.0;
-   out_8283696179185346057[10] = 1.0;
+   out_6175643942212596402[10] = 1.0;
-   out_8283696179185346057[11] = 0.0;
+   out_6175643942212596402[11] = 0.0;
-   out_8283696179185346057[12] = 0.0;
+   out_6175643942212596402[12] = 0.0;
-   out_8283696179185346057[13] = 0.0;
+   out_6175643942212596402[13] = 0.0;
-   out_8283696179185346057[14] = 0.0;
+   out_6175643942212596402[14] = 0.0;
-   out_8283696179185346057[15] = 0.0;
+   out_6175643942212596402[15] = 0.0;
-   out_8283696179185346057[16] = 0.0;
+   out_6175643942212596402[16] = 0.0;
-   out_8283696179185346057[17] = 0.0;
+   out_6175643942212596402[17] = 0.0;
-   out_8283696179185346057[18] = 0.0;
+   out_6175643942212596402[18] = 0.0;
-   out_8283696179185346057[19] = 0.0;
+   out_6175643942212596402[19] = 0.0;
-   out_8283696179185346057[20] = 1.0;
+   out_6175643942212596402[20] = 1.0;
-   out_8283696179185346057[21] = 0.0;
+   out_6175643942212596402[21] = 0.0;
-   out_8283696179185346057[22] = 0.0;
+   out_6175643942212596402[22] = 0.0;
-   out_8283696179185346057[23] = 0.0;
+   out_6175643942212596402[23] = 0.0;
-   out_8283696179185346057[24] = 0.0;
+   out_6175643942212596402[24] = 0.0;
-   out_8283696179185346057[25] = 0.0;
+   out_6175643942212596402[25] = 0.0;
-   out_8283696179185346057[26] = 0.0;
+   out_6175643942212596402[26] = 0.0;
-   out_8283696179185346057[27] = 0.0;
+   out_6175643942212596402[27] = 0.0;
-   out_8283696179185346057[28] = 0.0;
+   out_6175643942212596402[28] = 0.0;
-   out_8283696179185346057[29] = 0.0;
+   out_6175643942212596402[29] = 0.0;
-   out_8283696179185346057[30] = 1.0;
+   out_6175643942212596402[30] = 1.0;
-   out_8283696179185346057[31] = 0.0;
+   out_6175643942212596402[31] = 0.0;
-   out_8283696179185346057[32] = 0.0;
+   out_6175643942212596402[32] = 0.0;
-   out_8283696179185346057[33] = 0.0;
+   out_6175643942212596402[33] = 0.0;
-   out_8283696179185346057[34] = 0.0;
+   out_6175643942212596402[34] = 0.0;
-   out_8283696179185346057[35] = 0.0;
+   out_6175643942212596402[35] = 0.0;
-   out_8283696179185346057[36] = 0.0;
+   out_6175643942212596402[36] = 0.0;
-   out_8283696179185346057[37] = 0.0;
+   out_6175643942212596402[37] = 0.0;
-   out_8283696179185346057[38] = 0.0;
+   out_6175643942212596402[38] = 0.0;
-   out_8283696179185346057[39] = 0.0;
+   out_6175643942212596402[39] = 0.0;
-   out_8283696179185346057[40] = 1.0;
+   out_6175643942212596402[40] = 1.0;
-   out_8283696179185346057[41] = 0.0;
+   out_6175643942212596402[41] = 0.0;
-   out_8283696179185346057[42] = 0.0;
+   out_6175643942212596402[42] = 0.0;
-   out_8283696179185346057[43] = 0.0;
+   out_6175643942212596402[43] = 0.0;
-   out_8283696179185346057[44] = 0.0;
+   out_6175643942212596402[44] = 0.0;
-   out_8283696179185346057[45] = 0.0;
+   out_6175643942212596402[45] = 0.0;
-   out_8283696179185346057[46] = 0.0;
+   out_6175643942212596402[46] = 0.0;
-   out_8283696179185346057[47] = 0.0;
+   out_6175643942212596402[47] = 0.0;
-   out_8283696179185346057[48] = 0.0;
+   out_6175643942212596402[48] = 0.0;
-   out_8283696179185346057[49] = 0.0;
+   out_6175643942212596402[49] = 0.0;
-   out_8283696179185346057[50] = 1.0;
+   out_6175643942212596402[50] = 1.0;
-   out_8283696179185346057[51] = 0.0;
+   out_6175643942212596402[51] = 0.0;
-   out_8283696179185346057[52] = 0.0;
+   out_6175643942212596402[52] = 0.0;
-   out_8283696179185346057[53] = 0.0;
+   out_6175643942212596402[53] = 0.0;
-   out_8283696179185346057[54] = 0.0;
+   out_6175643942212596402[54] = 0.0;
-   out_8283696179185346057[55] = 0.0;
+   out_6175643942212596402[55] = 0.0;
-   out_8283696179185346057[56] = 0.0;
+   out_6175643942212596402[56] = 0.0;
-   out_8283696179185346057[57] = 0.0;
+   out_6175643942212596402[57] = 0.0;
-   out_8283696179185346057[58] = 0.0;
+   out_6175643942212596402[58] = 0.0;
-   out_8283696179185346057[59] = 0.0;
+   out_6175643942212596402[59] = 0.0;
-   out_8283696179185346057[60] = 1.0;
+   out_6175643942212596402[60] = 1.0;
-   out_8283696179185346057[61] = 0.0;
+   out_6175643942212596402[61] = 0.0;
-   out_8283696179185346057[62] = 0.0;
+   out_6175643942212596402[62] = 0.0;
-   out_8283696179185346057[63] = 0.0;
+   out_6175643942212596402[63] = 0.0;
-   out_8283696179185346057[64] = 0.0;
+   out_6175643942212596402[64] = 0.0;
-   out_8283696179185346057[65] = 0.0;
+   out_6175643942212596402[65] = 0.0;
-   out_8283696179185346057[66] = 0.0;
+   out_6175643942212596402[66] = 0.0;
-   out_8283696179185346057[67] = 0.0;
+   out_6175643942212596402[67] = 0.0;
-   out_8283696179185346057[68] = 0.0;
+   out_6175643942212596402[68] = 0.0;
-   out_8283696179185346057[69] = 0.0;
+   out_6175643942212596402[69] = 0.0;
-   out_8283696179185346057[70] = 1.0;
+   out_6175643942212596402[70] = 1.0;
-   out_8283696179185346057[71] = 0.0;
+   out_6175643942212596402[71] = 0.0;
-   out_8283696179185346057[72] = 0.0;
+   out_6175643942212596402[72] = 0.0;
-   out_8283696179185346057[73] = 0.0;
+   out_6175643942212596402[73] = 0.0;
-   out_8283696179185346057[74] = 0.0;
+   out_6175643942212596402[74] = 0.0;
-   out_8283696179185346057[75] = 0.0;
+   out_6175643942212596402[75] = 0.0;
-   out_8283696179185346057[76] = 0.0;
+   out_6175643942212596402[76] = 0.0;
-   out_8283696179185346057[77] = 0.0;
+   out_6175643942212596402[77] = 0.0;
-   out_8283696179185346057[78] = 0.0;
+   out_6175643942212596402[78] = 0.0;
-   out_8283696179185346057[79] = 0.0;
+   out_6175643942212596402[79] = 0.0;
-   out_8283696179185346057[80] = 1.0;
+   out_6175643942212596402[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_3453347444055346076) {
+void f_fun(double *state, double dt, double *out_2437881867604815540) {
-   out_3453347444055346076[0] = state[0];
+   out_2437881867604815540[0] = state[0];
-   out_3453347444055346076[1] = state[1];
+   out_2437881867604815540[1] = state[1];
-   out_3453347444055346076[2] = state[2];
+   out_2437881867604815540[2] = state[2];
-   out_3453347444055346076[3] = state[3];
+   out_2437881867604815540[3] = state[3];
-   out_3453347444055346076[4] = state[4];
+   out_2437881867604815540[4] = state[4];
-   out_3453347444055346076[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
+   out_2437881867604815540[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
-   out_3453347444055346076[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
+   out_2437881867604815540[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
-   out_3453347444055346076[7] = state[7];
+   out_2437881867604815540[7] = state[7];
-   out_3453347444055346076[8] = state[8];
+   out_2437881867604815540[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_1825222330008905672) {
+void F_fun(double *state, double dt, double *out_3645925778664752399) {
-   out_1825222330008905672[0] = 1;
+   out_3645925778664752399[0] = 1;
-   out_1825222330008905672[1] = 0;
+   out_3645925778664752399[1] = 0;
-   out_1825222330008905672[2] = 0;
+   out_3645925778664752399[2] = 0;
-   out_1825222330008905672[3] = 0;
+   out_3645925778664752399[3] = 0;
-   out_1825222330008905672[4] = 0;
+   out_3645925778664752399[4] = 0;
-   out_1825222330008905672[5] = 0;
+   out_3645925778664752399[5] = 0;
-   out_1825222330008905672[6] = 0;
+   out_3645925778664752399[6] = 0;
-   out_1825222330008905672[7] = 0;
+   out_3645925778664752399[7] = 0;
-   out_1825222330008905672[8] = 0;
+   out_3645925778664752399[8] = 0;
-   out_1825222330008905672[9] = 0;
+   out_3645925778664752399[9] = 0;
-   out_1825222330008905672[10] = 1;
+   out_3645925778664752399[10] = 1;
-   out_1825222330008905672[11] = 0;
+   out_3645925778664752399[11] = 0;
-   out_1825222330008905672[12] = 0;
+   out_3645925778664752399[12] = 0;
-   out_1825222330008905672[13] = 0;
+   out_3645925778664752399[13] = 0;
-   out_1825222330008905672[14] = 0;
+   out_3645925778664752399[14] = 0;
-   out_1825222330008905672[15] = 0;
+   out_3645925778664752399[15] = 0;
-   out_1825222330008905672[16] = 0;
+   out_3645925778664752399[16] = 0;
-   out_1825222330008905672[17] = 0;
+   out_3645925778664752399[17] = 0;
-   out_1825222330008905672[18] = 0;
+   out_3645925778664752399[18] = 0;
-   out_1825222330008905672[19] = 0;
+   out_3645925778664752399[19] = 0;
-   out_1825222330008905672[20] = 1;
+   out_3645925778664752399[20] = 1;
-   out_1825222330008905672[21] = 0;
+   out_3645925778664752399[21] = 0;
-   out_1825222330008905672[22] = 0;
+   out_3645925778664752399[22] = 0;
-   out_1825222330008905672[23] = 0;
+   out_3645925778664752399[23] = 0;
-   out_1825222330008905672[24] = 0;
+   out_3645925778664752399[24] = 0;
-   out_1825222330008905672[25] = 0;
+   out_3645925778664752399[25] = 0;
-   out_1825222330008905672[26] = 0;
+   out_3645925778664752399[26] = 0;
-   out_1825222330008905672[27] = 0;
+   out_3645925778664752399[27] = 0;
-   out_1825222330008905672[28] = 0;
+   out_3645925778664752399[28] = 0;
-   out_1825222330008905672[29] = 0;
+   out_3645925778664752399[29] = 0;
-   out_1825222330008905672[30] = 1;
+   out_3645925778664752399[30] = 1;
-   out_1825222330008905672[31] = 0;
+   out_3645925778664752399[31] = 0;
-   out_1825222330008905672[32] = 0;
+   out_3645925778664752399[32] = 0;
-   out_1825222330008905672[33] = 0;
+   out_3645925778664752399[33] = 0;
-   out_1825222330008905672[34] = 0;
+   out_3645925778664752399[34] = 0;
-   out_1825222330008905672[35] = 0;
+   out_3645925778664752399[35] = 0;
-   out_1825222330008905672[36] = 0;
+   out_3645925778664752399[36] = 0;
-   out_1825222330008905672[37] = 0;
+   out_3645925778664752399[37] = 0;
-   out_1825222330008905672[38] = 0;
+   out_3645925778664752399[38] = 0;
-   out_1825222330008905672[39] = 0;
+   out_3645925778664752399[39] = 0;
-   out_1825222330008905672[40] = 1;
+   out_3645925778664752399[40] = 1;
-   out_1825222330008905672[41] = 0;
+   out_3645925778664752399[41] = 0;
-   out_1825222330008905672[42] = 0;
+   out_3645925778664752399[42] = 0;
-   out_1825222330008905672[43] = 0;
+   out_3645925778664752399[43] = 0;
-   out_1825222330008905672[44] = 0;
+   out_3645925778664752399[44] = 0;
-   out_1825222330008905672[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
+   out_3645925778664752399[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
-   out_1825222330008905672[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_3645925778664752399[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_1825222330008905672[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_3645925778664752399[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_1825222330008905672[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_3645925778664752399[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_1825222330008905672[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
+   out_3645925778664752399[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
-   out_1825222330008905672[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_3645925778664752399[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_1825222330008905672[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_3645925778664752399[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_1825222330008905672[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_3645925778664752399[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_1825222330008905672[53] = -9.8100000000000005*dt;
+   out_3645925778664752399[53] = -9.8100000000000005*dt;
-   out_1825222330008905672[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
+   out_3645925778664752399[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
-   out_1825222330008905672[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_3645925778664752399[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_1825222330008905672[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_3645925778664752399[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_1825222330008905672[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_3645925778664752399[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_1825222330008905672[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
+   out_3645925778664752399[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
-   out_1825222330008905672[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_3645925778664752399[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_1825222330008905672[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
+   out_3645925778664752399[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
-   out_1825222330008905672[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_3645925778664752399[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_1825222330008905672[62] = 0;
+   out_3645925778664752399[62] = 0;
-   out_1825222330008905672[63] = 0;
+   out_3645925778664752399[63] = 0;
-   out_1825222330008905672[64] = 0;
+   out_3645925778664752399[64] = 0;
-   out_1825222330008905672[65] = 0;
+   out_3645925778664752399[65] = 0;
-   out_1825222330008905672[66] = 0;
+   out_3645925778664752399[66] = 0;
-   out_1825222330008905672[67] = 0;
+   out_3645925778664752399[67] = 0;
-   out_1825222330008905672[68] = 0;
+   out_3645925778664752399[68] = 0;
-   out_1825222330008905672[69] = 0;
+   out_3645925778664752399[69] = 0;
-   out_1825222330008905672[70] = 1;
+   out_3645925778664752399[70] = 1;
-   out_1825222330008905672[71] = 0;
+   out_3645925778664752399[71] = 0;
-   out_1825222330008905672[72] = 0;
+   out_3645925778664752399[72] = 0;
-   out_1825222330008905672[73] = 0;
+   out_3645925778664752399[73] = 0;
-   out_1825222330008905672[74] = 0;
+   out_3645925778664752399[74] = 0;
-   out_1825222330008905672[75] = 0;
+   out_3645925778664752399[75] = 0;
-   out_1825222330008905672[76] = 0;
+   out_3645925778664752399[76] = 0;
-   out_1825222330008905672[77] = 0;
+   out_3645925778664752399[77] = 0;
-   out_1825222330008905672[78] = 0;
+   out_3645925778664752399[78] = 0;
-   out_1825222330008905672[79] = 0;
+   out_3645925778664752399[79] = 0;
-   out_1825222330008905672[80] = 1;
+   out_3645925778664752399[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_7364871104219513908) {
+void h_25(double *state, double *unused, double *out_1347552076965832871) {
-   out_7364871104219513908[0] = state[6];
+   out_1347552076965832871[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_5259356069339540719) {
+void H_25(double *state, double *unused, double *out_6035190491488835800) {
-   out_5259356069339540719[0] = 0;
+   out_6035190491488835800[0] = 0;
-   out_5259356069339540719[1] = 0;
+   out_6035190491488835800[1] = 0;
-   out_5259356069339540719[2] = 0;
+   out_6035190491488835800[2] = 0;
-   out_5259356069339540719[3] = 0;
+   out_6035190491488835800[3] = 0;
-   out_5259356069339540719[4] = 0;
+   out_6035190491488835800[4] = 0;
-   out_5259356069339540719[5] = 0;
+   out_6035190491488835800[5] = 0;
-   out_5259356069339540719[6] = 1;
+   out_6035190491488835800[6] = 1;
-   out_5259356069339540719[7] = 0;
+   out_6035190491488835800[7] = 0;
-   out_5259356069339540719[8] = 0;
+   out_6035190491488835800[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_6181950591427252357) {
+void h_24(double *state, double *unused, double *out_7176999251413506765) {
-   out_6181950591427252357[0] = state[4];
+   out_7176999251413506765[0] = state[4];
-   out_6181950591427252357[1] = state[5];
+   out_7176999251413506765[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_1316215737251977382) {
+void H_24(double *state, double *unused, double *out_6210520902369554390) {
-   out_1316215737251977382[0] = 0;
+   out_6210520902369554390[0] = 0;
-   out_1316215737251977382[1] = 0;
+   out_6210520902369554390[1] = 0;
-   out_1316215737251977382[2] = 0;
+   out_6210520902369554390[2] = 0;
-   out_1316215737251977382[3] = 0;
+   out_6210520902369554390[3] = 0;
-   out_1316215737251977382[4] = 1;
+   out_6210520902369554390[4] = 1;
-   out_1316215737251977382[5] = 0;
+   out_6210520902369554390[5] = 0;
-   out_1316215737251977382[6] = 0;
+   out_6210520902369554390[6] = 0;
-   out_1316215737251977382[7] = 0;
+   out_6210520902369554390[7] = 0;
-   out_1316215737251977382[8] = 0;
+   out_6210520902369554390[8] = 0;
-   out_1316215737251977382[9] = 0;
+   out_6210520902369554390[9] = 0;
-   out_1316215737251977382[10] = 0;
+   out_6210520902369554390[10] = 0;
-   out_1316215737251977382[11] = 0;
+   out_6210520902369554390[11] = 0;
-   out_1316215737251977382[12] = 0;
+   out_6210520902369554390[12] = 0;
-   out_1316215737251977382[13] = 0;
+   out_6210520902369554390[13] = 0;
-   out_1316215737251977382[14] = 1;
+   out_6210520902369554390[14] = 1;
-   out_1316215737251977382[15] = 0;
+   out_6210520902369554390[15] = 0;
-   out_1316215737251977382[16] = 0;
+   out_6210520902369554390[16] = 0;
-   out_1316215737251977382[17] = 0;
+   out_6210520902369554390[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_4311037743139686772) {
+void h_30(double *state, double *unused, double *out_2798183469594716038) {
-   out_4311037743139686772[0] = state[4];
+   out_2798183469594716038[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_1657334272152076036) {
+void H_30(double *state, double *unused, double *out_881499850002780955) {
-   out_1657334272152076036[0] = 0;
+   out_881499850002780955[0] = 0;
-   out_1657334272152076036[1] = 0;
+   out_881499850002780955[1] = 0;
-   out_1657334272152076036[2] = 0;
+   out_881499850002780955[2] = 0;
-   out_1657334272152076036[3] = 0;
+   out_881499850002780955[3] = 0;
-   out_1657334272152076036[4] = 1;
+   out_881499850002780955[4] = 1;
-   out_1657334272152076036[5] = 0;
+   out_881499850002780955[5] = 0;
-   out_1657334272152076036[6] = 0;
+   out_881499850002780955[6] = 0;
-   out_1657334272152076036[7] = 0;
+   out_881499850002780955[7] = 0;
-   out_1657334272152076036[8] = 0;
+   out_881499850002780955[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_8569605146509354979) {
+void h_26(double *state, double *unused, double *out_7428563492211452550) {
-   out_8569605146509354979[0] = state[7];
+   out_7428563492211452550[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_9000859388213596943) {
+void H_26(double *state, double *unused, double *out_8670050263346659592) {
-   out_9000859388213596943[0] = 0;
+   out_8670050263346659592[0] = 0;
-   out_9000859388213596943[1] = 0;
+   out_8670050263346659592[1] = 0;
-   out_9000859388213596943[2] = 0;
+   out_8670050263346659592[2] = 0;
-   out_9000859388213596943[3] = 0;
+   out_8670050263346659592[3] = 0;
-   out_9000859388213596943[4] = 0;
+   out_8670050263346659592[4] = 0;
-   out_9000859388213596943[5] = 0;
+   out_8670050263346659592[5] = 0;
-   out_9000859388213596943[6] = 0;
+   out_8670050263346659592[6] = 0;
-   out_9000859388213596943[7] = 1;
+   out_8670050263346659592[7] = 1;
-   out_9000859388213596943[8] = 0;
+   out_8670050263346659592[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_2363197836558668263) {
+void h_27(double *state, double *unused, double *out_3654121190695012774) {
-   out_2363197836558668263[0] = state[3];
+   out_3654121190695012774[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_517429039648348875) {
+void H_27(double *state, double *unused, double *out_8339292750432500781) {
-   out_517429039648348875[0] = 0;
+   out_8339292750432500781[0] = 0;
-   out_517429039648348875[1] = 0;
+   out_8339292750432500781[1] = 0;
-   out_517429039648348875[2] = 0;
+   out_8339292750432500781[2] = 0;
-   out_517429039648348875[3] = 1;
+   out_8339292750432500781[3] = 1;
-   out_517429039648348875[4] = 0;
+   out_8339292750432500781[4] = 0;
-   out_517429039648348875[5] = 0;
+   out_8339292750432500781[5] = 0;
-   out_517429039648348875[6] = 0;
+   out_8339292750432500781[6] = 0;
-   out_517429039648348875[7] = 0;
+   out_8339292750432500781[7] = 0;
-   out_517429039648348875[8] = 0;
+   out_8339292750432500781[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_1498485639869833389) {
+void h_29(double *state, double *unused, double *out_2203489798066129607) {
-   out_1498485639869833389[0] = state[1];
+   out_2203489798066129607[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_2230791766517899908) {
+void H_29(double *state, double *unused, double *out_5654298094317683686) {
-   out_2230791766517899908[0] = 0;
+   out_5654298094317683686[0] = 0;
-   out_2230791766517899908[1] = 1;
+   out_5654298094317683686[1] = 1;
-   out_2230791766517899908[2] = 0;
+   out_5654298094317683686[2] = 0;
-   out_2230791766517899908[3] = 0;
+   out_5654298094317683686[3] = 0;
-   out_2230791766517899908[4] = 0;
+   out_5654298094317683686[4] = 0;
-   out_2230791766517899908[5] = 0;
+   out_5654298094317683686[5] = 0;
-   out_2230791766517899908[6] = 0;
+   out_5654298094317683686[6] = 0;
-   out_2230791766517899908[7] = 0;
+   out_5654298094317683686[7] = 0;
-   out_2230791766517899908[8] = 0;
+   out_5654298094317683686[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_6657345575881808179) {
+void h_28(double *state, double *unused, double *out_3781022243733688039) {
-   out_6657345575881808179[0] = state[0];
+   out_3781022243733688039[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_7313190783587430482) {
+void H_28(double *state, double *unused, double *out_3690667822752357435) {
-   out_7313190783587430482[0] = 1;
+   out_3690667822752357435[0] = 1;
-   out_7313190783587430482[1] = 0;
+   out_3690667822752357435[1] = 0;
-   out_7313190783587430482[2] = 0;
+   out_3690667822752357435[2] = 0;
-   out_7313190783587430482[3] = 0;
+   out_3690667822752357435[3] = 0;
-   out_7313190783587430482[4] = 0;
+   out_3690667822752357435[4] = 0;
-   out_7313190783587430482[5] = 0;
+   out_3690667822752357435[5] = 0;
-   out_7313190783587430482[6] = 0;
+   out_3690667822752357435[6] = 0;
-   out_7313190783587430482[7] = 0;
+   out_3690667822752357435[7] = 0;
-   out_7313190783587430482[8] = 0;
+   out_3690667822752357435[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_398394152601158533) {
+void h_31(double *state, double *unused, double *out_5423283149384518065) {
-   out_398394152601158533[0] = state[8];
+   out_5423283149384518065[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_5228710107462580291) {
+void H_31(double *state, double *unused, double *out_6004544529611875372) {
-   out_5228710107462580291[0] = 0;
+   out_6004544529611875372[0] = 0;
-   out_5228710107462580291[1] = 0;
+   out_6004544529611875372[1] = 0;
-   out_5228710107462580291[2] = 0;
+   out_6004544529611875372[2] = 0;
-   out_5228710107462580291[3] = 0;
+   out_6004544529611875372[3] = 0;
-   out_5228710107462580291[4] = 0;
+   out_6004544529611875372[4] = 0;
-   out_5228710107462580291[5] = 0;
+   out_6004544529611875372[5] = 0;
-   out_5228710107462580291[6] = 0;
+   out_6004544529611875372[6] = 0;
-   out_5228710107462580291[7] = 0;
+   out_6004544529611875372[7] = 0;
-   out_5228710107462580291[8] = 1;
+   out_6004544529611875372[8] = 1;
 }
 #include <eigen3/Eigen/Dense>
 #include <iostream>
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
   update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
 }
-void car_err_fun(double *nom_x, double *delta_x, double *out_6333242559483084265) {
+void car_err_fun(double *nom_x, double *delta_x, double *out_2765937094321424100) {
-  err_fun(nom_x, delta_x, out_6333242559483084265);
+  err_fun(nom_x, delta_x, out_2765937094321424100);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_829248915283082072) {
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_1382255689110899984) {
-  inv_err_fun(nom_x, true_x, out_829248915283082072);
+  inv_err_fun(nom_x, true_x, out_1382255689110899984);
 }
-void car_H_mod_fun(double *state, double *out_8283696179185346057) {
+void car_H_mod_fun(double *state, double *out_6175643942212596402) {
-  H_mod_fun(state, out_8283696179185346057);
+  H_mod_fun(state, out_6175643942212596402);
 }
-void car_f_fun(double *state, double dt, double *out_3453347444055346076) {
+void car_f_fun(double *state, double dt, double *out_2437881867604815540) {
-  f_fun(state,  dt, out_3453347444055346076);
+  f_fun(state,  dt, out_2437881867604815540);
 }
-void car_F_fun(double *state, double dt, double *out_1825222330008905672) {
+void car_F_fun(double *state, double dt, double *out_3645925778664752399) {
-  F_fun(state,  dt, out_1825222330008905672);
+  F_fun(state,  dt, out_3645925778664752399);
 }
-void car_h_25(double *state, double *unused, double *out_7364871104219513908) {
+void car_h_25(double *state, double *unused, double *out_1347552076965832871) {
-  h_25(state, unused, out_7364871104219513908);
+  h_25(state, unused, out_1347552076965832871);
 }
-void car_H_25(double *state, double *unused, double *out_5259356069339540719) {
+void car_H_25(double *state, double *unused, double *out_6035190491488835800) {
-  H_25(state, unused, out_5259356069339540719);
+  H_25(state, unused, out_6035190491488835800);
 }
-void car_h_24(double *state, double *unused, double *out_6181950591427252357) {
+void car_h_24(double *state, double *unused, double *out_7176999251413506765) {
-  h_24(state, unused, out_6181950591427252357);
+  h_24(state, unused, out_7176999251413506765);
 }
-void car_H_24(double *state, double *unused, double *out_1316215737251977382) {
+void car_H_24(double *state, double *unused, double *out_6210520902369554390) {
-  H_24(state, unused, out_1316215737251977382);
+  H_24(state, unused, out_6210520902369554390);
 }
-void car_h_30(double *state, double *unused, double *out_4311037743139686772) {
+void car_h_30(double *state, double *unused, double *out_2798183469594716038) {
-  h_30(state, unused, out_4311037743139686772);
+  h_30(state, unused, out_2798183469594716038);
 }
-void car_H_30(double *state, double *unused, double *out_1657334272152076036) {
+void car_H_30(double *state, double *unused, double *out_881499850002780955) {
-  H_30(state, unused, out_1657334272152076036);
+  H_30(state, unused, out_881499850002780955);
 }
-void car_h_26(double *state, double *unused, double *out_8569605146509354979) {
+void car_h_26(double *state, double *unused, double *out_7428563492211452550) {
-  h_26(state, unused, out_8569605146509354979);
+  h_26(state, unused, out_7428563492211452550);
 }
-void car_H_26(double *state, double *unused, double *out_9000859388213596943) {
+void car_H_26(double *state, double *unused, double *out_8670050263346659592) {
-  H_26(state, unused, out_9000859388213596943);
+  H_26(state, unused, out_8670050263346659592);
 }
-void car_h_27(double *state, double *unused, double *out_2363197836558668263) {
+void car_h_27(double *state, double *unused, double *out_3654121190695012774) {
-  h_27(state, unused, out_2363197836558668263);
+  h_27(state, unused, out_3654121190695012774);
 }
-void car_H_27(double *state, double *unused, double *out_517429039648348875) {
+void car_H_27(double *state, double *unused, double *out_8339292750432500781) {
-  H_27(state, unused, out_517429039648348875);
+  H_27(state, unused, out_8339292750432500781);
 }
-void car_h_29(double *state, double *unused, double *out_1498485639869833389) {
+void car_h_29(double *state, double *unused, double *out_2203489798066129607) {
-  h_29(state, unused, out_1498485639869833389);
+  h_29(state, unused, out_2203489798066129607);
 }
-void car_H_29(double *state, double *unused, double *out_2230791766517899908) {
+void car_H_29(double *state, double *unused, double *out_5654298094317683686) {
-  H_29(state, unused, out_2230791766517899908);
+  H_29(state, unused, out_5654298094317683686);
 }
-void car_h_28(double *state, double *unused, double *out_6657345575881808179) {
+void car_h_28(double *state, double *unused, double *out_3781022243733688039) {
-  h_28(state, unused, out_6657345575881808179);
+  h_28(state, unused, out_3781022243733688039);
 }
-void car_H_28(double *state, double *unused, double *out_7313190783587430482) {
+void car_H_28(double *state, double *unused, double *out_3690667822752357435) {
-  H_28(state, unused, out_7313190783587430482);
+  H_28(state, unused, out_3690667822752357435);
 }
-void car_h_31(double *state, double *unused, double *out_398394152601158533) {
+void car_h_31(double *state, double *unused, double *out_5423283149384518065) {
-  h_31(state, unused, out_398394152601158533);
+  h_31(state, unused, out_5423283149384518065);
 }
-void car_H_31(double *state, double *unused, double *out_5228710107462580291) {
+void car_H_31(double *state, double *unused, double *out_6004544529611875372) {
-  H_31(state, unused, out_5228710107462580291);
+  H_31(state, unused, out_6004544529611875372);
 }
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
   predict(in_x, in_P, in_Q, dt);

selfdrive/locationd/models/generated/car.h

@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void car_err_fun(double *nom_x, double *delta_x, double *out_6333242559483084265);
+void car_err_fun(double *nom_x, double *delta_x, double *out_2765937094321424100);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_829248915283082072);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_1382255689110899984);
-void car_H_mod_fun(double *state, double *out_8283696179185346057);
+void car_H_mod_fun(double *state, double *out_6175643942212596402);
-void car_f_fun(double *state, double dt, double *out_3453347444055346076);
+void car_f_fun(double *state, double dt, double *out_2437881867604815540);
-void car_F_fun(double *state, double dt, double *out_1825222330008905672);
+void car_F_fun(double *state, double dt, double *out_3645925778664752399);
-void car_h_25(double *state, double *unused, double *out_7364871104219513908);
+void car_h_25(double *state, double *unused, double *out_1347552076965832871);
-void car_H_25(double *state, double *unused, double *out_5259356069339540719);
+void car_H_25(double *state, double *unused, double *out_6035190491488835800);
-void car_h_24(double *state, double *unused, double *out_6181950591427252357);
+void car_h_24(double *state, double *unused, double *out_7176999251413506765);
-void car_H_24(double *state, double *unused, double *out_1316215737251977382);
+void car_H_24(double *state, double *unused, double *out_6210520902369554390);
-void car_h_30(double *state, double *unused, double *out_4311037743139686772);
+void car_h_30(double *state, double *unused, double *out_2798183469594716038);
-void car_H_30(double *state, double *unused, double *out_1657334272152076036);
+void car_H_30(double *state, double *unused, double *out_881499850002780955);
-void car_h_26(double *state, double *unused, double *out_8569605146509354979);
+void car_h_26(double *state, double *unused, double *out_7428563492211452550);
-void car_H_26(double *state, double *unused, double *out_9000859388213596943);
+void car_H_26(double *state, double *unused, double *out_8670050263346659592);
-void car_h_27(double *state, double *unused, double *out_2363197836558668263);
+void car_h_27(double *state, double *unused, double *out_3654121190695012774);
-void car_H_27(double *state, double *unused, double *out_517429039648348875);
+void car_H_27(double *state, double *unused, double *out_8339292750432500781);
-void car_h_29(double *state, double *unused, double *out_1498485639869833389);
+void car_h_29(double *state, double *unused, double *out_2203489798066129607);
-void car_H_29(double *state, double *unused, double *out_2230791766517899908);
+void car_H_29(double *state, double *unused, double *out_5654298094317683686);
-void car_h_28(double *state, double *unused, double *out_6657345575881808179);
+void car_h_28(double *state, double *unused, double *out_3781022243733688039);
-void car_H_28(double *state, double *unused, double *out_7313190783587430482);
+void car_H_28(double *state, double *unused, double *out_3690667822752357435);
-void car_h_31(double *state, double *unused, double *out_398394152601158533);
+void car_h_31(double *state, double *unused, double *out_5423283149384518065);
-void car_H_31(double *state, double *unused, double *out_5228710107462580291);
+void car_H_31(double *state, double *unused, double *out_6004544529611875372);
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
 void car_set_mass(double x);
 void car_set_rotational_inertia(double x);

selfdrive/locationd/models/generated/pose.h

@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void pose_err_fun(double *nom_x, double *delta_x, double *out_3574067617578089570);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_2822233615381903085);
-void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1515328263168530495);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3556797172859207138);
-void pose_H_mod_fun(double *state, double *out_207229337745545750);
+void pose_H_mod_fun(double *state, double *out_4158952900173024917);
-void pose_f_fun(double *state, double dt, double *out_3493541226789963130);
+void pose_f_fun(double *state, double dt, double *out_4480310224533440205);
-void pose_F_fun(double *state, double dt, double *out_5154051058888586616);
+void pose_F_fun(double *state, double dt, double *out_2252133042369597057);
-void pose_h_4(double *state, double *unused, double *out_5970382648807117116);
+void pose_h_4(double *state, double *unused, double *out_1225931617160521679);
-void pose_H_4(double *state, double *unused, double *out_4945289047293534985);
+void pose_H_4(double *state, double *unused, double *out_3404791898118312310);
-void pose_h_10(double *state, double *unused, double *out_520088425664385209);
+void pose_h_10(double *state, double *unused, double *out_8314558572170779017);
-void pose_H_10(double *state, double *unused, double *out_4050335309714945768);
+void pose_H_10(double *state, double *unused, double *out_3543553484029179274);
-void pose_h_13(double *state, double *unused, double *out_1434579794425570866);
+void pose_h_13(double *state, double *unused, double *out_7492025966781743409);
-void pose_H_13(double *state, double *unused, double *out_1111533583991010961);
+void pose_H_13(double *state, double *unused, double *out_4205839310198388619);
-void pose_h_14(double *state, double *unused, double *out_5370135948753942539);
+void pose_h_14(double *state, double *unused, double *out_8444019918627598165);
-void pose_H_14(double *state, double *unused, double *out_1862500614998162689);
+void pose_H_14(double *state, double *unused, double *out_6487580330413684606);
 void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }

selfdrive/modeld/compile_modeld.py

@@ -4,114 +4,106 @@ import atexit
 import os
 import pickle
 import time
-from collections import defaultdict, namedtuple
 from functools import partial
+from collections import namedtuple
 
 import numpy as np
 
-
 def _patch_tinygrad_fetch_fw():
   import hashlib
   import pathlib
-
   import zstandard
   from tinygrad import helpers
-
+  _orig = getattr(helpers, "fetch_fw", None)
-  original_fetch_fw = getattr(helpers, "fetch_fw", None)
+  if _orig is None:
-  if original_fetch_fw is None:
     return
-
   def fetch_fw(path, name, sha256):
-    firmware_path = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
+    p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
-    if firmware_path.is_file():
+    if p.is_file():
-      blob = zstandard.ZstdDecompressor().stream_reader(firmware_path.read_bytes()).read()
+      blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read()
       if hashlib.sha256(blob).hexdigest() == sha256:
         return blob
-    return original_fetch_fw(path, name, sha256)
+    return _orig(path, name, sha256)
-
   helpers.fetch_fw = fetch_fw
-
-
 _patch_tinygrad_fetch_fw()
 
+from tinygrad.tensor import Tensor
+from tinygrad.helpers import Context
 from tinygrad.device import Device
 from tinygrad.engine.jit import TinyJit
-from tinygrad.helpers import Context
-from tinygrad.tensor import Tensor
 
 
-NV12Frame = namedtuple("NV12Frame", ["width", "height", "stride", "y_height", "uv_height", "size"])
+NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size'])
-WARP_INPUTS = ["img_q", "big_img_q", "tfm", "big_tfm"]
+WARP_INPUTS = ['img_q', 'big_img_q', 'tfm', 'big_tfm']
-POLICY_INPUTS = ["feat_q", "desire_q", "desire", "traffic_convention", "action_t"]
+POLICY_INPUTS = ['feat_q', 'desire_q', 'desire', 'traffic_convention', 'action_t']
 
-WARP_DEV = os.getenv("WARP_DEV")
+UV_SCALE_MATRIX = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 1]], dtype=np.float32)
+UV_SCALE_MATRIX_INV = np.linalg.inv(UV_SCALE_MATRIX)
+
+WARP_DEV = os.getenv('WARP_DEV')
 
 
 def make_random_images(keys, shape, device=None):
-  return {key: Tensor.randint(shape, low=0, high=256, dtype="uint8", device=device).realize() for key in keys}
+  return {k: Tensor.randint(shape, low=0, high=256, dtype='uint8', device=device).realize() for k in keys}
 
 
-class _BlobTensorInputs(dict):
+def make_random_blob_images(keys, size, device=None):
-  _backing_arrays: dict[str, np.ndarray]
+  keepalive: list[np.ndarray] = []
+
+  def _make_random_blob_images():
+    nonlocal keepalive
+    keepalive = []
+    tensors = {}
+    for key in keys:
+      frame_np = (32 * np.random.randn(size).astype(np.float32) + 128).clip(0, 255).astype(np.uint8)
+      keepalive.append(frame_np)
+      # Match runtime's Tensor.from_blob camera input ABI so TinyJit captures the same view shape.
+      tensors[key] = Tensor.from_blob(frame_np.ctypes.data, (size,), dtype='uint8', device=device).realize()
+    return tensors
+
+  return _make_random_blob_images
 
 
-def make_random_blob_images(keys, shape, device=None):
+def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad, border_fill_val=None):
-  blob_shape = shape if isinstance(shape, tuple) else (shape,)
+  w_dst, h_dst = dst_shape
-  backing_arrays = {
+  h_src, w_src = src_shape
-    key: np.random.randint(0, 256, size=blob_shape, dtype=np.uint8)
-    for key in keys
-  }
-  inputs = _BlobTensorInputs({
-    key: Tensor.from_blob(array.ctypes.data, array.shape, dtype="uint8", device=device).realize()
-    for key, array in backing_arrays.items()
-  })
-  # Keep the numpy storage alive for the duration of the JIT capture/replay call.
-  inputs._backing_arrays = backing_arrays
-  return inputs
 
+  x = Tensor.arange(w_dst, device=WARP_DEV).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1)
+  y = Tensor.arange(h_dst, device=WARP_DEV).reshape(h_dst, 1).expand(h_dst, w_dst).reshape(-1)
 
-def warp_perspective_tinygrad(src_flat, matrix_inverse, dst_shape, src_shape, stride_pad, border_fill_val=None):
+  # inline 3x3 matmul as elementwise to avoid reduce op (enables fusion with gather)
-  width_dst, height_dst = dst_shape
+  src_x = M_inv[0, 0] * x + M_inv[0, 1] * y + M_inv[0, 2]
-  height_src, width_src = src_shape
+  src_y = M_inv[1, 0] * x + M_inv[1, 1] * y + M_inv[1, 2]
-
+  src_w = M_inv[2, 0] * x + M_inv[2, 1] * y + M_inv[2, 2]
-  x = Tensor.arange(width_dst, device=WARP_DEV).reshape(1, width_dst).expand(height_dst, width_dst).reshape(-1)
-  y = Tensor.arange(height_dst, device=WARP_DEV).reshape(height_dst, 1).expand(height_dst, width_dst).reshape(-1)
-
-  # Inline 3x3 matmul as elementwise to avoid reduce ops and enable fusion with gather.
-  src_x = matrix_inverse[0, 0] * x + matrix_inverse[0, 1] * y + matrix_inverse[0, 2]
-  src_y = matrix_inverse[1, 0] * x + matrix_inverse[1, 1] * y + matrix_inverse[1, 2]
-  src_w = matrix_inverse[2, 0] * x + matrix_inverse[2, 1] * y + matrix_inverse[2, 2]
 
   src_x = src_x / src_w
   src_y = src_y / src_w
 
   x_round = Tensor.round(src_x)
   y_round = Tensor.round(src_y)
-  x_nn_clipped = x_round.clip(0, width_src - 1).cast("int")
+  x_nn_clipped = x_round.clip(0, w_src - 1).cast('int')
-  y_nn_clipped = y_round.clip(0, height_src - 1).cast("int")
+  y_nn_clipped = y_round.clip(0, h_src - 1).cast('int')
-  idx = y_nn_clipped * (width_src + stride_pad) + x_nn_clipped
+  idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped
   sampled = src_flat[idx]
 
   if border_fill_val is None:
     return sampled
 
-  in_bounds = ((x_round >= 0) & (x_round <= width_src - 1) &
+  in_bounds = ((x_round >= 0) & (x_round <= w_src - 1) &
-               (y_round >= 0) & (y_round <= height_src - 1)).cast(sampled.dtype)
+               (y_round >= 0) & (y_round <= h_src - 1)).cast(sampled.dtype)
   return sampled * in_bounds + Tensor(border_fill_val, dtype=sampled.dtype) * (1 - in_bounds)
 
 
 def frames_to_tensor(frames):
-  height = (frames.shape[0] * 2) // 3
+  H = (frames.shape[0] * 2) // 3
-  width = frames.shape[1]
+  W = frames.shape[1]
-  return Tensor.cat(
+  in_img1 = Tensor.cat(frames[0:H:2, 0::2],
-    frames[0:height:2, 0::2],
+                       frames[1:H:2, 0::2],
-    frames[1:height:2, 0::2],
+                       frames[0:H:2, 1::2],
-    frames[0:height:2, 1::2],
+                       frames[1:H:2, 1::2],
-    frames[1:height:2, 1::2],
+                       frames[H:H+H//4].reshape((H//2, W//2)),
-    frames[height:height + height // 4].reshape((height // 2, width // 2)),
+                       frames[H+H//4:H+H//2].reshape((H//2, W//2)), dim=0).reshape((6, H//2, W//2))
-    frames[height + height // 4:height + height // 2].reshape((height // 2, width // 2)),
+  return in_img1
-    dim=0,
-  ).reshape((6, height // 2, width // 2))
 
 
 def make_frame_prepare(nv12: NV12Frame, model_w, model_h):
@@ -119,81 +111,65 @@ def make_frame_prepare(nv12: NV12Frame, model_w, model_h):
   uv_offset = stride * y_height
   stride_pad = stride - cam_w
 
-  def frame_prepare_tinygrad(input_frame, matrix_inverse):
+  def frame_prepare_tinygrad(input_frame, M_inv):
-    # UV_SCALE @ M_inv @ UV_SCALE_INV simplifies to elementwise scaling.
+    # UV_SCALE @ M_inv @ UV_SCALE_INV simplifies to elementwise scaling
-    matrix_inverse_uv = matrix_inverse * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=WARP_DEV)
+    M_inv_uv = M_inv * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=WARP_DEV)
-    # Deinterleave NV12 UV plane (UVUV... -> separate U, V).
+    # deinterleave NV12 UV plane (UVUV... -> separate U, V)
     uv = input_frame[uv_offset:uv_offset + uv_height * stride].reshape(uv_height, stride)
     with Context(SPLIT_REDUCEOP=0):
-      y = warp_perspective_tinygrad(
+      y = warp_perspective_tinygrad(input_frame[:cam_h*stride],
-        input_frame[:cam_h * stride],
+                                    M_inv, (model_w, model_h),
-        matrix_inverse,
+                                    (cam_h, cam_w), stride_pad).realize()
-        (model_w, model_h),
+      u = warp_perspective_tinygrad(uv[:cam_h//2, :cam_w:2].flatten(),
-        (cam_h, cam_w),
+                                    M_inv_uv, (model_w//2, model_h//2),
-        stride_pad,
+                                    (cam_h//2, cam_w//2), 0).realize()
-      ).realize()
+      v = warp_perspective_tinygrad(uv[:cam_h//2, 1:cam_w:2].flatten(),
-      u = warp_perspective_tinygrad(
+                                    M_inv_uv, (model_w//2, model_h//2),
-        uv[:cam_h // 2, :cam_w:2].flatten(),
+                                    (cam_h//2, cam_w//2), 0).realize()
-        matrix_inverse_uv,
-        (model_w // 2, model_h // 2),
-        (cam_h // 2, cam_w // 2),
-        0,
-      ).realize()
-      v = warp_perspective_tinygrad(
-        uv[:cam_h // 2, 1:cam_w:2].flatten(),
-        matrix_inverse_uv,
-        (model_w // 2, model_h // 2),
-        (cam_h // 2, cam_w // 2),
-        0,
-      ).realize()
     yuv = y.cat(u).cat(v).reshape((model_h * 3 // 2, model_w))
-    return frames_to_tensor(yuv)
+    tensor = frames_to_tensor(yuv)
-
+    return tensor
   return frame_prepare_tinygrad
 
 
-def make_tensor_inputs(vision_input_shapes, policy_input_shapes, frame_skip, device):
+def make_warp_input_queues(vision_input_shapes, frame_skip, device):
-  img = vision_input_shapes["img"]
+  img = vision_input_shapes['img']  # (1, 12, 128, 256)
   n_frames = img[1] // 6
   img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
 
-  features_buffer = policy_input_shapes["features_buffer"]
-  desire_pulse = policy_input_shapes["desire_pulse"]
-
-  return {
-    "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 * (features_buffer[1] - 1) + 1, features_buffer[0], features_buffer[2]), dtype=np.float32),
-      device=device,
-    ).contiguous().realize(),
-    "desire_q": Tensor(
-      np.zeros((frame_skip * desire_pulse[1], desire_pulse[0], desire_pulse[2]), dtype=np.float32),
-      device=device,
-    ).contiguous().realize(),
-  }
-
-
-def make_npy_inputs(policy_input_shapes):
-  desire_pulse = policy_input_shapes["desire_pulse"]
-  traffic_convention = policy_input_shapes["traffic_convention"]
-
   npy = {
-    "desire": np.zeros(desire_pulse[2], dtype=np.float32),
+    'tfm': np.zeros((3, 3), dtype=np.float32),
-    "traffic_convention": np.zeros(traffic_convention, dtype=np.float32),
+    'big_tfm': np.zeros((3, 3), dtype=np.float32),
-    "tfm": np.zeros((3, 3), dtype=np.float32),
-    "big_tfm": np.zeros((3, 3), dtype=np.float32),
   }
-  if "action_t" in policy_input_shapes:
+  input_queues = {
-    npy["action_t"] = np.zeros(policy_input_shapes["action_t"], dtype=np.float32)
+    'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-  npy_tensors = {key: Tensor(value, device="NPY").realize() for key, value in npy.items()}
+    'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-  return npy, npy_tensors
+    **{k: Tensor(v, device='NPY').realize() for k, v in npy.items()},
+  }
+  return input_queues, npy
 
 
 def make_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device):
-  tensor_inputs = make_tensor_inputs(vision_input_shapes, policy_input_shapes, frame_skip, device)
+  input_queues, npy = make_warp_input_queues(vision_input_shapes, frame_skip, device)
-  npy, npy_tensors = make_npy_inputs(policy_input_shapes)
+
-  return {**tensor_inputs, **npy_tensors}, npy
+  fb = policy_input_shapes['features_buffer']  # (1, 25, 512)
+  dp = policy_input_shapes['desire_pulse']  # (1, 25, 8)
+  tc = policy_input_shapes['traffic_convention']  # (1, 2)
+  #TODO action_t is hardcoded to match tc for future compatibility
+  at = tc
+
+  policy_npy = {
+    'desire': np.zeros(dp[2], dtype=np.float32),
+    'traffic_convention': np.zeros(tc, dtype=np.float32),
+    'action_t': np.zeros(at, dtype=np.float32),
+  }
+  npy.update(policy_npy)
+  input_queues.update({
+    '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 policy_npy.items()},
+  })
+  return input_queues, npy
 
 
 def shift_and_sample(buf, new_val, sample_fn):
@@ -223,44 +199,39 @@ def make_warp(nv12, model_w, model_h, frame_skip):
     img = shift_and_sample(img_q, warped_frame, sample_skip_fn)
     big_img = shift_and_sample(big_img_q, warped_big_frame, sample_skip_fn)
     return img, big_img
-
   return warp_enqueue
 
 
-def make_run_policy(vision_runner, off_policy_runner, on_policy_runner, vision_features_slice, frame_skip):
+def make_run_policy(model_runners, model_metadata, frame_skip):
   sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
   sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
+  vision_features_slice = model_metadata['vision']['output_slices']['hidden_state']
 
   def run_policy(img, big_img, feat_q, desire_q, desire, traffic_convention, action_t):
     desire = desire.to(Device.DEFAULT)
     traffic_convention = traffic_convention.to(Device.DEFAULT)
     action_t = action_t.to(Device.DEFAULT)
     Tensor.realize(desire, traffic_convention, action_t)
-
     desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
-    vision_out = next(iter(vision_runner({"img": img, "big_img": big_img}).values())).cast("float32")
+    vision_out = next(iter(model_runners['vision']({'img': img, 'big_img': 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)
 
     inputs = {
-      "features_buffer": feat_buf,
+      'features_buffer': feat_buf,
-      "desire_pulse": desire_buf,
+      'desire_pulse': desire_buf,
-      "traffic_convention": traffic_convention,
+      'traffic_convention': traffic_convention,
-      "action_t": action_t,
+      'action_t': action_t,
     }
-    on_policy_out = next(iter(on_policy_runner(inputs).values())).cast("float32")
+    on_policy_out = next(iter(model_runners['on_policy'](inputs).values())).cast('float32')
-    off_policy_out = next(iter(off_policy_runner(inputs).values())).cast("float32")
+    off_policy_out = next(iter(model_runners['off_policy'](inputs).values())).cast('float32')
     return vision_out, on_policy_out, off_policy_out
-
   return run_policy
 
 
-def compile_jit(jit, make_random_inputs, input_keys, frame_skip, vision_metadata, policy_metadata):
+def compile_jit(jit, make_random_inputs, input_keys, make_queues):
-  vision_input_shapes = vision_metadata["input_shapes"]
+  SEED = 42
-  policy_input_shapes = policy_metadata["input_shapes"]
-
-  seed = 42
   validation_rtol = 5e-3 if Device.DEFAULT == "QCOM" else 0.0
   validation_atol = 5e-3 if Device.DEFAULT == "QCOM" else 0.0
 
@@ -271,118 +242,104 @@ def compile_jit(jit, make_random_inputs, input_keys, frame_skip, vision_metadata
       return np.allclose(lhs, rhs, rtol=validation_rtol, atol=validation_atol, equal_nan=True)
     return np.array_equal(lhs, rhs)
 
-  def random_inputs_run(fn, current_seed, test_val=None, test_buffers=None, expect_match=True):
+  def random_inputs_run(fn, seed, test_val=None, test_buffers=None, expect_match=True):
-    input_queues, npy = make_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, Device.DEFAULT)
+    input_queues, npy = make_queues(Device.DEFAULT)
-    np.random.seed(current_seed)
+    np.random.seed(seed)
-    Tensor.manual_seed(current_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 idx in range(n_runs):
+    for i in range(n_runs):
-      for value in npy.values():
+      for v in npy.values():
-        value[:] = np.random.randn(*value.shape).astype(value.dtype)
+        v[:] = np.random.randn(*v.shape).astype(v.dtype)
       Device.default.synchronize()
       random_inputs = make_random_inputs()
-      start = time.perf_counter()
+      st = time.perf_counter()
-      outs = fn(**{key: input_queues[key] for key in input_keys}, **random_inputs)
+      outs = fn(**{k: input_queues[k] for k in input_keys}, **random_inputs)
-      mid = time.perf_counter()
+      mt = time.perf_counter()
       Device.default.synchronize()
-      end = time.perf_counter()
+      et = time.perf_counter()
-      print(f"  [{idx + 1}/{n_runs}] enqueue {(mid - start) * 1e3:6.2f} ms -- total {(end - start) * 1e3:6.2f} ms")
+      print(f"  [{i+1}/{n_runs}] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
 
-      if idx == 0:
+      if i == 0:
-        val = [np.copy(value.numpy()) for value in outs]
+        val = [np.copy(v.numpy()) for v in outs]
-        buffers = [np.copy(value.numpy().copy()) for value in input_queues.values()]
+        buffers = [np.copy(v.numpy().copy()) for v in input_queues.values()]
 
     if Device.DEFAULT != "QCOM":
       if test_val is not None:
-        match = all(arrays_match(lhs, rhs) for lhs, rhs in zip(val, test_val, strict=True))
+        match = all(arrays_match(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={current_seed})"
+        assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={seed})"
       if test_buffers is not None:
-        match = all(arrays_match(lhs, rhs) for lhs, rhs in zip(buffers, test_buffers, strict=True))
+        match = all(arrays_match(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={current_seed})"
+        assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={seed})"
     return val, buffers
 
-  print("capture + replay")
+  print('capture + replay')
-  test_val, test_buffers = random_inputs_run(jit, seed)
+  test_val, test_buffers = random_inputs_run(jit, SEED)
-  print("pickle round trip")
+  print('pickle round trip')
   jit = pickle.loads(pickle.dumps(jit))
-  random_inputs_run(jit, seed, test_val, test_buffers, expect_match=True)
+  random_inputs_run(jit, SEED, test_val, test_buffers, expect_match=True)
-  random_inputs_run(jit, seed + 1, test_val, test_buffers, expect_match=False)
+  random_inputs_run(jit, SEED+1, test_val, test_buffers, expect_match=False)
   return jit
 
 
-def _parse_size(size):
+def _parse_size(s):
-  width, height = size.lower().split("x")
+  w, h = s.lower().split('x')
-  return int(width), int(height)
+  return int(w), int(h)
 
 
 def read_file_chunked_to_shm(path):
   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:
+  with open(shm_path, 'wb') as f:
     f.write(read_file_chunked(path))
   return shm_path
 
 
 if __name__ == "__main__":
   from tinygrad.nn.onnx import OnnxRunner
-  from openpilot.selfdrive.modeld.get_model_metadata import make_metadata_dict
   from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
+  from openpilot.selfdrive.modeld.get_model_metadata import make_metadata_dict
+  p = argparse.ArgumentParser()
+  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,
+                 help='camera resolutions WxH (one or more)')
+  p.add_argument('--vision-onnx', required=True)
+  p.add_argument('--off-policy-onnx', required=True)
+  p.add_argument('--on-policy-onnx', required=True)
+  p.add_argument('--output', required=True)
+  p.add_argument('--frame-skip', type=int, required=True)
+  args = p.parse_args()
 
-  parser = argparse.ArgumentParser()
+  model_paths = {
-  parser.add_argument("--model-size", type=_parse_size, required=True, help="model input WxH")
+    'vision': read_file_chunked_to_shm(args.vision_onnx),
-  parser.add_argument("--camera-resolutions", type=_parse_size, nargs="+", required=True, help="camera resolutions WxH (one or more)")
+    'off_policy': read_file_chunked_to_shm(args.off_policy_onnx),
-  parser.add_argument("--vision-onnx", required=True)
+    'on_policy': read_file_chunked_to_shm(args.on_policy_onnx),
-  parser.add_argument("--off-policy-onnx", required=True)
+  }
-  parser.add_argument("--on-policy-onnx", required=True)
-  parser.add_argument("--output", required=True)
-  parser.add_argument("--frame-skip", type=int, required=True)
-  args = parser.parse_args()
-
-  out = defaultdict(dict)
-  vision_path = read_file_chunked_to_shm(args.vision_onnx)
-  off_policy_path = read_file_chunked_to_shm(args.off_policy_onnx)
-  on_policy_path = read_file_chunked_to_shm(args.on_policy_onnx)
   model_w, model_h = args.model_size
 
-  vision_runner = OnnxRunner(vision_path)
+  model_runners = {name: OnnxRunner(path) for name, path in model_paths.items()}
-  off_policy_runner = OnnxRunner(off_policy_path)
+  out = {'metadata': {name: make_metadata_dict(path) for name, path in model_paths.items()}}
-  on_policy_runner = OnnxRunner(on_policy_path)
-  vision_metadata = make_metadata_dict(vision_path)
-  off_policy_metadata = make_metadata_dict(off_policy_path)
-  on_policy_metadata = make_metadata_dict(on_policy_path)
-  assert off_policy_metadata["input_shapes"] == on_policy_metadata["input_shapes"]
 
-  run_policy_jit = TinyJit(
+  assert out['metadata']['off_policy']['input_shapes'] == out['metadata']['on_policy']['input_shapes']
-    make_run_policy(
-      vision_runner,
-      off_policy_runner,
-      on_policy_runner,
-      vision_metadata["output_slices"]["hidden_state"],
-      args.frame_skip,
-    ),
-    prune=True,
-  )
 
-  out["metadata"]["vision"] = vision_metadata
+  run_policy_jit = TinyJit(make_run_policy(model_runners, out['metadata'], args.frame_skip), prune=True)
-  out["metadata"]["off_policy"] = off_policy_metadata
-  out["metadata"]["on_policy"] = on_policy_metadata
-  out["tensor_inputs"] = make_tensor_inputs(vision_metadata["input_shapes"], on_policy_metadata["input_shapes"], args.frame_skip, Device.DEFAULT)
 
-  make_random_model_inputs = partial(make_random_images, keys=["img", "big_img"], shape=vision_metadata["input_shapes"]["img"])
+  make_policy_queues = partial(make_input_queues, out['metadata']['vision']['input_shapes'],
-  out["run_policy"] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS, args.frame_skip, vision_metadata, on_policy_metadata)
+                               out['metadata']['on_policy']['input_shapes'], args.frame_skip)
+  make_random_model_inputs = partial(make_random_images, keys=['img', 'big_img'], shape=out['metadata']['vision']['input_shapes']['img'])
+  out['run_policy'] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS,
+                                  make_policy_queues)
 
   for cam_w, cam_h in args.camera_resolutions:
     nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-    # Capture warp against blob-backed frames so the JIT ABI matches runtime VisionBuf inputs.
+    make_random_warp_inputs = make_random_blob_images(keys=['frame', 'big_frame'], size=nv12.size, device=WARP_DEV)
-    make_random_warp_inputs = partial(make_random_blob_images, keys=["frame", "big_frame"], shape=nv12.size, device=WARP_DEV)
     warp_enqueue = TinyJit(make_warp(nv12, model_w, model_h, args.frame_skip), prune=True)
-    out[(cam_w, cam_h)] = compile_jit(warp_enqueue, make_random_warp_inputs, WARP_INPUTS, args.frame_skip, vision_metadata, on_policy_metadata)
+    make_warp_queues = partial(make_warp_input_queues, out['metadata']['vision']['input_shapes'], args.frame_skip)
+    out[(cam_w,cam_h)] = compile_jit(warp_enqueue, make_random_warp_inputs, WARP_INPUTS, make_warp_queues)
 
   with open(args.output, "wb") as f:
     pickle.dump(out, f)

selfdrive/modeld/modeld_v16.py

@@ -4,7 +4,7 @@ from __future__ import annotations
 import os
 import pickle
 import time
-from pathlib import Path
+from pathlib import Path #I hate this
 
 from openpilot.system.hardware import TICI
 
@@ -17,6 +17,8 @@ from cereal import car, log
 from msgq.visionipc import VisionBuf, VisionIpcClient, VisionStreamType
 from opendbc.car.car_helpers import get_demo_car_params
 from setproctitle import setproctitle
+from tinygrad.dtype import dtypes
+from tinygrad.engine.jit import get_out_buffers_for_ei
 from tinygrad.tensor import Tensor
 
 from openpilot.common.file_chunker import read_file_chunked
@@ -27,17 +29,18 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.selfdrive.controls.lib.drive_helpers import smooth_value
+from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, get_curvature_from_plan, smooth_value
-from openpilot.selfdrive.modeld.compile_modeld import POLICY_INPUTS, WARP_INPUTS, make_npy_inputs, make_tensor_inputs
+from openpilot.selfdrive.modeld.compile_modeld import POLICY_INPUTS, make_input_queues
-from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 from openpilot.selfdrive.modeld.fill_model_msg import PublishState, fill_model_msg, fill_pose_msg
 from openpilot.selfdrive.modeld.helpers import get_tg_input_devices
+from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, DrivingModelFrame
 from openpilot.selfdrive.modeld.parse_model_outputs import Parser
+from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
 from openpilot.starpilot.assets.model_manager import ModelManager
 from openpilot.starpilot.common.model_versions import uses_combined_driving_artifacts
 from openpilot.starpilot.common.starpilot_variables import MODELS_PATH, get_starpilot_toggles, params_memory
 from openpilot.system import sentry
-from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
 
 
 PROCESS_NAME = "selfdrive.modeld.modeld"
@@ -113,9 +116,25 @@ def _combined_model_path(model_id: str, use_builtin_model: bool) -> Path:
 
 
 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, v_ego: float) -> log.ModelDataV2.Action:
-  desired_curv_unscaled, desired_accel = model_output["action"][0]
+  if "action" in model_output:
-  desired_curvature = float(desired_curv_unscaled) / max(1.0, v_ego) ** 2
+    desired_curv_unscaled, desired_accel = model_output["action"][0]
-  should_stop = (v_ego < 0.3 and desired_accel < 0.1)
+    desired_curvature = float(desired_curv_unscaled) / max(1.0, v_ego) ** 2
+    should_stop = (v_ego < 0.3 and desired_accel < 0.1)
+  else:
+    plan = model_output["plan"][0]
+    desired_accel, should_stop = get_accel_from_plan(
+      plan[:, Plan.VELOCITY][:, 0],
+      plan[:, Plan.ACCELERATION][:, 0],
+      ModelConstants.T_IDXS,
+      action_t=DT_MDL,
+    )
+    desired_curvature = get_curvature_from_plan(
+      plan[:, Plan.T_FROM_CURRENT_EULER][:, 2],
+      plan[:, Plan.ORIENTATION_RATE][:, 2],
+      ModelConstants.T_IDXS,
+      v_ego,
+      DT_MDL,
+    )
 
   desired_accel = smooth_value(float(desired_accel), prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
   if v_ego > MIN_LAT_CONTROL_SPEED:
@@ -143,7 +162,7 @@ class FrameMeta:
 class ModelState:
   prev_desire: np.ndarray
 
-  def __init__(self, cam_w: int, cam_h: int, usbgpu: bool):
+  def __init__(self, context: CLContext, usbgpu: bool):
     params = Params()
     model_id_raw = _resolve_mirrored_param(params, "Model", "DrivingModel") or BUILTIN_MODEL_KEY
     self.model_id = _canonical_model_id(model_id_raw)
@@ -184,70 +203,73 @@ class ModelState:
     self.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
     input_devices = get_tg_input_devices(PROCESS_NAME, usbgpu)
     self.WARP_DEV, self.QUEUE_DEV = input_devices["WARP_DEV"], input_devices["QUEUE_DEV"]
-    tensor_inputs = jits.get("tensor_inputs")
+    self.input_queues, self.npy = make_input_queues(
-    if tensor_inputs is None:
+      self.vision_input_shapes, self.policy_input_shapes, self.frame_skip, device=self.QUEUE_DEV
-      tensor_inputs = make_tensor_inputs(self.vision_input_shapes, self.policy_input_shapes, self.frame_skip, device=self.QUEUE_DEV)
+    )
-    self.npy, npy_tensors = make_npy_inputs(self.policy_input_shapes)
+    self.frames = {name: DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP) for name in self.vision_input_names}
-    self.input_queues = {**tensor_inputs, **npy_tensors}
+    self.vision_inputs: dict[str, Tensor] = {}
-    self.full_frames: dict[str, Tensor] = {}
-    self._blob_cache: dict[tuple[str, int], Tensor] = {}
     self.parser = Parser()
-    self.frame_buf_params = {key: get_nv12_info(cam_w, cam_h) for key in ("img", "big_img")}
     self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
-
+    self.run_policy = jits["run_policy"]
-    camera_jit = jits[(cam_w, cam_h)]
-    self.split_warp_layout = "run_policy" in jits and not isinstance(camera_jit, dict)
-    if self.split_warp_layout:
-      self.run_policy = jits["run_policy"]
-      self.warp_enqueue = camera_jit
-    else:
-      self.run_policy = camera_jit["run_policy"]
-      self.warp_enqueue = camera_jit["warp_enqueue"]
 
   def slice_outputs(self, model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]:
     return {key: model_outputs[np.newaxis, value] for key, value in output_slices.items()}
 
-  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:
+  def read_captured_outputs(self) -> tuple[np.ndarray, np.ndarray, np.ndarray] | None:
-    for key in bufs.keys():
+    captured = getattr(self.run_policy, "captured", None)
-      ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data
+    ret_output_map = getattr(captured, "ret_output_map", None)
-      yuv_size = self.frame_buf_params[key][3]
+    if captured is None or ret_output_map is None or len(ret_output_map) != 3:
-      cache_key = (key, ptr)
+      return None
-      if cache_key not in self._blob_cache:
-        self._blob_cache[cache_key] = Tensor.from_blob(ptr, (yuv_size,), dtype="uint8", device=self.WARP_DEV)
-      self.full_frames[key] = self._blob_cache[cache_key]
 
+    jit_outs = []
+    for ji in captured.jit_cache:
+      jit_outs.extend(get_out_buffers_for_ei(ji))
+
+    outputs = []
+    for idx in ret_output_map:
+      if idx is None or idx >= len(jit_outs):
+        return None
+      outputs.append(np.frombuffer(bytes(jit_outs[idx].as_memoryview()), dtype=np.float32).copy())
+    return tuple(outputs)
+
+  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:
     inputs[self.desire_key][0] = 0
     self.npy["desire"][:] = np.where(inputs[self.desire_key] - self.prev_desire > 0.99, inputs[self.desire_key], 0)
     self.prev_desire[:] = inputs[self.desire_key]
     self.npy["traffic_convention"][:] = inputs["traffic_convention"]
     if "action_t" in self.npy:
       self.npy["action_t"][:] = inputs["action_t"]
-    self.npy["tfm"][:, :] = transforms["img"][:, :]
-    self.npy["big_tfm"][:, :] = transforms["big_img"][:, :]
 
-    if self.split_warp_layout:
+    if prepare_only:
-      img, big_img = self.warp_enqueue(
+      return None
-        **{key: self.input_queues[key] for key in WARP_INPUTS},
+
-        frame=self.full_frames["img"],
+    imgs_cl = {name: self.frames[name].prepare(bufs[name], transforms[name].flatten()) for name in self.vision_input_names}
-        big_frame=self.full_frames["big_img"],
+    if TICI:
-      )
+      for key in imgs_cl:
-      if prepare_only:
+        if key not in self.vision_inputs:
-        return None
+          self.vision_inputs[key] = qcom_tensor_from_opencl_address(
-      policy_inputs = {key: self.input_queues[key] for key in POLICY_INPUTS if key in self.input_queues}
+            imgs_cl[key].mem_address,
-      vision_output, policy_output, off_policy_output = self.run_policy(**policy_inputs, img=img, big_img=big_img)
+            self.vision_input_shapes[key],
+            dtype=dtypes.uint8,
+          )
     else:
-      if prepare_only:
+      for key in imgs_cl:
-        self.warp_enqueue(**self.input_queues, frame=self.full_frames["img"], big_frame=self.full_frames["big_img"])
+        frame_input = self.frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.vision_input_shapes[key])
-        return None
+        self.vision_inputs[key] = Tensor(frame_input, dtype=dtypes.uint8).realize()
-      vision_output, policy_output, off_policy_output = self.run_policy(
-        **self.input_queues,
-        frame=self.full_frames["img"],
-        big_frame=self.full_frames["big_img"],
-      )
 
-    vision_output = vision_output.numpy().flatten()
+    vision_output, policy_output, off_policy_output = self.run_policy(
-    policy_output = policy_output.numpy().flatten()
+      **{key: self.input_queues[key] for key in POLICY_INPUTS if key in self.input_queues},
-    off_policy_output = off_policy_output.numpy().flatten()
+      img=self.vision_inputs["img"],
+      big_img=self.vision_inputs["big_img"],
+    )
+
+    captured_outputs = self.read_captured_outputs()
+    if captured_outputs is not None:
+      vision_output, policy_output, off_policy_output = captured_outputs
+    else:
+      vision_output = vision_output.numpy().flatten()
+      policy_output = policy_output.numpy().flatten()
+      off_policy_output = off_policy_output.numpy().flatten()
 
     vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(vision_output, self.vision_output_slices))
     off_policy_outputs_dict = self.parser.parse_off_policy_outputs(self.slice_outputs(off_policy_output, self.off_policy_output_slices))
@@ -294,8 +316,10 @@ def main(demo=False):
     cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
 
   start_time = time.monotonic()
+  cloudlog.warning("setting up CL context")
+  cl_context = CLContext()
   cloudlog.warning("loading combined model")
-  model = ModelState(vipc_client_main.width, vipc_client_main.height, usbgpu)
+  model = ModelState(cl_context, usbgpu)
   cloudlog.warning(f"combined model loaded in {time.monotonic() - start_time:.1f}s, modeld starting")
 
   pm = messaging.PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "starpilotModelV2"])

selfdrive/selfdrived/events.py

@@ -1083,7 +1083,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
       "",
       AlertStatus.normal, AlertSize.full,
       Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
-    ET.USER_DISABLE: ImmediateDisableAlert("Reverse Gear"),
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
     ET.NO_ENTRY: NoEntryAlert("Reverse Gear"),
   },
 
@@ -1478,7 +1478,7 @@ if HARDWARE.get_device_type() == 'mici':
         "",
         AlertStatus.normal, AlertSize.full,
         Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
-      ET.USER_DISABLE: ImmediateDisableAlert("Reverse"),
+      ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
       ET.NO_ENTRY: NoEntryAlert("Reverse"),
     },
   })

selfdrive/selfdrived/selfdrived.py

@@ -18,6 +18,7 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.common.gps import get_gps_location_service
 
 from openpilot.selfdrive.car.car_specific import CarSpecificEvents
+from openpilot.selfdrive.car.cruise_state import should_flag_cruise_mismatch
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 from openpilot.selfdrive.selfdrived.events import Events, ET
 from openpilot.selfdrive.selfdrived.helpers import ExcessiveActuationCheck
@@ -463,7 +464,8 @@ class SelfdriveD:
         self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise
       )
       effective_pcm_cruise = self.CP.pcmCruise or preap_software_cruise
-      cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not effective_pcm_cruise) and not pacifica_hybrid_aol
+      cruise_mismatch = should_flag_cruise_mismatch(self.CP, CS.cruiseState.enabled, self.enabled,
+                                                    effective_pcm_cruise) and not pacifica_hybrid_aol
       self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
       if self.cruise_mismatch_counter > int(6. / DT_CTRL):
         self.events.add(EventName.cruiseMismatch)

selfdrive/selfdrived/tests/test_alerts.py

@@ -13,6 +13,8 @@ from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
 from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS
 
 AlertSize = log.SelfdriveState.AlertSize
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+EventName = log.OnroadEvent.EventName
 
 OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/selfdrived/alerts_offroad.json")
 
@@ -104,6 +106,14 @@ class TestAlerts:
         if event_type not in (ET.WARNING, ET.PERMANENT, ET.PRE_ENABLE):
           assert a.creation_delay == 0.
 
+  def test_reverse_gear_user_disable_is_normal_disengage(self):
+    alert = EVENTS[EventName.reverseGear][ET.USER_DISABLE]
+    assert isinstance(alert, Alert)
+    assert alert.alert_text_1 == ""
+    assert alert.alert_text_2 == ""
+    assert alert.alert_size == AlertSize.none
+    assert alert.audible_alert == AudibleAlert.disengage
+
   def test_offroad_alerts(self):
     params = Params()
     for a in self.offroad_alerts:

selfdrive/ui/layouts/settings/starpilot/aethergrid.py

@@ -481,6 +481,37 @@ def draw_soft_card(rect: rl.Rectangle, fill: rl.Color, border: rl.Color, radius:
   _draw_rounded_stroke(rect, border, radius_px=radius_px, segments=segments)
 
 
+def draw_status_badges(
+  start_x: float,
+  y: float,
+  items: list[str],
+  style: PanelStyle,
+  *,
+  height: float = 28.0,
+  font_size: int = 15,
+  gap: float = 8.0,
+  padding_x: float = 18.0,
+  text_color: rl.Color = AetherListColors.HEADER,
+):
+  badge_x = start_x
+  font = gui_app.font(FontWeight.BOLD)
+  for item in items:
+    text_size = measure_text_cached(font, item, font_size)
+    badge_w = text_size.x + padding_x
+    badge_rect = rl.Rectangle(badge_x, y, badge_w, height)
+
+    fill_color = _with_alpha(style.accent, 24)
+    border_color = _with_alpha(style.accent, 80)
+    _draw_rounded_fill(badge_rect, fill_color, radius_px=8)
+    _draw_rounded_stroke(badge_rect, border_color, radius_px=8)
+
+    text_x = badge_rect.x + (badge_rect.width - text_size.x) / 2
+    text_y = badge_rect.y + (badge_rect.height - text_size.y) / 2
+    rl.draw_text_ex(font, item, rl.Vector2(round(text_x), round(text_y)), font_size, 0, text_color)
+
+    badge_x += badge_w + gap
+
+
 def draw_list_row_shell(
   rect: rl.Rectangle,
   *,

selfdrive/ui/layouts/settings/starpilot/longitudinal.py

@@ -240,6 +240,11 @@ class StarPilotLongitudinalLayout(_SettingsPage):
                    get_value=lambda: f"{self._params.get_int('ForceStopDistanceOffset'):+d} ft",
                    on_click=lambda: self._show_slider("ForceStopDistanceOffset", -20, 20, unit=" ft"),
                    visible=lambda: self._params.get_bool("QOLLongitudinal") and self._params.get_bool("ForceStops")),
+        SettingRow("RadarTakeoffs", "toggle", tr_noop("Radar for Takeoffs"),
+                   subtitle=tr_noop("Turns on/off using radar data to track leads at standstill, making following/takeoffs more responsive once leads move."),
+                   get_state=lambda: self._params.get_bool("RadarTakeoffs"),
+                   set_state=lambda s: self._params.put_bool("RadarTakeoffs", s),
+                   visible=lambda: self._params.get_bool("QOLLongitudinal") and starpilot_state.car_state.hasRadar),
       ], tab_key="daily", column_pair="daily"),
       SettingSection(tr_noop("Standstill & Gears"), [
         SettingRow("ForceStandstill", "toggle", tr_noop("Force Standstill"),

selfdrive/ui/layouts/settings/starpilot/vehicle.py

@@ -3,32 +3,36 @@ from __future__ import annotations
 import pyray as rl
 
 from openpilot.system.hardware import HARDWARE
-from openpilot.system.ui.lib.application import FontWeight, MouseEvent, MousePos, gui_app
+from openpilot.system.ui.lib.application import FontWeight, gui_app
 from openpilot.system.ui.lib.multilang import tr, tr_noop
 from openpilot.system.ui.lib.scroll_panel2 import GuiScrollPanel2
 from openpilot.system.ui.widgets import DialogResult, Widget
 from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog
 from openpilot.system.ui.widgets.label import gui_label
+from openpilot.system.ui.lib.text_measure import measure_text_cached
 from openpilot.system.ui.widgets.option_dialog import MultiOptionDialog
 from openpilot.selfdrive.ui.layouts.settings.starpilot.panel import _SettingsPage
 from openpilot.selfdrive.ui.layouts.settings.starpilot.aethergrid import (
-  AETHER_LIST_METRICS,
+  AetherListMetrics,
   AetherInteractiveMixin,
   AetherListColors,
   AetherScrollbar,
   AetherSliderDialog,
   panel_style_from_color,
-  _point_hits,
   draw_list_group_shell,
   draw_list_scroll_fades,
-  draw_metric_strip,
   draw_section_header,
   draw_selection_list_row,
   draw_settings_list_row,
   draw_settings_panel_header,
   draw_soft_card,
-  draw_tab_bar,
   init_list_panel,
+  TileGrid,
+  ToggleTile,
+  _with_alpha,
+  _draw_rounded_fill,
+  _draw_rounded_stroke,
+  draw_status_badges,
 )
 from openpilot.selfdrive.ui.lib.starpilot_state import starpilot_state
 from openpilot.selfdrive.ui.mici.layouts.settings.fingerprint_catalog import (
@@ -64,21 +68,33 @@ def _lock_doors_timer_labels():
   return labels
 
 
-SECTION_GAP = AETHER_LIST_METRICS.section_gap
+CUSTOM_METRICS = AetherListMetrics(
-SECTION_HEADER_HEIGHT = AETHER_LIST_METRICS.section_header_height
+  max_content_width=1560,
-SECTION_HEADER_GAP = AETHER_LIST_METRICS.section_header_gap
+  outer_margin_x=18,
-ROW_HEIGHT = AETHER_LIST_METRICS.row_height
+  outer_margin_y=10,
-FADE_HEIGHT = AETHER_LIST_METRICS.fade_height
+  panel_padding_x=16,
+  panel_padding_top=16,
+  panel_padding_bottom=12,
+  header_height=164,
+  section_gap=12,
+  section_header_height=28,
+  section_header_gap=8,
+  row_height=104,
+  utility_row_height=88,
+)
+
+SECTION_GAP = CUSTOM_METRICS.section_gap
+SECTION_HEADER_HEIGHT = CUSTOM_METRICS.section_header_height
+SECTION_HEADER_GAP = CUSTOM_METRICS.section_header_gap
+ROW_HEIGHT = CUSTOM_METRICS.row_height
+FADE_HEIGHT = CUSTOM_METRICS.fade_height
 PANEL_STYLE = panel_style_from_color("#64748B")
 
 
 class VehicleSettingsManagerView(AetherInteractiveMixin, Widget):
-  HEADER_SUBTITLE_HEIGHT = 24
+  HEADER_SUBTITLE_HEIGHT = 22
-  HEADER_SUMMARY_GAP = 12
+  HEADER_SUMMARY_GAP = 10
-  HEADER_CARD_HEIGHT = 108
+  HEADER_CARD_HEIGHT = 100
-  TAB_HEIGHT = 68
-  TAB_GAP = 10
-  TAB_BOTTOM_GAP = 18
   TWO_COLUMN_BREAKPOINT = 1180
   COLUMN_GAP = 22
 
@@ -89,15 +105,130 @@ class VehicleSettingsManagerView(AetherInteractiveMixin, Widget):
     self._scrollbar = AetherScrollbar()
     self._content_height = 0.0
     self._scroll_offset = 0.0
-    self._active_tab_key = "identity"
     self._shell_rect = rl.Rectangle(0, 0, 0, 0)
     self._scroll_rect = rl.Rectangle(0, 0, 0, 0)
 
-    self._tab_defs = [
+    self._toggle_grid = TileGrid(columns=2, padding=12, min_tile_width=100)
-      {"id": "identity", "title": tr("Identity")},
+    self._toggle_grid.set_touch_valid_callback(lambda: self._scroll_panel.is_touch_valid())
-      {"id": "features", "title": tr("Features")},
+    self._child(self._toggle_grid)
-      {"id": "controls", "title": tr("Controls")},
+
-    ]
+    self._last_make = ""
+    self._last_model = ""
+
+  def _build_driving_toggles(self) -> list[dict]:
+    cs = starpilot_state.car_state
+    toggles = []
+
+    toggles.append({
+      "title": tr("Disable Fingerprinting"),
+      "subtitle": tr("Manually select vehicle instead of auto-detecting."),
+      "get_state": lambda: self._controller._params.get_bool("ForceFingerprint"),
+      "set_state": lambda s: self._controller._on_toggle("ForceFingerprint"),
+    })
+
+    toggles.append({
+      "title": tr("Disable openpilot Long"),
+      "subtitle": tr("Revert to stock longitudinal control."),
+      "get_state": lambda: self._controller._params.get_bool("DisableOpenpilotLongitudinal"),
+      "set_state": lambda s: self._controller._on_toggle("DisableOpenpilotLongitudinal"),
+    })
+
+    if cs.isGM and (cs.hasPedal or cs.canUsePedal):
+      toggles.append({
+        "title": tr("Pedal for Long"),
+        "get_state": lambda: self._controller._params.get_bool("GMPedalLongitudinal"),
+        "set_state": lambda s: self._controller._on_toggle("GMPedalLongitudinal"),
+      })
+      toggles.append({
+        "title": tr("Offsets on Dash Spoof"),
+        "get_state": lambda: self._controller._params.get_bool("GMDashSpoofOffsets"),
+        "set_state": lambda s: self._controller._on_toggle("GMDashSpoofOffsets"),
+      })
+    if cs.isGM:
+      toggles.append({
+        "title": tr("Remote Start Panda"),
+        "get_state": lambda: self._controller._params.get_bool("RemoteStartBootsComma"),
+        "set_state": lambda s: self._controller._on_toggle("RemoteStartBootsComma"),
+      })
+    if cs.isGM and cs.isVolt and not cs.hasSNG:
+      toggles.append({
+        "title": tr("Volt SNG Hack"),
+        "get_state": lambda: self._controller._params.get_bool("VoltSNG"),
+        "set_state": lambda s: self._controller._on_toggle("VoltSNG"),
+      })
+
+    if cs.isSubaru:
+      toggles.append({
+        "title": tr("Stop and Go"),
+        "get_state": lambda: self._controller._params.get_bool("SubaruSNG"),
+        "set_state": lambda s: self._controller._on_toggle("SubaruSNG"),
+      })
+
+    if cs.isToyota:
+      toggles.append({
+        "title": tr("Auto Lock Doors"),
+        "get_state": lambda: self._controller._params.get_bool("LockDoors"),
+        "set_state": lambda s: self._controller._on_toggle("LockDoors"),
+      })
+      toggles.append({
+        "title": tr("Auto Unlock Doors"),
+        "get_state": lambda: self._controller._params.get_bool("UnlockDoors"),
+        "set_state": lambda s: self._controller._on_toggle("UnlockDoors"),
+      })
+    if cs.isToyota and not cs.hasSNG:
+      toggles.append({
+        "title": tr("Stop-and-Go Hack"),
+        "get_state": lambda: self._controller._params.get_bool("SNGHack"),
+        "set_state": lambda s: self._controller._on_toggle("SNGHack"),
+      })
+    if cs.isToyota and cs.hasOpenpilotLongitudinal:
+      toggles.append({
+        "title": tr("FrogsGoMoo Tweak"),
+        "get_state": lambda: self._controller._params.get_bool("FrogsGoMoosTweak"),
+        "set_state": lambda s: self._controller._on_toggle("FrogsGoMoosTweak"),
+      })
+
+    if cs.isBolt and cs.hasPedal:
+      toggles.append({
+        "title": tr("Remap Cancel Button"),
+        "subtitle": tr("Treat the Cancel button as an extra mappable steering-wheel button."),
+        "get_state": lambda: self._controller._params.get_bool("RemapCancelToDistance"),
+        "set_state": lambda s: self._controller._on_toggle("RemapCancelToDistance"),
+      })
+
+    if cs.isHKGCanFd and cs.hasOpenpilotLongitudinal:
+      toggles.append({
+        "title": tr("Nostalgia Mode"),
+        "subtitle": tr("Use the left paddle to pause openpilot acceleration and braking."),
+        "get_state": lambda: self._controller._params.get_bool("NostalgiaMode"),
+        "set_state": lambda s: self._controller._on_toggle("NostalgiaMode"),
+      })
+
+    return toggles
+
+  def _rebuild_toggle_grid(self):
+    self._toggle_grid.clear()
+    toggles = self._build_driving_toggles()
+    self._toggle_grid._columns = len(toggles)
+    for toggle_def in toggles:
+      tile = ToggleTile(
+        title=toggle_def["title"],
+        get_state=toggle_def["get_state"],
+        set_state=toggle_def["set_state"],
+        bg_color=PANEL_STYLE.accent,
+        desc=toggle_def.get("subtitle", ""),
+        is_enabled=toggle_def.get("is_enabled"),
+        disabled_label=toggle_def.get("disabled_label", ""),
+      )
+      self._toggle_grid.add_tile(tile)
+
+  def _check_rebuild_grid(self):
+    current_make = self._controller._get_display_make()
+    current_model = self._controller._get_display_model()
+    if current_make != self._last_make or current_model != self._last_model:
+      self._last_make = current_make
+      self._last_model = current_model
+      self._rebuild_toggle_grid()
 
   def _uses_two_columns(self, width: float) -> bool:
     return width >= self.TWO_COLUMN_BREAKPOINT
@@ -122,41 +253,15 @@ class VehicleSettingsManagerView(AetherInteractiveMixin, Widget):
     if not target_id:
       return
     prefix, _, value = target_id.partition(":")
-    if prefix == "tab":
-      self._active_tab_key = value
-      return
     if prefix == "toggle":
       self._controller._on_toggle(value)
     elif prefix == "select":
       self._controller._on_select(value)
 
-  def _tab_subtitle(self, tab_id: str) -> str:
-    cs = starpilot_state.car_state
-    if tab_id == "identity":
-      return tr("Make, model, and fingerprint")
-    if tab_id == "features":
-      count = 1
-      if cs.isGM: count += 4
-      if cs.isGM and cs.isVolt and not cs.hasSNG: count += 1
-      if cs.isHKG and cs.isHKGCanFd: count += 2
-      if cs.isSubaru: count += 1
-      if cs.isToyota: count += 4
-      if cs.isToyota and not cs.hasSNG: count += 1
-      if cs.isToyota and cs.hasOpenpilotLongitudinal: count += 1
-      if cs.isHKGCanFd and cs.hasOpenpilotLongitudinal: count += 1
-      return tr("{} settings").format(count)
-    if tab_id == "controls":
-      count = 8
-      if not cs.isSubaru:
-        count += 1
-      if cs.hasModeStarButtons: count += 6
-      return tr("{} buttons").format(count)
-    return ""
-
   def _render(self, rect: rl.Rectangle):
     self.set_rect(rect)
 
-    frame, scroll_rect, content_width = init_list_panel(rect, PANEL_STYLE)
+    frame, scroll_rect, content_width = init_list_panel(rect, PANEL_STYLE, CUSTOM_METRICS)
     self._shell_rect = frame.shell
     self._scroll_rect = scroll_rect
 
@@ -180,48 +285,40 @@ class VehicleSettingsManagerView(AetherInteractiveMixin, Widget):
                                 tr("Configure vehicle fingerprint, driving features, and steering controls."),
                                 subtitle_size=22)
 
-    summary_y = rect.y + 48 + self.HEADER_SUBTITLE_HEIGHT + self.HEADER_SUMMARY_GAP
+    summary_y = rect.y + 44 + self.HEADER_SUBTITLE_HEIGHT + self.HEADER_SUMMARY_GAP
     summary_rect = rl.Rectangle(rect.x, summary_y, rect.width, min(self.HEADER_CARD_HEIGHT, rect.y + rect.height - summary_y))
     self._draw_summary_card(summary_rect)
 
   def _draw_summary_card(self, rect: rl.Rectangle):
     draw_soft_card(rect, PANEL_STYLE.surface_fill, PANEL_STYLE.surface_border)
-    inset = 18
-    left_x = rect.x + inset
-    left_w = rect.width * 0.40
 
+    inset = 24
+    avail_w = rect.width - inset * 2
+    col1_w = avail_w * 0.32
+    col2_w = avail_w * 0.20
+    col3_w = avail_w * 0.24
+    col4_w = avail_w * 0.24
+
+    col1_x = rect.x + inset
+    col2_x = col1_x + col1_w
+    col3_x = col2_x + col2_w
+    col4_x = col3_x + col3_w
+
+    # 1. Current Vehicle
     make = self._controller._get_display_make()
     model = self._controller._get_display_model()
     vehicle_name = f"{make} {model}" if make != tr("None") else tr("No vehicle selected")
 
-    gui_label(rl.Rectangle(left_x, rect.y + 10, left_w, 22), tr("Current Vehicle"), 20, AetherListColors.MUTED, FontWeight.MEDIUM)
+    gui_label(rl.Rectangle(col1_x, rect.y + 16, col1_w - 16, 18), tr("CURRENT VEHICLE"), 14, AetherListColors.MUTED, FontWeight.MEDIUM)
-    gui_label(rl.Rectangle(left_x, rect.y + 34, left_w, 30), vehicle_name, 26, AetherListColors.HEADER, FontWeight.BOLD)
+    gui_label(rl.Rectangle(col1_x, rect.y + 38, col1_w - 16, 28), vehicle_name, 22, AetherListColors.HEADER, FontWeight.BOLD)
 
+    # 2. Fingerprint
+    fingerprint_state = tr("Forced") if self._controller._params.get_bool("ForceFingerprint") else tr("Auto")
+    gui_label(rl.Rectangle(col2_x, rect.y + 16, col2_w - 16, 18), tr("FINGERPRINT"), 14, AetherListColors.MUTED, FontWeight.MEDIUM)
+    gui_label(rl.Rectangle(col2_x, rect.y + 38, col2_w - 16, 26), fingerprint_state, 20, AetherListColors.HEADER, FontWeight.SEMI_BOLD)
+
+    # 3. Hardware
     cs = starpilot_state.car_state
-    metrics = []
-    if cs.hasRadar:
-      metrics.append((tr("Radar"), tr("Yes")))
-    if cs.hasOpenpilotLongitudinal:
-      metrics.append((tr("Long"), tr("Yes")))
-    if cs.hasBSM:
-      metrics.append((tr("BSM"), tr("Yes")))
-    if cs.hasSNG:
-      metrics.append((tr("SNG"), tr("Yes")))
-
-    if metrics:
-      draw_metric_strip(
-        rl.Rectangle(left_x, rect.y + 72, max(240.0, rect.width * 0.38), 30),
-        metrics,
-        style=PANEL_STYLE,
-        label_top_offset=0,
-        value_top_offset=14,
-        divider_top_offset=2,
-        divider_bottom_offset=16,
-      )
-
-    right_x = rect.x + rect.width * 0.42
-    right_w = rect.width * 0.58 - inset
-
     hardware_items = []
     if cs.canUsePedal:
       hardware_items.append(tr("Pedal"))
@@ -232,105 +329,178 @@ class VehicleSettingsManagerView(AetherInteractiveMixin, Widget):
     if cs.hasZSS:
       hardware_items.append(tr("ZSS"))
 
-    hw_text = ", ".join(hardware_items) if hardware_items else tr("Standard")
+    gui_label(rl.Rectangle(col3_x, rect.y + 16, col3_w - 16, 18), tr("HARDWARE"), 14, AetherListColors.MUTED, FontWeight.MEDIUM)
-    gui_label(rl.Rectangle(right_x, rect.y + 10, right_w, 22), tr("Hardware"), 20, AetherListColors.MUTED, FontWeight.MEDIUM)
+    if not hardware_items:
-    gui_label(rl.Rectangle(right_x, rect.y + 34, right_w, 26), hw_text, 24, AetherListColors.HEADER, FontWeight.MEDIUM)
+      gui_label(rl.Rectangle(col3_x, rect.y + 38, col3_w - 16, 26), tr("Standard"), 18, AetherListColors.MUTED, FontWeight.MEDIUM)
+    else:
+      draw_status_badges(col3_x, rect.y + 37, hardware_items, PANEL_STYLE)
 
-    fingerprint_state = tr("Forced") if self._controller._params.get_bool("ForceFingerprint") else tr("Auto")
+    # 4. Capabilities
-    gui_label(rl.Rectangle(right_x, rect.y + 66, right_w, 20), tr("Fingerprint"), 18, AetherListColors.MUTED, FontWeight.MEDIUM)
+    gui_label(rl.Rectangle(col4_x, rect.y + 16, col4_w - 16, 18), tr("CAPABILITIES"), 14, AetherListColors.MUTED, FontWeight.MEDIUM)
-    gui_label(rl.Rectangle(right_x, rect.y + 84, right_w, 20), fingerprint_state, 18, AetherListColors.HEADER, FontWeight.MEDIUM)
+
+    metrics = []
+    if cs.hasRadar:
+      metrics.append(tr("Radar"))
+    if cs.hasOpenpilotLongitudinal:
+      metrics.append(tr("Long"))
+    if cs.hasBSM:
+      metrics.append(tr("BSM"))
+    if cs.hasSNG:
+      metrics.append(tr("SNG"))
+
+    if not metrics:
+      gui_label(rl.Rectangle(col4_x, rect.y + 38, col4_w - 16, 26), tr("Standard"), 18, AetherListColors.MUTED, FontWeight.MEDIUM)
+    else:
+      draw_status_badges(col4_x, rect.y + 37, metrics, PANEL_STYLE)
 
   def _measure_content_height(self, width: float) -> float:
-    content_height = self._measure_active_tab_height(width)
+    self._check_rebuild_grid()
-    return self.TAB_HEIGHT + self.TAB_BOTTOM_GAP + content_height
+    cs = starpilot_state.car_state
 
-  def _measure_active_tab_height(self, width: float) -> float:
+    # Left Column heights
-    if self._active_tab_key == "identity":
+    identity_rows = 2
-      return self._section_block_height(self._section_height(3, ROW_HEIGHT))
+    if cs.isToyota:
-    if self._active_tab_key == "features":
+      identity_rows += 2
-      rows = self._build_driving_rows()
+    identity_h = self._section_block_height(self._section_height(identity_rows, ROW_HEIGHT))
+
+    steering_rows = self._build_steering_rows()
+    steering_h = self._section_block_height(self._section_height(len(steering_rows), ROW_HEIGHT))
+
+    left_h = identity_h + SECTION_GAP + steering_h
+
+    # Right Column/Features height
+    tiles_height = 0.0
+    if self._toggle_grid.tiles:
+      N = len(self._toggle_grid.tiles)
+      gap = self._toggle_grid.gap
       if self._uses_two_columns(width):
-        max_per_col = (len(rows) + 1) // 2
+        cols = 2
-        return self._section_block_height(self._section_height(max_per_col, ROW_HEIGHT))
+        tile_rows = (N + cols - 1) // cols
-      return self._section_block_height(self._section_height(len(rows), ROW_HEIGHT))
+        tile_gaps = gap * (tile_rows - 1) if tile_rows > 0 else 0
-    if self._active_tab_key == "controls":
+        tiles_content_h = tile_rows * 130 + tile_gaps
-      rows = self._build_steering_rows()
+        tiles_height = self._section_block_height(tiles_content_h + 24)
-      if self._uses_two_columns(width):
+      else:
-        max_per_col = (len(rows) + 1) // 2
+        avail_w = width - 24
-        return self._section_block_height(self._section_height(max_per_col, ROW_HEIGHT))
+        cols = 3
-      return self._section_block_height(self._section_height(len(rows), ROW_HEIGHT))
+        tile_rows = (N + cols - 1) // cols
-    return 0
+        tile_gaps = gap * (tile_rows - 1) if tile_rows > 0 else 0
+        tiles_content_h = tile_rows * 130 + tile_gaps
+        tiles_height = SECTION_GAP + self._section_block_height(tiles_content_h + 24)
+
+    if self._uses_two_columns(width):
+      return max(left_h, tiles_height)
+    return left_h + tiles_height
 
   def _draw_scroll_content(self, rect: rl.Rectangle, width: float):
     self._interactive_rects.clear()
     y = rect.y + self._scroll_offset
-    self._draw_tabs(rl.Rectangle(rect.x, y, width, self.TAB_HEIGHT))
+    self._draw_panel_content(y, rect.x, width)
-    y += self.TAB_HEIGHT + self.TAB_BOTTOM_GAP
 
-    if self._active_tab_key == "identity":
+  def _draw_panel_content(self, y: float, x: float, width: float):
-      self._draw_identity_tab(y, rect.x, width)
+    self._check_rebuild_grid()
-    elif self._active_tab_key == "features":
+    cs = starpilot_state.car_state
-      self._draw_features_tab(y, rect.x, width)
-    else:
-      self._draw_controls_tab(y, rect.x, width)
 
-  def _draw_tabs(self, rect: rl.Rectangle):
+    identity_rows = [
-    draw_tab_bar(
-      rect, self._tab_defs, self._active_tab_key, self._interactive_state,
-      subtitle_fn=self._tab_subtitle, style=PANEL_STYLE,
-    )
-
-  def _draw_identity_tab(self, y: float, x: float, width: float):
-    rows = [
       {"target_id": "select:CarMake", "type": "select", "title": tr("Car Make"),
        "get_value": self._controller._get_display_make, "pill_width": 160},
       {"target_id": "select:CarModel", "type": "select", "title": tr("Car Model"),
        "get_value": self._controller._get_display_model, "pill_width": 160},
-      {"target_id": "toggle:ForceFingerprint", "type": "toggle", "title": tr("Disable Fingerprinting"),
-       "subtitle": tr("Manually select vehicle instead of auto-detecting."),
-       "get_state": lambda: self._controller._params.get_bool("ForceFingerprint")},
     ]
-    draw_section_header(rl.Rectangle(x, y, width, SECTION_HEADER_HEIGHT), tr("Vehicle Identity"), style=PANEL_STYLE)
+    if cs.isToyota:
-    y += SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP
+      identity_rows.append({"target_id": "select:LockDoorsTimer", "type": "select",
-    container_rect = rl.Rectangle(x, y, width, len(rows) * ROW_HEIGHT)
+                    "title": tr("Lock Doors Timer"),
-    draw_list_group_shell(container_rect, style=PANEL_STYLE)
+                    "get_value": lambda: _lock_doors_timer_labels().get(float(self._controller._params.get_int("LockDoorsTimer")), f"{self._controller._params.get_int('LockDoorsTimer')}s"),
-    for index, row in enumerate(rows):
+                    "pill_width": 100})
-      row_rect = rl.Rectangle(x, y + index * ROW_HEIGHT, width, ROW_HEIGHT)
+      identity_rows.append({"target_id": "select:ClusterOffset", "type": "select",
-      self._draw_row(row_rect, row, is_last=index == len(rows) - 1)
+                    "title": tr("Dashboard Speed Offset"),
+                    "get_value": lambda: f"{self._controller._params.get_float('ClusterOffset'):.3f}x",
+                    "pill_width": 120})
+
+    steering_rows = self._build_steering_rows()
 
-  def _draw_features_tab(self, y: float, x: float, width: float):
-    rows = self._build_driving_rows()
-    if not rows:
-      return
     if self._uses_two_columns(width):
       column_w = self._column_width(width)
-      mid = len(rows) // 2
-      self._draw_row_group(y, x, column_w, rows[:mid])
-      self._draw_row_group(y, x + column_w + self.COLUMN_GAP, column_w, rows[mid:])
-    else:
-      self._draw_row_group(y, x, width, rows)
 
-  def _draw_controls_tab(self, y: float, x: float, width: float):
+      # Left Column: Vehicle Identity & Steering Controls
-    rows = self._build_steering_rows()
+      curr_y = y
-    if not rows:
+      draw_section_header(rl.Rectangle(x, curr_y, column_w, SECTION_HEADER_HEIGHT), tr("Vehicle Identity"), style=PANEL_STYLE)
-      return
+      curr_y += SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP
-    if self._uses_two_columns(width):
+      container_rect = rl.Rectangle(x, curr_y, column_w, len(identity_rows) * ROW_HEIGHT)
-      column_w = self._column_width(width)
+      draw_list_group_shell(container_rect, style=PANEL_STYLE)
-      mid = len(rows) // 2
+      for index, row in enumerate(identity_rows):
-      self._draw_row_group(y, x, column_w, rows[:mid])
+        row_rect = rl.Rectangle(x, curr_y + index * ROW_HEIGHT, column_w, ROW_HEIGHT)
-      self._draw_row_group(y, x + column_w + self.COLUMN_GAP, column_w, rows[mid:])
+        self._draw_row(row_rect, row, is_last=index == len(identity_rows) - 1)
-    else:
+      curr_y += len(identity_rows) * ROW_HEIGHT
-      self._draw_row_group(y, x, width, rows)
 
-  def _draw_row_group(self, y: float, x: float, width: float, rows: list[dict]):
+      curr_y += SECTION_GAP
-    if not rows:
+      draw_section_header(rl.Rectangle(x, curr_y, column_w, SECTION_HEADER_HEIGHT), tr("Steering Controls"), style=PANEL_STYLE)
-      return y
+      curr_y += SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP
-    container_rect = rl.Rectangle(x, y, width, len(rows) * ROW_HEIGHT)
+      container_rect = rl.Rectangle(x, curr_y, column_w, len(steering_rows) * ROW_HEIGHT)
-    draw_list_group_shell(container_rect, style=PANEL_STYLE)
+      draw_list_group_shell(container_rect, style=PANEL_STYLE)
-    for index, row in enumerate(rows):
+      for index, row in enumerate(steering_rows):
-      row_rect = rl.Rectangle(x, y + index * ROW_HEIGHT, width, ROW_HEIGHT)
+        row_rect = rl.Rectangle(x, curr_y + index * ROW_HEIGHT, column_w, ROW_HEIGHT)
-      self._draw_row(row_rect, row, is_last=index == len(rows) - 1)
+        self._draw_row(row_rect, row, is_last=index == len(steering_rows) - 1)
-    return y + len(rows) * ROW_HEIGHT
+      left_end_y = curr_y + len(steering_rows) * ROW_HEIGHT
+
+      # Right Column: Features
+      if self._toggle_grid.tiles:
+        rx = x + column_w + self.COLUMN_GAP
+        draw_section_header(rl.Rectangle(rx, y, column_w, SECTION_HEADER_HEIGHT), tr("Features"), style=PANEL_STYLE)
+        right_container_y = y + SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP
+
+        N = len(self._toggle_grid.tiles)
+        cols = 2
+        self._toggle_grid._columns = cols
+        gap = self._toggle_grid.gap
+        tile_rows = (N + cols - 1) // cols
+        tile_gaps = gap * (tile_rows - 1) if tile_rows > 0 else 0
+        tiles_content_h = tile_rows * 130 + tile_gaps
+
+        needed_height = tiles_content_h + 24
+        left_content_height = left_end_y - right_container_y
+        container_h = max(needed_height, left_content_height)
+
+        draw_list_group_shell(rl.Rectangle(rx, right_container_y, column_w, container_h), style=PANEL_STYLE)
+        self._toggle_grid.set_parent_rect(self._scroll_rect)
+        self._toggle_grid.render(rl.Rectangle(rx + 12, right_container_y + 12, column_w - 24, container_h - 24))
+    else:
+      # Single Column Stacked Layout
+      draw_section_header(rl.Rectangle(x, y, width, SECTION_HEADER_HEIGHT), tr("Vehicle Identity"), style=PANEL_STYLE)
+      y += SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP
+      container_rect = rl.Rectangle(x, y, width, len(identity_rows) * ROW_HEIGHT)
+      draw_list_group_shell(container_rect, style=PANEL_STYLE)
+      for index, row in enumerate(identity_rows):
+        row_rect = rl.Rectangle(x, y + index * ROW_HEIGHT, width, ROW_HEIGHT)
+        self._draw_row(row_rect, row, is_last=index == len(identity_rows) - 1)
+      y += len(identity_rows) * ROW_HEIGHT
+
+      y += SECTION_GAP
+      draw_section_header(rl.Rectangle(x, y, width, SECTION_HEADER_HEIGHT), tr("Steering Controls"), style=PANEL_STYLE)
+      y += SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP
+      container_rect = rl.Rectangle(x, y, width, len(steering_rows) * ROW_HEIGHT)
+      draw_list_group_shell(container_rect, style=PANEL_STYLE)
+      for index, row in enumerate(steering_rows):
+        row_rect = rl.Rectangle(x, y + index * ROW_HEIGHT, width, ROW_HEIGHT)
+        self._draw_row(row_rect, row, is_last=index == len(steering_rows) - 1)
+      y += len(steering_rows) * ROW_HEIGHT
+
+      if self._toggle_grid.tiles:
+        y += SECTION_GAP
+        draw_section_header(rl.Rectangle(x, y, width, SECTION_HEADER_HEIGHT), tr("Features"), style=PANEL_STYLE)
+        y += SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP
+
+        N = len(self._toggle_grid.tiles)
+        cols = 3
+        self._toggle_grid._columns = cols
+        gap = self._toggle_grid.gap
+        avail_w = width - 24
+        tile_rows = (N + cols - 1) // cols
+        tile_gaps = gap * (tile_rows - 1) if tile_rows > 0 else 0
+        tiles_content_h = tile_rows * 130 + tile_gaps
+
+        draw_list_group_shell(rl.Rectangle(x, y, width, tiles_content_h + 24), style=PANEL_STYLE)
+        self._toggle_grid.set_parent_rect(self._scroll_rect)
+        self._toggle_grid.render(rl.Rectangle(x + 12, y + 12, avail_w, tiles_content_h))
 
   def _draw_row(self, rect: rl.Rectangle, row: dict, is_last: bool):
     target_id = row["target_id"]
@@ -364,60 +534,6 @@ class VehicleSettingsManagerView(AetherInteractiveMixin, Widget):
         style=PANEL_STYLE,
       )
 
-  def _build_driving_rows(self) -> list[dict]:
-    cs = starpilot_state.car_state
-    rows = []
-    rows.append({"target_id": "toggle:DisableOpenpilotLongitudinal", "type": "toggle",
-                  "title": tr("Disable openpilot Long"), "subtitle": tr("Revert to stock longitudinal control."),
-                  "get_state": lambda: self._controller._params.get_bool("DisableOpenpilotLongitudinal")})
-
-    if cs.isGM and (cs.hasPedal or cs.canUsePedal):
-      rows.append({"target_id": "toggle:GMPedalLongitudinal", "type": "toggle",
-                    "title": tr("Pedal for Long"), "get_state": lambda: self._controller._params.get_bool("GMPedalLongitudinal")})
-      rows.append({"target_id": "toggle:GMDashSpoofOffsets", "type": "toggle",
-                    "title": tr("Offsets on Dash Spoof"), "get_state": lambda: self._controller._params.get_bool("GMDashSpoofOffsets")})
-    if cs.isGM:
-      rows.append({"target_id": "toggle:LongPitch", "type": "toggle",
-                    "title": tr("Smooth Pedal on Hills"), "get_state": lambda: self._controller._params.get_bool("LongPitch")})
-      rows.append({"target_id": "toggle:RemoteStartBootsComma", "type": "toggle",
-                    "title": tr("Remote Start Panda"), "get_state": lambda: self._controller._params.get_bool("RemoteStartBootsComma")})
-    if cs.isGM and cs.isVolt and not cs.hasSNG:
-      rows.append({"target_id": "toggle:VoltSNG", "type": "toggle",
-                    "title": tr("Volt SNG Hack"), "get_state": lambda: self._controller._params.get_bool("VoltSNG")})
-    if cs.isSubaru:
-      rows.append({"target_id": "toggle:SubaruSNG", "type": "toggle",
-                    "title": tr("Stop and Go"), "get_state": lambda: self._controller._params.get_bool("SubaruSNG")})
-    if cs.isToyota:
-      rows.append({"target_id": "toggle:LockDoors", "type": "toggle",
-                    "title": tr("Auto Lock Doors"), "get_state": lambda: self._controller._params.get_bool("LockDoors")})
-      rows.append({"target_id": "toggle:UnlockDoors", "type": "toggle",
-                    "title": tr("Auto Unlock Doors"), "get_state": lambda: self._controller._params.get_bool("UnlockDoors")})
-      rows.append({"target_id": "select:LockDoorsTimer", "type": "select",
-                    "title": tr("Lock Doors Timer"),
-                    "get_value": lambda: _lock_doors_timer_labels().get(float(self._controller._params.get_int("LockDoorsTimer")), f"{self._controller._params.get_int('LockDoorsTimer')}s"),
-                    "pill_width": 100})
-      rows.append({"target_id": "select:ClusterOffset", "type": "select",
-                    "title": tr("Dashboard Speed Offset"),
-                    "get_value": lambda: f"{self._controller._params.get_float('ClusterOffset'):.3f}x",
-                    "pill_width": 120})
-    if cs.isToyota and not cs.hasSNG:
-      rows.append({"target_id": "toggle:SNGHack", "type": "toggle",
-                    "title": tr("Stop-and-Go Hack"), "get_state": lambda: self._controller._params.get_bool("SNGHack")})
-    if cs.isToyota and cs.hasOpenpilotLongitudinal:
-      rows.append({"target_id": "toggle:FrogsGoMoosTweak", "type": "toggle",
-                    "title": tr("FrogsGoMoo Tweak"), "get_state": lambda: self._controller._params.get_bool("FrogsGoMoosTweak")})
-
-    if cs.isBolt and cs.hasPedal:
-      rows.append({"target_id": "toggle:RemapCancelToDistance", "type": "toggle",
-                    "title": tr("Remap Cancel Button"), "subtitle": tr("Treat the Cancel button as an extra mappable steering-wheel button."),
-                    "get_state": lambda: self._controller._params.get_bool("RemapCancelToDistance")})
-    if cs.isHKGCanFd and cs.hasOpenpilotLongitudinal:
-      rows.append({"target_id": "toggle:NostalgiaMode", "type": "toggle",
-                    "title": tr("Nostalgia Mode"),
-                    "subtitle": tr("Use the left paddle to pause openpilot acceleration and braking."),
-                    "get_state": lambda: self._controller._params.get_bool("NostalgiaMode")})
-    return rows
-
   def _build_steering_rows(self) -> list[dict]:
     cs = starpilot_state.car_state
     rows = []

selfdrive/ui/mici/onroad/augmented_road_view.py

@@ -307,14 +307,6 @@ class StandstillTimerOverlay:
 
     self._standstill_duration = int(now - self._standstill_started_at)
 
-  @staticmethod
-  def _format_duration_text(total_seconds: int) -> tuple[str, str]:
-    minutes = total_seconds // 60
-    seconds = total_seconds % 60
-    minute_text = f"{minutes} minute" if minutes == 1 else f"{minutes} minutes"
-    second_text = f"{seconds} second" if seconds == 1 else f"{seconds} seconds"
-    return minute_text, second_text
-
   def _draw_centered_text(self, rect: rl.Rectangle, text: str, y: float, font: rl.Font, font_size: int, color: rl.Color) -> None:
     text_size = rl.measure_text_ex(font, text, font_size, 0)
     text_pos = rl.Vector2(rect.x + rect.width / 2 - text_size.x / 2, rect.y + y - text_size.y / 2)
@@ -334,13 +326,14 @@ class StandstillTimerOverlay:
     if self._standstill_duration == 0:
       return False
 
-    minute_text, second_text = self._format_duration_text(self._standstill_duration)
+    minutes = self._standstill_duration // 60
+    seconds = self._standstill_duration % 60
+    duration_text = f"{minutes:02d}:{seconds:02d}"
     duration_color = self._get_duration_color()
     max_text_width = max(rect.width - 36, 120)
-    minute_font_size = self._fit_font_size(self._font_bold, minute_text, int(rect.height * 0.34), max_text_width, 28)
+
-    second_font_size = self._fit_font_size(self._font_medium, second_text, int(rect.height * 0.15), max_text_width, 16)
+    duration_font_size = self._fit_font_size(self._font_bold, duration_text, int(rect.height * 0.34), max_text_width, 28)
-    self._draw_centered_text(rect, minute_text, rect.height * 0.42, self._font_bold, minute_font_size, duration_color)
+    self._draw_centered_text(rect, duration_text, rect.height * 0.42, self._font_bold, duration_font_size, duration_color)
-    self._draw_centered_text(rect, second_text, rect.height * 0.62, self._font_medium, second_font_size, rl.Color(255, 255, 255, 242))
     return True
 
 

selfdrive/ui/onroad/starpilot/aethergauge.py

@@ -99,10 +99,14 @@ def _with_alpha(color: rl.Color, a: int) -> rl.Color:
 def _pulse(freq: float) -> float:
   return 0.5 + 0.5 * math.sin(rl.get_time() * freq)
 
+def _get_road_height(data: 'AetherGaugeData | None') -> float:
+  is_curve = data.indicator_type == IndicatorType.ROAD_CURVE if data else False
+  return ROAD_HEIGHT if is_curve else (45.0 * SCALE)
+
 def _road_xy(distance: float, icx: float, bottom: float, data: 'AetherGaugeData') -> tuple[float, float, float]:
   t = max(0.0, min(1.0, _get_t_from_x(distance)))
   offset = _get_perspective_offset(t, data)
-  return t, icx + offset, bottom - t * ROAD_HEIGHT
+  return t, icx + offset, bottom - t * _get_road_height(data)
 
 def _get_t_from_x(x: float) -> float:
   x_clamped = max(X_MIN, min(X_MAX, x))
@@ -110,11 +114,8 @@ def _get_t_from_x(x: float) -> float:
 
 def _get_perspective_offset(t: float, data: 'AetherGaugeData | None') -> float:
   curvature = 0.0
-  if data:
+  if data and data.indicator_type == IndicatorType.ROAD_CURVE:
-    if data.indicator_type == IndicatorType.ROAD_CURVE:
+    curvature = data.indicator_value
-      curvature = data.indicator_value
-    elif _sm_valid("starpilotPlan"):
-      curvature = ui_state.sm["starpilotPlan"].roadCurvature
 
   path_y_far = PATH_Y_FAR_SCALE * curvature
   max_offset_top = math.tanh(path_y_far * PERSPECTIVE_GAIN) * PERSPECTIVE_MAX_OFFSET
@@ -311,7 +312,8 @@ class AetherGauge:
     ]
     if TEST_CYCLE:
       self._sources.insert(0, (_test_cycle_active, _test_cycle_data))
-    self._stop_smooth_s = 0.0
+    self._chevron_accum = 0.0
+    self._lead_chevron_accum = 0.0
 
   def get_active_data(self) -> AetherGaugeData | None:
     for is_active, get_data in self._sources:
@@ -319,42 +321,42 @@ class AetherGauge:
         return get_data()
     return None
 
-  def render(self, rect: rl.Rectangle, font_bold: rl.Font, font_medium: rl.Font, current_speed: float):
+  def render(self, rect: rl.Rectangle, font_bold: rl.Font, font_medium: rl.Font, current_speed: float, cx: float | None = None, bottom: float | None = None):
     data = self.get_active_data()
     if not data:
       return
 
-    cx = rect.x + rect.width / 2
+    if cx is None or bottom is None:
-    cy_speed = rect.y + 180 * SCALE
+      base_cx = rect.x + rect.width / 2
-
+      cy_speed = rect.y + 180 * SCALE
-    speed_text = str(round(current_speed))
+      speed_text = str(round(current_speed))
-    speed_text_size = measure_text_cached(font_bold, speed_text, int(176 * SCALE))
+      speed_text_size = measure_text_cached(font_bold, speed_text, int(176 * SCALE))
-    icon_cx = cx - speed_text_size.x / 2 - 70.0 * SCALE
+      icx = base_cx - speed_text_size.x / 2 - 70.0 * SCALE
+      icy = cy_speed - 39.5 * SCALE
+    else:
+      icx = cx
+      icy = bottom - ROAD_HALF_SIZE
 
     if data.indicator_type in (IndicatorType.ROAD_CURVE, IndicatorType.FORCE_STOP, IndicatorType.LEAD, IndicatorType.STOP_LIGHT):
-      self._render_unified_road(rect, icon_cx, cy_speed - 39.5 * SCALE, data, font_bold, font_medium)
+      self._render_unified_road(rect, icx, icy, data, font_bold, font_medium)
 
   def _render_unified_road(self, rect, icx, icy, data, font_bold, font_medium):
     bottom = icy + ROAD_HALF_SIZE
 
-    if data.indicator_type == IndicatorType.FORCE_STOP:
+    if data.indicator_type in (IndicatorType.FORCE_STOP, IndicatorType.STOP_LIGHT):
-      raw_s = max(0.0, min(1.0, (STOP_DISTANCE_MAX - data.indicator_value) / STOP_DISTANCE_MAX))
+      distance = 15.0
-      if abs(raw_s - self._stop_smooth_s) > STOP_SNAP_THRESHOLD:
-        self._stop_smooth_s = raw_s
-      else:
-        self._stop_smooth_s += (raw_s - self._stop_smooth_s) * STOP_LERP_RATE
-      distance = STOP_DISTANCE_MAX - self._stop_smooth_s * STOP_DISTANCE_MAX
     else:
       distance = data.indicator_value
 
     points_left = []
     points_right = []
+    road_h = _get_road_height(data)
 
     for i in range(ROAD_SEGMENTS + 1):
       t = i / ROAD_SEGMENTS
       offset = _get_perspective_offset(t, data)
       cx_t = icx + offset
-      y_t = bottom - t * ROAD_HEIGHT
+      y_t = bottom - t * road_h
       w_t = ROAD_W_BOTTOM - t * (ROAD_W_BOTTOM - ROAD_W_TOP)
 
       points_left.append(rl.Vector2(cx_t - w_t, y_t))
@@ -402,7 +404,7 @@ class AetherGauge:
     rl.draw_line_ex(p_left, p_right, max(1.5 * SCALE, 3.5 * SCALE * fade), COLOR_STOP_LINE_CORE)
 
   def _draw_lead_car(self, icx, bottom, data):
-    t_lead, cx_lead, cy_lead = _road_xy(data.indicator_value, icx, bottom, data)
+    t_lead, cx_lead, cy_lead = _road_xy(6.5, icx, bottom, data)
     t_lead = max(0.15, min(0.85, t_lead))
 
     car_scale = 0.45 + (1.0 - t_lead) * 0.55
@@ -415,7 +417,7 @@ class AetherGauge:
     if is_stopped:
       border_color = COLOR_LEAD_STOPPED
     elif is_slower:
-      border_color = _with_alpha(data.color, int(160 + 95 * _pulse(3.0)))
+      border_color = _with_alpha(data.color, int(160 + 95 * _pulse(1.2)))
     else:
       border_color = data.color
 
@@ -436,12 +438,12 @@ class AetherGauge:
     for tl_x, glow_x in [(int(cx_lead - W * 0.45), int(cx_lead - W * 0.38)),
                           (int(cx_lead + W * 0.3), int(cx_lead + W * 0.38))]:
       if is_stopped:
-        pulse = _pulse(8.0)
+        pulse = _pulse(2.5)
         r_glow = int(W * 0.08 * (1.0 + 0.4 * pulse))
         rl.draw_circle(glow_x, glow_y, r_glow, rl.Color(255, 30, 60, int(150 + 105 * pulse)))
         c_tl = rl.Color(255, 220, 220, 255)
       elif is_slower:
-        pulse = _pulse(3.0)
+        pulse = _pulse(1.2)
         r_glow = int(W * 0.06 * (1.0 + 0.2 * pulse))
         rl.draw_circle(glow_x, glow_y, r_glow, rl.Color(255, 140, 30, int(100 + 80 * pulse)))
         c_tl = rl.Color(255, 160, 60, int(180 + 75 * pulse))
@@ -458,12 +460,17 @@ class AetherGauge:
     rl.draw_rectangle(int(cx_lead + W * 0.28), wheel_y, wheel_w, wheel_h, wheel_c)
 
     # Following chevrons (from lead car back toward viewer)
-    progress = (rl.get_time() * 1.5) % 1.0
+    dt = rl.get_frame_time()
+    v_ego = _get_val("carState", "vEgo", 0.0)
+    flow_speed_factor = min(1.8, v_ego * 0.08)
+    self._lead_chevron_accum = (self._lead_chevron_accum + dt * flow_speed_factor * 1.8) % 1.0
+    progress = self._lead_chevron_accum
     for i in range(2):
       t = t_lead * (1.0 - ((i + progress) / 2) % 1.0)
 
       cx_t = icx + _get_perspective_offset(t, data)
-      cy_t = bottom - t * ROAD_HEIGHT
+      road_h = _get_road_height(data)
+      cy_t = bottom - t * road_h
 
       chevron_w = 12.0 * SCALE - t * 5.0 * SCALE
       chevron_thick = max(1.5 * SCALE, 3.5 * SCALE - t * 1.5 * SCALE)
@@ -473,12 +480,16 @@ class AetherGauge:
       alpha = max(0, min(255, int(data.color.a * (1.0 - t / t_lead) * math.sin(t / t_lead * math.pi))))
       c_color = _with_alpha(data.color, alpha)
 
-      rl.draw_line_ex(rl.Vector2(lx, cy_t + chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color)
+      rl.draw_line_ex(rl.Vector2(lx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color)
-      rl.draw_line_ex(rl.Vector2(rx, cy_t + chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color)
+      rl.draw_line_ex(rl.Vector2(rx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color)
 
   def _draw_standard_chevrons(self, icx, bottom, distance, data):
     is_stop = data.indicator_type in (IndicatorType.FORCE_STOP, IndicatorType.STOP_LIGHT)
-    progress = (rl.get_time() * 1.2) % 1.0
+    dt = rl.get_frame_time()
+    v_ego = _get_val("carState", "vEgo", 0.0)
+    flow_speed_factor = min(1.5, v_ego * 0.08)
+    self._chevron_accum = (self._chevron_accum + dt * flow_speed_factor * 1.5) % 1.0
+    progress = self._chevron_accum
     t_stop = max(0.05, min(1.0, distance / STOP_DISTANCE_MAX)) if is_stop else 1.0
     max_t = t_stop if is_stop else 1.0
 
@@ -491,8 +502,9 @@ class AetherGauge:
       cx_t = icx + _get_perspective_offset(t, data)
       cx_next = icx + _get_perspective_offset(t_next, data)
 
-      cy_t = bottom - t * ROAD_HEIGHT
+      road_h = _get_road_height(data)
-      cy_next = bottom - t_next * ROAD_HEIGHT
+      cy_t = bottom - t * road_h
+      cy_next = bottom - t_next * road_h
 
       dx = cx_next - cx_t
       dy = cy_next - cy_t
@@ -506,10 +518,10 @@ class AetherGauge:
       chevron_h = chevron_w * 0.6
       chevron_thick = max(2.0 * SCALE, 4.0 * SCALE - t * 2.0 * SCALE)
 
-      lx = cx_t - dir_right_x * chevron_w - dir_up_x * chevron_h
+      lx = cx_t - dir_right_x * chevron_w + dir_up_x * chevron_h
-      ly = cy_t - dir_right_y * chevron_w - dir_up_y * chevron_h
+      ly = cy_t - dir_right_y * chevron_w + dir_up_y * chevron_h
-      rx = cx_t + dir_right_x * chevron_w - dir_up_x * chevron_h
+      rx = cx_t + dir_right_x * chevron_w + dir_up_x * chevron_h
-      ry = cy_t + dir_right_y * chevron_w - dir_up_y * chevron_h
+      ry = cy_t + dir_right_y * chevron_w + dir_up_y * chevron_h
 
       alpha_factor = math.sin((t / max_t) * math.pi) if max_t > 0.01 else 0.0
       alpha = max(0, min(255, int(data.color.a * alpha_factor)))
@@ -545,7 +557,7 @@ class AetherGauge:
     ]
     for y, name, active_c, inactive_c in bulbs:
       if name == "red" and active_light == "red":
-        glow_pulse = _pulse(5.0)
+        glow_pulse = _pulse(1.5)
         rl.draw_circle_v(rl.Vector2(cx_light, y), r_bulb + 3.0 * SCALE * glow_pulse, rl.Color(255, 30, 60, 45))
         rl.draw_circle_v(rl.Vector2(cx_light, y), r_bulb + 6.0 * SCALE * glow_pulse, rl.Color(255, 30, 60, 15))
       c = active_c if active_light == name else inactive_c
@@ -558,7 +570,8 @@ class AetherGauge:
     offset_stop = _get_perspective_offset(t_stop_gauge, data) if data else 0.0
     cx_stop = cx + offset_stop
 
-    cy_road = bottom - t_stop_gauge * ROAD_HEIGHT
+    road_h = _get_road_height(data)
+    cy_road = bottom - t_stop_gauge * road_h
     y_sign = cy_road - 25.0 * SCALE * smoothed_s
 
     r_min = 6.0 * SCALE

selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py

@@ -59,7 +59,16 @@ class StarPilotOnroadView(AugmentedRoadView):
     self._personality_button.render()
     self._render_standstill_timer()
     self._render_developer_metrics()
-    self._aethergauge.render(self._content_rect, self._font_bold, self._font_medium, current_speed=self._hud_renderer.speed)
+    dm = self.driver_state_renderer
+    if dm and dm.is_visible and dm.position_x != 0.0:
+      dm_top = dm.position_y - 96  # top of DM icon
+      self._aethergauge.render(
+        self._content_rect, self._font_bold, self._font_medium,
+        current_speed=self._hud_renderer.speed,
+        cx=dm.position_x, bottom=dm_top - 105
+      )
+    else:
+      self._aethergauge.render(self._content_rect, self._font_bold, self._font_medium, current_speed=self._hud_renderer.speed)
     self._render_bottom_row_widgets()
     self._render_pedals()
 

starpilot/common/safe_mode.py

@@ -79,6 +79,7 @@ SAFE_MODE_MANAGED_KEYS = (
   "QOLLongitudinal",
   "ForceStops",
   "ForceStandstill",
+  "RadarTakeoffs",
   "IncreasedStoppedDistance",
   "MapGears",
   "MapAcceleration",

starpilot/common/starpilot_variables.py

@@ -1128,6 +1128,7 @@ class StarPilotVariables:
     toggle.force_stops = self.get_value("ForceStops", condition=quality_of_life_longitudinal)
     toggle.force_stop_distance_offset = self.get_value("ForceStopDistanceOffset", cast=int, condition=(quality_of_life_longitudinal and toggle.force_stops))
     toggle.force_standstill = self.get_value("ForceStandstill", condition=quality_of_life_longitudinal)
+    toggle.radar_takeoffs = self.get_value("RadarTakeoffs", condition=quality_of_life_longitudinal)
     toggle.increase_stopped_distance = self.get_value("IncreasedStoppedDistance", cast=float, condition=quality_of_life_longitudinal, conversion=distance_conversion)
     map_gears = self.get_value("MapGears", condition=quality_of_life_longitudinal)
     toggle.map_acceleration = self.get_value("MapAcceleration", condition=map_gears)

starpilot/controls/lib/speed_limit_controller.py

@@ -73,6 +73,7 @@ class SpeedLimitController:
     self.mapbox_token = self.starpilot_planner.params.get("MapboxSecretKey", encoding="utf-8")
 
     self.previous_target = self.starpilot_planner.params.get_float("PreviousSpeedLimit")
+    self.last_valid_limit = self.previous_target if self.previous_target > 0 else 0
 
     self.executor = ThreadPoolExecutor(max_workers=1)
     self.mapbox_future = None
@@ -90,11 +91,21 @@ class SpeedLimitController:
     return self.target == 0 and bool(getattr(self.starpilot_toggles, "slc_fallback_experimental_mode", False))
 
   @property
-  def offset(self):
+  def target_to_use(self):
+    if self.source == "None" and self.target > 0 and self.last_valid_limit > 0:
+      if self.target >= self.last_valid_limit:
+        return self.last_valid_limit
+    return self.target
+
+  def get_offset(self, target_speed):
     if self.starpilot_toggles is None:
       return 0
     offset_map = OFFSET_MAP_METRIC if self.starpilot_toggles.is_metric else OFFSET_MAP_IMPERIAL
-    return next((getattr(self.starpilot_toggles, offset) for low, high, offset in offset_map if low < self.target < high), 0)
+    return next((getattr(self.starpilot_toggles, offset) for low, high, offset in offset_map if low <= target_speed < high), 0)
+
+  @property
+  def offset(self):
+    return self.get_offset(self.target)
 
   @property
   def override_mode_enabled(self):
@@ -103,7 +114,8 @@ class SpeedLimitController:
     return self.starpilot_toggles.speed_limit_controller_override_manual or self.starpilot_toggles.speed_limit_controller_override_set_speed
 
   def override_active(self, v_ego, gas_pressed):
-    target_with_offset = self.target + self.offset
+    target_to_use = self.target_to_use
+    target_with_offset = target_to_use + self.get_offset(target_to_use)
     if target_with_offset <= 0 or not self.override_mode_enabled:
       return False
     return self.overridden_speed > target_with_offset or (gas_pressed and v_ego > target_with_offset)
@@ -113,6 +125,8 @@ class SpeedLimitController:
       return
     if not had_override and self.overridden_speed <= 0:
       return
+    if abs(desired_target - self.last_valid_limit) < 0.1:
+      return
 
     # A new posted limit starts a new segment, so the previous segment's gas override
     # should not carry through until the driver releases and reapplies the pedal.
@@ -122,7 +136,7 @@ class SpeedLimitController:
       self.override_requires_gas_release = True
 
   def get_mapbox_speed_limit(self, now, time_validated, v_ego, sm):
-    if not self.starpilot_planner.gps_valid or not self.mapbox_token or (sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg) >= 45:
+    if not self.starpilot_planner.gps_valid or not self.mapbox_token or abs(sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg) >= 45:
       self.mapbox_limit = 0
       self.segment_distance = 0
       return
@@ -144,6 +158,7 @@ class SpeedLimitController:
       try:
         if not is_url_pingable(self.mapbox_host):
           self.segment_distance = 1000
+          successful = True
           return None
 
         if time_validated:
@@ -156,7 +171,7 @@ class SpeedLimitController:
             })
 
         self.mapbox_requests["total_requests"] += 1
-        self.starpilot_planner.params.put_nonblocking("MapBoxRequests", self.mapbox_requests)
+        self.starpilot_planner.params.put_nonblocking("MapBoxRequests", json.dumps(self.mapbox_requests))
 
         current_bearing = self.starpilot_planner.gps_position.get("bearing")
         current_latitude = self.starpilot_planner.gps_position.get("latitude")
@@ -217,15 +232,20 @@ class SpeedLimitController:
           segment_distance = distances[0]
 
           speed_data = annotation.get("maxspeed", [])
-          speed_limit_kph = 0
           if speed_data:
             first_segment_speed = speed_data[0]
-            speed_limit_kph = (first_segment_speed.get("speed") if first_segment_speed.get("speed") != "none" else 0) or 0
+            try:
-
+              raw_speed = float(first_segment_speed.get("speed") if first_segment_speed.get("speed") != "none" else 0.0)
-          if speed_limit_kph > 0:
+            except (ValueError, TypeError):
-            self.mapbox_limit = speed_limit_kph * CV.KPH_TO_MS
+              raw_speed = 0.0
-            self.segment_distance = segment_distance
+            unit = first_segment_speed.get("unit", "km/h")
-            return
+            if raw_speed > 0:
+              if unit == "mph":
+                self.mapbox_limit = raw_speed * CV.MPH_TO_MS
+              else:
+                self.mapbox_limit = raw_speed * CV.KPH_TO_MS
+              self.segment_distance = segment_distance
+              return
 
         self.mapbox_limit = 0
         self.segment_distance = v_ego
@@ -275,12 +295,12 @@ class SpeedLimitController:
       self.previous_target = desired_target
       self.previous_road_name = current_road_name
 
-    elif desired_target < self.target and not self.starpilot_toggles.speed_limit_confirmation_lower:
+    elif desired_target < self.target and (desired_source == "None" or not self.starpilot_toggles.speed_limit_confirmation_lower):
       self.source = desired_source
       self.target = desired_target
       self.clear_override_for_source_limit(desired_source, desired_target, had_override)
 
-    elif desired_target > self.target and not self.starpilot_toggles.speed_limit_confirmation_higher:
+    elif desired_target > self.target and (desired_source == "None" or not self.starpilot_toggles.speed_limit_confirmation_higher):
       self.source = desired_source
       self.target = desired_target
       self.clear_override_for_source_limit(desired_source, desired_target, had_override)
@@ -300,7 +320,7 @@ class SpeedLimitController:
       self.previous_target = self.target
       self.previous_road_name = current_road_name
 
-      self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", self.target)
+      self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", float(self.target))
 
   def update_limits(self, dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm, display_only=False):
     self.update_map_speed_limit(v_ego, sm)
@@ -343,7 +363,7 @@ class SpeedLimitController:
       desired_source = "None"
       desired_target = 0
 
-    if desired_target == 0 or self.target == 0:
+    if desired_target == 0:
       if self.mapbox_requests["total_requests"] < self.mapbox_requests["max_requests"] and self.starpilot_toggles.slc_mapbox_filler:
         self.get_mapbox_speed_limit(now, time_validated, v_ego, sm)
 
@@ -351,7 +371,7 @@ class SpeedLimitController:
           desired_source = "Mapbox"
           desired_target = self.mapbox_limit
 
-      if not display_only and (desired_target == 0 or self.target == 0):
+      if not display_only and desired_target == 0:
         if self.previous_target > 0 and self.starpilot_toggles.slc_fallback_previous_speed_limit:
           desired_source = self.previous_source
           desired_target = self.previous_target
@@ -384,12 +404,16 @@ class SpeedLimitController:
 
     if abs(desired_target - self.previous_target) >= 1 or (current_road_name != self.previous_road_name and current_road_name != ""):
       self.handle_limit_change(desired_source, desired_target, current_road_name, v_ego, sm)
-    elif desired_source != self.source and abs(desired_target - self.target) < 1:
+    elif desired_source != self.source and (abs(desired_target - self.target) < 1 or self.target == 0):
       self.source = desired_source
+      self.target = desired_target
     else:
       self.speed_limit_changed_timer = 0
       self.unconfirmed_speed_limit = 0
 
+    if self.source != "None" and self.target > 0:
+      self.last_valid_limit = self.target
+
     self._slc_adopt_counter += 1
     if self._slc_adopt_counter % 4 == 0 and self.starpilot_planner.params_memory.get_bool("SLCAdoptSpeedLimit"):
       self.starpilot_planner.params_memory.remove("SLCAdoptSpeedLimit")
@@ -402,7 +426,7 @@ class SpeedLimitController:
         self.previous_target = desired_target
         self.speed_limit_changed_timer = 0
         self.unconfirmed_speed_limit = 0
-        self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", self.target)
+        self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", float(self.target))
         self.starpilot_planner.params_memory.put_float("SLCForceCruiseSpeed", self.target + self.offset)
 
   def update_map_speed_limit(self, v_ego, sm):
@@ -439,14 +463,16 @@ class SpeedLimitController:
     if not sm["carState"].gasPressed:
       self.override_requires_gas_release = False
 
-    self.override_slc = self.overridden_speed > self.target + self.offset > 0 and v_ego > self.target + self.offset
+    target_to_use = self.target_to_use
-    self.override_slc |= not self.override_requires_gas_release and sm["carState"].gasPressed and v_ego > self.target + self.offset > 0
+    offset = self.get_offset(target_to_use)
+    self.override_slc = self.override_slc and self.overridden_speed > target_to_use + offset > 0
+    self.override_slc |= not self.override_requires_gas_release and sm["carState"].gasPressed and v_ego > target_to_use + offset > 0
 
     if self.override_slc:
       if self.starpilot_toggles.speed_limit_controller_override_manual:
         if sm["carState"].gasPressed:
           self.overridden_speed = max(v_ego + v_ego_diff, self.overridden_speed)
-        self.overridden_speed = float(np.clip(self.overridden_speed, self.target + self.offset, v_cruise + v_cruise_diff))
+        self.overridden_speed = float(np.clip(self.overridden_speed, target_to_use + offset, v_cruise + v_cruise_diff))
       elif self.starpilot_toggles.speed_limit_controller_override_set_speed:
         self.overridden_speed = v_cruise + v_cruise_diff
 

starpilot/system/the_pond/assets/components/tools/device_settings_layout.json

@@ -1085,6 +1085,14 @@
         "ui_type": "toggle",
         "parent_key": "QOLLongitudinal"
       },
+      {
+        "key": "RadarTakeoffs",
+        "label": "Radar for Takeoffs",
+        "description": "Turns on/off using radar data to track leads at standstill, making following/takeoffs more responsive once leads move.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "parent_key": "QOLLongitudinal"
+      },
       {
         "key": "IncreasedStoppedDistance",
         "label": "Increase Stopped Distance by:",

starpilot/system/the_pond/factory_reset.py

@@ -0,0 +1,29 @@
+import subprocess
+import time
+
+
+_DELETE_TIMEOUT_S = 1800
+_DELETE_RETRY_ATTEMPTS = 20
+_DELETE_RETRY_DELAY_S = 0.25
+_DIRECTORY_NOT_EMPTY_ERROR = "directory not empty"
+
+
+def remove_path(path):
+  # Managed processes can recreate entries under /data/params while rm is finishing.
+  for attempt in range(_DELETE_RETRY_ATTEMPTS):
+    result = subprocess.run(
+      ["sudo", "rm", "-rf", "--", path],
+      capture_output=True,
+      text=True,
+      timeout=_DELETE_TIMEOUT_S,
+      check=False,
+    )
+    if result.returncode == 0:
+      return
+
+    error_text = (result.stderr or result.stdout or "sudo rm -rf failed").strip()
+    can_retry = _DIRECTORY_NOT_EMPTY_ERROR in error_text.lower()
+    if not can_retry or attempt == _DELETE_RETRY_ATTEMPTS - 1:
+      raise RuntimeError(f"Failed to remove {path}: {error_text}")
+
+    time.sleep(_DELETE_RETRY_DELAY_S)

starpilot/system/the_pond/tests/test_factory_reset.py

@@ -0,0 +1,44 @@
+import subprocess
+
+import pytest
+
+from openpilot.starpilot.system.the_pond import factory_reset
+
+
+def test_remove_path_retries_directory_not_empty(monkeypatch):
+  results = iter(
+    [
+      subprocess.CompletedProcess([], 1, stderr="rm: cannot remove '/data/params': Directory not empty"),
+      subprocess.CompletedProcess([], 0),
+    ]
+  )
+  calls = []
+  sleeps = []
+
+  def fake_run(*args, **kwargs):
+    calls.append((args, kwargs))
+    return next(results)
+
+  monkeypatch.setattr(factory_reset.subprocess, "run", fake_run)
+  monkeypatch.setattr(factory_reset.time, "sleep", sleeps.append)
+
+  factory_reset.remove_path("/data/params")
+
+  assert len(calls) == 2
+  assert calls[0][0][0] == ["sudo", "rm", "-rf", "--", "/data/params"]
+  assert sleeps == [factory_reset._DELETE_RETRY_DELAY_S]
+
+
+def test_remove_path_does_not_retry_non_transient_error(monkeypatch):
+  calls = []
+
+  def fake_run(*args, **kwargs):
+    calls.append((args, kwargs))
+    return subprocess.CompletedProcess([], 1, stderr="rm: cannot remove '/data/params': Permission denied")
+
+  monkeypatch.setattr(factory_reset.subprocess, "run", fake_run)
+
+  with pytest.raises(RuntimeError, match="Permission denied"):
+    factory_reset.remove_path("/data/params")
+
+  assert len(calls) == 1

starpilot/system/the_pond/the_pond.py

@@ -78,6 +78,7 @@ from openpilot.starpilot.common.testing_grounds import (
   TESTING_GROUNDS_STATE_PATH as SHARED_TESTING_GROUNDS_STATE_PATH,
 )
 from openpilot.starpilot.navigation.destination_store import normalize_destination_payload, update_recent_destinations
+from openpilot.starpilot.system.the_pond.factory_reset import remove_path as _run_factory_reset_delete
 from openpilot.starpilot.system.the_pond import utilities
 
 DISCORD_WEBHOOK_URL = os.getenv("DISCORD_WEBHOOK_URL")
@@ -633,7 +634,6 @@ _FAST_UPDATE_REBOOT_NOTICE_SECONDS = 6.0
 _FAST_UPDATE_FETCH_TIMEOUT_S = 60
 _FAST_BRANCH_SWITCH_FETCH_TIMEOUT_S = 60
 _FAST_ROLLBACK_FETCH_TIMEOUT_S = 60
-_FACTORY_RESET_DELETE_TIMEOUT_S = 1800
 _GIT_PROGRESS_PERCENT_RE = re.compile(r'([A-Za-z][A-Za-z /_-]+):\s*([0-9]{1,3})%')
 _GIT_SUBMODULE_SECTION_RE = re.compile(r'^\s*\[submodule\s+"[^"]+"\]\s*$', re.MULTILINE)
 _ROLLBACK_REF = "refs/starpilot/rollback"
@@ -1572,18 +1572,6 @@ def _set_fast_update_error_state(message, exception):
     progressDetail="Update failed. See Last Error below.",
   )
 
-def _run_factory_reset_delete(path):
-  result = subprocess.run(
-    ["sudo", "rm", "-rf", path],
-    capture_output=True,
-    text=True,
-    timeout=_FACTORY_RESET_DELETE_TIMEOUT_S,
-    check=False,
-  )
-  if result.returncode != 0:
-    error_text = (result.stderr or result.stdout or "sudo rm -rf failed").strip()
-    raise RuntimeError(f"Failed to remove {path}: {error_text}")
-
 def _factory_reset_worker():
   started_at = time.time()
 

starpilot/ui/qt/offroad/longitudinal_settings.cc

@@ -159,6 +159,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
     {"ForceStops", tr("Force Stop at \"Detected\" Stop Lights/Signs"), tr("<b>Force openpilot to stop whenever the driving model \"detects\" a red light or stop sign.</b><br><br><i><b>Disclaimer</b>: openpilot does not explicitly detect traffic lights or stop signs. In \"Experimental Mode\", openpilot makes end-to-end driving decisions from camera input, which means it may stop even when there's no clear reason!</i>"), ""},
     {"ForceStopDistanceOffset", tr("Force Stop Distance Offset"), tr("<b>Tune where Force Stops bring the car to rest.</b> Positive values let the car roll further before stopping (longer stop, closer to the line). Negative values stop the car sooner (more buffer before the line)."), ""},
     {"ForceStandstill", tr("Force Standstill State"), tr("<b>Keep openpilot in the standstill state until you press the gas pedal or the Resume/+ cruise button.</b><br><br>This applies to any engaged stop, not just red lights or stop signs."), ""},
+    {"RadarTakeoffs", tr("Radar for Takeoffs"), tr("<b>Turns on/off using radar data to track leads at standstill</b>, making following/takeoffs more responsive once leads move."), ""},
     {"IncreasedStoppedDistance", tr("Increase Stopped Distance by:"), tr("<b>Add extra space when stopped behind vehicles.</b> Increase for more room; decrease for shorter gaps."), ""},
     {"MapGears", tr("Map Accel/Decel to Gears"), tr("<b>Map the Acceleration or Deceleration profiles to the vehicle's \"Eco\" and \"Sport\" gear modes.</b>"), ""},
     {"SetSpeedOffset", tr("Offset Set Speed by:"), tr("<b>Increase the set speed by the chosen offset.</b> For example, set +5 if you usually drive 5 over the limit."), ""},
@@ -1000,6 +1001,10 @@ void StarPilotLongitudinalPanel::updateToggles() {
         setVisible &= parent->hasRadar;
       }
 
+      else if (key == "RadarTakeoffs") {
+        setVisible &= parent->hasRadar;
+      }
+
       else if (key == "MapGears") {
         setVisible &= parent->isToyota || parent->isHKG;
         setVisible &= !parent->isTSK;

starpilot/ui/qt/offroad/longitudinal_settings.h

@@ -34,7 +34,7 @@ private:
   QSet<QString> curveSpeedKeys = {"CalibratedLateralAcceleration", "CalibrationProgress", "ResetCurveData", "ShowCSCStatus"};
   QSet<QString> customDrivingPersonalityKeys = {"AggressivePersonalityProfile", "RelaxedPersonalityProfile", "StandardPersonalityProfile", "TrafficPersonalityProfile"};
   QSet<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "HumanAcceleration", "CoastUpToLeads", "HumanLaneChanges", "LeadDetectionThreshold", "TacoTune", "NavLongitudinalAllowed"};
-  QSet<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "ForceStopDistanceOffset", "ForceStandstill", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset", "WeatherPresets"};
+  QSet<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "ForceStopDistanceOffset", "ForceStandstill", "RadarTakeoffs", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset", "WeatherPresets"};
   QSet<QString> relaxedPersonalityKeys = {"RelaxedFollow", "RelaxedFollowHigh", "RelaxedJerkAcceleration", "RelaxedJerkDeceleration", "RelaxedJerkDanger", "RelaxedJerkSpeed", "RelaxedJerkSpeedDecrease", "ResetRelaxedPersonality"};
   QSet<QString> speedLimitControllerKeys = {"SLCOffsets", "SLCFallback", "SLCOverride", "SLCPriority", "SLCQOL", "SLCVisuals"};
   QSet<QString> speedLimitControllerOffsetsKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "Offset5", "Offset6", "Offset7"};

tinygrad_repo/examples/openpilot/compile3.py

@@ -1,5 +1,4 @@
 import os, sys, pickle, time, re
-from pathlib import Path
 import numpy as np
 if "FLOAT16" not in os.environ: os.environ["FLOAT16"] = "1"
 if "IMAGE" not in os.environ: os.environ["IMAGE"] = "2"
@@ -132,8 +131,7 @@ def bench(run, inputs):
       run(**inputs).numpy()
 
 if __name__ == "__main__":
-  local_onnx = Path(OPENPILOT_MODEL).expanduser()
+  onnx_file = fetch(OPENPILOT_MODEL)
-  onnx_file = str(local_onnx if local_onnx.exists() else fetch(OPENPILOT_MODEL))
   inputs, outputs = compile(onnx_file)
 
   with open(OUTPUT, "rb") as f: pickle_loaded = pickle.load(f)

tools/joystick/joystick_control.py

@@ -52,7 +52,7 @@ class Joystick:
       self.flip_map = {'ABS_RZ': accel_axis}
     else:
       accel_axis = 'ABS_RX'
-      steer_axis = 'ABS_Z'
+      steer_axis = 'ABS_X'
       self.flip_map = {'ABS_RY': accel_axis}
 
     self.min_axis_value = {accel_axis: 0., steer_axis: 0.}