add Torque

troque

Latcontrol torque: update tuning (commaai#24357)

* Little more chill

* Update ref

* Update refs

Latcontrol torque: max_torque rename (commaai#24265)

toggle notes

misc

revert LQR removal

wrong ordinal

spacing fix

Co-Authored-By: Willem Melching <willem.melching@gmail.com>

cereal/car.capnp

@@ -428,6 +428,7 @@ struct CarParams {
     pid @26 :LateralPIDTuning;
     indi @27 :LateralINDITuning;
     lqr @40 :LateralLQRTuning;
+    torque @66 :LateralTorqueTuning;
   }
 
   steerLimitAlert @28 :Bool;
@@ -476,11 +477,20 @@ struct CarParams {
     kf @4 :Float32;
   }
 
+  struct LateralTorqueTuning {
+    useSteeringAngle @0 :Bool;
+    kp @1 :Float32;
+    ki @2 :Float32;
+    friction @3 :Float32;
+    kf @4 :Float32;
+  }
+
   struct LongitudinalPIDTuning {
     kpBP @0 :List(Float32);
     kpV @1 :List(Float32);
     kiBP @2 :List(Float32);
     kiV @3 :List(Float32);
+    kf @6 :Float32;
     deadzoneBP @4 :List(Float32);
     deadzoneV @5 :List(Float32);
   }

cereal/log.capnp

@@ -579,6 +579,7 @@ struct ControlsState @0x97ff69c53601abf1 {
     lqrState @55 :LateralLQRState;
     angleState @58 :LateralAngleState;
     debugState @59 :LateralDebugState;
+    torqueState @60 :LateralTorqueState;
   }
 
   enum OpenpilotState @0xdbe58b96d2d1ac61 {
@@ -630,6 +631,18 @@ struct ControlsState @0x97ff69c53601abf1 {
     steeringAngleDesiredDeg @9 :Float32;
    }
 
+   struct LateralTorqueState {
+     active @0 :Bool;
+     error @1 :Float32;
+     errorRate @8 :Float32;
+     p @2 :Float32;
+     i @3 :Float32;
+     d @4 :Float32;
+     f @5 :Float32;
+     output @6 :Float32;
+     saturated @7 :Bool;
+    }
+
   struct LateralLQRState {
     active @0 :Bool;
     steeringAngleDeg @1 :Float32;

panda/board/safety/safety_toyota.h

@@ -3,13 +3,13 @@ const int TOYOTA_MAX_TORQUE = 1500;       // max torque cmd allowed ever
 
 // rate based torque limit + stay within actually applied
 // packet is sent at 100hz, so this limit is 1000/sec
-const int TOYOTA_MAX_RATE_UP = 10;        // ramp up slow
-const int TOYOTA_MAX_RATE_DOWN = 25;      // ramp down fast
+const int TOYOTA_MAX_RATE_UP = 15;        // ramp up slow
+const int TOYOTA_MAX_RATE_DOWN = 25;      // ramp down fast (we do not use 50 just allow throuht ~ kumar)
 const int TOYOTA_MAX_TORQUE_ERROR = 350;  // max torque cmd in excess of torque motor
 
 // real time torque limit to prevent controls spamming
 // the real time limit is 1500/sec
-const int TOYOTA_MAX_RT_DELTA = 375;      // max delta torque allowed for real time checks
+const int TOYOTA_MAX_RT_DELTA = 450;      // max delta torque allowed for real time checks
 const uint32_t TOYOTA_RT_INTERVAL = 250000;    // 250ms between real time checks
 
 // longitudinal limits

release/files_common

@@ -239,6 +239,7 @@ selfdrive/controls/lib/lane_planner.py
 selfdrive/controls/lib/latcontrol_angle.py
 selfdrive/controls/lib/latcontrol_indi.py
 selfdrive/controls/lib/latcontrol_lqr.py
+selfdrive/controls/lib/latcontrol_torque.py
 selfdrive/controls/lib/latcontrol_pid.py
 selfdrive/controls/lib/latcontrol.py
 selfdrive/controls/lib/lateral_planner.py

selfdrive/car/interfaces.py

@@ -100,6 +100,7 @@ def get_std_params(candidate, fingerprint):
     ret.stoppingControl = True
     ret.longitudinalTuning.deadzoneBP = [0.]
     ret.longitudinalTuning.deadzoneV = [0.]
+    ret.longitudinalTuning.kf = 1.
     ret.longitudinalTuning.kpBP = [0.]
     ret.longitudinalTuning.kpV = [1.]
     ret.longitudinalTuning.kiBP = [0.]

selfdrive/car/tests/test_car_interfaces.py

@@ -39,8 +39,8 @@ def test_car_interfaces(self, car_name):
       tuning = car_params.lateralTuning.which()
       if tuning == 'pid':
         self.assertTrue(len(car_params.lateralTuning.pid.kpV))
-      elif tuning == 'lqr':
-        self.assertTrue(len(car_params.lateralTuning.lqr.a))
+      elif tuning == 'torque':
+        self.assertTrue(car_params.lateralTuning.torque.kf > 0)
       elif tuning == 'indi':
         self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV))
 

selfdrive/car/toyota/interface.py

@@ -36,7 +36,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       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
 
@@ -46,15 +45,15 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       ret.steerRatio = 17.4
       tire_stiffness_factor = 0.5533
       ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
-      set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06)
 
     elif candidate in (CAR.RAV4, CAR.RAV4H):
       stop_and_go = True if (candidate in CAR.RAV4H) else False
       ret.wheelbase = 2.65
       ret.steerRatio = 16.88   # 14.5 is spec end-to-end
       tire_stiffness_factor = 0.5533
       ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
-      set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06)
 
     elif candidate == CAR.COROLLA:
       ret.wheelbase = 2.70
