mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-13 05:12:11 +08:00
Refine GM pedal/paddle tuning and EV accel profile behavior
This commit is contained in:
firestar5683
@@ -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]
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user