From 77160afa1a4bb8dad3998cb7883713eddae4d684 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 6 Dec 2025 15:58:03 -0600 Subject: [PATCH] Updates Reverse Fix New Lateral Torque Tuning Advanced Lateral Panel Fix Fix --- frogpilot/ui/qt/offroad/lateral_settings.cc | 7 ++-- selfdrive/car/interfaces.py | 2 +- selfdrive/controls/controlsd.py | 10 ++++++ selfdrive/controls/lib/events.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 37 +++++++++------------ 5 files changed, 31 insertions(+), 27 deletions(-) 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/interfaces.py b/selfdrive/car/interfaces.py index f32ca9ec1..924db4270 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -37,7 +37,7 @@ 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 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/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/events.py b/selfdrive/controls/lib/events.py index 760e2082d..22bf69e9e 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -978,7 +978,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { EventName.reverseGear: { ET.PERMANENT: Alert( - "Wrong\nGear", + "Reverse\nGear", "", AlertStatus.normal, AlertSize.full, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5), diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d2fffedf9..d0398edd0 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.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 @@ -22,14 +22,16 @@ from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_G # to be overcome to move it at all, this is compensated for too. KP = 0.6 -KI = 0.3 -KD = 0.0 +KI = 0.28 + 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] LP_FILTER_CUTOFF_HZ = 1.2 +JERK_LOOKAHEAD_SECONDS = 0.19 +JERK_GAIN = 0.3 LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 -VERSION = 0 +VERSION = 1 class LatControlTorque(LatControl): def __init__(self, CP, CI, dt): @@ -37,13 +39,13 @@ 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.previous_measurement = 0.0 - self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) + 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) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -69,17 +71,15 @@ 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 + lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2)) + raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx+1] - self.lat_accel_request_buffer[lookahead_idx-1]) / (2 * self.dt) + desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk) future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2 self.lat_accel_request_buffer.append(future_desired_lateral_accel) 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 measurement = measured_curvature * CS.vEgo ** 2 - measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt) - self.previous_measurement = measurement - - setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel error = setpoint - measurement # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly @@ -87,15 +87,10 @@ 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 - # 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 + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, 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, 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 +105,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