Lite Brite

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -866,7 +866,7 @@ IONIQ_6_CURVY_TURN_IN_TRIM_LAT_START = 1.0
 IONIQ_6_CURVY_TURN_IN_TRIM_LAT_END = 2.5
 IONIQ_6_CURVY_TURN_IN_TRIM_LAT_ONSET_WIDTH = 0.18
 IONIQ_6_CURVY_TURN_IN_TRIM_LAT_CUTOFF_WIDTH = 0.30
-IONIQ_6_2023_UNWIND_FF_REDUCTION_MAX = 0.18
+IONIQ_6_2023_UNWIND_FF_REDUCTION_MAX = 0.24
 IONIQ_6_2023_UNWIND_FF_OVERSHOOT = 0.15
 IONIQ_6_2023_UNWIND_FF_OVERSHOOT_WIDTH = 0.18
 IONIQ_6_2023_UNWIND_FF_JERK = 0.10

selfdrive/controls/lib/lead_follow_policy.py

@@ -28,6 +28,9 @@ FOLLOW_TRANSITION_MIN_TTC = 6.0
 FOLLOW_HEADWAY_MARGIN = 0.90
 FOLLOW_SIGN_CROSS_STEP = 0.10
 FOLLOW_TRANSITION_MAX_BRAKE = 0.25
+FOLLOW_STEADY_DEADBAND_MAX_TARGET = 0.28
+FOLLOW_STEADY_DEADBAND_MAX_CLOSING = 0.75
+FOLLOW_STEADY_DEADBAND_MAX_HEADWAY_MARGIN = 0.90
 
 
 @dataclass(frozen=True)
