From be1c67550c343a1d89f903f8de59b5a95032a469 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 10 Mar 2026 20:13:09 -0500 Subject: [PATCH] Planner --- frogpilot/controls/lib/frogpilot_following.py | 20 +++- selfdrive/controls/lib/longcontrol.py | 20 ---- .../lib/longitudinal_mpc_lib/long_mpc.py | 7 +- .../controls/lib/longitudinal_planner.py | 107 +++++++++++++++--- .../test/longitudinal_maneuvers/plant.py | 27 ++++- 5 files changed, 136 insertions(+), 45 deletions(-) diff --git a/frogpilot/controls/lib/frogpilot_following.py b/frogpilot/controls/lib/frogpilot_following.py index 5ff6eb5cb..0d6ccadd8 100644 --- a/frogpilot/controls/lib/frogpilot_following.py +++ b/frogpilot/controls/lib/frogpilot_following.py @@ -73,9 +73,23 @@ class FrogPilotFollowing: self.following_lead = self.frogpilot_planner.tracking_lead and self.frogpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego - self.disable_throttle = self.frogpilot_planner.tracking_lead and not self.following_lead - self.disable_throttle &= self.frogpilot_planner.lead_one.dRel + 6.0 < (self.t_follow * 2 * 2) * v_ego - self.disable_throttle &= self.frogpilot_planner.lead_one.vLead < v_ego * 0.75 + self.disable_throttle = False + if self.frogpilot_planner.tracking_lead and self.frogpilot_planner.lead_one.status: + lead_distance = self.frogpilot_planner.lead_one.dRel + v_lead = self.frogpilot_planner.lead_one.vLead + closing_speed = max(0.0, v_ego - v_lead) + desired_gap = float(desired_follow_distance(v_ego, v_lead, self.t_follow)) + ttc = lead_distance / max(closing_speed, 1e-3) if closing_speed > 0.1 else 1e6 + + # Keep a mild coasting behavior only for far/low-risk slower leads. + coast_window_open = lead_distance > desired_gap + max(4.0, 0.2 * v_ego) + coast_window_far = lead_distance < desired_gap + max(25.0, 1.2 * v_ego) + gentle_closing = closing_speed < max(2.0, 0.12 * v_ego) + + self.disable_throttle = (not self.following_lead and v_ego > 5.0 and coast_window_open and + coast_window_far and gentle_closing) + # Never coast when we are entering a potentially late-braking scenario. + self.disable_throttle &= ttc > 6.0 and lead_distance > desired_gap + 6.0 if sm["controlsState"].enabled and self.frogpilot_planner.tracking_lead: self.update_follow_values(self.frogpilot_planner.lead_one.dRel, v_ego, self.frogpilot_planner.lead_one.vLead, frogpilot_toggles) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index e8d1d5182..9f05d4ff3 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -207,26 +207,6 @@ class LongControl: else: output_accel = raw_output_accel - if self.long_control_state == LongCtrlState.pid: - # Smooth acceleration and deceleration with urgency-based rate limiting - base_rate = 1.0 - - if output_accel < self.last_output_accel: # Deceleration requested - decel_needed = self.last_output_accel - output_accel - # Use a safe default for ACCEL_MIN if not available, to prevent division by zero - max_decel = abs(CarControllerParams.ACCEL_MIN) if CarControllerParams.ACCEL_MIN != 0 else 4.0 - urgency = min(1.0, decel_needed / max_decel) - - # Adjust rate based on urgency (1.0 m/s^3 for low urgency, up to 4.0 m/s^3 for high urgency) - max_rate = 1.0 + 3.0 * urgency - else: - max_rate = base_rate # Acceleration is always smooth - - max_accel_change = max_rate * DT_CTRL - output_accel = clip(output_accel, - self.last_output_accel - max_accel_change, - self.last_output_accel + max_accel_change) - self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) return self.last_output_accel diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 533fb1d45..cc409f8fb 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -460,10 +460,11 @@ class LongitudinalMpc: def update(self, lead_one, lead_two, v_cruise, x, v, a, j, t_follow, tracking_lead, personality=log.LongitudinalPersonality.standard): v_ego = self.x0[1] - self.status = lead_one.status and tracking_lead or lead_two.status + self.status = lead_one.status or lead_two.status - lead_xv_0 = self.process_lead(lead_one, tracking_lead) - lead_xv_1 = self.process_lead(lead_two, v_ego) + # Always process valid leads for safety; trackingLead can still be used by higher-level logic/UI. + lead_xv_0 = self.process_lead(lead_one, lead_one.status) + lead_xv_1 = self.process_lead(lead_two, lead_two.status) # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 1d1f46b8e..0708c07ab 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -116,9 +116,11 @@ class LongitudinalPlanner: # Lead stability tracking self.prev_lead_dist = None self.last_big_brake_t = 0.0 + self.last_lead_brake_cmd_t = 0.0 self.stable_lead = False # Smoothed lead distance self.lead_dist_f = None + self.last_safety_log_t = 0.0 # Uncertainty slope tracking @@ -198,6 +200,26 @@ class LongitudinalPlanner: accel_limits = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration] steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP) + + # Safety override: keep profile comfort limits, but increase available braking + # when lead-closing risk rises so chill profiles cannot under-brake. + lead_one = sm['radarState'].leadOne + if lead_one.status: + lead_dist = float(lead_one.dRel) + rel_v = max(0.0, v_ego - float(lead_one.vLead)) + ttc = lead_dist / max(rel_v, 0.1) if rel_v > 0.1 else 1e6 + desired_gap = sm['frogpilotPlan'].tFollow * v_ego + 6.0 + + floor_ttc = interp(ttc, [1.6, 2.8, 4.0, 6.0, 10.0], + [ACCEL_MIN, -2.6, -1.8, -1.2, accel_limits_turns[0]]) + floor_rel_v = interp(rel_v, [0.0, 1.0, 2.5, 5.0, 8.0], + [accel_limits_turns[0], -1.1, -1.7, -2.5, ACCEL_MIN]) + gap_shortfall = max(0.0, desired_gap - lead_dist) + floor_gap = interp(gap_shortfall, [0.0, 2.0, 5.0, 9.0], + [accel_limits_turns[0], -1.2, -2.0, -2.8]) + + safety_floor = min(accel_limits_turns[0], floor_ttc, floor_rel_v, floor_gap) + accel_limits_turns[0] = max(ACCEL_MIN, safety_floor) else: accel_limits = [ACCEL_MIN, ACCEL_MAX] accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] @@ -206,6 +228,7 @@ class LongitudinalPlanner: self.v_desired_filter.x = v_ego # Clip aEgo to cruise limits to prevent large accelerations when becoming active self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]) + self.last_lead_brake_cmd_t = 0.0 # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) @@ -232,8 +255,14 @@ class LongitudinalPlanner: lead_dist = self.lead_one.dRel if self.lead_one.status else 50.0 - # Smooth lead distance (EMA) to avoid chatter in thresholds - alpha = max(0.02, min(0.15, 0.05 + 0.002 * v_ego)) + # Keep only light smoothing on lead distance so ACC reacts quickly like stock. + closing_speed = max(0.0, v_ego - self.lead_one.vLead) if self.lead_one.status else 0.0 + opening_speed = max(0.0, self.lead_one.vLead - v_ego) if self.lead_one.status else 0.0 + alpha = interp(v_ego, [0.0, 8.0, 15.0, 25.0, 35.0], [0.22, 0.28, 0.34, 0.42, 0.48]) + if closing_speed > 0.8: + alpha = max(alpha, interp(closing_speed, [0.8, 2.0, 4.0], [0.48, 0.58, 0.66])) + elif opening_speed > 1.0: + alpha = min(alpha, interp(opening_speed, [1.0, 2.5, 4.0], [alpha, 0.22, 0.18])) if self.lead_dist_f is None: self.lead_dist_f = float(lead_dist) else: @@ -309,9 +338,29 @@ class LongitudinalPlanner: uncertainty = self.uncert_slow.x uncertainty_accel = min(self.uncert_slow.x, self.uncert_fast.x) - self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, - sm['frogpilotPlan'].dangerJerk, - sm['frogpilotPlan'].speedJerk, + accel_jerk_w = sm['frogpilotPlan'].accelerationJerk + danger_jerk_w = sm['frogpilotPlan'].dangerJerk + speed_jerk_w = sm['frogpilotPlan'].speedJerk + + # In stable, low-risk car-following, increase smoothing to reduce rubberbanding. + if self.lead_one.status and self.stable_lead: + lead_dist_used = self.lead_dist_f if self.lead_dist_f is not None else self.lead_one.dRel + desired_gap = sm['frogpilotPlan'].tFollow * v_ego + 6.0 + gap_err = abs(lead_dist_used - desired_gap) + rel_v_abs = abs(v_ego - self.lead_one.vLead) + closing_v = max(0.0, v_ego - self.lead_one.vLead) + ttc = lead_dist_used / max(closing_v, 0.1) if closing_v > 0.1 else 1e6 + + gap_ok = gap_err < interp(v_ego, [0.0, 10.0, 20.0, 35.0], [1.0, 2.0, 3.5, 5.0]) + rel_v_ok = rel_v_abs < interp(v_ego, [0.0, 10.0, 20.0, 35.0], [0.30, 0.60, 0.90, 1.20]) + low_risk = (ttc > 3.0) and gap_ok and rel_v_ok + if low_risk: + accel_jerk_w *= interp(v_ego, [0.0, 10.0, 20.0, 35.0], [1.00, 1.08, 1.18, 1.26]) + speed_jerk_w *= interp(v_ego, [0.0, 10.0, 20.0, 35.0], [1.00, 1.04, 1.10, 1.16]) + + self.mpc.set_weights(accel_jerk_w, + danger_jerk_w, + speed_jerk_w, prev_accel_constraint, personality=sm['controlsState'].personality, v_ego=v_ego, @@ -339,10 +388,14 @@ class LongitudinalPlanner: # Safety checks for rubber-banding mitigation max_jerk = np.max(np.abs(self.mpc.j_solution)) max_accel_change = np.max(np.abs(np.diff(self.mpc.a_solution))) - if max_jerk > 5.0: # m/s^3 - cloudlog.warning(f"High jerk detected: {max_jerk:.2f} m/s^3") - if max_accel_change > 2.0: # m/s^2 - cloudlog.warning(f"High acceleration change: {max_accel_change:.2f} m/s^2") + now_t = time.monotonic() + if now_t - self.last_safety_log_t > 2.0: + if max_jerk > 5.0: # m/s^3 + cloudlog.warning(f"High jerk detected: {max_jerk:.2f} m/s^3") + self.last_safety_log_t = now_t + if max_accel_change > 2.0: # m/s^2 + cloudlog.warning(f"High acceleration change: {max_accel_change:.2f} m/s^2") + self.last_safety_log_t = now_t # Interpolate 0.05 seconds and save as starting point for next iteration a_prev = self.a_desired @@ -352,16 +405,36 @@ class LongitudinalPlanner: # Anticipatory pre-brake to avoid "coming in hot" when closing on a lead if self.lead_one.status: rel_v = max(0.0, v_ego - self.lead_one.vLead) - # dynamic time headway adds a small buffer when uncertainty is elevated - base_th = 1.6 - th = base_th + 0.6 * max(0.0, uncertainty - 0.42) - desired_gap = th * v_ego - if (self.lead_dist_f is not None and self.lead_dist_f < desired_gap and rel_v > 0.5): - k_rel, k_unc = 0.04, 0.20 - pre_brake = k_rel * rel_v + k_unc * max(0.0, uncertainty - 0.42) - pre_brake = min(pre_brake, 0.06) + lead_dist_f = self.lead_dist_f if self.lead_dist_f is not None else self.lead_one.dRel + ttc = lead_dist_f / max(rel_v, 0.1) if rel_v > 0.1 else 1e6 + desired_gap = sm['frogpilotPlan'].tFollow * v_ego + 6.0 + gap_shortfall = max(0.0, desired_gap - lead_dist_f) + + pre_brake_dist_trigger = desired_gap + interp(v_ego, [0.0, 10.0, 20.0, 30.0], [5.0, 5.8, 6.8, 8.0]) + if rel_v > 0.5 and lead_dist_f < pre_brake_dist_trigger: + pre_brake = 0.0 + pre_brake += interp(rel_v, [0.5, 2.0, 5.0, 8.0], [0.0, 0.02, 0.06, 0.11]) + pre_brake += interp(ttc, [1.4, 2.2, 3.5, 5.0, 7.5], [0.16, 0.09, 0.04, 0.01, 0.0]) + pre_brake += interp(gap_shortfall, [0.0, 2.0, 6.0, 10.0], [0.0, 0.015, 0.04, 0.07]) + pre_brake += 0.10 * max(0.0, uncertainty - 0.35) + # Mild low-speed soften to avoid excess early braking while retaining high-speed safety. + pre_brake *= interp(v_ego, [0.0, 8.0, 15.0, 25.0], [0.50, 0.68, 0.88, 1.00]) + pre_brake = min(pre_brake, interp(v_ego, [0.0, 5.0, 15.0, 30.0], [0.05, 0.08, 0.13, 0.16])) self.a_desired = float(self.a_desired - pre_brake) + # Shape accel release after low-speed lead-brake events to reduce stop-and-go brake->surge snapback. + if v_ego < 8.0 and rel_v > 0.2 and lead_dist_f < desired_gap + 2.5 and self.a_desired < -0.35: + self.last_lead_brake_cmd_t = now_t + + t_since_brake = now_t - self.last_lead_brake_cmd_t + release_window = interp(v_ego, [0.0, 3.0, 6.0, 8.0], [0.6, 0.7, 0.8, 0.9]) + low_risk_release = ttc > 2.0 and rel_v < interp(v_ego, [0.0, 3.0, 6.0, 8.0], [0.3, 0.45, 0.6, 0.75]) + near_lead = lead_dist_f < desired_gap + 2.0 + if 0.0 < t_since_brake < release_window and v_ego < 8.0 and near_lead and low_risk_release and self.a_desired > -0.05: + release_cap_t = interp(t_since_brake, [0.0, 0.15, 0.35, 0.60, release_window], [0.05, 0.14, 0.24, 0.34, 0.48]) + release_cap_v = interp(v_ego, [0.0, 3.0, 6.0, 8.0], [0.15, 0.24, 0.34, 0.42]) + self.a_desired = float(min(self.a_desired, min(release_cap_t, release_cap_v))) + # Small deadzone around zero accel to kill micro-dithers if -0.05 < self.a_desired < 0.05: self.a_desired = 0.0 diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 7596e1947..e6053368c 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import time +import types import numpy as np from cereal import log @@ -62,6 +63,7 @@ class Plant: radar = messaging.new_message('radarState') control = messaging.new_message('controlsState') car_state = messaging.new_message('carState') + lp = messaging.new_message('liveParameters') car_control = messaging.new_message('carControl') model = messaging.new_message('modelV2') a_lead = (v_lead - self.v_lead_prev)/self.ts @@ -113,22 +115,43 @@ class Plant: model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)] control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off + control.controlsState.enabled = bool(self.enabled) control.controlsState.vCruise = float(v_cruise * 3.6) control.controlsState.experimentalMode = self.e2e control.controlsState.personality = self.personality control.controlsState.forceDecel = self.force_decel car_state.carState.vEgo = float(self.speed) car_state.carState.standstill = self.speed < 0.01 - car_state.carState.vCruise = float(v_cruise * 3.6) + # Backward/forward compatible cruise field for host-side maneuver tests + if hasattr(car_state.carState, "vCruise"): + car_state.carState.vCruise = float(v_cruise * 3.6) + elif hasattr(car_state.carState, "cruiseState") and hasattr(car_state.carState.cruiseState, "speed"): + car_state.carState.cruiseState.speed = float(v_cruise) car_control.carControl.orientationNED = [0., float(pitch), 0.] # ******** get controlsState messages for plotting *** + frogpilot_plan = types.SimpleNamespace( + vCruise=float(v_cruise), + minAcceleration=-3.5, + maxAcceleration=2.0, + disableThrottle=False, + accelerationJerk=5.0, + dangerJerk=5.0, + speedJerk=5.0, + tFollow=1.45, + trackingLead=True, + forcingStopLength=2.0, + ) + lp.liveParameters.angleOffsetDeg = 0.0 + sm = {'radarState': radar.radarState, 'carState': car_state.carState, 'carControl': car_control.carControl, 'controlsState': control.controlsState, + 'liveParameters': lp.liveParameters, + 'frogpilotPlan': frogpilot_plan, 'modelV2': model.modelV2} - self.planner.update(sm) + self.planner.update(False, sm, types.SimpleNamespace(model_version='v11', taco_tune=False)) self.speed = self.planner.v_desired_filter.x self.acceleration = self.planner.a_desired self.speeds = self.planner.v_desired_trajectory.tolist()