@@ -87,10 +86,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       tire_stiffness_factor = 0.7933
       ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
       if Params().get_bool('LqrTune'):
-        set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS_TSS2)
-        ret.steerActuatorDelay = 0.3
-        ret.steerRateCost = 1.25
-        ret.steerLimitTimer = 0.5
+        set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.4, FRICTION=0.05)
       else:
         set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
 
@@ -123,13 +119,13 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       ret.steerRatio = 14.3
       tire_stiffness_factor = 0.7933
       ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG  # Average between ICE and Hybrid
-      set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_TORQUE=2.5, FRICTION=0.06)
 
       # 2019+ Rav4 TSS2 uses two different steering racks and specific tuning seems to be necessary.
       # See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891
       for fw in car_fw:
         if fw.ecu == "eps" and (fw.fwVersion.startswith(b'\x02') or fw.fwVersion in [b'8965B42181\x00\x00\x00\x00\x00\x00']):
-          set_lat_tune(ret.lateralTuning, LatTunes.PID_I)
+          set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_TORQUE=2.5, FRICTION=0.06)
           break
 
     elif candidate in (CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2):
@@ -138,7 +134,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       ret.steerRatio = 13.9
       tire_stiffness_factor = 0.444  # not optimized yet
       ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
-      set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.0, FRICTION=0.06)
 
     elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH):
       stop_and_go = True
@@ -186,10 +182,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
       if Params().get_bool('LqrTune'):
-        set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS_TSS2)
-        ret.steerActuatorDelay = 0.3
-        ret.steerRateCost = 1.25
-        ret.steerLimitTimer = 0.5
+        set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.0, FRICTION=0.06)
       else:
         set_lat_tune(ret.lateralTuning, LatTunes.PID_N)
 

selfdrive/car/toyota/tunes.py

@@ -25,7 +25,8 @@ class LatTunes(Enum):
   PID_M = 14
   PID_N = 15
   INDI_PRIUS_TSS2 = 16
-
+  INDI_RAV4_TSS2 = 17
+  TORQUE = 18
 
 
 ###### LONG ######
