From 031e3fa6090a86db3a490293ada668bebc35f353 Mon Sep 17 00:00:00 2001 From: James <91348155+FrogAi@users.noreply.github.com> Date: Mon, 1 Dec 2025 12:00:00 -0700 Subject: [PATCH] Advanced lateral tuning --- frogpilot/common/frogpilot_variables.py | 18 ++++++++++++++++++ selfdrive/controls/controlsd.py | 3 +++ selfdrive/locationd/lagd.py | 4 +++- selfdrive/locationd/paramsd.py | 2 +- selfdrive/locationd/torqued.py | 4 ++-- 5 files changed, 27 insertions(+), 4 deletions(-) diff --git a/frogpilot/common/frogpilot_variables.py b/frogpilot/common/frogpilot_variables.py index ddcd95f60..cd91db878 100644 --- a/frogpilot/common/frogpilot_variables.py +++ b/frogpilot/common/frogpilot_variables.py @@ -21,6 +21,7 @@ from opendbc.car.toyota.values import ToyotaFrogPilotFlags from openpilot.common.basedir import BASEDIR from openpilot.common.constants import CV from openpilot.common.params import Params +from openpilot.selfdrive.controls.lib.latcontrol_torque import KP from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.system.hardware import HARDWARE from openpilot.system.hardware.power_monitoring import VBATT_PAUSE_CHARGING @@ -197,14 +198,20 @@ class FrogPilotVariables: toggle.car_make = CP.brand toggle.car_model = CP.carFingerprint + friction = CP.lateralTuning.torque.friction has_bsm = CP.enableBsm toggle.has_pedal = CP.enableGasInterceptorDEPRECATED has_radar = not CP.radarUnavailable toggle.has_sdsu = toggle.car_make == "toyota" and bool(FPCP.flags & ToyotaFrogPilotFlags.SMART_DSU.value) has_sng = CP.autoResumeSng toggle.has_zss = toggle.car_make == "toyota" and bool(FPCP.flags & ToyotaFrogPilotFlags.ZSS.value) + is_angle_car = CP.steerControlType == car.CarParams.SteerControlType.angle + latAccelFactor = CP.lateralTuning.torque.latAccelFactor toggle.openpilot_longitudinal = CP.openpilotLongitudinalControl and not toggle.disable_openpilot_long pcm_cruise = CP.pcmCruise + steerActuatorDelay = CP.steerActuatorDelay + steerKp = KP + steerRatio = CP.steerRatio msg_bytes = self.params.get("LiveTorqueParameters") if msg_bytes: @@ -224,6 +231,17 @@ class FrogPilotVariables: toggle.hide_speed_limit = self.get_value("HideSpeedLimit", condition=advanced_custom_ui) toggle.use_wheel_speed = self.get_value("WheelSpeed", condition=advanced_custom_ui) + advanced_lateral_tuning = self.get_value("AdvancedLateralTune") + toggle.steerActuatorDelay = self.get_value("SteerDelay", cast=float, condition=advanced_lateral_tuning, default=steerActuatorDelay, min=0.01, max=1.0) + toggle.use_custom_steerActuatorDelay = bool(round(toggle.steerActuatorDelay, 2) != round(steerActuatorDelay, 2)) + toggle.friction = self.get_value("SteerFriction", cast=float, condition=advanced_lateral_tuning, default=friction, min=0, max=1) + toggle.use_custom_friction = bool(round(toggle.friction, 2) != round(friction, 2)) and is_torque_car + toggle.steerKp = [[0], [self.get_value("SteerKP", cast=float, condition=advanced_lateral_tuning and is_torque_car and not is_angle_car, default=steerKp, min=steerKp * 0.5, max=steerKp * 1.5)]] + toggle.latAccelFactor = self.get_value("SteerLatAccel", cast=float, condition=advanced_lateral_tuning, default=latAccelFactor, min=latAccelFactor * 0.5, max=latAccelFactor * 1.5) + toggle.use_custom_latAccelFactor = bool(round(toggle.latAccelFactor, 2) != round(latAccelFactor, 2)) and is_torque_car + toggle.steerRatio = self.get_value("SteerRatio", cast=float, condition=advanced_lateral_tuning, default=steerRatio, min=steerRatio * 0.5, max=steerRatio * 1.5) + toggle.use_custom_steerRatio = bool(round(toggle.steerRatio, 2) != round(steerRatio, 2)) + toggle.alert_volume_controller = self.get_value("AlertVolumeControl") toggle.disengage_volume = self.get_value("DisengageVolume", cast=float, condition=toggle.alert_volume_controller) toggle.engage_volume = self.get_value("EngageVolume", cast=float, condition=toggle.alert_volume_controller) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f5edf68ad..9e141a709 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -75,6 +75,9 @@ class Controls: self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose) # FrogPilot variables + if hasattr(self.LaC, "pid") and self.CP.lateralTuning.which() != "pid": + self.LaC.pid._k_p = self.frogpilot_toggles.steerKp + self.frogpilot_toggles = get_frogpilot_toggles(self.sm) def state_control(self): diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 9e7b559ab..f4b4572f8 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -216,7 +216,9 @@ class LateralLagEstimator: else: liveDelay.status = log.LiveDelayData.Status.unestimated - if liveDelay.status == log.LiveDelayData.Status.estimated: + if self.frogpilot_toggles.use_custom_steerActuatorDelay: + liveDelay.lateralDelay = self.frogpilot_toggles.steerActuatorDelay + elif liveDelay.status == log.LiveDelayData.Status.estimated: liveDelay.lateralDelay = valid_mean_lag else: liveDelay.lateralDelay = self.initial_lag diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 921727172..dbdb9d774 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -166,7 +166,7 @@ class VehicleParamsLearner: liveParameters = msg.liveParameters liveParameters.posenetValid = True liveParameters.sensorValid = sensors_valid - liveParameters.steerRatio = float(x[States.STEER_RATIO].item()) + liveParameters.steerRatio = float(x[States.STEER_RATIO].item() if not self.frogpilot_toggles.use_custom_steerRatio else self.frogpilot_toggles.steerRatio) liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item()) liveParameters.roll = float(self.roll) liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 86f391c25..22b101377 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -232,9 +232,9 @@ class TorqueEstimator(ParameterEstimator): if with_points: liveTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist() - liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x) + liveTorqueParameters.latAccelFactorFiltered = float(self.filtered_params['latAccelFactor'].x if not self.frogpilot_toggles.use_custom_latAccelFactor else self.frogpilot_toggles.latAccelFactor) liveTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x) - liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x) + liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x if not self.frogpilot_toggles.use_custom_friction else self.frogpilot_toggles.friction) liveTorqueParameters.totalBucketPoints = len(self.filtered_points) liveTorqueParameters.calPerc = self.filtered_points.get_valid_percent() liveTorqueParameters.decay = self.decay