From 25903a0d1a64a1f256d5c6ce8d6a06feef0262f5 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 19 Aug 2026 15:04:21 -0500 Subject: [PATCH] Lite Brite --- .../controls/lib/latcontrol_vehicle_tunes.py | 2 +- selfdrive/controls/lib/lead_follow_policy.py | 32 +++++++++++++++++-- .../controls/lib/longcontrol_vehicle_tunes.py | 15 ++++++--- .../lib/longitudinal_mpc_lib/long_mpc.py | 13 ++++++++ .../controls/tests/test_lead_follow_policy.py | 12 +++++++ selfdrive/controls/tests/test_longcontrol.py | 31 ++++++++++++++++++ .../tests/test_longitudinal_planner.py | 11 +++++++ .../controls/tests/test_starpilot_vcruise.py | 25 +++++++++++++++ selfdrive/modeld/modeld.py | 5 +-- starpilot/controls/lib/starpilot_vcruise.py | 29 +++++++++++++---- .../tools/device_settings_layout.json | 5 +++ .../tests/test_device_settings_layout.py | 13 ++++++++ 12 files changed, 174 insertions(+), 19 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 52ede8fb4..a9f16038c 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/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 diff --git a/selfdrive/controls/lib/lead_follow_policy.py b/selfdrive/controls/lib/lead_follow_policy.py index 516c659c0..5958896c2 100644 --- a/selfdrive/controls/lib/lead_follow_policy.py +++ b/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) diff --git a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py index 74b3d4e7e..3c57fc5a3 100644 --- a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py +++ b/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 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 4167390b0..ff724c92a 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/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]) diff --git a/selfdrive/controls/tests/test_lead_follow_policy.py b/selfdrive/controls/tests/test_lead_follow_policy.py index 5c388f80c..3cb129e74 100644 --- a/selfdrive/controls/tests/test_lead_follow_policy.py +++ b/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) diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index ef4566524..7a33eaa6c 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/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) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index b77e5ef63..0b8887771 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/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) diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 09f578b8d..0c972a90a 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/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) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 039f4e854..e45898a9c 100755 --- a/selfdrive/modeld/modeld.py +++ b/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') diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index bcc205818..9c16f75b8 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/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. diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json index c03b7740d..5d52a88b9 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json +++ b/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" }, { diff --git a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py index d9d5fdcea..86da94fe6 100644 --- a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py +++ b/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)"]