Rerevert torque control (commaai#24649)

* Revert "Revert torque control (commaai#24565)"

This reverts commit 9f8b037.

* Move tune out of car specific stuff

* Update ref commit

release/files_common

@@ -239,7 +239,6 @@ selfdrive/controls/lib/events.py
 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/car/tests/test_car_interfaces.py

@@ -38,8 +38,6 @@ 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':

selfdrive/car/tests/test_models.py

@@ -117,8 +117,6 @@ def test_car_params(self):
         self.assertTrue(len(self.CP.lateralTuning.pid.kpV))
       elif tuning == 'torque':
         self.assertTrue(self.CP.lateralTuning.torque.kf > 0)
-      elif tuning == 'lqr':
-        self.assertTrue(len(self.CP.lateralTuning.lqr.a))
       elif tuning == 'indi':
         self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV))
       else:

selfdrive/car/toyota/interface.py

@@ -43,15 +43,15 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
       ret.steerRatio = 17.4
       tire_stiffness_factor = 0.5533
       ret.mass = 3340. * 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
@@ -83,7 +83,10 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
       ret.steerRatio = 13.7
       tire_stiffness_factor = 0.7933
       ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
-      set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
+      if candidate in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2):
+        set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.4, FRICTION=0.05)
+      else:
+        set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
 
     elif candidate in (CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2):
       stop_and_go = True
@@ -132,7 +135,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
       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.07)
 
     elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH):
       stop_and_go = True

selfdrive/car/toyota/tunes.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 from enum import Enum
-
+from selfdrive.controls.lib.latcontrol_torque import set_torque_tune
 
 class LongTunes(Enum):
   PEDAL = 0
@@ -52,22 +52,7 @@ def set_long_tune(tune, name):
 ###### LAT ######
 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.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
+    set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION)
   elif name == LatTunes.INDI_PRIUS:
     tune.init('indi')
     tune.indi.innerLoopGainBP = [0.]

selfdrive/controls/controlsd.py

@@ -22,7 +22,6 @@
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
-from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
 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
@@ -145,8 +144,6 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None):
       self.LaC = LatControlPID(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'indi':
       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)
 
@@ -739,8 +736,6 @@ def publish_logs(self, CS, start_time, CC, lac_log):
       controlsState.lateralControlState.pidState = lac_log
     elif lat_tuning == 'torque':
       controlsState.lateralControlState.torqueState = lac_log
-    elif lat_tuning == 'lqr':
-      controlsState.lateralControlState.lqrState = lac_log
     elif lat_tuning == 'indi':
       controlsState.lateralControlState.indiState = lac_log
 

selfdrive/controls/lib/latcontrol_torque.py

@@ -22,6 +22,15 @@
 JERK_THRESHOLD = 0.2
 
 
+def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=.1):
+  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
+
+
 class LatControlTorque(LatControl):
   def __init__(self, CP, CI):
     super().__init__(CP, CI)

selfdrive/controls/lib/tests/test_latcontrol.py

@@ -9,15 +9,15 @@
 from selfdrive.car.toyota.values import CAR as TOYOTA
 from selfdrive.car.nissan.values import CAR as NISSAN
 from selfdrive.controls.lib.latcontrol_pid import LatControlPID
-from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
+from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 
 
 class TestLatControl(unittest.TestCase):
 
-  @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
+  @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlTorque), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
   def test_saturation(self, car_name, controller):
     CarInterface, CarController, CarState = interfaces[car_name]
     CP = CarInterface.get_params(car_name)

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-b8c35486e8354713221d4237e97e5abced6f5228
+336d77ad17b90af17b7eb24cc832e80b62d05a24