From 8e7ce87be8f306a21fe3fb103fee2871bbfe175c Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 15 Jul 2026 15:57:21 -0500 Subject: [PATCH] owalawattabotta --- selfdrive/car/card.py | 8 ++- selfdrive/car/cruise.py | 5 ++ selfdrive/car/tests/test_cruise_speed.py | 46 ++++++++++++- .../controls/lib/longitudinal_planner.py | 6 +- .../tests/test_longitudinal_planner.py | 67 +++++++++++++++++++ 5 files changed, 126 insertions(+), 6 deletions(-) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 2b28af3e7..6570f5674 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -22,7 +22,10 @@ from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.common.constants import CV -from openpilot.selfdrive.car.cruise import VCruiseHelper, IMPERIAL_INCREMENT, V_CRUISE_MAX, V_CRUISE_MIN +from openpilot.selfdrive.car.cruise import ( + VCruiseHelper, IMPERIAL_INCREMENT, V_CRUISE_MAX, V_CRUISE_MIN, + is_speed_limit_confirmation_pending, +) from openpilot.selfdrive.car.redneck_cruise import RedneckCruise, select_redneck_target_speed from openpilot.selfdrive.car.car_specific import MockCarState @@ -232,11 +235,12 @@ class Car: self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise ) if not preap_software_cruise: + speed_limit_confirmation_pending = is_speed_limit_confirmation_pending(self.sm['starpilotPlan']) self.v_cruise_helper.update_v_cruise( CS, self.sm['carControl'].enabled, self.is_metric, - self.sm['starpilotPlan'].speedLimitChanged, + speed_limit_confirmation_pending, self.starpilot_toggles, FPCS, ) diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index ed036b107..9945c34f0 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -32,6 +32,11 @@ CRUISE_INTERVAL_SIGN = { ACCEL_CRUISE_BUTTONS = (ButtonType.accelCruise,) +def is_speed_limit_confirmation_pending(starpilot_plan) -> bool: + """Only consume cruise buttons when SLC has a limit awaiting confirmation.""" + return bool(starpilot_plan.speedLimitChanged and starpilot_plan.unconfirmedSlcSpeedLimit >= 1) + + class VCruiseHelper: def __init__(self, CP, FPCP=None): self.CP = CP diff --git a/selfdrive/car/tests/test_cruise_speed.py b/selfdrive/car/tests/test_cruise_speed.py index 10b385fac..c00e677c8 100644 --- a/selfdrive/car/tests/test_cruise_speed.py +++ b/selfdrive/car/tests/test_cruise_speed.py @@ -6,7 +6,10 @@ import numpy as np from parameterized import parameterized_class from types import SimpleNamespace from cereal import log -from openpilot.selfdrive.car.cruise import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT +from openpilot.selfdrive.car.cruise import ( + VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT, + is_speed_limit_confirmation_pending, +) from cereal import car from openpilot.common.constants import CV from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver @@ -15,6 +18,20 @@ ButtonEvent = car.CarState.ButtonEvent ButtonType = car.CarState.ButtonEvent.Type +@pytest.mark.parametrize(("speed_limit_changed", "unconfirmed_speed_limit", "expected"), [ + (False, 0.0, False), + (True, 0.0, False), + (True, 15.0, True), +]) +def test_speed_limit_confirmation_pending(speed_limit_changed, unconfirmed_speed_limit, expected): + plan = SimpleNamespace( + speedLimitChanged=speed_limit_changed, + unconfirmedSlcSpeedLimit=unconfirmed_speed_limit, + ) + + assert is_speed_limit_confirmation_pending(plan) is expected + + def run_cruise_simulation(cruise, e2e, personality, t_end=20.): man = Maneuver( '', @@ -313,6 +330,33 @@ class TestVCruiseHelper: assert self.v_cruise_helper.v_cruise_kph == initial_v_cruise_kph + def test_stale_speed_limit_change_does_adjust_cruise(self): + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) + initial_v_cruise_kph = self.v_cruise_helper.v_cruise_kph + plan = SimpleNamespace(speedLimitChanged=True, unconfirmedSlcSpeedLimit=0.0) + + pressed_cs = car.CarState(cruiseState={"available": True}) + pressed_cs.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=True)] + self.v_cruise_helper.update_v_cruise( + pressed_cs, + enabled=True, + is_metric=False, + speed_limit_changed=is_speed_limit_confirmation_pending(plan), + starpilot_toggles=self.starpilot_toggles, + ) + + released_cs = car.CarState(cruiseState={"available": True}) + released_cs.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=False)] + self.v_cruise_helper.update_v_cruise( + released_cs, + enabled=True, + is_metric=False, + speed_limit_changed=is_speed_limit_confirmation_pending(plan), + starpilot_toggles=self.starpilot_toggles, + ) + + assert self.v_cruise_helper.v_cruise_kph > initial_v_cruise_kph + def test_missing_custom_cruise_toggles_fall_back_to_single_step(self): self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) initial_v_cruise_kph = self.v_cruise_helper.v_cruise_kph diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 9c087bf7e..4c218d3d7 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -3134,9 +3134,9 @@ class LongitudinalPlanner: output_a_target = min(output_a_target, lead_catchup_accel_cap) if lead_control_active and np.isfinite(v_cruise) and any(lead.status for lead in (self.lead_one, self.lead_two)): - # Keep follow/catchup behavior from pulling past the cruise target. Using the - # same action horizon as the planner preserves normal accel farther below set speed. - cruise_accel_cap = (v_cruise - v_ego + 0.01) / max(action_t, self.dt) + # This is only an acceleration cap. A negative cap would manufacture hard + # braking on abrupt cruise-target drops instead of letting the MPC decelerate. + cruise_accel_cap = max(0.0, (v_cruise - v_ego + 0.01) / max(action_t, self.dt)) output_a_target = min(output_a_target, cruise_accel_cap) if vision_brake_cap_active: diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index edfe35c31..91486da03 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -230,6 +230,73 @@ def test_acc_mode_matches_no_lead_baseline_for_far_vision_only_lead_without_trac np.testing.assert_allclose(far_vision_outputs, no_lead_outputs, atol=1e-6) +def test_cruise_accel_cap_does_not_manufacture_braking_after_set_speed_drop_with_lead(): + v_ego = 20.115 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead( + status=True, + d_rel=68.95, + v_lead=18.74, + a_lead=-0.32, + radar=True, + model_prob=0.991, + y_rel=-0.15, + ) + lead_two = make_lead( + status=True, + d_rel=68.95, + v_lead=18.74, + a_lead=-0.32, + radar=True, + model_prob=0.993, + y_rel=-0.15, + ) + sm = make_sm( + v_ego, + desired_accel=-0.05, + min_accel=-1.0, + experimental_mode=True, + tracking_lead=True, + lead_one=lead_one, + lead_two=lead_two, + ) + sm["starpilotPlan"].vCruise = 15.646 + sm["starpilotPlan"].tFollow = 1.0 + + planner.update(sm, make_toggles()) + + assert planner.output_a_target > -1.0 + assert planner.output_a_target <= 0.0 + + +def test_cruise_accel_cap_preserves_close_lead_braking_after_set_speed_drop(): + v_ego = 20.115 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=-0.05, + min_accel=-1.0, + experimental_mode=True, + tracking_lead=True, + lead_one=make_lead( + status=True, + d_rel=20.0, + v_lead=0.0, + a_lead=-1.0, + radar=True, + model_prob=0.99, + ), + ) + sm["starpilotPlan"].vCruise = 15.646 + sm["starpilotPlan"].tFollow = 1.0 + + planner.update(sm, make_toggles()) + + assert planner.output_a_target <= -3.0 + + def test_soften_far_radar_lead_accel_reduces_gentle_far_brake(): softened = soften_far_radar_lead_accel(114.8, 28.88, -0.75, 29.26, 1.45, radar=True) assert softened > -0.35