diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 20db0f0f8..6e0919f1f 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -302,6 +302,9 @@ class LongitudinalMpc: self.current_filter_time = LEAD_FILTER_TIME_LOW self.lead_a_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt) self.lead_v_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt) + # Slew-limited filter factor to avoid abrupt 0.50↔1.00 jumps + self.filter_time_factor = 1.0 + self.slew_per_sec = 1.0 # Instance variables to avoid global modifications self.current_x_ego_cost = X_EGO_OBSTACLE_COSTS[0] self.current_j_ego_cost = J_EGO_COSTS[0] @@ -357,7 +360,9 @@ class LongitudinalMpc: for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - 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): + 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): # Update parameters based on current speed with interpolation for smooth scaling speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph @@ -390,12 +395,31 @@ class LongitudinalMpc: speed_jerk *= dist_factor # Scene complexity adjustment based on model uncertainty - complexity_factor = 1.0 - filter_time_factor = 1.0 + prev_filter_time_factor = getattr(self, 'prev_filter_time_factor', 1.0) + # Target factor from uncertainty + if uncertainty <= 0.45: + tgt_factor = 1.0 + elif uncertainty >= 0.70: + tgt_factor = 0.0 + else: + tgt_factor = float(np.interp(uncertainty, [0.45, 0.70], [1.0, 0.30])) - if uncertainty > 1.0: # High uncertainty indicates complex scene - complexity_factor = 1.5 # Boost responsiveness - filter_time_factor = 0.0 # Disable smoothing for immediate response + if accel_reengage: + tgt_factor = min(tgt_factor, 0.5) + + # Slew-limit changes to avoid step-wise filter jumps + max_step = self.slew_per_sec * self.dt + delta = np.clip(tgt_factor - self.filter_time_factor, -max_step, max_step) + self.filter_time_factor += float(delta) + filter_time_factor = float(self.filter_time_factor) + + # When uncertainty is moderately elevated, allow accel but cap jerk by increasing jerk cost + if 0.45 <= uncertainty < 0.60: + scale = float(np.interp(uncertainty, [0.45, 0.60], [1.2, 1.5])) + speed_jerk *= scale + + if abs(filter_time_factor - prev_filter_time_factor) > 1e-3: + cloudlog.error(f"LON_FILTER; filter_time_factor={filter_time_factor:.2f}; uncertainty={uncertainty:.3f}; v_ego={v_ego:.2f} mps; lead_dist={lead_dist:.2f} m; accel_reengage={accel_reengage}") if self.mode == 'acc': a_change_cost = acceleration_jerk if prev_accel_constraint else 0 @@ -410,7 +434,7 @@ class LongitudinalMpc: self.set_cost_weights(cost_weights, constraint_cost_weights) # Adjust filter time constants for complex scenes - if abs(filter_time_factor - getattr(self, 'prev_filter_time_factor', 1.0)) > 0.1: + if abs(filter_time_factor - getattr(self, 'prev_filter_time_factor', 1.0)) > 0.05: current_a = self.lead_a_filter.x if hasattr(self.lead_a_filter, 'x') else 0.0 current_v = self.lead_v_filter.x if hasattr(self.lead_v_filter, 'x') else 0.0 new_filter_time = self.current_filter_time * filter_time_factor diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index cf161471c..2911d7eae 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import math import numpy as np +import time from openpilot.common.numpy_fast import clip, interp import cereal.messaging as messaging @@ -106,6 +107,26 @@ class LongitudinalPlanner: self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 + # logging cadence & state + self.last_uncert_log_t = 0.0 + self.prev_uncert_over = False + + # ---- Rubberband mitigation state ---- + # Two uncertainty tracks (slow/fast) for asymmetric gating + self.uncert_slow = FirstOrderFilter(0.0, 1.6, self.dt) # ~lam=0.6 + self.uncert_fast = FirstOrderFilter(0.0, 0.9, self.dt) # faster cool-down for accel decisions + # Lead stability tracking + self.prev_lead_dist = None + self.last_big_brake_t = 0.0 + self.stable_lead = False + # Temporary accel nudge window + self.accel_nudge_until = 0.0 + + # Hysteresis gate + dwell for accel re-engage and smoothed lead distance + self.accel_gate = False + self._t_arm = 0.0 + self._t_disarm = 0.0 + self.lead_dist_f = None @property def mlsim(self): @@ -216,31 +237,137 @@ 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)) + if self.lead_dist_f is None: + self.lead_dist_f = float(lead_dist) + else: + self.lead_dist_f += alpha * (float(lead_dist) - self.lead_dist_f) + + # Lead stability estimation and recent-brake timer + now_t = time.monotonic() + # relative speed (ego - lead) positive when closing + v_rel = (v_ego - self.lead_one.vLead) if self.lead_one.status else 0.0 + if self.prev_lead_dist is None: + d_rel_dot = 0.0 + else: + d_rel_dot = (lead_dist - self.prev_lead_dist) / max(self.dt, 1e-3) + self.prev_lead_dist = lead_dist + + # Remember time of last non-trivial model brake risk + if 'raw_brake_max' in locals() and raw_brake_max is not None and raw_brake_max > 0.02: + self.last_big_brake_t = now_t + + # Stable lead heuristic (short window, cheap to compute) + recently_braked = (now_t - self.last_big_brake_t) < 0.7 + self.stable_lead = ( + self.lead_one.status and + abs(v_rel) < 0.5 and + abs(d_rel_dot) < 0.5 and + not recently_braked + ) + # Calculate scene uncertainty from model desire prediction entropy and disengage predictions uncertainty = 0.0 if hasattr(sm['modelV2'], 'meta'): - # Desire prediction entropy (maneuver uncertainty) + # Desire prediction entropy (maneuver uncertainty), normalized to [0, 1] desire_entropy = 0.0 if hasattr(sm['modelV2'].meta, 'desirePrediction'): desire_probs = sm['modelV2'].meta.desirePrediction if len(desire_probs) > 1: - desire_probs = np.array(desire_probs) - desire_probs = desire_probs / np.sum(desire_probs) # Normalize - desire_entropy = -np.sum(desire_probs * np.log(desire_probs + 1e-10)) + probs = np.asarray(desire_probs, dtype=float) + total = float(np.sum(probs)) + if total > 1e-6: + p = probs / total + entropy = -np.sum(p * np.log(p + 1e-10)) + max_entropy = np.log(len(p)) + desire_entropy = float(entropy / max(max_entropy, 1e-6)) # normalized entropy in [0,1] + else: + desire_entropy = 0.0 # guard against all-zero vector # Disengage prediction risk (intervention likelihood) disengage_risk = 0.0 + raw_brake_max = -1.0 + lam = -1.0 if hasattr(sm['modelV2'].meta, 'disengagePredictions'): # Use brake press probabilities as primary risk indicator brake_probs = sm['modelV2'].meta.disengagePredictions.brakePressProbs if len(brake_probs) > 0: - disengage_risk = np.max(brake_probs) # Peak risk over time horizon + # Exponentially decayed max over the full horizon + probs = np.asarray(brake_probs, dtype=float) + # Clip tiny brake blips so they don't inflate uncertainty + if float(np.max(probs)) < 0.015: + probs = probs * 0.5 + raw_brake_max = float(np.max(probs)) + # Time vector assuming model horizon step = DT_MDL + t = np.arange(len(probs), dtype=float) * DT_MDL + lam = 0.6 # decay rate per second (tunable: 0.5–0.9 typical) + weights = np.exp(-lam * t) + disengage_risk = float(np.max(probs * weights)) - # Combined uncertainty metric - uncertainty = desire_entropy + disengage_risk + # Combined uncertainty metric (range roughly 0..2), with dual-track filtering + raw_uncertainty = desire_entropy + disengage_risk + # Update filters + self.uncert_slow.update(raw_uncertainty) + self.uncert_fast.update(raw_uncertainty) + # Use a more permissive track for accel decisions + 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, prev_accel_constraint, - personality=sm['controlsState'].personality, v_ego=v_ego, lead_dist=lead_dist, uncertainty=uncertainty) + # now_t defined earlier + over = uncertainty > 1.0 + # Log on threshold edge or at ~1 Hz + if over != self.prev_uncert_over or (now_t - self.last_uncert_log_t) > 1.0: + try: + cloudlog.error( + f"LON_UNCERT; v_ego={v_ego:.2f} mps; desireEntropy={desire_entropy:.3f}; " + f"brakeRawMax={(raw_brake_max if 'raw_brake_max' in locals() else -1.0):.3f}; " + f"brakeDecayed={(disengage_risk if 'disengage_risk' in locals() else -1.0):.3f}; " + f"lam={(lam if 'lam' in locals() else -1.0):.2f}; uncertainty={uncertainty:.3f}; over={over}" + ) + except Exception as e: + cloudlog.warning(f"LON_UNCERT log error: {e}") + self.prev_uncert_over = over + self.last_uncert_log_t = now_t + + # Asymmetric accel release with hysteresis + dwell to prevent on/off pulsing + rise_dwell_s, fall_dwell_s = 0.6, 0.4 + good = ( + (self.a_desired > 0.0) and + self.stable_lead and + (uncertainty <= 0.425) and + (desire_entropy < 0.41) + ) + + # dwell timers for robust gating + if good and not self.accel_gate: + if now_t - self._t_arm >= rise_dwell_s: + self.accel_gate = True + else: + self._t_arm = now_t + + if (not good) and self.accel_gate: + if now_t - self._t_disarm >= fall_dwell_s: + self.accel_gate = False + else: + self._t_disarm = now_t + + if self.accel_gate: + # Ensure some positive headroom for MPC to exit coasting + accel_limits_turns[1] = max(accel_limits_turns[1], 0.2) + # Short self-canceling nudge to unstick (applied post-MPC) + if now_t > self.accel_nudge_until: + self.accel_nudge_until = now_t + 0.45 + + self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, + sm['frogpilotPlan'].dangerJerk, + sm['frogpilotPlan'].speedJerk, + prev_accel_constraint, + personality=sm['controlsState'].personality, + v_ego=v_ego, + lead_dist=self.lead_dist_f if self.lead_dist_f is not None else lead_dist, + uncertainty=uncertainty, + accel_reengage=self.accel_gate) 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 @@ -273,6 +400,27 @@ class LongitudinalPlanner: self.a_desired = float(interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 + # 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) + self.a_desired = float(self.a_desired - pre_brake) + + # Apply tiny feed-forward nudge when released and safe + if now_t < self.accel_nudge_until and self.a_desired > -0.1: + self.a_desired = float(min(self.a_desired + 0.12, get_max_accel(v_ego))) + + # Small deadzone around zero accel to kill micro-dithers + if -0.05 < self.a_desired < 0.05: + self.a_desired = 0.0 + def publish(self, classic_model, tinygrad_model, sm, pm, frogpilot_toggles): plan_send = messaging.new_message('longitudinalPlan')