Add indi breakpoints (#19664)

* Added BP, V to latcontrol_indi

* hyundai

* toyota

* Tests for INDI outerLoopGain
old-commit-hash: 0a65e87394

selfdrive/car/hyundai/interface.py

@@ -73,10 +73,14 @@ class CarInterface(CarInterfaceBase):
       ret.wheelbase = 3.01
       ret.steerRatio = 16.5
       ret.lateralTuning.init('indi')
-      ret.lateralTuning.indi.innerLoopGain = 3.5
+      ret.lateralTuning.indi.innerLoopGainBP = [0.]
-      ret.lateralTuning.indi.outerLoopGain = 2.0
+      ret.lateralTuning.indi.innerLoopGainV = [3.5]
-      ret.lateralTuning.indi.timeConstant = 1.4
+      ret.lateralTuning.indi.outerLoopGainBP = [0.]
-      ret.lateralTuning.indi.actuatorEffectiveness = 2.3
+      ret.lateralTuning.indi.outerLoopGainV = [2.0]
+      ret.lateralTuning.indi.timeConstantBP = [0.]
+      ret.lateralTuning.indi.timeConstantV = [1.4]
+      ret.lateralTuning.indi.actuatorEffectivenessBP = [0.]
+      ret.lateralTuning.indi.actuatorEffectivenessV = [2.3]
       ret.minSteerSpeed = 60 * CV.KPH_TO_MS
     elif candidate == CAR.KONA:
       ret.lateralTuning.pid.kf = 0.00005
@@ -157,10 +161,14 @@ class CarInterface(CarInterfaceBase):
     # Genesis
     elif candidate == CAR.GENESIS_G70:
       ret.lateralTuning.init('indi')
-      ret.lateralTuning.indi.innerLoopGain = 2.5
+      ret.lateralTuning.indi.innerLoopGainBP = [0.]
-      ret.lateralTuning.indi.outerLoopGain = 3.5
+      ret.lateralTuning.indi.innerLoopGainV = [2.5]
-      ret.lateralTuning.indi.timeConstant = 1.4
+      ret.lateralTuning.indi.outerLoopGainBP = [0.]
-      ret.lateralTuning.indi.actuatorEffectiveness = 1.8
+      ret.lateralTuning.indi.outerLoopGainV = [3.5]
+      ret.lateralTuning.indi.timeConstantBP = [0.]
+      ret.lateralTuning.indi.timeConstantV = [1.4]
+      ret.lateralTuning.indi.actuatorEffectivenessBP = [0.]
+      ret.lateralTuning.indi.actuatorEffectivenessV = [1.8]
       ret.steerActuatorDelay = 0.1
       ret.mass = 1640.0 + STD_CARGO_KG
       ret.wheelbase = 2.84

selfdrive/car/tests/test_car_interfaces.py

@@ -39,7 +39,7 @@ class TestCarInterfaces(unittest.TestCase):
       elif tuning == 'lqr':
         self.assertTrue(len(car_params.lateralTuning.lqr.a))
       elif tuning == 'indi':
-        self.assertGreater(car_params.lateralTuning.indi.outerLoopGain, 1e-3)
+        self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV))
 
       # Run car interface
       CC = car.CarControl.new_message()

selfdrive/car/toyota/interface.py

@@ -45,10 +45,14 @@ class CarInterface(CarInterfaceBase):
       ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
 
       ret.lateralTuning.init('indi')
-      ret.lateralTuning.indi.innerLoopGain = 4.0
+      ret.lateralTuning.indi.innerLoopGainBP = [0.]
-      ret.lateralTuning.indi.outerLoopGain = 3.0
+      ret.lateralTuning.indi.innerLoopGainV = [4.0]
-      ret.lateralTuning.indi.timeConstant = 1.0
+      ret.lateralTuning.indi.outerLoopGainBP = [0.]
-      ret.lateralTuning.indi.actuatorEffectiveness = 1.0
+      ret.lateralTuning.indi.outerLoopGainV = [3.0]
+      ret.lateralTuning.indi.timeConstantBP = [0.]
+      ret.lateralTuning.indi.timeConstantV = [1.0]
+      ret.lateralTuning.indi.actuatorEffectivenessBP = [0.]
+      ret.lateralTuning.indi.actuatorEffectivenessV = [1.0]
       ret.steerActuatorDelay = 0.5
 
     elif candidate in [CAR.RAV4, CAR.RAV4H]:

selfdrive/controls/lib/latcontrol_indi.py

@@ -3,7 +3,7 @@ import numpy as np
 
 from cereal import log
 from common.realtime import DT_CTRL
-from common.numpy_fast import clip
+from common.numpy_fast import clip, interp
 from selfdrive.car.toyota.values import CarControllerParams
 from selfdrive.car import apply_toyota_steer_torque_limits
 from selfdrive.controls.lib.drive_helpers import get_steer_max
@@ -28,16 +28,18 @@ class LatControlINDI():
                   [7.29394177e+00, 1.39159419e-02],
                   [1.71022442e+01, 3.38495381e-02]])
 
+    self.speed = 0.
+
     self.K = K
     self.A_K = A - np.dot(K, C)
     self.x = np.array([[0.], [0.], [0.]])
 
     self.enforce_rate_limit = CP.carName == "toyota"
 
-    self.RC = CP.lateralTuning.indi.timeConstant
+    self._RC = (CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV)
-    self.G = CP.lateralTuning.indi.actuatorEffectiveness
+    self._G = (CP.lateralTuning.indi.actuatorEffectivenessBP, CP.lateralTuning.indi.actuatorEffectivenessV)
-    self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain
+    self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV)
-    self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain
+    self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV)
     self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL)
 
     self.sat_count_rate = 1.0 * DT_CTRL
@@ -45,10 +47,27 @@ class LatControlINDI():
 
     self.reset()
 
+  @property
+  def RC(self):
+    return interp(self.speed, self._RC[0], self._RC[1])
+
+  @property
+  def G(self):
+    return interp(self.speed, self._G[0], self._G[1])
+
+  @property
+  def outer_loop_gain(self):
+    return interp(self.speed, self._outer_loop_gain[0], self._outer_loop_gain[1])
+
+  @property
+  def inner_loop_gain(self):
+    return interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1])
+
   def reset(self):
     self.delayed_output = 0.
     self.output_steer = 0.
     self.sat_count = 0.0
+    self.speed = 0.
 
   def _check_saturation(self, control, check_saturation, limit):
     saturated = abs(control) == limit
@@ -63,6 +82,7 @@ class LatControlINDI():
     return self.sat_count > self.sat_limit
 
   def update(self, active, CS, CP, path_plan):
+    self.speed = CS.vEgo
     # Update Kalman filter
     y = np.array([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]])
     self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)

selfdrive/test/test_models.py

@@ -91,7 +91,7 @@ class TestCarModel(unittest.TestCase):
     elif tuning == 'lqr':
       self.assertTrue(len(self.CP.lateralTuning.lqr.a))
     elif tuning == 'indi':
-      self.assertGreater(self.CP.lateralTuning.indi.outerLoopGain, 1e-3)
+      self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV))
 
     self.assertTrue(self.CP.enableCamera)