LongyLongNeck

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -398,7 +398,8 @@ class LongitudinalMpc:
 
   def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True,
                   personality=log.LongitudinalPersonality.standard, v_ego=0.0, lead_dist=50.0,
-                  uncertainty=0.0, accel_reengage=False, panic_bypass=False):
+                  uncertainty=0.0, accel_reengage=False, panic_bypass=False,
+                  filter_time_factor_floor=0.0):
     # Update parameters based on current speed with interpolation for smooth scaling
     speed_mph = v_ego * CV.MS_TO_MPH  # Convert m/s to mph
 
@@ -446,6 +447,8 @@ class LongitudinalMpc:
     # Hard bypass of smoothing when approaching fast or magnitude trips
     if panic_bypass:
       tgt_factor = 0.0
+    else:
+      tgt_factor = max(tgt_factor, float(filter_time_factor_floor))
 
     # Slew-limit changes to avoid step-wise filter jumps
     max_step = self.slew_per_sec * self.dt

selfdrive/controls/lib/longitudinal_planner.py

@@ -118,6 +118,23 @@ LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN = 0.15
 # Uncertainty-based filter disable thresholds
 UNCERT_SLOPE_TRIG = 0.12  # per second
 UNCERT_MAG_TRIG = 0.50
+UNCERT_PANIC_MIN_CLOSING_SPEED = 2.0
+UNCERT_PANIC_MIN_CLOSING_SPEED_GAIN = 0.08
+UNCERT_PANIC_MAX_GAP_BUFFER_MIN = 8.0
+UNCERT_PANIC_MAX_GAP_BUFFER_GAIN = 0.35
+STEADY_FOLLOW_SMOOTHING_MIN_SPEED = 22.0
+STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED = 0.15
+STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED = 1.8
+STEADY_FOLLOW_SMOOTHING_MIN_HEADWAY = 0.95
+STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET = 0.35
+STEADY_FOLLOW_SMOOTHING_HEADWAY_ABOVE_TARGET = 0.90
+STEADY_FOLLOW_SMOOTHING_MAX_LEAD_BRAKE = 0.35
+STEADY_FOLLOW_SMOOTHING_MIN_MODEL_PROB = 0.7
+STEADY_FOLLOW_SMOOTHING_FILTER_FACTOR_FLOOR = 0.24
+STEADY_FOLLOW_BRAKE_CAP_MIN_HEADWAY = 1.05
+STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET = 0.90
+STEADY_FOLLOW_BRAKE_CAP_MIN_DECEL = 0.18
+STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL = 0.32
 
 # Lookup table for turns
 _A_TOTAL_MAX_V = [3.5, 3.5, 3.2]
@@ -286,6 +303,7 @@ class LongitudinalPlanner:
     # Uncertainty slope tracking
     self._uncert_last = 0.0
     self._uncert_last_t = None
+    self._panic_bypass_log_t = 0.0
     self.effective_t_follow = None
     self.vision_low_speed_stop_hold_until = 0.0
     self.vision_lead_approach_confirm_t = 0.0
@@ -638,6 +656,37 @@ class LongitudinalPlanner:
     gap_factor = float(np.clip(max(gap_error, 0.0) / max(gap_buffer, 0.1), 0.0, 1.0))
     return float(np.interp(gap_factor, [0.0, 1.0], [near_cap, edge_cap]))
 