@@ -184,6 +187,25 @@ def _transition_target(lead, v_ego: float, t_follow: float, previous: float, tar
   return limited if abs(limited - float(target)) > 1e-6 else None
 
 
+def _steady_follow_deadband(lead, v_ego: float, t_follow: float, previous: float, target: float) -> float:
+  """Remove only small sign reversals in an already matched, non-urgent follow."""
+  if not _matched(lead, v_ego, t_follow):
+    return float(target)
+  if float(lead.vLead) - float(v_ego) > 0.15:
+    return float(target)
+  if max(0.0, float(v_ego) - float(lead.vLead)) > FOLLOW_STEADY_DEADBAND_MAX_CLOSING:
+    return float(target)
+  if _headway(lead, v_ego) - float(t_follow) > FOLLOW_STEADY_DEADBAND_MAX_HEADWAY_MARGIN:
+    return float(target)
+  if (
+    float(previous) * float(target) < 0.0 and
+    abs(float(previous)) <= FOLLOW_STEADY_DEADBAND_MAX_TARGET and
+    abs(float(target)) <= FOLLOW_STEADY_DEADBAND_MAX_TARGET
+  ):
+    return 0.0
+  return float(target)
+
+
 def apply(lead_one, lead_two, *, source: str, active: bool, v_ego: float, t_follow: float,
           previous_target: float, raw_target: float, tracking: bool, post_departure: bool,
           blocked: bool, panic_bypass: bool) -> FollowResult:
@@ -202,7 +224,11 @@ def apply(lead_one, lead_two, *, source: str, active: bool, v_ego: float, t_foll
     target = max(target, floor)
   if cap is not None:
     target = min(target, cap)
-  transition = _transition_target(lead, v_ego, t_follow, previous_target, target)
-  if transition is not None:
-    target = transition
+  deadband_target = _steady_follow_deadband(lead, v_ego, t_follow, previous_target, target)
+  if deadband_target != target:
+    target = deadband_target
+  else:
+    transition = _transition_target(lead, v_ego, t_follow, previous_target, target)
+    if transition is not None:
+      target = transition
   return FollowResult(lead, cap, floor, target)

selfdrive/controls/lib/longcontrol_vehicle_tunes.py

@@ -27,11 +27,11 @@ GM_TRUCK_TARGET_FILTER_DROP_BYPASS = 0.45
 TOYOTA_SIENNA_TARGET_FILTER_MIN_SPEED = 12.0
 TOYOTA_SIENNA_TARGET_FILTER_UP_TAU = 0.32
 TOYOTA_SIENNA_TARGET_FILTER_DOWN_TAU = 0.24
-TOYOTA_SIENNA_LOW_SPEED_ACCEL_UP_TAU = 0.35
+TOYOTA_SIENNA_LOW_SPEED_ACCEL_UP_TAU = 0.50
 TOYOTA_SIENNA_TARGET_FILTER_BRAKE_BYPASS = -0.75
 TOYOTA_SIENNA_TARGET_FILTER_DROP_BYPASS = 0.65
-TOYOTA_SIENNA_COMFORT_FILTER_MIN_SPEED = 5.0
-TOYOTA_SIENNA_COMFORT_FILTER_MIN_DISTANCE = 10.0
+TOYOTA_SIENNA_COMFORT_FILTER_MIN_SPEED = 1.0
+TOYOTA_SIENNA_COMFORT_FILTER_MIN_DISTANCE = 7.0
 TOYOTA_SIENNA_COMFORT_FILTER_MIN_TTC = 4.5
 TOYOTA_SIENNA_COMFORT_FILTER_MAX_CLOSING_SPEED = 4.0
 TOYOTA_SIENNA_COMFORT_FILTER_MAX_LEAD_BRAKE = 2.5
@@ -295,7 +295,9 @@ class LongControlVehicleTuning:
 
     if v_ego < TOYOTA_SIENNA_TARGET_FILTER_MIN_SPEED and not comfort_filter_active:
       if a_target > 0.0:
-        if not self.toyota_sienna_target_filter_initialized or self.toyota_sienna_filtered_a_target < 0.0:
+        if not self.toyota_sienna_target_filter_initialized or (
+          self.toyota_sienna_filtered_a_target < 0.0 and comfort_lead is None
+        ):
           self.toyota_sienna_filtered_a_target = 0.0
           self.toyota_sienna_target_filter_initialized = True
         alpha = DT_CTRL / (TOYOTA_SIENNA_LOW_SPEED_ACCEL_UP_TAU + DT_CTRL)
@@ -304,6 +306,11 @@ class LongControlVehicleTuning:
         )
         return self.toyota_sienna_filtered_a_target
 
+      if comfort_lead is not None:
+        self.toyota_sienna_filtered_a_target = float(a_target)
+        self.toyota_sienna_target_filter_initialized = True
+        return float(a_target)
+
       self.toyota_sienna_target_filter_initialized = False
       return a_target
 

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -138,6 +138,8 @@ LEAD_ACCEL_TAU = 1.5
 FCW_MIN_MODEL_PROB = 0.9
 FCW_MIN_CLOSING_SPEED = 0.5
 FCW_MAX_TTC = 4.0
+MODEL_LEAD_TRAJECTORY_MAX_LEAD_BRAKE = 0.5
+MODEL_LEAD_TRAJECTORY_MAX_CLOSING_TTC = 7.0
 
 
 # Fewer timestamps don't hurt performance and lead to
@@ -178,6 +180,17 @@ def build_model_lead_trajectory(model_lead, radar_lead, v_ego):
   if not np.isfinite(raw_d_rel) or not np.isfinite(raw_v_lead):
     return None
 
+  # The model path is a comfort prediction, not the raw safety measurement.
+  # When the measured lead is already braking or the gap is closing quickly,
+  # keep the legacy raw-lead path so an optimistic model horizon cannot delay
+  # the first braking response.
+  raw_lead_brake = max(0.0, -float(getattr(radar_lead, "aLeadK", 0.0)))
+  closing_speed = max(0.0, float(v_ego) - raw_v_lead)
+  ttc = raw_d_rel / max(closing_speed, 1e-3) if closing_speed > 0.1 else float("inf")
+  if (raw_lead_brake > MODEL_LEAD_TRAJECTORY_MAX_LEAD_BRAKE or
+      (closing_speed > 0.75 and ttc < MODEL_LEAD_TRAJECTORY_MAX_CLOSING_TTC)):
+    return None
+
   # The model contributes future deltas only. This preserves raw lead source
   # selection and keeps the current lead distance/speed safety anchor intact.
   x_lead_traj = raw_d_rel + (model_x - model_x[0])

selfdrive/controls/tests/test_lead_follow_policy.py

@@ -60,6 +60,18 @@ def test_follow_policy_limits_small_post_lead_reversal():
   assert result.target - (-0.08) <= 0.18
 
 
+def test_follow_policy_deadbands_small_steady_sign_reversal():
+  result = run(lead(d_rel=37.2, v_lead=24.0, radar=True), v_ego=24.0, previous=0.24, raw=-0.24)
+
+  assert result.target == pytest.approx(0.0)
+
+
+def test_follow_policy_deadband_does_not_mask_closing_lead():
+  result = run(lead(d_rel=37.2, v_lead=22.5, a_lead=-1.0, radar=True), v_ego=24.0, previous=0.24, raw=-0.24)
+
+  assert result.target < 0.0
+
+
 def test_follow_policy_never_relaxes_material_braking():
   result = run(lead(d_rel=25.0, v_lead=18.0), v_ego=25.0, previous=0.30, raw=-1.2)
   assert result.target == pytest.approx(-1.2)

selfdrive/controls/tests/test_longcontrol.py

@@ -1235,6 +1235,37 @@ def test_toyota_sienna_target_filter_smooths_mild_high_speed_handoffs():
 
   assert -0.20 < filtered < 0.30
 
+def test_toyota_sienna_target_filter_smooths_nonurgent_low_speed_lead_braking():
+  CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
+  tuning = vehicle_tunes.LongControlVehicleTuning(CP)
+  lead = SimpleNamespace(status=True, yRel=0.0, dRel=10.0, vLead=3.5, aLeadK=-0.4)
+
+  tuning.shape_toyota_sienna_accel_target(0.45, 5.0, False, leads=(lead,))
+  filtered = tuning.shape_toyota_sienna_accel_target(-1.0, 5.0, False, leads=(lead,))
+
+  assert -1.0 < filtered < 0.45
+
+
+def test_toyota_sienna_target_filter_keeps_urgent_low_speed_braking():
+  CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
+  tuning = vehicle_tunes.LongControlVehicleTuning(CP)
+  lead = SimpleNamespace(status=True, yRel=0.0, dRel=5.0, vLead=0.0, aLeadK=-1.5)
+
+  filtered = tuning.shape_toyota_sienna_accel_target(-1.0, 3.0, False, leads=(lead,))
+
+  assert filtered == pytest.approx(-1.0)
+
+
+def test_toyota_sienna_target_filter_ramps_out_of_low_speed_braking_with_lead_present():
+  CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
+  tuning = vehicle_tunes.LongControlVehicleTuning(CP)
+  lead = SimpleNamespace(status=True, yRel=0.0, dRel=6.5, vLead=1.5, aLeadK=-0.2)
+
+  tuning.shape_toyota_sienna_accel_target(-0.8, 1.5, False, leads=(lead,))
+  release = tuning.shape_toyota_sienna_accel_target(0.2, 1.5, False, leads=(lead,))
+
+  assert -0.8 < release < 0.0
+
 
 def test_toyota_sienna_target_filter_unwinds_braking_before_acceleration():
   CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -376,6 +376,17 @@ def test_model_lead_trajectory_falls_back_without_raw_lead_or_valid_shape():
   assert build_model_lead_trajectory(short_model_lead, raw_lead, 20.0) is None
 
 
+@pytest.mark.parametrize("d_rel,v_lead,a_lead", [
+  (42.0, 18.0, -0.6),
+  (8.0, 0.0, 0.0),
+])
+def test_model_lead_trajectory_falls_back_for_urgent_raw_lead(d_rel, v_lead, a_lead):
+  raw_lead = make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=a_lead, model_prob=0.99)
+  _, model_lead = make_model_lead()
+
+  assert build_model_lead_trajectory(model_lead, raw_lead, 20.0) is None
+
+
 def set_model_launch_trajectory(model, *, wait_time: float = 0.6, accel: float = 1.0):
   times = np.asarray(ModelConstants.T_IDXS, dtype=float)
   moving_time = np.maximum(times - wait_time, 0.0)

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -507,6 +507,31 @@ def test_force_stop_stays_committed_while_moving_even_if_scene_opens():
   assert vcruise.forcing_stop
 
 
