diff --git a/frogpilot/controls/lib/frogpilot_acceleration.py b/frogpilot/controls/lib/frogpilot_acceleration.py index b575a9d04..4acd350ae 100644 --- a/frogpilot/controls/lib/frogpilot_acceleration.py +++ b/frogpilot/controls/lib/frogpilot_acceleration.py @@ -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 diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 10adbe44f..643128f9a 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -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 diff --git a/selfdrive/car/torque_data/override.toml b/selfdrive/car/torque_data/override.toml index b37817b4c..3a1fa3523 100644 --- a/selfdrive/car/torque_data/override.toml +++ b/selfdrive/car/torque_data/override.toml @@ -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] diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index ee918b166..70486f660 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -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