From 056544acbeb4d96eacbb366dc601085a6713fdc9 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 8 Mar 2026 16:33:54 -0500 Subject: [PATCH] Refine GM pedal/paddle tuning and EV accel profile behavior --- .../controls/lib/frogpilot_acceleration.py | 8 +- selfdrive/car/gm/carcontroller.py | 178 ++++++++++++++---- selfdrive/car/gm/interface.py | 41 +++- selfdrive/controls/lib/longcontrol.py | 41 +++- .../lib/longitudinal_mpc_lib/long_mpc.py | 56 +----- .../controls/lib/longitudinal_planner.py | 27 +-- 6 files changed, 224 insertions(+), 127 deletions(-) diff --git a/frogpilot/controls/lib/frogpilot_acceleration.py b/frogpilot/controls/lib/frogpilot_acceleration.py index 137d6306b..2c1569016 100644 --- a/frogpilot/controls/lib/frogpilot_acceleration.py +++ b/frogpilot/controls/lib/frogpilot_acceleration.py @@ -53,10 +53,10 @@ A_CRUISE_MIN_SPORT = A_CRUISE_MIN * 2 # MPH = [0.0, 11, 22, 34, 45, 56, 89] A_CRUISE_MAX_BP_CUSTOM = [0.0, 5., 10., 15., 20., 25., 40.] -A_CRUISE_MAX_VALS_ECO_EV = [1.0, 1.0, 1.0, 1.0, 1.12, 1.12, 1.45] -A_CRUISE_MAX_VALS_STANDARD_EV = [1.15, 1.15, 1.15, 1.15, 1.30, 1.30, 1.72] -A_CRUISE_MAX_VALS_SPORT_EV = [1.25, 1.25, 1.25, 1.25, 1.45, 1.5, 2.0] -A_CRUISE_MAX_VALS_SPORT_PLUS_EV = [1.35, 1.35, 1.35, 1.35, 1.60, 1.60, 2.10] +A_CRUISE_MAX_VALS_ECO_EV = [1.15, 1.15, 1.15, 1.15, 1.30, 1.30, 1.72] +A_CRUISE_MAX_VALS_STANDARD_EV = [1.25, 1.25, 1.25, 1.25, 1.45, 1.50, 2.00] +A_CRUISE_MAX_VALS_SPORT_EV = [1.35, 1.35, 1.35, 1.35, 1.60, 1.60, 2.10] +A_CRUISE_MAX_VALS_SPORT_PLUS_EV = [1.55, 1.55, 1.55, 1.55, 1.84, 1.84, 2.42] A_CRUISE_MAX_VALS_ECO_GAS = [2.0, 1.5, 1.0, 0.8, 0.6, 0.4, 0.2] A_CRUISE_MAX_VALS_SPORT_GAS = [3.0, 2.5, 2.0, 1.5, 1.0, 0.8, 0.6] A_CRUISE_MAX_VALS_ECO_TRUCK = [3.00, 1.05, 0.60, 0.50, 0.50, 0.45, 0.35] diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index a5a6e1a87..67e9704ff 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -79,50 +79,154 @@ class CarController(CarControllerBase): self.regen_paddle_pressed = False self.aego = 0.0 self.regen_paddle_timer = 0 + self.regen_press_counter = 0 + self.regen_release_counter = 0 + self.regen_min_on_frames = 0 + self.regen_min_off_frames = 0 self.planner_regen_hold = False self.paddle_handoff_frames = 0 + self.pedal_active_last = False + self.regen_mode_blend = 0.0 + self.maneuver_paddle_mode = "auto" def calc_pedal_command(self, accel: float, long_active: bool, car_velocity) -> Tuple[float, bool]: if not long_active: self.planner_regen_hold = False + self.regen_paddle_pressed = False + self.regen_paddle_timer = 0 + self.regen_press_counter = 0 + self.regen_release_counter = 0 + self.regen_min_on_frames = 0 + self.regen_min_off_frames = 0 + self.regen_mode_blend = 0.0 + self.pedal_active_last = False + self.pedal_steady = 0.0 return 0., False - # Regen paddle hysteresis (frame-based): hold 10 frames, with decrement dead-zone - if not hasattr(self, 'regen_paddle_timer'): - self.regen_paddle_timer = 0 # frames + # Regen paddle state machine: speed-aware thresholds + min on/off duration for robust paddle transitions. + press_cmd_threshold = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [-0.90, -0.82, -0.72, -0.65]) + release_cmd_threshold = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [-0.10, -0.17, -0.24, -0.30]) + press_aego_threshold = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [-0.95, -0.86, -0.76, -0.70]) + release_aego_threshold = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [-0.16, -0.23, -0.30, -0.36]) - # Regen paddle hysteresis (frame‑based): count frames when decelerating hard, decrement only when truly released - if self.aego < -0.7: - self.regen_paddle_timer += 1 - elif self.aego > -0.3: - self.regen_paddle_timer = max(self.regen_paddle_timer - 1, 0) - # else: hold timer between -0.7 and -0.3 + press_confirm_frames = int(round(interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [8.0, 6.0, 5.0, 4.0]))) + release_confirm_frames = int(round(interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [18.0, 15.0, 12.0, 10.0]))) + min_on_frames = int(round(interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [34.0, 27.0, 20.0, 16.0]))) + min_off_frames = int(round(interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [16.0, 14.0, 12.0, 10.0]))) - # Base paddle press hysteresis - self.regen_paddle_pressed = self.regen_paddle_timer >= 10 # 10 frames - press_regen_paddle = self.regen_paddle_pressed or self.planner_regen_hold + want_press = self.planner_regen_hold or accel <= press_cmd_threshold or self.aego <= press_aego_threshold + want_release = (not self.planner_regen_hold) and accel >= release_cmd_threshold and self.aego >= release_aego_threshold + if want_press: + self.regen_press_counter += 1 + else: + self.regen_press_counter = max(self.regen_press_counter - 1, 0) + + if want_release: + self.regen_release_counter += 1 + else: + self.regen_release_counter = max(self.regen_release_counter - 1, 0) + + # Strong planner request can skip most of debounce delay. + if self.planner_regen_hold and accel <= (press_cmd_threshold - 0.30): + self.regen_press_counter = max(self.regen_press_counter, press_confirm_frames) + + if self.regen_min_on_frames > 0: + self.regen_min_on_frames -= 1 + if self.regen_min_off_frames > 0: + self.regen_min_off_frames -= 1 + + switched_state = False + if self.regen_paddle_pressed: + if self.regen_min_on_frames == 0 and self.regen_release_counter >= release_confirm_frames: + self.regen_paddle_pressed = False + self.regen_min_off_frames = min_off_frames + self.regen_release_counter = 0 + switched_state = True + else: + if self.regen_min_off_frames == 0 and self.regen_press_counter >= press_confirm_frames: + self.regen_paddle_pressed = True + self.regen_min_on_frames = min_on_frames + self.regen_press_counter = 0 + switched_state = True + + self.regen_paddle_timer = self.regen_press_counter + press_regen_paddle = self.regen_paddle_pressed + + if self.maneuver_paddle_mode == "off": + self.regen_paddle_pressed = False + self.regen_press_counter = 0 + self.regen_release_counter = 0 + self.regen_min_on_frames = 0 + self.regen_mode_blend = 0.0 + press_regen_paddle = False + elif self.maneuver_paddle_mode == "force": + forced_press = accel < -0.02 + self.regen_paddle_pressed = forced_press + press_regen_paddle = forced_press # Regen gain ratios from bin-averaged 60–0 deceleration sweep; Calculates stronger decel from paddle speed_mps = [0.559, 1.678, 2.797, 3.916, 5.035, 6.154, 7.273, 8.392, 9.511, 10.63, 11.749, 12.868, 13.987, 15.106, 16.225, 17.344, 18.463, 19.582, 20.701, 21.820, 22.939, 24.058, 25.177, 26.296] - regen_gain_ratio = [1.01, 1.01, 1.02, 1.05, 1.08, 1.345979, 1.369975, - 1.376302, 1.388052, 1.370367, 1.388498, 1.386030, 1.405950, 1.387555, - 1.390392, 1.394946, 1.414915, 1.428535, 1.439611, 1.440106, 1.441438, - 1.439395, 1.446909, 1.445738] + regen_gain_ratio = [1.01, 1.01, 1.02, 1.05, 1.08, 1.31, 1.33, + 1.34, 1.35, 1.36, 1.37, 1.38, 1.39, 1.39, + 1.40, 1.40, 1.41, 1.42, 1.43, 1.43, 1.44, + 1.44, 1.45, 1.45] gain = interp(car_velocity, speed_mps, regen_gain_ratio) - pedaloffset = interp(car_velocity, [0., 3, 6, 30], [0.10, 0.175, 0.240, 0.240]) + accel_gain = interp(car_velocity, [0.0, 3.0, 8.0, 20.0], [0.47, 0.52, 0.57, 0.61]) + pedaloffset = interp(car_velocity, [0.0, 1.0, 3.0, 6.0, 15.0, 30.0], [0.085, 0.11, 0.17, 0.23, 0.235, 0.23]) - # Compute raw pedal gas - raw_pedal_gas = clip((pedaloffset + (accel / gain) * 0.6), 0.0, 1.0) if press_regen_paddle else clip((pedaloffset + accel * 0.6), 0.0, 1.0) + # Blend between non-paddle and paddle lookup behavior to avoid a hard mode switch. + regen_blend_target = 1.0 if press_regen_paddle else 0.0 + regen_blend_up_step = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [0.04, 0.06, 0.09, 0.12]) + regen_blend_down_step = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [0.05, 0.08, 0.11, 0.14]) + if switched_state: + regen_blend_up_step *= 0.35 + regen_blend_down_step *= 0.35 + if regen_blend_target > self.regen_mode_blend: + self.regen_mode_blend = min(self.regen_mode_blend + regen_blend_up_step, regen_blend_target) + else: + self.regen_mode_blend = max(self.regen_mode_blend - regen_blend_down_step, regen_blend_target) - # --- Immediate application of raw pedal gas, no blending --- - pedal_gas = raw_pedal_gas - # Safety cap: ramp from 22% at 0 m/s to 37.25% at 10 mph (4.47 m/s), then allow full throttle - pedal_gas_max = interp(car_velocity, [0.0, 4.47, 4.48], [0.22, 0.3725, 1.0]) - pedal_gas = clip(pedal_gas, 0.0, pedal_gas_max) + if switched_state and press_regen_paddle: + # Snap quickly toward paddle-compensated mapping to avoid transient over-decel at paddle handoff. + press_blend_snap = interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [0.70, 0.78, 0.86, 0.92]) + self.regen_mode_blend = max(self.regen_mode_blend, press_blend_snap) + + accel_term_scale = (1.0 - self.regen_mode_blend) + (self.regen_mode_blend / max(gain, 1e-3)) + # De-sensitize small commands asymmetrically: keep decel smoother while preserving accel pickup. + if accel >= 0.0: + small_cmd_scale = interp(abs(accel), [0.0, 0.35, 0.8, 1.5, 2.5], [0.58, 0.68, 0.82, 0.93, 1.0]) + else: + small_cmd_scale = interp(abs(accel), [0.0, 0.35, 0.8, 1.5, 2.5], [0.44, 0.54, 0.70, 0.89, 1.0]) + accel_cmd = accel * small_cmd_scale + if (not press_regen_paddle) and accel < -2.0: + accel_cmd *= interp(abs(accel), [2.0, 2.5, 3.0], [1.0, 1.03, 1.06]) + raw_pedal_gas = clip(pedaloffset + accel_cmd * accel_gain * accel_term_scale, 0.0, 1.0) + + # Safety cap with continuous low-speed transition (removes the near-step around ~4.5 m/s). + pedal_gas_max = interp(car_velocity, [0.0, 1.0, 2.5, 4.5, 6.0, 8.0, 12.0], [0.20, 0.235, 0.29, 0.365, 0.52, 0.78, 1.0]) + target_pedal_gas = clip(raw_pedal_gas, 0.0, pedal_gas_max) + + # Blend and rate-limit command changes for smoother transients while preserving clip bounds. + if not self.pedal_active_last: + pedal_gas = target_pedal_gas + self.pedal_active_last = True + else: + urgency = clip(abs(accel) / 2.0, 0.0, 1.0) + rate_up = interp(car_velocity, [0.0, 3.0, 8.0, 20.0], [0.007, 0.012, 0.022, 0.036]) + 0.011 * urgency + if accel > 1.2: + # Recover high-command launch response without changing low-command smoothness. + rate_up += interp(car_velocity, [0.0, 4.0, 12.0, 25.0], [0.006, 0.005, 0.003, 0.002]) + rate_down = interp(car_velocity, [0.0, 3.0, 8.0, 20.0], [0.008, 0.014, 0.026, 0.045]) + 0.015 * urgency + if switched_state: + rate_up *= 0.75 + rate_down *= 0.75 + pedal_gas = clip(target_pedal_gas, self.pedal_steady - rate_down, self.pedal_steady + rate_up) + + self.pedal_steady = pedal_gas return pedal_gas, press_regen_paddle @@ -132,6 +236,12 @@ class CarController(CarControllerBase): actuators = CC.actuators accel = brake_accel = actuators.accel press_regen_paddle = False + + # Longitudinal maneuvers can force paddle behavior for A/B testing. + if self.frame % 25 == 0: + mode = self.params_.get("LongitudinalManeuverPaddleMode", encoding="utf-8") + mode = (mode or "auto").strip().lower() + self.maneuver_paddle_mode = mode if mode in ("auto", "off", "force") else "auto" kaofui_cars = SDGM_CAR | ASCM_INT | { CAR.CHEVROLET_VOLT, CAR.CHEVROLET_VOLT_2019, @@ -152,14 +262,18 @@ class CarController(CarControllerBase): # Planner-driven regen hold: gate by car support and OP long active, use commanded accel thresholds if (self.CP.enableGasInterceptor and self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and self.CP.openpilotLongitudinalControl and CC.longActive): - # Match original hysteresis intent: vehicle can usually stop without paddle up to ~1.0 m/s^2 - # Use the same thresholds as the aEgo-based hysteresis, but on commanded accel for preemption - planner_press_threshold = -0.7 - planner_release_threshold = -0.3 - if accel <= planner_press_threshold: - self.planner_regen_hold = True - elif accel >= planner_release_threshold: + if self.maneuver_paddle_mode == "off": self.planner_regen_hold = False + elif self.maneuver_paddle_mode == "force": + self.planner_regen_hold = accel < -0.02 + else: + # Pre-arm paddle on strong commanded decel with speed-aware hysteresis. + planner_press_threshold = interp(CS.out.vEgo, [0.0, 4.0, 12.0, 25.0], [-0.95, -0.82, -0.70, -0.62]) + planner_release_threshold = interp(CS.out.vEgo, [0.0, 4.0, 12.0, 25.0], [-0.14, -0.22, -0.30, -0.36]) + if accel <= planner_press_threshold: + self.planner_regen_hold = True + elif accel >= planner_release_threshold: + self.planner_regen_hold = False else: self.planner_regen_hold = False @@ -184,7 +298,7 @@ class CarController(CarControllerBase): self.CP.openpilotLongitudinalControl and CC.longActive and self.CP.enableGasInterceptor and - (self.regen_paddle_timer >= 10 or self.planner_regen_hold) # hysteresis or planner hint + self.regen_paddle_pressed ) use_panda_paddle_sched = ( self.CP.enableGasInterceptor and diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 2f24fbaeb..6cb8df61f 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -5,6 +5,7 @@ import numpy as np from panda import Panda from openpilot.common.conversions import Conversions as CV +from openpilot.common.numpy_fast import interp from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus, GMFlags, CC_ONLY_CAR, SDGM_CAR, ASCM_INT, CC_REGEN_PADDLE_CAR, set_red_panda_canbus @@ -39,6 +40,13 @@ VOLT_LIKE_CARS = { CAR.CHEVROLET_MALIBU_HYBRID_CC, } +BOLT_PEDAL_LONG_CARS = { + CAR.CHEVROLET_BOLT_CC_2017, + CAR.CHEVROLET_BOLT_CC_2019_2021, + CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL, + CAR.CHEVROLET_BOLT_CC_2022_2023, +} + NON_LINEAR_TORQUE_PARAMS = { CAR.CHEVROLET_BOLT_ACC_2022_2023: { "left": [2.6531724862969748, 1.1, 0.1919764879840985, 0.0], @@ -78,6 +86,18 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): + if CP.enableGasInterceptor and bool(CP.flags & GMFlags.PEDAL_LONG.value): + if CP.carFingerprint in BOLT_PEDAL_LONG_CARS: + accel_min = interp(current_speed, [0.0, 1.5, 4.0, 8.0, 15.0, 30.0], + [-0.85, -1.2, -1.90, -2.50, -2.82, -2.95]) + accel_max = interp(current_speed, [0.0, 1.5, 4.0, 8.0, 15.0], + [0.54, 0.74, 1.03, 1.46, CarControllerParams.ACCEL_MAX]) + else: + accel_min = interp(current_speed, [0.0, 1.5, 4.0, 8.0, 15.0, 30.0], + [-0.95, -1.3, -1.85, -2.3, -2.6, -2.8]) + accel_max = interp(current_speed, [0.0, 1.5, 4.0, 8.0, 15.0], + [0.60, 0.85, 1.15, 1.60, CarControllerParams.ACCEL_MAX]) + return accel_min, accel_max return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX # Determined by iteratively plotting and minimizing error for f(angle, speed) = steer. @@ -483,17 +503,26 @@ class CarInterface(CarInterfaceBase): gm_safety_cfg.safetyParam |= Panda.FLAG_GM_PEDAL_LONG # Note: Low speed, stop and go not tested. Should be fairly smooth on highway if candidate in (CAR.CHEVROLET_MALIBU_CC, CAR.CHEVROLET_MALIBU_HYBRID_CC): + ret.longitudinalTuning.kpBP = [0.0, 5.0, 35.0] + ret.longitudinalTuning.kpV = [0.06, 0.05, 0.04] ret.longitudinalTuning.kiBP = [0.0, 5., 35.] - ret.longitudinalTuning.kiV = [0.0, 0.35, 0.5] + ret.longitudinalTuning.kiV = [0.0, 0.30, 0.45] ret.longitudinalTuning.kfDEPRECATED = 0.15 ret.stoppingDecelRate = 0.8 ret.minEnableSpeed = -1 ret.pcmCruise = False ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long else: - ret.longitudinalTuning.kiBP = [0., 3., 6., 35.] - ret.longitudinalTuning.kiV = [0.125, 0.175, 0.225, 0.33] - ret.longitudinalTuning.kfDEPRECATED = 0.25 + ret.longitudinalTuning.kpBP = [0.0, 5.0, 15.0, 35.0] + ret.longitudinalTuning.kpV = [0.09, 0.08, 0.06, 0.045] + ret.longitudinalTuning.kiBP = [0.0, 3.0, 6.0, 35.0] + ret.longitudinalTuning.kiV = [0.09, 0.13, 0.19, 0.28] + if candidate in BOLT_PEDAL_LONG_CARS: + ret.longitudinalTuning.kpV = [0.095, 0.085, 0.065, 0.050] + ret.longitudinalTuning.kiV = [0.07, 0.10, 0.15, 0.24] + ret.longitudinalTuning.kfDEPRECATED = 0.20 + else: + ret.longitudinalTuning.kfDEPRECATED = 0.25 ret.stoppingDecelRate = 0.8 else: # Pedal used for SNG, ACC for longitudinal control otherwise gm_safety_cfg.safetyParam |= Panda.FLAG_GM_HW_CAM_LONG @@ -504,8 +533,10 @@ class CarInterface(CarInterfaceBase): if ret.enableGasInterceptor and candidate == CAR.CHEVROLET_MALIBU_HYBRID_CC: ret.flags |= GMFlags.PEDAL_LONG.value gm_safety_cfg.safetyParam |= Panda.FLAG_GM_PEDAL_LONG + ret.longitudinalTuning.kpBP = [0.0, 5.0, 35.0] + ret.longitudinalTuning.kpV = [0.06, 0.05, 0.04] ret.longitudinalTuning.kiBP = [0.0, 5., 35.] - ret.longitudinalTuning.kiV = [0.0, 0.18, 0.25] + ret.longitudinalTuning.kiV = [0.0, 0.30, 0.45] ret.longitudinalTuning.kfDEPRECATED = 0.15 ret.stoppingDecelRate = 0.8 ret.minEnableSpeed = -1 diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 68ce9d35d..e8d1d5182 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -5,7 +5,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_dead from openpilot.selfdrive.controls.lib.pid import PIDController from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.selfdrive.car.gm.values import CarControllerParams +from openpilot.selfdrive.car.gm.values import CarControllerParams, GMFlags CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -102,8 +102,9 @@ class LongControl: self.v_pid = 0.0 self._mode_setup() self.last_output_accel = 0.0 - - + self.last_a_target = 0.0 + self.integrator_hold_frames = 0 + self.is_gm_pedal_long = bool(CP.carName == "gm" and CP.enableGasInterceptor and (CP.flags & GMFlags.PEDAL_LONG.value)) def update_mpc_mode(self, experimental_mode): new_mode = 'blended' if experimental_mode else 'acc' @@ -134,6 +135,31 @@ class LongControl: def reset(self): self.pid.reset() + self.last_a_target = 0.0 + self.integrator_hold_frames = 0 + + def _get_pedal_long_freeze(self, a_target, error, v_ego, accel_limits): + if not self.is_gm_pedal_long: + self.last_a_target = a_target + self.integrator_hold_frames = 0 + return False + + handoff_threshold = interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.35, 0.45, 0.55, 0.70]) + if abs(a_target - self.last_a_target) > handoff_threshold: + hold_frames = int(round(interp(v_ego, [0.0, 4.0, 12.0, 25.0], [25.0, 20.0, 14.0, 10.0]))) + self.integrator_hold_frames = max(self.integrator_hold_frames, hold_frames) + self.last_a_target = a_target + + if self.integrator_hold_frames > 0: + self.integrator_hold_frames -= 1 + + sat_buffer = 0.03 + at_neg_sat = self.last_output_accel <= (accel_limits[0] + sat_buffer) + at_pos_sat = self.last_output_accel >= (accel_limits[1] - sat_buffer) + sat_pushing_lower = at_neg_sat and error < -0.05 + sat_pushing_upper = at_pos_sat and error > 0.05 + + return self.integrator_hold_frames > 0 or sat_pushing_lower or sat_pushing_upper def update(self, active, CS, a_target, should_stop, accel_limits, frogpilot_toggles): """Update longitudinal control. This updates the state machine and runs a PID loop""" @@ -162,7 +188,9 @@ class LongControl: error = a_target - CS.aEgo self.update_mpc_mode(self.experimental_mode) feedforward = a_target * self.feedforward_gain - raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=feedforward) + freeze_integrator = self._get_pedal_long_freeze(a_target, error, CS.vEgo, accel_limits) + raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=feedforward, + freeze_integrator=freeze_integrator) if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended': @@ -206,6 +234,8 @@ class LongControl: """Reset PID controller and change setpoint""" self.pid.reset() self.v_pid = v_pid + self.last_a_target = 0.0 + self.integrator_hold_frames = 0 def update_old_long(self, active, CS, long_plan, accel_limits, t_since_plan, frogpilot_toggles): """Update longitudinal control. This updates the state machine and runs a PID loop""" @@ -255,10 +285,9 @@ class LongControl: # TODO too complex, needs to be simplified and tested on toyotas prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_1sec < 0.7 and v_target_1sec < self.v_pid deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP, self.CP.longitudinalTuning.deadzoneV) - freeze_integrator = prevent_overshoot - error = self.v_pid - CS.vEgo error_deadzone = apply_deadzone(error, deadzone) + freeze_integrator = prevent_overshoot or self._get_pedal_long_freeze(a_target, error_deadzone, CS.vEgo, accel_limits) feedforward = a_target * self.feedforward_gain output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, feedforward=feedforward, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index b54420ffb..533fb1d45 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -297,13 +297,10 @@ class LongitudinalMpc: self.dt = dt self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.source = SOURCES[2] - # Initialize smoothing filters with default time constants + # Keep a fixed lead filter time; disable speed/uncertainty follow-smoothing modulation. 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] @@ -362,6 +359,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, panic_bypass=False): + _ = uncertainty, accel_reengage, panic_bypass # compatibility args (follow-smoothing path removed) # Update parameters based on current speed with interpolation for smooth scaling speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph @@ -374,53 +372,12 @@ class LongitudinalMpc: dist_adapt_array = [0.0, DIST_ADAPTS[1], DIST_ADAPTS[2], DIST_ADAPTS[3]] self.current_dist_adapt = get_speed_based_param(speed_mph, dist_adapt_array) - # Update filter time constants with interp and recreate filters if needed - if speed_mph < 47: - self.current_filter_time = 0.0 - else: - self.current_filter_time = interp(speed_mph, [47, 65], [0.0, LEAD_FILTER_TIME_HIGH]) - if abs(self.current_filter_time - getattr(self, 'prev_filter_time', 0)) > 0.1: # Only update if significant change - # Recreate filters with new time constant while preserving current values - 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 - self.lead_a_filter = FirstOrderFilter(current_a, self.current_filter_time, self.dt) - self.lead_v_filter = FirstOrderFilter(current_v, self.current_filter_time, self.dt) - self.prev_filter_time = self.current_filter_time - # Adaptive jerk factors for distance with interp scaling dist_factor = 1.0 + self.current_dist_adapt * (20.0 / max(lead_dist, 5.0)) acceleration_jerk *= dist_factor danger_jerk *= dist_factor speed_jerk *= dist_factor - # Scene complexity adjustment based on model uncertainty - 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 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) - 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 self.mode == 'acc': a_change_cost = acceleration_jerk if prev_accel_constraint else 0 cost_weights = [self.current_x_ego_cost, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, speed_jerk] @@ -433,15 +390,6 @@ class LongitudinalMpc: raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') 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.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 - self.lead_a_filter = FirstOrderFilter(current_a, new_filter_time, self.dt) - self.lead_v_filter = FirstOrderFilter(current_v, new_filter_time, self.dt) - self.prev_filter_time_factor = filter_time_factor - def set_cur_state(self, v, a): v_prev = self.x0[1] self.x0[1] = v diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index e5da54aeb..1d1f46b8e 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -25,9 +25,6 @@ ALLOW_THROTTLE_THRESHOLD = 0.4 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.] @@ -124,8 +121,6 @@ class LongitudinalPlanner: self.lead_dist_f = None # Uncertainty slope tracking - self._uncert_last = 0.0 - self._uncert_last_t = None @property def mlsim(self): @@ -314,25 +309,6 @@ class LongitudinalPlanner: uncertainty = self.uncert_slow.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 - self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].dangerJerk, sm['frogpilotPlan'].speedJerk, @@ -340,8 +316,7 @@ class LongitudinalPlanner: 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, - panic_bypass=panic_bypass) + uncertainty=uncertainty) 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