+  def get_matched_follow_brake_cap(self, lead, v_ego, base_t_follow):
+    if lead is None or not lead.status or v_ego < STEADY_FOLLOW_SMOOTHING_MIN_SPEED:
+      return None
+
+    closing_speed = max(0.0, float(v_ego) - float(lead.vLead))
+    if not (STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED <= closing_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED):
+      return None
+
+    lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
+    if lead_brake > STEADY_FOLLOW_SMOOTHING_MAX_LEAD_BRAKE:
+      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 < STEADY_FOLLOW_SMOOTHING_MIN_MODEL_PROB:
+      return None
+
+    actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
+    if actual_headway < max(STEADY_FOLLOW_BRAKE_CAP_MIN_HEADWAY, float(base_t_follow) - STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET):
+      return None
+    if actual_headway > float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET:
+      return None
+
+    cap_decel = float(np.interp(
+      closing_speed,
+      [STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED, STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED],
+      [STEADY_FOLLOW_BRAKE_CAP_MIN_DECEL, STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL],
+    ))
+    headway_deficit = float(np.clip((float(base_t_follow) - actual_headway) / STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET, 0.0, 1.0))
+    cap_decel = min(STEADY_FOLLOW_BRAKE_CAP_MAX_DECEL, cap_decel + 0.05 * headway_deficit)
+    return -cap_decel
+
   @staticmethod
   def raw_close_lead_needs_control(lead, v_ego):
     if lead is None or not lead.status:
@@ -720,8 +769,10 @@ class LongitudinalPlanner:
 
     if not self.allow_throttle:
       clipped_accel_coast = max(accel_coast, accel_limits_turns[0])
-      clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast])
-      accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp)
+      # Hold the output cap to the physical coasting limit until throttle is
+      # allowed again. Relaxing back toward positive accel while the gate is
+      # still closed can stall downhill coastdown well above the target speed.
+      accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast)
     no_throttle_output_max = accel_limits_turns[1]
 
     if force_slow_decel:
@@ -840,15 +891,57 @@ class LongitudinalPlanner:
     self._uncert_last = uncertainty
     self._uncert_last_t = now_t
 
-    closing_fast = lead_one_active and (v_ego - self.lead_one.vLead) > 0.5
-    # Trigger if either slope is high or magnitude is high; require a valid lead and closing
-    panic_bypass = closing_fast and (uncert_slope > UNCERT_SLOPE_TRIG or uncertainty >= UNCERT_MAG_TRIG)
+    panic_close_window = False
+    closing_fast = False
+    desired_gap = None
+    closing_speed = 0.0
+    if lead_one_active:
+      desired_gap = float(desired_follow_distance(v_ego, self.lead_one.vLead, effective_t_follow))
+      close_gap_window = max(UNCERT_PANIC_MAX_GAP_BUFFER_MIN,
+                             UNCERT_PANIC_MAX_GAP_BUFFER_GAIN * float(v_ego))
+      panic_close_window = float(self.lead_one.dRel) <= desired_gap + close_gap_window
+      closing_speed = max(0.0, v_ego - self.lead_one.vLead)
+      closing_fast = closing_speed >= max(
+        UNCERT_PANIC_MIN_CLOSING_SPEED,
+        UNCERT_PANIC_MIN_CLOSING_SPEED_GAIN * float(v_ego),
+      )
+
+    # Only bypass lead smoothing when we're closing meaningfully and already
+    # near the follow window. Far or nearly pace-matched leads should stay on
+    # the smoothed path so the planner doesn't flip-flop between accel and brake.
+    panic_bypass = panic_close_window and closing_fast and (
+      uncert_slope > UNCERT_SLOPE_TRIG or uncertainty >= UNCERT_MAG_TRIG
+    )
+
+    steady_follow_filter_floor = 0.0
+    if lead_one_active and desired_gap is not None and not panic_bypass:
+      lead_brake = max(0.0, -float(getattr(self.lead_one, "aLeadK", 0.0)))
+      lead_prob = float(getattr(self.lead_one, "modelProb", 1.0 if bool(getattr(self.lead_one, "radar", False)) else 0.0))
+      actual_headway = float(self.lead_one.dRel) / max(float(v_ego), 1e-3)
+      matched_follow_window = (
+        v_ego >= STEADY_FOLLOW_SMOOTHING_MIN_SPEED and
+        STEADY_FOLLOW_SMOOTHING_MIN_CLOSING_SPEED <= closing_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED and
+        actual_headway >= max(STEADY_FOLLOW_SMOOTHING_MIN_HEADWAY,
+                              effective_t_follow - STEADY_FOLLOW_SMOOTHING_HEADWAY_BELOW_TARGET) and
+        actual_headway <= effective_t_follow + STEADY_FOLLOW_SMOOTHING_HEADWAY_ABOVE_TARGET and
+        lead_brake <= STEADY_FOLLOW_SMOOTHING_MAX_LEAD_BRAKE and
+        (bool(getattr(self.lead_one, "radar", False)) or lead_prob >= STEADY_FOLLOW_SMOOTHING_MIN_MODEL_PROB)
+      )
+      if matched_follow_window:
+        steady_follow_filter_floor = STEADY_FOLLOW_SMOOTHING_FILTER_FACTOR_FLOOR
 
     if panic_bypass:
