lat_control_steer_model: use model without memory

of the form

    steering_angle = B * u + R * p

    with ['can_cmds_squared', 'cmds_by_vsquared', 'cmds_squared_by_vsquared']
    p = ['roll', 'roll_squared', 'roll_by_speed', 'roll_by_vsquared']

Note: coefficients in B must be positive

cereal

@@ -1 +1 @@
-Subproject commit 28d458a9af49b38bd0a9052f09fbe927324320fb
+Subproject commit 2336d872a7b18c614f33bcb5802a73b4dcd6c1ae

selfdrive/car/toyota/interface.py

@@ -130,7 +130,8 @@ class CarInterface(CarInterfaceBase):
       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.PID_D)
+      set_lat_tune(ret.lateralTuning, LatTunes.STEER_MODEL)
 
     elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH):
       stop_and_go = True

selfdrive/car/toyota/tunes.py

@@ -24,6 +24,7 @@ class LatTunes(Enum):
   PID_L = 13
   PID_M = 14
   PID_N = 15
+  STEER_MODEL = 16
 
 
 ###### LONG ######
@@ -72,6 +73,9 @@ def set_lat_tune(tune, name):
     tune.lqr.l = [0.3233671, 0.3185757]
     tune.lqr.dcGain = 0.002237852961363602
 
+  elif name == LatTunes.STEER_MODEL:
+    tune.init('steerModel')
+    tune.steerModel.modelparam = [ 1.71925238e+01, 1.41763391e+03, 9.77812550e+02, 1.88123015e-01, -9.82756467e-02, -1.26513806e+00, -1.68255566e+02]
   elif 'PID' in str(name):
     tune.init('pid')
     tune.pid.kiBP = [0.0]

selfdrive/controls/controlsd.py

@@ -21,6 +21,7 @@ from selfdrive.controls.lib.latcontrol_pid import LatControlPID
 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_steer_model import LatControlSteerModel
 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 @@ class Controls:
       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() == 'steerModel':
+      self.LaC = LatControlSteerModel(self.CP, self.CI)
 
     self.initialized = False
     self.state = State.disabled
@@ -669,6 +672,8 @@ class Controls:
       controlsState.lateralControlState.lqrState = lac_log
     elif lat_tuning == 'indi':
       controlsState.lateralControlState.indiState = lac_log
+    elif lat_tuning == 'steerModel':
+      controlsState.lateralControlState.steerModelState = lac_log
 
     self.pm.send('controlsState', dat)
 

selfdrive/controls/lib/latcontrol_steer_model.py

@@ -0,0 +1,60 @@
+import math
+import numpy as np
+
+from selfdrive.controls.lib.drive_helpers import get_steer_max
+from cereal import log
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
+
+
+controls_list = ['can_cmds_squared', 'cmds_by_vsquared', 'cmds_squared_by_vsquared']
+NU = len(controls_list)
+
+live_param_list = ['roll', 'roll_squared', 'roll_by_speed', 'roll_by_vsquared']
+N_live_param = len(live_param_list)
+
+
+class LatControlSteerModel(LatControl):
+  def __init__(self, CP, CI):
+    super().__init__(CP, CI)
+
+    model_param = np.asarray(list(CP.lateralTuning.steerModel.modelparam))
+    self.B = model_param[0:NU]
+    self.R = model_param[NU:NU+N_live_param]
+    assert(NU+N_live_param == len(model_param))
+
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+    model_log = log.ControlsState.LateralSteerModelState.new_message()
+
+    steers_max = get_steer_max(CP, CS.vEgo)
+
+    # offset does not contribute to resistive torque
+    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
+
+    model_log.steeringAngleDesiredDeg = angle_steers_des
+
+    if CS.vEgo < MIN_STEER_SPEED or not active:
+      output_steer = 0.0
+      model_log.active = False
+      self.reset()
+    else:
+      # NOTE: live_param_list dependent.
+      # live_param_list = ['roll', 'roll_squared', 'roll_by_speed', 'roll_by_vsquared']
+      torque_prev = last_actuators.steer
+      roll_deg = np.degrees(params.roll)
+      live_param = np.array([roll_deg, roll_deg * abs(roll_deg), roll_deg / CS.vEgo, roll_deg/(CS.vEgo**2)])
+      Rp = self.R @ live_param
+
+      # NOTE: controls_list dependent.
+      # controls_list = ['can_cmds_squared', 'cmds_by_vsquared', 'cmds_squared_by_vsquared']
+      du_dtorque = np.array([2*abs(torque_prev), 1/(CS.vEgo**2), 2*abs(torque_prev)/(CS.vEgo**2) ])
+      B_tilde = self.B @ du_dtorque
+      output_steer = float( (1/B_tilde) * (angle_steers_des_no_offset - Rp) )
+
+      model_log.active = True
+
+    # model_log.steeringAngleDeg = float(self.xcurrent[0])
+    model_log.output = output_steer
+    model_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
+
+    return output_steer, angle_steers_des, model_log

selfdrive/controls/lib/tests/test_latcontrol.py

@@ -12,12 +12,14 @@ from selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from selfdrive.controls.lib.latcontrol_steer_model import LatControlSteerModel
 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, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI),
+                         (NISSAN.LEAF, LatControlAngle), (TOYOTA.COROLLA_TSS2, LatControlSteerModel)])
   def test_saturation(self, car_name, controller):
     CarInterface, CarController, CarState = interfaces[car_name]
     CP = CarInterface.get_params(car_name)

selfdrive/test/test_models.py

@@ -120,6 +120,8 @@ class TestCarModel(unittest.TestCase):
         self.assertTrue(len(self.CP.lateralTuning.lqr.a))
       elif tuning == 'indi':
         self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV))
+      elif tuning == 'steerModel':
+        self.assertTrue(len(self.CP.lateralTuning.steerModel.modelparam))
       else:
         raise Exception("unkown tuning")