import math import numpy as np from collections import deque from cereal import log from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.interfaces import FRICTION_THRESHOLD, get_friction_threshold 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 # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to # torque applied to the steering rack. It does not correlate to # wheel slip, or to speed. # This controller applies torque to achieve desired lateral # accelerations. To compensate for the low speed effects the # proportional gain is increased at low speeds by the PID controller. # Additionally, there is friction in the steering wheel that needs # to be overcome to move it at all, this is compensated for too. KP = 0.7 KI = 0.35 INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30] KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP] LOW_SPEED_X = [0, 10, 20, 30] LOW_SPEED_Y = [12, 10.5, 8, 5] MAX_LAT_JERK_UP = 2.5 # m/s^3 LP_FILTER_CUTOFF_HZ = 1.2 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_LEFT_FRICTION_GAIN = 0.08 SILVERADO_LEFT_P_REDUCTION = 0.05 SILVERADO_LEFTWARD_SIGN = 1.0 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_KP_LOW_SPEED_BP = [0.0, 15.0 * CV.MPH_TO_MS, 35.0 * CV.MPH_TO_MS] SILVERADO_KP_LOW_SPEED_V = [0.92, 0.96, 1.0] SILVERADO_KP_HI_SPEED_BP = [35.0 * CV.MPH_TO_MS, 70.0 * CV.MPH_TO_MS] SILVERADO_KP_HI_SPEED_V = [1.00, 1.04] SILVERADO_KI_GAIN = 1.25 SILVERADO_LSF_MULT_MAX = 1.6 SILVERADO_KP_FLOOR = 0.08 SILVERADO_FF_LOW_SPEED_BP = [0.0, 20.0 * CV.MPH_TO_MS, 30.0 * CV.MPH_TO_MS] SILVERADO_FF_LOW_SPEED_V = [1.1, 1.05, 1.0] SILVERADO_I_SETPOINT_DB = 0.30 SILVERADO_I_ERROR_DB = 0.12 SILVERADO_I_DB_LEFT_MULT = 1.15 SILVERADO_I_DECAY = 0.008 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): super().__init__(CP, CI, dt) self.torque_params = CP.lateralTuning.torque self.torque_from_lateral_accel = CI.torque_from_lateral_accel() self.lateral_accel_from_torque = CI.lateral_accel_from_torque() self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1/self.dt) self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt) self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len) self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt) self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) 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 self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction self.update_limits() def update_limits(self): self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params), self.lateral_accel_from_torque(-self.steer_max, self.torque_params)) def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, llk, model_data, frogpilot_toggles): pid_log = log.ControlsState.LateralTorqueState.new_message() pid_log.version = VERSION if not active: output_torque = 0.0 pid_log.active = False self.pid.reset() 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 curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 delay_frames = int(np.clip(lat_delay / self.dt, 1, self.lat_accel_request_buffer_len)) expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames] future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 self.lat_accel_request_buffer.append(future_desired_lateral_accel) raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt) raw_lateral_jerk = np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) 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) measurement_rate = np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP) self.previous_measurement = measurement low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2 error = setpoint - measurement if self.is_silverado: base_kp = np.interp(CS.vEgo, self.base_kp[0], self.base_kp[1]) # Cap low-speed gain amplification to prevent oscillations at very low kp. effective_kp = max(base_kp, SILVERADO_KP_FLOOR) lsf_mult = 1.0 + low_speed_factor / effective_kp lsf_mult = min(lsf_mult, SILVERADO_LSF_MULT_MAX) error_with_lsf = error * lsf_mult leftward = (SILVERADO_LEFTWARD_SIGN * error_with_lsf) > 0.0 # Silverado-only split tuning to reduce left bias, wander, and low-lat oscillations. base_kp_mult = SILVERADO_KP_POS_MULT if leftward else SILVERADO_KP_NEG_MULT low_speed_kp_mult = np.interp(CS.vEgo, SILVERADO_KP_LOW_SPEED_BP, SILVERADO_KP_LOW_SPEED_V) hi_speed_kp_mult = np.interp(CS.vEgo, SILVERADO_KP_HI_SPEED_BP, SILVERADO_KP_HI_SPEED_V) kp_mult = base_kp_mult * low_speed_kp_mult * ((1.0 - SILVERADO_LEFT_P_REDUCTION) if leftward else 1.0) kp_mult *= hi_speed_kp_mult ki_mult = (SILVERADO_KI_POS_MULT if leftward else SILVERADO_KI_NEG_MULT) * SILVERADO_KI_GAIN 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]]] else: current_kp = np.interp(CS.vEgo, self.pid._k_p[0], self.pid._k_p[1]) error_with_lsf = error * (1 + low_speed_factor / max(current_kp, 1e-3)) leftward = False # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly pid_log.error = float(error_with_lsf) 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_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 if self.is_silverado: # Mild low-speed feedforward boost to reduce P+I reliance. ff *= np.interp(CS.vEgo, SILVERADO_FF_LOW_SPEED_BP, SILVERADO_FF_LOW_SPEED_V) 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 and leftward: friction *= (1.0 + SILVERADO_LEFT_FRICTION_GAIN) 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 or unwind_detected) if self.is_silverado: setpoint_db = SILVERADO_I_SETPOINT_DB error_db = SILVERADO_I_ERROR_DB if leftward: setpoint_db *= SILVERADO_I_DB_LEFT_MULT error_db *= SILVERADO_I_DB_LEFT_MULT near_center = abs(setpoint) < setpoint_db and abs(error_with_lsf) < error_db if near_center: self.pid.i *= (1.0 - SILVERADO_I_DECAY) 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) pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) pid_log.d = float(self.pid.d) pid_log.f = float(self.pid.f) pid_log.output = float(-output_torque) # TODO: log lat accel? pid_log.actualLateralAccel = float(measurement) pid_log.desiredLateralAccel = float(setpoint) 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