@@ -51,8 +52,15 @@ def set_long_tune(tune, name):
 
 
 ###### LAT ######
-def set_lat_tune(tune, name):
-  if name == LatTunes.INDI_PRIUS:
+def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1):
+  if name == LatTunes.TORQUE:
+    tune.init('torque')
+    tune.torque.useSteeringAngle = True
+    tune.torque.kp = 1.0 / MAX_LAT_ACCEL
+    tune.torque.kf = 1.0 / MAX_LAT_ACCEL
+    tune.torque.ki = 0.1 / MAX_LAT_ACCEL
+    tune.torque.friction = FRICTION
+  elif name == LatTunes.INDI_PRIUS:
     tune.init('indi')
     tune.indi.innerLoopGainBP = [0.]
     tune.indi.innerLoopGainV = [4.0]
@@ -75,16 +83,18 @@ def set_lat_tune(tune, name):
     tune.indi.actuatorEffectivenessV = [2, 3]
 
 
-  elif name == LatTunes.LQR_RAV4:
-    tune.init('lqr')
-    tune.lqr.scale = 1500.0
-    tune.lqr.ki = 0.05
-    tune.lqr.a = [0., 1., -0.22619643, 1.21822268]
-    tune.lqr.b = [-1.92006585e-04, 3.95603032e-05]
-    tune.lqr.c = [1., 0.]
-    tune.lqr.k = [-110.73572306, 451.22718255]
-    tune.lqr.l = [0.3233671, 0.3185757]
-    tune.lqr.dcGain = 0.002237852961363602
+  elif name == LatTunes.INDI_RAV4_TSS2:
+    tune.init('indi')
+    tune.indi.innerLoopGainBP = [5.0,  8.3,   11.1,  12.10,  13.9,  16.7,  18,    20,    25,    30]
+    tune.indi.innerLoopGainV = [3.13,  5.2,   6.66,  7.8,    8.8,   10,    10.8,  12,    15,  15]
+    tune.indi.outerLoopGainBP = [5.0,  8.3,   11.1,  12.10,  13.9,  16.7,  18,    20,    25,    30]
+    tune.indi.outerLoopGainV = [2.9, 4.97,   6.51,  7.65,  8.58,  9.78,  10.58, 11.83, 14.9, 14.998]
+     # inner - outer difference 0.23, 0.23,  0.15,  0.15,  0.22,  0.22 , 0.22,  0.16,  0.1,  0.002
+    tune.indi.timeConstantBP = [5.0,  8.33,   11.1,  13.9,   16.7,  17.5,  18,    20,  22.5, 23.88, 23.89,  30,  40]
+    tune.indi.timeConstantV = [0.1461, 0.2427, 0.27871, 0.3, 0,3,   0.3,   0.5, 0.6250, 0.85, 1.1,  2.2,   2.6,  3.2]
+    #tune.indi.timeConstantV = [0.1461, 0.2427, 0.27871, 0.312, 0,315, 0.32, 0.525, 0.6250, 0.85, 1.1, 1.2, 2.3, 2.6]
+    tune.indi.actuatorEffectivenessBP = [10, 13, 14, 25, 33]
+    tune.indi.actuatorEffectivenessV = [15, 13, 13, 15, 15]
 
   elif 'PID' in str(name):
     tune.init('pid')

selfdrive/car/toyota/values.py

@@ -15,11 +15,10 @@ class CarControllerParams:
   ACCEL_MIN = -3.5  # m/s2
 
   STEER_MAX = 1500
-  STEER_DELTA_UP = 10       # 1.5s time to peak torque
+  STEER_DELTA_UP = 15       # 1.5s time to peak torque
   STEER_DELTA_DOWN = 25     # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
   STEER_ERROR_MAX = 350     # max delta between torque cmd and torque motor
 
-
 class ToyotaFlags(IntFlag):
   HYBRID = 1
 

selfdrive/controls/controlsd.py