+def test_force_stop_reanchors_when_model_reopens_path_without_stop_action():
+  planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
+  planner.model_length = 40.0
+  vcruise.tracked_model_length = 10.0
+  sm = make_sm(standstill=False)
+  sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
+
+  result = update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=1.5)
+
+  assert vcruise.tracked_model_length == pytest.approx(40.0)
+  assert result > 5.0
+
+
+def test_force_stop_does_not_reanchor_committed_model_stop():
+  planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
+  planner.model_length = 40.0
+  vcruise.tracked_model_length = 10.0
+  sm = make_sm(standstill=False)
+  sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=True))
+
+  update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=1.5)
+
+  assert vcruise.tracked_model_length < 10.0
+
+
 def test_force_stop_releases_after_cem_light_clears_while_moving():
   planner, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=True)
   sm = make_sm(standstill=False)

selfdrive/modeld/modeld.py

@@ -933,10 +933,7 @@ def main(demo=False):
     mt2 = time.perf_counter()
     model_execution_time = mt2 - mt1
 
-    if model_output is not None and vipc_dropped_frames > 0:
-      cloudlog.error(f"suppressing model output after dropping {vipc_dropped_frames} frames")
-
-    if model_output is not None and vipc_dropped_frames == 0:
+    if model_output is not None:
       modelv2_send = messaging.new_message('modelV2')
       starpilot_modelv2_send = messaging.new_message('starpilotModelV2')
       drivingdata_send = messaging.new_message('drivingModelData')

starpilot/controls/lib/starpilot_vcruise.py

@@ -71,6 +71,7 @@ FORCE_STOP_TURN_VETO_MAX_SPEED = 18.0 * CV.MPH_TO_MS
 FORCE_STOP_TURN_VETO_STEERING_ANGLE = 25.0
 FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE = 0.003
 FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME = 4.0
