diff --git a/frogpilot/ui/qt/offroad/lateral_settings.cc b/frogpilot/ui/qt/offroad/lateral_settings.cc index d99244d54..3db5dc80e 100644 --- a/frogpilot/ui/qt/offroad/lateral_settings.cc +++ b/frogpilot/ui/qt/offroad/lateral_settings.cc @@ -340,7 +340,6 @@ void FrogPilotLateralPanel::updateToggles() { } } - bool forcingAutoTune = !parent->hasAutoTune && params.getBool("ForceAutoTune"); bool forcingAutoTuneOff = parent->hasAutoTune && params.getBool("ForceAutoTuneOff"); bool forcingTorqueController = !parent->isAngleCar && params.getBool("ForceTorqueController"); bool usingNNFF = parent->hasNNFFLog && params.getBool("LateralTune") && params.getBool("NNFF"); @@ -401,7 +400,7 @@ void FrogPilotLateralPanel::updateToggles() { else if (key == "SteerFriction") { setVisible &= parent->friction != 0; - setVisible &= parent->hasAutoTune ? forcingAutoTuneOff : !forcingAutoTune; + setVisible &= parent->hasAutoTune ? forcingAutoTuneOff : true; setVisible &= parent->isTorqueCar || forcingTorqueController; setVisible &= !usingNNFF; } @@ -413,14 +412,14 @@ void FrogPilotLateralPanel::updateToggles() { else if (key == "SteerLatAccel") { setVisible &= parent->latAccelFactor != 0; - setVisible &= parent->hasAutoTune ? forcingAutoTuneOff : !forcingAutoTune; + setVisible &= parent->hasAutoTune ? forcingAutoTuneOff : true; setVisible &= parent->isTorqueCar || forcingTorqueController; setVisible &= !usingNNFF; } else if (key == "SteerRatio") { setVisible &= parent->steerRatio != 0; - setVisible &= parent->hasAutoTune ? forcingAutoTuneOff : !forcingAutoTune; + setVisible &= parent->hasAutoTune ? forcingAutoTuneOff : true; } toggle->setVisible(setVisible); diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 3556d905b..ed505e270 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -8,7 +8,7 @@ from openpilot.common.conversions import Conversions as CV 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 -from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LateralAccelFromTorqueCallbackType +from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LateralAccelFromTorqueCallbackType, get_friction_threshold from openpilot.selfdrive.controls.lib.drive_helpers import get_friction @@ -28,6 +28,18 @@ CAM_MSG = 0x320 # AEBCmd # TODO: Is this always linked to camera presence? ACCELERATOR_POS_MSG = 0xbe +VOLT_LIKE_CARS = ( + CAR.CHEVROLET_VOLT, + CAR.CHEVROLET_VOLT_2019, + CAR.CHEVROLET_VOLT_CC, + CAR.CHEVROLET_VOLT_CAMERA, + CAR.CHEVROLET_VOLT_ASCM, + CAR.CHEVROLET_MALIBU, + CAR.CHEVROLET_MALIBU_SDGM, + CAR.CHEVROLET_MALIBU_CC, + CAR.CHEVROLET_MALIBU_HYBRID_CC, +) + 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], @@ -50,7 +62,7 @@ class CarInterface(CarInterfaceBase): return 0.10006696 * sigmoid * (v_ego + 3.12485927) def get_steer_feedforward_function(self): - if self.CP.carFingerprint in (CAR.CHEVROLET_VOLT, CAR.CHEVROLET_VOLT_2019, CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_VOLT_CAMERA, CAR.CHEVROLET_VOLT_ASCM): + if self.CP.carFingerprint in VOLT_LIKE_CARS: return self.get_steer_feedforward_volt else: return CarInterfaceBase.get_steer_feedforward_default @@ -182,6 +194,11 @@ class CarInterface(CarInterfaceBase): if candidate in (CAR.CHEVROLET_VOLT, CAR.CHEVROLET_VOLT_CC, CAR.CHEVROLET_VOLT_CAMERA): ret.minEnableSpeed = -1 + + if candidate == CAR.CHEVROLET_VOLT_2019 and not ret.openpilotLongitudinalControl: + ret.minEnableSpeed = -1 + + if candidate in VOLT_LIKE_CARS: ret.lateralTuning.pid.kpBP = [0., 40.] ret.lateralTuning.pid.kpV = [0., 0.17] ret.lateralTuning.pid.kiBP = [0.] @@ -264,7 +281,7 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - elif candidate in (CAR.CHEVROLET_TRAVERSE, CAR.CHEVROLET_BLAZER, CAR.CHEVROLET_MALIBU_SDGM): + elif candidate in (CAR.CHEVROLET_TRAVERSE, CAR.CHEVROLET_BLAZER): ret.steerActuatorDelay = 0.2 if not ret.openpilotLongitudinalControl: ret.minEnableSpeed = -1. # engage speed is decided by pcm @@ -282,19 +299,9 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.CADILLAC_CT6_CC: CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - elif candidate in (CAR.CHEVROLET_MALIBU_CC, CAR.CHEVROLET_MALIBU_HYBRID_CC): - ret.steerActuatorDelay = 0.2 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - elif candidate == CAR.CHEVROLET_TRAX: CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - elif candidate == CAR.CHEVROLET_VOLT_2019: - ret.steerActuatorDelay = 0.2 - if not ret.openpilotLongitudinalControl: - ret.minEnableSpeed = -1. # engage speed is decided by pcm - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - if ret.enableGasInterceptor and frogpilot_toggles.gm_pedal_longitudinal: ret.networkLocation = NetworkLocation.fwdCamera ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index f32ca9ec1..c0100c538 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -37,7 +37,12 @@ EventName = car.CarEvent.EventName MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS ACCEL_MAX = 2.0 ACCEL_MIN = -3.5 -FRICTION_THRESHOLD = 0.3 +FRICTION_THRESHOLD = 0.09 + +def get_friction_threshold(v_ego): + # Interpolate friction threshold from 0.09 at 50 mph to 0.15 at 75 mph + from openpilot.common.numpy_fast import interp + return interp(v_ego, [50 * CV.MPH_TO_MS, 75 * CV.MPH_TO_MS], [0.09, 0.15]) TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/params.toml') TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override.toml') diff --git a/selfdrive/car/torque_data/override.toml b/selfdrive/car/torque_data/override.toml index 975b9c9ac..1b79d243b 100644 --- a/selfdrive/car/torque_data/override.toml +++ b/selfdrive/car/torque_data/override.toml @@ -45,8 +45,6 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "CADILLAC_XT4" = [1.45, 1.6, 0.2] "CADILLAC_XT6" = [1.33, 1.9, 0.16] "CHEVROLET_BOLT_EUV" = [1.0, 2.0, 0.175] -"CHEVROLET_MALIBU_CC" = [1.85, 1.85, 0.075] -"CHEVROLET_MALIBU_HYBRID_CC" = [1.85, 1.85, 0.075] "CHEVROLET_SILVERADO" = [1.9, 1.9, 0.112] "CHEVROLET_BLAZER" = [1.33, 1.33, 0.18] "CHEVROLET_TRAILBLAZER" = [1.33, 1.9, 0.16] diff --git a/selfdrive/car/torque_data/substitute.toml b/selfdrive/car/torque_data/substitute.toml index 2577d1611..ca1370968 100644 --- a/selfdrive/car/torque_data/substitute.toml +++ b/selfdrive/car/torque_data/substitute.toml @@ -57,6 +57,8 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "CADILLAC_ATS" = "CHEVROLET_VOLT" "CHEVROLET_MALIBU" = "CHEVROLET_VOLT" "CHEVROLET_MALIBU_SDGM" = "CHEVROLET_VOLT" +"CHEVROLET_MALIBU_CC" = "CHEVROLET_VOLT" +"CHEVROLET_MALIBU_HYBRID_CC" = "CHEVROLET_VOLT" "HOLDEN_ASTRA" = "CHEVROLET_VOLT" "CHEVROLET_VOLT_CC" = "CHEVROLET_VOLT" "CHEVROLET_VOLT_CAMERA" = "CHEVROLET_VOLT" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a812f780c..6200c7718 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -626,6 +626,16 @@ class Controls: if self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or self.frogpilot_toggles.force_auto_tune): self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, torque_params.frictionCoefficientFiltered) + elif self.frogpilot_toggles.use_custom_friction or self.frogpilot_toggles.use_custom_latAccelFactor: + lat_accel_factor = self.frogpilot_toggles.latAccelFactor if self.frogpilot_toggles.use_custom_latAccelFactor else self.CP.lateralTuning.torque.latAccelFactor + friction = self.frogpilot_toggles.friction if self.frogpilot_toggles.use_custom_friction else self.CP.lateralTuning.torque.friction + self.LaC.update_live_torque_params(lat_accel_factor, self.CP.lateralTuning.torque.latAccelOffset, friction) + else: + # Use manual parameter values from settings panel + lat_accel_factor = self.params.get_float("SteerLatAccel") + friction = self.params.get_float("SteerFriction") + steer_ratio = self.params.get_float("SteerRatio") + self.LaC.update_live_torque_params(lat_accel_factor, self.CP.lateralTuning.torque.latAccelOffset, friction) if self.sm.updated['liveDelay'] and hasattr(self.LaC, "update_live_delay"): self.LaC.update_live_delay(self.sm['liveDelay'].lateralDelay) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d2fffedf9..e309ef4b2 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -3,9 +3,9 @@ import numpy as np from collections import deque from cereal import log -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.selfdrive.car.interfaces import FRICTION_THRESHOLD +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 from openpilot.selfdrive.controls.lib.pid import PIDController from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY @@ -23,13 +23,19 @@ from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_G KP = 0.6 KI = 0.3 -KD = 0.0 + 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 = 0 +VERSION = 2 class LatControlTorque(LatControl): def __init__(self, CP, CI, dt): @@ -37,13 +43,15 @@ class LatControlTorque(LatControl): 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, KD, rate=1/self.dt) + 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 * LP_FILTER_CUTOFF_HZ), self.dt) + self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -61,6 +69,10 @@ class LatControlTorque(LatControl): 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) else: measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY @@ -69,33 +81,33 @@ class LatControlTorque(LatControl): 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] - # TODO factor out lateral jerk from error to later replace it with delay independent alternative 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 - desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay + setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay 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 - setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel + low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2 + 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)) # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly - pid_log.error = float(error) + 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 - # TODO jerk is weighted by lat_delay for legacy reasons, but should be made independent of it - ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + ff += get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, get_friction_threshold(CS.vEgo), self.torque_params) freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 - output_lataccel = self.pid.update(pid_log.error, - -measurement_rate, - feedforward=ff, - speed=CS.vEgo, - freeze_integrator=freeze_integrator) + 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 @@ -110,4 +122,4 @@ class LatControlTorque(LatControl): pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) # TODO left is positive in this convention - return -output_torque, 0.0, pid_log + return -output_torque, 0.0, pid_log \ No newline at end of file