Interpolate ki/kp for steering PID loop (#200)

* Interpolate ki/kp for steering PID loop

Very much needed for the Volt port: car ping-pongs with low kp
on high speeeds, and the loop is unstable with high kp on
low speeds.

Also, removes "number or array?" logic from PIController,
now that all the callers use interpolation ofr ki/kp.

* Pass speed to steering PID loop for ki/kp interpolation

* Remove unused numbers import

cereal/car.capnp

@@ -303,8 +303,12 @@ struct CarParams {
   tireStiffnessRear @14 :Float32;    # [N/rad] rear tire coeff of stiff
 
   # Kp and Ki for the lateral control
-  steerKp @15 :Float32;
+  steerKpBP @42 :List(Float32);
-  steerKi @16 :Float32;
+  steerKpV @43 :List(Float32);
+  steerKiBP @44 :List(Float32);
+  steerKiV @45 :List(Float32);
+  steerKpDEPRECATED @15 :Float32;
+  steerKiDEPRECATED @16 :Float32;
   steerKf @25 :Float32;
 
   # Kp and Ki for the longitudinal control

selfdrive/car/honda/interface.py

@@ -151,6 +151,7 @@ class CarInterface(object):
     tireStiffnessFront_civic = 85400
     tireStiffnessRear_civic = 90000
 
+    ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
     if candidate == CAR.CIVIC:
       stop_and_go = True
       ret.mass = mass_civic
@@ -159,7 +160,7 @@ class CarInterface(object):
       ret.steerRatio = 13.0
       # Civic at comma has modified steering FW, so different tuning for the Neo in that car
       is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e']
-      ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
+      ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]
 
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [3.6, 2.4, 1.5]
@@ -173,7 +174,7 @@ class CarInterface(object):
       ret.steerRatio = 15.3
       # Acura at comma has modified steering FW, so different tuning for the Neo in that car
       is_fw_modified = os.getenv("DONGLE_ID") in ['85a6c74d4ad9c310']
-      ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
+      ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]
 
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]
@@ -185,7 +186,7 @@ class CarInterface(object):
       ret.wheelbase = 2.62
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 15.3
-      ret.steerKp, ret.steerKi = 0.8, 0.24
+      ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
 
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]
@@ -197,7 +198,7 @@ class CarInterface(object):
       ret.wheelbase = 2.68
       ret.centerToFront = ret.wheelbase * 0.38
       ret.steerRatio = 15.0
-      ret.steerKp, ret.steerKi = 0.8, 0.24
+      ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
 
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]
@@ -209,7 +210,7 @@ class CarInterface(object):
       ret.wheelbase = 3.00
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 14.35
-      ret.steerKp, ret.steerKi = 0.6, 0.18
+      ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]
 
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]
@@ -221,7 +222,7 @@ class CarInterface(object):
       ret.wheelbase = 2.81
       ret.centerToFront = ret.wheelbase * 0.41
       ret.steerRatio = 16.0
-      ret.steerKp, ret.steerKi = 0.38, 0.11
+      ret.steerKpV, ret.steerKiV = [[0.38], [0.11]]
 
       ret.longitudinalKpBP = [0., 5., 35.]
       ret.longitudinalKpV = [1.2, 0.8, 0.5]

selfdrive/car/toyota/interface.py

@@ -72,12 +72,13 @@ class CarInterface(object):
     tireStiffnessFront_civic = 85400
     tireStiffnessRear_civic = 90000
 
+    ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
     if candidate == CAR.PRIUS:
       ret.safetyParam = 66  # see conversion factor for STEER_TORQUE_EPS in dbc file
       ret.wheelbase = 2.70
       ret.steerRatio = 14.5  # TODO: find exact value for Prius
       ret.mass = 3045./2.205 + std_cargo