@@ -21,6 +21,7 @@
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
 from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from selfdrive.controls.lib.events import Events, ET
 from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
 from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -133,6 +134,8 @@ def __init__(self, sm=None, pm=None, can_sock=None):
       self.LaC = LatControlINDI(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'lqr':
       self.LaC = LatControlLQR(self.CP, self.CI)
+    elif self.CP.lateralTuning.which() == 'torque':
+      self.LaC = LatControlTorque(self.CP, self.CI)
 
     self.initialized = False
     self.state = State.disabled
@@ -501,7 +504,7 @@ def state_control(self, CS):
                                                                              lat_plan.curvatures,
                                                                              lat_plan.curvatureRates)
       actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.VM, params, self.last_actuators,
-                                                                             desired_curvature, desired_curvature_rate)
+                                                                             desired_curvature, desired_curvature_rate, self.sm['liveLocationKalman'])
     else:
       lac_log = log.ControlsState.LateralDebugState.new_message()
       if self.sm.rcv_frame['testJoystick'] > 0 and self.active:
@@ -666,6 +669,8 @@ def publish_logs(self, CS, start_time, actuators, lac_log):
       controlsState.lateralControlState.pidState = lac_log
     elif lat_tuning == 'lqr':
       controlsState.lateralControlState.lqrState = lac_log
+    elif lat_tuning == 'torque':
+      controlsState.lateralControlState.torqueState = lac_log
     elif lat_tuning == 'indi':
       controlsState.lateralControlState.indiState = lac_log
 

selfdrive/controls/lib/latcontrol.py

@@ -13,7 +13,7 @@ def __init__(self, CP, CI):
     self.sat_count = 0.
 
   @abstractmethod
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
     pass
 
   def reset(self):

selfdrive/controls/lib/latcontrol_angle.py

@@ -7,7 +7,7 @@
 
 
 class LatControlAngle(LatControl):
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
     angle_log = log.ControlsState.LateralAngleState.new_message()
 
     if CS.vEgo < MIN_STEER_SPEED or not active:

selfdrive/controls/lib/latcontrol_indi.py

@@ -65,7 +65,7 @@ def reset(self):
     self.steer_filter.x = 0.
     self.speed = 0.
 
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
     self.speed = CS.vEgo
     # Update Kalman filter
     y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])

selfdrive/controls/lib/latcontrol_pid.py

@@ -1,6 +1,6 @@
 import math
 
-from selfdrive.controls.lib.pid import PIController
+from selfdrive.controls.lib.pid import PIDController
 from selfdrive.controls.lib.drive_helpers import get_steer_max
 from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
 from cereal import log
@@ -9,7 +9,7 @@
 class LatControlPID(LatControl):
   def __init__(self, CP, CI):
     super().__init__(CP, CI)
-    self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
+    self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
                             (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
                             k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0)
     self.get_steer_feedforward = CI.get_steer_feedforward_function()
@@ -18,16 +18,17 @@ def reset(self):
     super().reset()
     self.pid.reset()
 
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
     pid_log = log.ControlsState.LateralPIDState.new_message()
     pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     pid_log.steeringRateDeg = float(CS.steeringRateDeg)
 
     angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
     angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
+    error = angle_steers_des - CS.steeringAngleDeg
 
     pid_log.steeringAngleDesiredDeg = angle_steers_des
-    pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
+    pid_log.angleError = error
     if CS.vEgo < MIN_STEER_SPEED or not active:
       output_steer = 0.0
       pid_log.active = False
@@ -40,10 +41,8 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature,
       # offset does not contribute to resistive torque
       steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
 
-      deadzone = 0.0
-
-      output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, override=CS.steeringPressed,
-                                     feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
+      output_steer = self.pid.update(error, override=CS.steeringPressed,
+                                     feedforward=steer_feedforward, speed=CS.vEgo)
       pid_log.active = True
       pid_log.p = self.pid.p
       pid_log.i = self.pid.i