Advanced lateral tuning

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)

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):

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

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)

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