-      try:
-        cloudlog.error(f"LON_SLOPE; slope={uncert_slope:.3f}/s; uncertainty={uncertainty:.3f}; v_ego={v_ego:.2f}; v_rel={(v_ego - self.lead_one.vLead) if lead_one_active else 0.0:.2f}; lead_dist={self.lead_dist_f if self.lead_dist_f is not None else -1:.2f}; trigger=True")
-      except Exception:
-        pass
+      if now_t - self._panic_bypass_log_t > 5.0:
+        self._panic_bypass_log_t = now_t
+        try:
+          cloudlog.warning(
+            "LON_SLOPE close bypass: "
+            f"slope={uncert_slope:.3f}/s uncertainty={uncertainty:.3f} "
+            f"v_ego={v_ego:.2f} v_rel={(v_ego - self.lead_one.vLead) if lead_one_active else 0.0:.2f} "
+            f"lead_dist={self.lead_dist_f if self.lead_dist_f is not None else -1:.2f}"
+          )
+        except Exception:
+          pass
 
     personality = get_longitudinal_personality(sm)
 
@@ -860,7 +953,8 @@ class LongitudinalPlanner:
                          v_ego=v_ego,
                          lead_dist=self.lead_dist_f if lead_one_active and self.lead_dist_f is not None else 50.0,
                          uncertainty=uncertainty,
-                         panic_bypass=panic_bypass)
+                         panic_bypass=panic_bypass,
+                         filter_time_factor_floor=steady_follow_filter_floor)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     # After deciding the MPC mode via get_mpc_mode(), ensure MPC uses that mode when not mlsim
@@ -1040,6 +1134,12 @@ class LongitudinalPlanner:
     if vision_brake_cap_active:
       output_accel_min = min(output_accel_min, vision_cap_accel_min)
 
+    if lead_one_active and not panic_bypass:
+      matched_follow_brake_cap = self.get_matched_follow_brake_cap(self.lead_one, v_ego, sm['starpilotPlan'].tFollow)
+      if matched_follow_brake_cap is not None:
+        self.a_desired = max(self.a_desired, matched_follow_brake_cap)
+        output_a_target = max(output_a_target, matched_follow_brake_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))
 

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -9,7 +9,7 @@ from cereal import log
 from opendbc.car.honda.interface import CarInterface
 from opendbc.car.honda.values import CAR
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
-from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_vehicle_min_accel
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import soften_far_radar_lead_accel
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 
@@ -29,7 +29,7 @@ def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead
   return lead
 
 
-def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0):
+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()
   t_idxs = ModelConstants.T_IDXS
 
@@ -49,6 +49,7 @@ def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0):
   model.acceleration.t = [float(t) for t in t_idxs]
 
   model.meta.disengagePredictions.gasPressProbs = [float(gas_press_prob)] * 6
+  model.meta.disengagePredictions.brakePressProbs = [float(brake_press_prob)] * 6
   model.action.desiredAcceleration = desired_accel
   model.action.shouldStop = False
   return model
@@ -56,7 +57,7 @@ def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0):
 
 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, disable_throttle: bool = False):
