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;
-  steerKi @16 :Float32;
+  steerKpBP @42 :List(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 @@ def get_params(candidate, fingerprint):
     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 @@ def get_params(candidate, fingerprint):
       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 @@ def get_params(candidate, fingerprint):
       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 @@ def get_params(candidate, fingerprint):
       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 @@ def get_params(candidate, fingerprint):
       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 @@ def get_params(candidate, fingerprint):
       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 @@ def get_params(candidate, fingerprint):
       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,36 +72,37 @@ def get_params(candidate, fingerprint):
     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]:
       ret.safetyParam = 73  # see conversion factor for STEER_TORQUE_EPS in dbc file
       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:
       ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file
       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:
       ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file
       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 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs
       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 @@ def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, s
 
   @property
   def k_p(self):
-    if isinstance(self._k_p, numbers.Number):
-      kp = self._k_p
-    else:
-      kp = interp(self.speed, self._k_p[0], self._k_p[1])
-
-    return kp
+    return interp(self.speed, self._k_p[0], self._k_p[1])
 
   @property
   def k_i(self):
-    if isinstance(self._k_i, numbers.Number):
-      ki = self._k_i
-    else:
-      ki = interp(self.speed, self._k_i[0], self._k_i[1])
-
-    return ki
+    return interp(self.speed, self._k_i[0], self._k_i[1])
 
   def _check_saturation(self, control, override, error):
     saturated = (control < self.neg_limit) or (control > self.pos_limit)