+FORCE_STOP_DISTANCE_REANCHOR_MIN_GAP = 3.0  # m — ignore small model-horizon noise
 
 # Knob bounds (mirror of UI slider; defense in depth)
 OFFSET_FT_MIN = -20
@@ -599,18 +600,32 @@ class StarPilotVCruise:
         v_cruise = 0.0
       else:
         # Kinematic distance estimator (also published as forcingStopLength).
-        # Decay one-to-one with motion, clamp by current model_length so we adopt
-        # the model's view when it regains sight, and snap closer to DASH_SEED_M
-        # when the dashboard signal is active and the model agrees a stop is near.
+        # Decay one-to-one with motion. A force-stop cycle may otherwise retain
+        # an old short horizon forever, even after the model has reopened the
+        # path. Recover only for the CEM/model path and only when the model's
+        # explicit stop action is clear; a committed model stop remains sticky.
         self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0.0)
-        self.tracked_model_length = min(self.tracked_model_length, self.starpilot_planner.model_length)
+        model_length = float(self.starpilot_planner.model_length)
+        try:
+          model_wants_stop = bool(sm["modelV2"].action.shouldStop)
+        except (KeyError, AttributeError, TypeError):
+          model_wants_stop = False
+        if (
+          not dash_active and
+          self.tracked_model_length > force_stop_handoff_m and
+          not model_wants_stop and
+          model_length > self.tracked_model_length + FORCE_STOP_DISTANCE_REANCHOR_MIN_GAP
+        ):
+          self.tracked_model_length = model_length
+        else:
+          self.tracked_model_length = min(self.tracked_model_length, model_length)
         if dash_active:
-          if self.starpilot_planner.model_length < DASH_MODEL_AGREE_M:
+          if model_length < DASH_MODEL_AGREE_M:
             self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M)
           # inside the seed the model range is the better line estimate; letting it pull
           # tracked back up is what keeps an early snap from parking us short of the sign
-          if self.starpilot_planner.model_length < DASH_SEED_M:
-            self.tracked_model_length = self.starpilot_planner.model_length
+          if model_length < DASH_SEED_M:
+            self.tracked_model_length = model_length
 
         # A car stopped in the next lane marks the stop bar better than the model does.
         # Shortening clamp only — it can pull the stop in, never push it out.

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

@@ -1375,6 +1375,7 @@
         "description": "Automatically adjust driving behavior based on real-time weather. Helps maintain comfort and safety in low visibility, rain, or snow.",
         "data_type": "bool",
         "ui_type": "toggle",
+        "is_parent_toggle": true,
         "parent_key": "QOLLongitudinal",
         "settings_tier": "simple"
       },
@@ -1617,6 +1618,7 @@
         "description": "Ask before changing to a new speed limit. To accept, tap the flashing on-screen widget or press the Cruise Increase button. To deny, press the Cruise Decrease button or ignore the prompt for 30 seconds.",
         "data_type": "bool",
         "ui_type": "toggle",
+        "is_parent_toggle": true,
         "parent_key": "SpeedLimitController",
         "settings_tier": "advanced"
       },
@@ -2376,6 +2378,7 @@
         "description": "Clear the active navigation destination when the device goes offroad.",
         "data_type": "bool",
         "ui_type": "toggle",
+        "is_parent_toggle": true,
         "settings_tier": "simple"
       },
       {
@@ -3942,6 +3945,7 @@
         "description": "WARNING: This will prevent your drives from being uploaded to comma connect which will impact debugging and official support from comma!\n\nPrevent the device from uploading driving data.",
         "data_type": "bool",
         "ui_type": "toggle",
+        "is_parent_toggle": true,
         "parent_key": "DeviceManagement",
         "settings_tier": "simple"
       },
@@ -4184,6 +4188,7 @@
         "description": "Show advanced settings in Galaxy.",
         "data_type": "bool",
         "ui_type": "toggle",
+        "is_parent_toggle": true,
         "settings_tier": "simple"
       },
       {

starpilot/system/the_galaxy/tests/test_device_settings_layout.py

@@ -95,6 +95,19 @@ def test_every_galaxy_setting_has_a_shared_settings_tier():
   assert None not in tiers
 
 
+def test_every_setting_parent_exposes_a_manage_control():
+  layout = _layout()
+
+  for section in layout:
+    params = section.get("params", [])
+    parent_keys = {param.get("parent_key") for param in params if param.get("parent_key")}
+    params_by_key = {param["key"]: param for param in params}
+    for parent_key in parent_keys:
+      assert params_by_key[parent_key].get("is_parent_toggle") is True, (
+        f"{section['name']} parent {parent_key} must expose its child settings"
+      )
+
+
 def test_requested_simple_and_advanced_settings_tiers():
   sections = _params_by_section(_layout())
   lateral = sections["Lateral (Steering)"]