owalawattabotta

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,
       )

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

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

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:

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