+            gas_press_prob: float = 1.0, brake_press_prob: float = 0.0, disable_throttle: bool = False):
   return {
     "carControl": SimpleNamespace(orientationNED=[0.0, 0.0, 0.0]),
     "carState": SimpleNamespace(
@@ -72,7 +73,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta
       forceDecel=False,
     ),
     "liveParameters": SimpleNamespace(angleOffsetDeg=0.0),
-    "modelV2": make_model(v_ego, desired_accel, gas_press_prob=gas_press_prob),
+    "modelV2": make_model(v_ego, desired_accel, gas_press_prob=gas_press_prob, brake_press_prob=brake_press_prob),
     "radarState": SimpleNamespace(
       leadOne=lead_one if lead_one is not None else make_lead(status=False),
       leadTwo=lead_two if lead_two is not None else make_lead(status=False),
@@ -912,3 +913,85 @@ def test_allow_throttle_hysteresis_filters_gas_prob_chatter():
   planner.update(sm, toggles)
   assert planner.model_allow_throttle
   assert planner.allow_throttle
+
+
+def test_no_throttle_cap_stays_at_coast_limit_until_throttle_returns():
+  v_ego = 8.5
+
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  sm = make_sm(v_ego, desired_accel=0.0, min_accel=-3.0, experimental_mode=False, gas_press_prob=0.0)
+  sm["carControl"].orientationNED = [0.0, 0.1, 0.0]
+  toggles = make_toggles()
+
+  planner.update(sm, toggles)
+
+  accel_coast = max(get_vehicle_min_accel(CP, v_ego), get_coast_accel(sm["carControl"].orientationNED[1]))
+
+  assert not planner.allow_throttle
+  assert planner.output_a_target == pytest.approx(accel_coast, abs=1e-3)
+
+
+def test_far_near_speed_follow_keeps_uncertainty_smoothing_active():
+  v_ego = 30.0
+
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  sm = make_sm(
+    v_ego,
+    desired_accel=0.0,
+    min_accel=-1.0,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=78.0, v_lead=29.2, radar=False, model_prob=0.96),
+  )
+  sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=1.0, brake_press_prob=0.52)
+  toggles = make_toggles()
+
+  for _ in range(12):
+    planner.update(sm, toggles)
+
+  assert planner.mpc.filter_time_factor > 0.75
+
+
+def test_near_speed_follow_keeps_some_smoothing_under_high_uncertainty():
+  v_ego = 31.0
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=53.0, v_lead=29.8, radar=False, model_prob=0.96)
+  sm = make_sm(
+    v_ego,
+    desired_accel=0.0,
+    min_accel=-1.0,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=lead,
+    brake_press_prob=0.85,
+  )
+
+  for _ in range(16):
+    planner.update(sm, make_toggles())
+
+  assert planner.mpc.filter_time_factor >= 0.24
+
+
+def test_near_speed_follow_soft_brake_cap_limits_matched_follow_pulse():
+  v_ego = 31.4
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=44.0, v_lead=30.1, radar=False, model_prob=0.96)
+  sm = make_sm(
+    v_ego,
+    desired_accel=0.0,
+    min_accel=-1.0,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=lead,
+    brake_press_prob=0.85,
+  )
+
+  for _ in range(16):
+    planner.update(sm, make_toggles())
+
+  assert planner.mpc.filter_time_factor >= 0.24
+  assert planner.output_a_target >= -0.33

selfdrive/test/process_replay/process_replay.py

@@ -465,7 +465,8 @@ CONFIGS = [
   ),
   ProcessConfig(
     proc_name="plannerd",
-    pubs=["modelV2", "carControl", "carState", "controlsState", "liveParameters", "radarState", "selfdriveState"],
+    pubs=["modelV2", "carControl", "carState", "controlsState", "liveParameters", "radarState", "selfdriveState",
+          "starpilotCarState", "starpilotPlan"],
     subs=["longitudinalPlan", "driverAssistance"],
     ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"],
     init_callback=get_car_params_callback,

selfdrive/test/process_replay/test_process_replay_starpilot.py

@@ -0,0 +1,12 @@
+import os
+
+os.environ["DEBUG"] = "0"
+
+from openpilot.selfdrive.test.process_replay.process_replay import get_process_config
+
+
+def test_plannerd_replay_includes_starpilot_inputs():
+  cfg = get_process_config("plannerd")
+
+  assert "starpilotPlan" in cfg.pubs
+  assert "starpilotCarState" in cfg.pubs