diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 6e0919f1f..419a53c1c 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -362,7 +362,7 @@ 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): + uncertainty=0.0, accel_reengage=False, panic_bypass=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 @@ -407,6 +407,10 @@ class LongitudinalMpc: if accel_reengage: tgt_factor = min(tgt_factor, 0.5) + # Hard bypass of smoothing when approaching fast or magnitude trips + if panic_bypass: + tgt_factor = 0.0 + # 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) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 22d779b5d..0df315da7 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -24,6 +24,10 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] ALLOW_THROTTLE_THRESHOLD = 0.5 MIN_ALLOW_THROTTLE_SPEED = 2.5 +# Uncertainty-based filter disable thresholds +UNCERT_SLOPE_TRIG = 0.12 # per second +UNCERT_MAG_TRIG = 0.50 + # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] @@ -128,6 +132,10 @@ class LongitudinalPlanner: self._t_disarm = 0.0 self.lead_dist_f = None + # Uncertainty slope tracking + self._uncert_last = 0.0 + self._uncert_last_t = None + @property def mlsim(self): return self.generation in ("v8", "v10", "v11") @@ -313,7 +321,26 @@ class LongitudinalPlanner: 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) + uncertainty_accel = min(self.uncert_slow.x, self.uncert_fast.x) + + # --- Slope-based panic bypass --- + if self._uncert_last_t is None: + uncert_slope = 0.0 + else: + dt_u = max(1e-3, now_t - self._uncert_last_t) + uncert_slope = (uncertainty - self._uncert_last) / dt_u + self._uncert_last = uncertainty + self._uncert_last_t = now_t + + closing_fast = (self.lead_one.status 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) + + 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 self.lead_one.status 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 # now_t defined earlier over = uncertainty > 1.0 @@ -368,7 +395,8 @@ class LongitudinalPlanner: 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) + accel_reengage=self.accel_gate, + panic_bypass=panic_bypass) 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