-      ret.steerKp, ret.steerKi = 0.6, 0.05
+      ret.steerKpV, ret.steerKiV = [[0.6], [0.05]]
       ret.steerKf = 0.00006   # full torque for 10 deg at 80mph means 0.00007818594
       ret.steerRateCost = 2.
     elif candidate in [CAR.RAV4, CAR.RAV4H]:
@@ -85,7 +86,7 @@ class CarInterface(object):
       ret.wheelbase = 2.65
       ret.steerRatio = 14.5 # Rav4 2017
       ret.mass = 3650./2.205 + std_cargo  # mean between normal and hybrid
-      ret.steerKp, ret.steerKi = 0.6, 0.05
+      ret.steerKpV, ret.steerKiV = [[0.6], [0.05]]
       ret.steerKf = 0.00006   # full torque for 10 deg at 80mph means 0.00007818594
       ret.steerRateCost = 1.
     elif candidate == CAR.COROLLA:
@@ -93,7 +94,7 @@ class CarInterface(object):
       ret.wheelbase = 2.70
       ret.steerRatio = 17.8
       ret.mass = 2860./2.205 + std_cargo  # mean between normal and hybrid
-      ret.steerKp, ret.steerKi = 0.2, 0.05
+      ret.steerKpV, ret.steerKiV = [[0.2], [0.05]]
       ret.steerKf = 0.00003   # full torque for 20 deg at 80mph means 0.00007818594
       ret.steerRateCost = 1.
     elif candidate == CAR.LEXUS_RXH:
@@ -101,7 +102,7 @@ class CarInterface(object):
       ret.wheelbase = 2.79
       ret.steerRatio = 16.  # official specs say 14.8, but it does not seem right
       ret.mass = 4481./2.205 + std_cargo  # mean between min and max
-      ret.steerKp, ret.steerKi = 0.6, 0.1
+      ret.steerKpV, ret.steerKiV = [[0.6], [0.1]]
       ret.steerKf = 0.00006   # full torque for 10 deg at 80mph means 0.00007818594
       ret.steerRateCost = .8
 

selfdrive/controls/lib/latcontrol.py

@@ -26,7 +26,9 @@ def get_steer_max(CP, v_ego):
 
 class LatControl(object):
   def __init__(self, VM):
-    self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0)
+    self.pid = PIController((VM.CP.steerKpBP, VM.CP.steerKpV),
+                            (VM.CP.steerKiBP, VM.CP.steerKiV),
+                            k_f=VM.CP.steerKf, pos_limit=1.0)
     self.last_cloudlog_t = 0.0
     self.setup_mpc(VM.CP.steerRateCost)
 
@@ -103,7 +105,7 @@ class LatControl(object):
       self.pid.pos_limit = steers_max
       self.pid.neg_limit = -steers_max
       steer_feedforward = self.angle_steers_des * v_ego**2  # proportional to realigning tire momentum (~ lateral accel)
-      output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward)
+      output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward, speed=v_ego)
 
     self.sat_flag = self.pid.saturated
     return output_steer

selfdrive/controls/lib/pid.py

@@ -1,6 +1,5 @@
 import numpy as np
 from common.numpy_fast import clip, interp
-import numbers
 
 def apply_deadzone(error, deadzone):
   if error > deadzone:
@@ -30,21 +29,11 @@ class PIController(object):
 
   @property
   def k_p(self):
-    if isinstance(self._k_p, numbers.Number):
+    return interp(self.speed, self._k_p[0], self._k_p[1])
-      kp = self._k_p
-    else:
-      kp = interp(self.speed, self._k_p[0], self._k_p[1])
-
-    return kp
 
   @property
   def k_i(self):
-    if isinstance(self._k_i, numbers.Number):
+    return interp(self.speed, self._k_i[0], self._k_i[1])
-      ki = self._k_i
-    else:
-      ki = interp(self.speed, self._k_i[0], self._k_i[1])
-
-    return ki
 
   def _check_saturation(self, control, override, error):
     saturated = (control < self.neg_limit) or (control > self.pos_limit)