From d708a134bdff9db5cc65491dd9129a709e30ad34 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 27 May 2022 10:21:02 -0700 Subject: [PATCH] Tssp prius torque control (#24669) * use llk * use steering sensor at low speed stil * Try more simple * rm prius tune * updated ref --- selfdrive/car/toyota/interface.py | 3 +-- selfdrive/car/toyota/tunes.py | 12 +----------- selfdrive/controls/lib/latcontrol_torque.py | 4 +++- selfdrive/test/process_replay/ref_commit | 2 +- 4 files changed, 6 insertions(+), 15 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index d8e642ba6..3d59102e4 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -38,8 +38,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.74 # unknown end-to-end spec tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS) - ret.steerActuatorDelay = 0.3 + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.7, FRICTION=0.06) elif candidate == CAR.PRIUS_V: stop_and_go = True diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py index 2cfdd3164..811b3a6e1 100644 --- a/selfdrive/car/toyota/tunes.py +++ b/selfdrive/car/toyota/tunes.py @@ -50,19 +50,9 @@ def set_long_tune(tune, name): ###### LAT ###### -def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1): +def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1, use_steering_angle=True): if name == LatTunes.TORQUE: set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION) - elif name == LatTunes.INDI_PRIUS: - tune.init('indi') - tune.indi.innerLoopGainBP = [0.] - tune.indi.innerLoopGainV = [4.0] - tune.indi.outerLoopGainBP = [0.] - tune.indi.outerLoopGainV = [3.0] - tune.indi.timeConstantBP = [0.] - tune.indi.timeConstantV = [1.0] - tune.indi.actuatorEffectivenessBP = [0.] - tune.indi.actuatorEffectivenessV = [1.0] elif 'PID' in str(name): tune.init('pid') tune.pid.kiBP = [0.0] diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 502d0abf0..820862af9 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -55,7 +55,9 @@ class LatControlTorque(LatControl): if self.use_steering_angle: actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) else: - actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo + actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo + actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) desired_lateral_accel = desired_curvature * CS.vEgo ** 2 desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 actual_lateral_accel = actual_curvature * CS.vEgo ** 2 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 55ce9832c..de8599a58 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -336d77ad17b90af17b7eb24cc832e80b62d05a24 +0956446adfa91506f0a3d88f893e041bfb2890c1