mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-03 22:53:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

TorqueController

Browse Source
This commit is contained in:
firestar5683
2026-01-13 22:28:41 -06:00
parent 0f58f6a199
commit d42cbb8f76
4 changed files with 116 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
frogpilot/controls/lib/frogpilot_acceleration.py
+1 -1
View File
@@ -50,7 +50,7 @@ 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.0, 1.0, 1.0]
A_CRUISE_MAX_VALS_SPORT_EV = [1.25, 1.25, 1.25, 1.25, 1.5, 1.5, 2.0]
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 = [4.5, 1.13, 0.65, 0.60, 0.5, 0.42, 0.35]
A_CRUISE_MAX_VALS_SPORT_GAS = [5.5, 1.2, 0.65, 0.60, 0.55, 0.45, 0.40]
def get_max_accel_eco(v_ego, ev_tuning=True):
cruise_vals = A_CRUISE_MAX_VALS_ECO_EV if ev_tuning else A_CRUISE_MAX_VALS_ECO_GAS
selfdrive/car/gm/interface.py
+43 -13
View File
@@ -27,10 +27,22 @@ CAM_MSG = 0x320 # AEBCmd
ACCELERATOR_POS_MSG = 0xbe
NON_LINEAR_TORQUE_PARAMS = {
CAR.CHEVROLET_BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
CAR.CHEVROLET_BOLT_CC: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
CAR.GMC_ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772],
CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122]
CAR.CHEVROLET_BOLT_EUV: {
"left": [2.6531724862969748, 1.1, 0.1919764879840985, 0.0],
"right": [2.7031724862969748, 1.0, 0.1469764879840985, 0.0],
},
CAR.CHEVROLET_BOLT_CC: {
"left": [2.6531724862969748, 1.1, 0.1919764879840985, 0.0],
"right": [2.7031724862969748, 1.0, 0.1469764879840985, 0.0],
},
CAR.GMC_ACADIA: {
"left": [4.78003305, 1.0, 0.3122, 0.05591772],
"right": [4.78003305, 1.0, 0.3122, 0.05591772],
},
CAR.CHEVROLET_SILVERADO: {
"left": [3.29974374, 1.1, 0.25571356, 0.0],
"right": [3.34974374, 1.0, 0.21071356, 0.0],
},
}
@@ -60,7 +72,9 @@ class CarInterface(CarInterfaceBase):
# This has big effect on the stability about 0 (noise when going straight)
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
assert non_linear_torque_params, "The params are not defined"
a, b, c, d = non_linear_torque_params
# Left is positive
side_key = "left" if lateral_acceleration >= 0 else "right"
a, b, c, d = non_linear_torque_params[side_key]
sig_input = a * lateral_acceleration
sig = np.sign(sig_input) * (1 / (1 + exp(-fabs(sig_input))) - 0.5)
steer_torque = (sig * b) + (lateral_acceleration * c) + d
@@ -116,11 +130,12 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalTuning.kiBP = [5., 35., 60.]
if candidate in CAMERA_ACC_CAR:
ret.experimentalLongitudinalAvailable = candidate not in CC_ONLY_CAR
# For ACC models with pedal interceptor, behave like CC_ONLY_CAR
ret.experimentalLongitudinalAvailable = (candidate not in CC_ONLY_CAR) and not ret.enableGasInterceptor
ret.networkLocation = NetworkLocation.fwdCamera
ret.radarUnavailable = True # no radar
# Use pcmCruise by default; this may be overridden below if a pedal interceptor is detected
ret.pcmCruise = True
# Only use pcmCruise if no pedal interceptor (bolt_cc style behavior)
ret.pcmCruise = not ret.enableGasInterceptor
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM
# Use default minEnableSpeed for ACC models (will be overridden by pedal interceptor section if present)
ret.minEnableSpeed = 5 * CV.KPH_TO_MS
@@ -215,17 +230,32 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if ret.enableGasInterceptor:
# ACC Bolts use pedal for full longitudinal control, not just sng
ret.flags |= GMFlags.PEDAL_LONG.value
# Bolt-only lateral tuning overrides
ret.lateralTuning.torque.kp = 1.03
ret.lateralTuning.torque.ki = 1.07
ret.lateralTuning.torque.kd = 0.93
ret.lateralTuning.torque.kfDEPRECATED = 0.02
if candidate == CAR.CHEVROLET_SILVERADO:
# Enable pedal interceptor for ACC models when detected
if candidate in CAMERA_ACC_CAR and ret.enableGasInterceptor:
# ACC models with pedal interceptor get full pedal longitudinal control
ret.flags |= GMFlags.PEDAL_LONG.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC
elif candidate == CAR.CHEVROLET_SILVERADO:
# On the Bolt, the ECM and camera independently check that you are either above 5 kph or at a stop
# with foot on brake to allow engagement, but this platform only has that check in the camera.
# TODO: check if this is split by EV/ICE with more platforms in the future
if ret.openpilotLongitudinalControl:
ret.minEnableSpeed = -1.
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
# Silverado torque lateral tuning overrides (stored in unused torque fields).
ret.lateralTuning.torque.kp = 1.03 # torque_ff_scale_pos
ret.lateralTuning.torque.ki = 1.07 # torque_ff_scale_neg
ret.lateralTuning.torque.kd = 0.93 # torque_ki_mult
ret.lateralTuning.torque.kfDEPRECATED = 0.02 # torque_deadzone_boost_neg (lat accel)
# Silverado-only: reduce base friction for left bias
ret.lateralTuning.torque.friction *= 0.93
elif candidate in (CAR.CHEVROLET_EQUINOX, CAR.CHEVROLET_EQUINOX_CC):
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
@@ -279,7 +309,7 @@ class CarInterface(CarInterfaceBase):
ret.stoppingControl = True
ret.autoResumeSng = True
if candidate in CC_ONLY_CAR: #pedal interceptor tuning
if candidate in CC_ONLY_CAR or (candidate in CAMERA_ACC_CAR and ret.enableGasInterceptor): #pedal interceptor tuning
ret.flags |= GMFlags.PEDAL_LONG.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_PEDAL_LONG
# Note: Low speed, stop and go not tested. Should be fairly smooth on highway
selfdrive/car/torque_data/override.toml
+1 -1
View File
@@ -43,7 +43,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
"CADILLAC_ESCALADE" = [1.899999976158142, 1.842270016670227, 0.1120000034570694]
"CADILLAC_ESCALADE_ESV_2019" = [1.15, 1.3, 0.2]
"CADILLAC_XT4" = [1.45, 1.6, 0.2]
"CHEVROLET_BOLT_EUV" = [2.0, 2.0, 0.05]
"CHEVROLET_BOLT_EUV" = [2.0, 2.0, 0.09]
"CHEVROLET_MALIBU_CC" = [1.85, 1.85, 0.075]
"CHEVROLET_SILVERADO" = [1.9, 1.9, 0.112]
"CHEVROLET_TRAILBLAZER" = [1.33, 1.9, 0.16]
selfdrive/controls/lib/latcontrol_torque.py
+71 -2
View File
@@ -7,6 +7,7 @@ from openpilot.selfdrive.car.interfaces import FRICTION_THRESHOLD, get_friction_
from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED, get_friction
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
from openpilot.selfdrive.car.gm.values import CAR as GM_CAR
from openpilot.selfdrive.controls.lib.pid import PIDController
from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
@@ -36,6 +37,21 @@ JERK_LOOKAHEAD_SECONDS = 0.19
JERK_GAIN = 0.22
LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
VERSION = 2
DEBUG_TORQUE_TUNE = False
FF_SCALE_BLEND_LAT_ACCEL = 0.05
DEADZONE_BOOST_LAT_ACCEL = 0.08
UNWIND_D_DES_THRESHOLD = -1.0
UNWIND_LAT_ACCEL_NEAR_ZERO = 0.3
SILVERADO_FRICTION_POS_MULT = 1.03
SILVERADO_FRICTION_NEG_MULT = 0.95
SILVERADO_KP_POS_MULT = 0.95
SILVERADO_KP_NEG_MULT = 1.07
SILVERADO_KI_POS_MULT = 0.70
SILVERADO_KI_NEG_MULT = 0.50
SILVERADO_I_LATACCEL_MIN = 0.20
SILVERADO_I_ERR_MIN = 0.08
BOLT_CARS = (GM_CAR.CHEVROLET_BOLT_EUV, GM_CAR.CHEVROLET_BOLT_CC, GM_CAR.CHEVROLET_SILVERADO)
class LatControlTorque(LatControl):
def __init__(self, CP, CI, dt):
@@ -53,6 +69,24 @@ class LatControlTorque(LatControl):
self.previous_measurement = 0.0
self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt)
self.low_speed_reset_threshold = max(CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED)
self.debug_counter = 0
self.prev_desired_lateral_accel = 0.0
self.is_bolt = CP.carFingerprint in BOLT_CARS
self.is_silverado = CP.carFingerprint == GM_CAR.CHEVROLET_SILVERADO
self.torque_ff_scale_pos = 1.0
self.torque_ff_scale_neg = 1.0
self.torque_deadzone_boost_neg = 0.0
self.torque_ki_mult = 1.0
if self.is_bolt:
self.torque_ff_scale_pos = float(self.torque_params.kp)
self.torque_ff_scale_neg = float(self.torque_params.ki)
self.torque_ki_mult = float(self.torque_params.kd)
self.torque_deadzone_boost_neg = float(getattr(self.torque_params, "kfDEPRECATED", 0.0))
if self.torque_ki_mult > 0.0 and self.torque_ki_mult != 1.0:
self.pid._k_i = [self.pid._k_i[0], [k * self.torque_ki_mult for k in self.pid._k_i[1]]]
self.base_kp = [self.pid._k_p[0][:], self.pid._k_p[1][:]]
self.base_ki = [self.pid._k_i[0][:], self.pid._k_i[1][:]]
def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
self.torque_params.latAccelFactor = latAccelFactor
@@ -74,6 +108,7 @@ class LatControlTorque(LatControl):
self.previous_measurement = 0.0
self.measurement_rate_filter.x = 0.0
self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
self.prev_desired_lateral_accel = 0.0
else:
measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
@@ -89,6 +124,10 @@ class LatControlTorque(LatControl):
desired_lateral_jerk = np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay
desired_lateral_accel_rate = (setpoint - self.prev_desired_lateral_accel) / self.dt
unwind_detected = (desired_lateral_accel_rate < UNWIND_D_DES_THRESHOLD and
abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO)
self.prev_desired_lateral_accel = setpoint
measurement = measured_curvature * CS.vEgo ** 2
measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
@@ -96,6 +135,12 @@ class LatControlTorque(LatControl):
self.previous_measurement = measurement
low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2
if self.is_silverado:
# Silverado-only split tuning to reduce left bias, wander, and low-lat oscillations.
kp_mult = SILVERADO_KP_POS_MULT if setpoint >= 0.0 else SILVERADO_KP_NEG_MULT
ki_mult = SILVERADO_KI_POS_MULT if setpoint >= 0.0 else SILVERADO_KI_NEG_MULT
self.pid._k_p = [self.base_kp[0], [k * kp_mult for k in self.base_kp[1]]]
self.pid._k_i = [self.base_ki[0], [k * ki_mult for k in self.base_ki[1]]]
current_kp = np.interp(CS.vEgo, self.pid._k_p[0], self.pid._k_p[1])
error = setpoint - measurement
error_with_lsf = error * (1 + low_speed_factor / max(current_kp, 1e-3))
@@ -105,11 +150,29 @@ class LatControlTorque(LatControl):
ff = gravity_adjusted_future_lateral_accel
# latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
ff -= self.torque_params.latAccelOffset
ff += get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, get_friction_threshold(CS.vEgo), self.torque_params)
ff_scale = 1.0
if self.is_bolt:
ff_scale = np.interp(ff, [-FF_SCALE_BLEND_LAT_ACCEL, 0.0, FF_SCALE_BLEND_LAT_ACCEL],
[self.torque_ff_scale_neg, 1.0, self.torque_ff_scale_pos])
ff *= ff_scale
friction = get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone,
get_friction_threshold(CS.vEgo), self.torque_params)
if self.is_silverado:
friction *= SILVERADO_FRICTION_POS_MULT if setpoint >= 0.0 else SILVERADO_FRICTION_NEG_MULT
ff += friction
deadzone_boost_active = False
if self.is_bolt and self.torque_deadzone_boost_neg > 0.0 and gravity_adjusted_future_lateral_accel < 0.0:
if abs(gravity_adjusted_future_lateral_accel) < DEADZONE_BOOST_LAT_ACCEL:
boost_scale = np.interp(abs(gravity_adjusted_future_lateral_accel), [0.0, DEADZONE_BOOST_LAT_ACCEL], [1.0, 0.0])
ff -= self.torque_deadzone_boost_neg * boost_scale
deadzone_boost_active = True
if CS.vEgo < self.low_speed_reset_threshold:
self.pid.reset()
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < self.low_speed_reset_threshold
freeze_integrator = (steer_limited_by_safety or CS.steeringPressed or
CS.vEgo < self.low_speed_reset_threshold or unwind_detected)
if self.is_silverado and abs(setpoint) < SILVERADO_I_LATACCEL_MIN and abs(error) < SILVERADO_I_ERR_MIN:
freeze_integrator = True
output_lataccel = self.pid.update(pid_log.error, error_rate=-measurement_rate, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
@@ -124,5 +187,11 @@ class LatControlTorque(LatControl):
pid_log.desiredLateralJerk = float(desired_lateral_jerk)
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
if DEBUG_TORQUE_TUNE and self.is_bolt:
self.debug_counter += 1
if self.debug_counter % 50 == 0:
print(f"bolt_torque ff_scale={ff_scale:.3f} pos={self.torque_ff_scale_pos:.3f} "
f"neg={self.torque_ff_scale_neg:.3f} deadzone_boost_active={deadzone_boost_active}")
# TODO left is positive in this convention
return -output_torque, 0.0, pid_log