Volt: Perfect lateral and 98% perfect longitudinal

cereal/car.capnp

@@ -441,16 +441,21 @@ struct CarParams {
     kpV @1 :List(Float32);
     kiBP @2 :List(Float32);
     kiV @3 :List(Float32);
-    kf @4 :Float32;
+    kdBP @4 :List(Float32) = [0.];
+    kdV @5 :List(Float32) = [0.];
+    kf @6 :Float32;
   }
 
   struct LongitudinalPIDTuning {
     kpBP @0 :List(Float32);
     kpV @1 :List(Float32);
     kiBP @2 :List(Float32);
     kiV @3 :List(Float32);
-    deadzoneBP @4 :List(Float32);
-    deadzoneV @5 :List(Float32);
+    kdBP @4 :List(Float32) = [0.];
+    kdV @5 :List(Float32) = [0.];
+    kf @8 :Float32;
+    deadzoneBP @6 :List(Float32);
+    deadzoneV @7 :List(Float32);
   }
 
   struct LateralINDITuning {

selfdrive/car/gm/interface.py

@@ -1,4 +1,6 @@
 #!/usr/bin/env python3
+
+from math import fabs, sin
 from cereal import car
 from common.numpy_fast import interp
 from selfdrive.config import Conversions as CV
@@ -58,13 +60,35 @@ def calc_accel_override(a_ego, a_target, v_ego, v_target):
 
     return float(max(max_accel, a_target / FOLLOW_AGGRESSION)) * min(speedLimiter, accelLimiter)
 
+  # Volt determined by iteratively plotting and minimizing error for f(angle, speed) = steer.
+  @staticmethod
+  def get_steer_feedforward_volt(desired_angle, v_ego):
+    # maps [-inf,inf] to [-1,1]: sigmoid(34.4 deg) = sigmoid(1) = 0.5
+    # 1 / 0.02904609 = 34.4 deg ~= 36 deg ~= 1/10 circle? Arbitrary?
+    desired_angle *= 0.02904609
+    sigmoid = desired_angle / (1 + fabs(desired_angle))
+    return 0.10006696 * sigmoid * (v_ego + 3.12485927)
+
+  def get_steer_feedforward_function(self):
+    if self.CP.carFingerprint == CAR.VOLT:
+      return self.get_steer_feedforward_volt
+    else:
+      return CarInterfaceBase.get_steer_feedforward_default
+
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
     ret.carName = "gm"
     ret.safetyModel = car.CarParams.SafetyModel.gm
     ret.enableCruise = False  # stock cruise control is kept off
 
+
+    ret.stoppingControl = True
+    ret.startAccel = 0.8
+
+    ret.steerLimitTimer = 0.4
+    ret.radarTimeStep = 1/15  # GM radar runs at 15Hz instead of standard 20Hz
+
     # GM port is a community feature
     # TODO: make a port that uses a car harness and it only intercepts the camera
     ret.communityFeature = True
@@ -84,14 +108,37 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.steerRateCost = 1.0
     ret.steerActuatorDelay = 0.1  # Default delay, not measured yet
 
+    ret.longitudinalTuning.kpBP = [5., 35.]
+    ret.longitudinalTuning.kpV = [2.4, 1.5]
+    ret.longitudinalTuning.kiBP = [0.]
+    ret.longitudinalTuning.kiV = [0.36]
+
     if candidate == CAR.VOLT:
       # supports stop and go, but initial engage must be above 18mph (which include conservatism)
       ret.minEnableSpeed = -1
       ret.mass = 1607. + STD_CARGO_KG
       ret.wheelbase = 2.69
-      ret.steerRatio = 15.7
+      ret.steerRatio = 17.7  # Stock 15.7, LiveParameters
+      ret.steerRateCost = 1.0
+      tire_stiffness_factor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters
       ret.steerRatioRear = 0.
-      ret.centerToFront = ret.wheelbase * 0.4  # wild guess
+      ret.centerToFront = 0.45 * ret.wheelbase # from Volt Gen 1
+
+      ret.lateralTuning.pid.kpBP = [0., 40.]
+      ret.lateralTuning.pid.kpV = [0.0, .20]
+      ret.lateralTuning.pid.kiBP = [0.0, 40.]
+      ret.lateralTuning.pid.kiV = [0.025, 0.022]
+      ret.lateralTuning.pid.kdBP = [i * CV.MPH_TO_MS for i in [15., 30., 55.]]
+      ret.lateralTuning.pid.kdV = [0.15, 0.26, 0.32]
+      ret.lateralTuning.pid.kf = 1. # !!! ONLY for sigmoid feedforward !!!
+      ret.steerActuatorDelay = 0.2
+
+      # Only tuned to reduce oscillations. TODO.
+      ret.longitudinalTuning.kpV = [1.7, 1.3]
+      ret.longitudinalTuning.kiBP = [5., 35.]
+      ret.longitudinalTuning.kiV = [0.32, 0.34]
+      ret.longitudinalTuning.kdV = [0.8, 0.2]
+      ret.longitudinalTuning.kdBP = [5., 25.]
 
     elif candidate == CAR.MALIBU:
       # supports stop and go, but initial engage must be above 18mph (which include conservatism)
@@ -144,17 +191,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
                                                                          tire_stiffness_factor=tire_stiffness_factor)
 
-    ret.longitudinalTuning.kpBP = [5., 35.]
-    ret.longitudinalTuning.kpV = [2.4, 1.5]
-    ret.longitudinalTuning.kiBP = [0.]
-    ret.longitudinalTuning.kiV = [0.36]
-
-    ret.stoppingControl = True
-    ret.startAccel = 0.8
-
-    ret.steerLimitTimer = 0.4
-    ret.radarTimeStep = 0.0667  # GM radar runs at 15Hz instead of standard 20Hz
-
     return ret
 
   # returns a car.CarState

selfdrive/car/interfaces.py

@@ -47,6 +47,16 @@ def compute_gb(accel, speed):
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
     raise NotImplementedError
+
+  @staticmethod
+  def get_steer_feedforward_default(desired_angle, v_ego):
+    # Proportional to realigning tire momentum: lateral acceleration.
+    # TODO: something with lateralPlan.curvatureRates
+    return desired_angle * (v_ego**2)
+
+  @staticmethod
+  def get_steer_feedforward_function():
+    return CarInterfaceBase.get_steer_feedforward_default
 
   # returns a set of default params to avoid repetition in car specific params
   @staticmethod

selfdrive/controls/controlsd.py

@@ -111,7 +111,7 @@ def __init__(self, sm=None, pm=None, can_sock=None):
     self.VM = VehicleModel(self.CP)
 
     if self.CP.lateralTuning.which() == 'pid':
-      self.LaC = LatControlPID(self.CP)
+      self.LaC = LatControlPID(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'indi':
       self.LaC = LatControlINDI(self.CP)
     elif self.CP.lateralTuning.which() == 'lqr':

selfdrive/controls/lib/latcontrol_pid.py

@@ -1,39 +1,48 @@
-from selfdrive.controls.lib.pid import PIController
+from selfdrive.controls.lib.pid import PIDController
 from selfdrive.controls.lib.drive_helpers import get_steer_max
+from selfdrive.config import Conversions as CV
 from cereal import car
 from cereal import log
 from selfdrive.kegman_conf import kegman_conf
 
 
 class LatControlPID():
-  def __init__(self, CP):
+  def __init__(self, CP, CI):
     self.kegman = kegman_conf(CP)
+    self.CI = CI
     self.deadzone = float(self.kegman.conf['deadzone'])
-    self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
+    self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
                             (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
+                            (CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
                             k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0,
-                            sat_limit=CP.steerLimitTimer)
+                            sat_limit=CP.steerLimitTimer, derivative_period=0.1)
+    self.get_steer_feedforward = CI.get_steer_feedforward_function()
     self.angle_steers_des = 0.
     self.mpc_frame = 0
 
   def reset(self):
     self.pid.reset()
 
   def live_tune(self, CP):
-    self.mpc_frame += 1
-    if self.mpc_frame % 300 == 0:
-      # live tuning through /data/openpilot/tune.py overrides interface.py settings
-      self.kegman = kegman_conf()
-      if self.kegman.conf['tuneGernby'] == "1":
-        self.steerKpV = [float(self.kegman.conf['Kp'])]
-        self.steerKiV = [float(self.kegman.conf['Ki'])]
-        self.steerKf = float(self.kegman.conf['Kf'])
-        self.pid = PIController((CP.lateralTuning.pid.kpBP, self.steerKpV),
-                            (CP.lateralTuning.pid.kiBP, self.steerKiV),
-                            k_f=self.steerKf, pos_limit=1.0)
-        self.deadzone = float(self.kegman.conf['deadzone'])
+    if self.get_steer_feedforward == self.CI.get_steer_feedforward_default:
+      self.mpc_frame += 1
+      if self.mpc_frame % 300 == 0:
+        # live tuning through /data/openpilot/tune.py overrides interface.py settings
+        self.kegman = kegman_conf()
+        if self.kegman.conf['tuneGernby'] == "1":
+          self.steerKpV = [float(self.kegman.conf['Kp'])]
+          self.steerKiV = [float(self.kegman.conf['Ki'])]
+          self.steerKdV = [float(self.kegman.conf['Kd'])]
+            # custom feedforward values are not allowed to be tuned.
+          self.steerKf = float(self.kegman.conf['Kf'])
+          self.pid = PIDController((CP.lateralTuning.pid.kpBP, self.steerKpV),
+                                  (CP.lateralTuning.pid.kiBP, self.steerKiV),
+                                  (CP.lateralTuning.pid.kdBP, self.steerKdV),
+                                  k_f=self.steerKf, pos_limit=self.pid.pos_limit, neg_limit=self.pid.neg_limit,
+                                  sat_limit=CP.steerLimitTimer, derivative_period=0.1)
+          self.deadzone = float(self.kegman.conf['deadzone'])
 
-      self.mpc_frame = 0    
+        self.mpc_frame = 0    
 
 
   def update(self, active, CS, CP, lat_plan):
@@ -52,11 +61,16 @@ def update(self, active, CS, CP, lat_plan):
       steers_max = get_steer_max(CP, CS.vEgo)
       self.pid.pos_limit = steers_max
       self.pid.neg_limit = -steers_max
-      steer_feedforward = self.angle_steers_des   # feedforward desired angle
-      if CP.steerControlType == car.CarParams.SteerControlType.torque:
-        # TODO: feedforward something based on lat_plan.rateSteers
-        steer_feedforward -= lat_plan.angleOffsetDeg # subtract the offset, since it does not contribute to resistive torque
-        steer_feedforward *= CS.vEgo**2  # proportional to realigning tire momentum (~ lateral accel)
+
+      steer_feedforward = self.get_steer_feedforward(self.angle_steers_des, CS.vEgo)
+
+      # torque for steer rate. ~0 angle, steer rate ~= steer command.
+      steer_rate_actual = CS.steeringRateDeg
+      steer_rate_desired = lat_plan.lat_plan.steeringAngleDeg
+      speed_mph =  CS.vEgo * CV.MS_TO_MPH
+      steer_rate_max = 0.0389837 * speed_mph**2 - 5.34858 * speed_mph + 223.831
+
+      steer_feedforward += ((steer_rate_desired - steer_rate_actual) / steer_rate_max)
 
       deadzone = self.deadzone    
 

selfdrive/controls/lib/longcontrol.py

@@ -1,7 +1,7 @@
 from cereal import log
 from common.numpy_fast import clip, interp
 from common.params import Params
-from selfdrive.controls.lib.pid import PIController
+from selfdrive.controls.lib.pid import PIDController
 from selfdrive.kegman_conf import kegman_conf
 
 kegman = kegman_conf()
@@ -59,7 +59,7 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
 class LongControl():
   def __init__(self, CP, compute_gb):
     self.long_control_state = LongCtrlState.off  # initialized to off
-    self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
+    self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
                             rate=RATE,
                             sat_limit=0.8,

selfdrive/controls/lib/pid.py

@@ -1,4 +1,5 @@
 import numpy as np
+from numbers import Number
 from common.numpy_fast import clip, interp
 
 def apply_deadzone(error, deadzone):
@@ -10,11 +11,18 @@ def apply_deadzone(error, deadzone):
     error = 0.
   return error
 
-class PIController():
-  def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
+class PIDController():
+  def __init__(self, k_p=0., k_i=0., k_d=0., k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None, derivative_period=1.):
     self._k_p = k_p  # proportional gain
     self._k_i = k_i  # integral gain
+    self._k_d = k_d  # derivative gain
     self.k_f = k_f  # feedforward gain
+    if isinstance(self._k_p, Number):
+      self._k_p = [[0], [self._k_p]]
+    if isinstance(self._k_i, Number):
+      self._k_i = [[0], [self._k_i]]
+    if isinstance(self._k_d, Number):
+      self._k_d = [[0], [self._k_d]]
 
     self.pos_limit = pos_limit
     self.neg_limit = neg_limit
@@ -25,6 +33,9 @@ def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, s
     self.sat_limit = sat_limit
     self.convert = convert
 
+    self._d_period = round(derivative_period * rate)  # period of time for derivative calculation (seconds converted to frames)
+    self._d_period_recip = 1. / self._d_period
+
     self.reset()
 
   @property
@@ -35,6 +46,10 @@ def k_p(self):
   def k_i(self):
     return interp(self.speed, self._k_i[0], self._k_i[1])
 
+  @property
+  def k_d(self):
+    return interp(self.speed, self._k_d[0], self._k_d[1])
+
   def _check_saturation(self, control, check_saturation, error):
     saturated = (control < self.neg_limit) or (control > self.pos_limit)
 
@@ -54,13 +69,20 @@ def reset(self):
     self.sat_count = 0.0
     self.saturated = False
     self.control = 0
+    self.errors = []
 
   def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False):
     self.speed = speed
 
     error = float(apply_deadzone(setpoint - measurement, deadzone))
     self.p = error * self.k_p
     self.f = feedforward * self.k_f
+
+    kd = self.k_d
+    d = 0
+    if len(self.errors) >= self._d_period and kd > 0.:  # makes sure we have enough history for period
+      d = (error - self.errors[-self._d_period]) * self._d_period_recip  # get deriv in terms of 100hz (tune scale doesn't change)
+      d *= kd
 
     if override:
       self.i -= self.i_unwind_rate * float(np.sign(self.i))
@@ -78,11 +100,15 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri
          not freeze_integrator:
         self.i = i
 
-    control = self.p + self.f + self.i
+    control = self.p + self.f + self.i + d
     if self.convert is not None:
       control = self.convert(control, speed=self.speed)
 
     self.saturated = self._check_saturation(control, check_saturation, error)
 
+    self.errors.append(float(error))
+    while len(self.errors) > self._d_period:
+      self.errors.pop(0)
+
     self.control = clip(control, self.neg_limit, self.pos_limit)
     return self.control

selfdrive/kegman_conf.py

@@ -21,6 +21,9 @@ def init_config(self, CP):
       if self.conf['Ki'] == "-1":
         self.conf['Ki'] = str(round(CP.lateralTuning.pid.kiV[0],3))
         write_conf = True
+      if self.conf.get('Kd',"-1") == "-1":
+        self.conf['Kd'] = str(round(CP.lateralTuning.pid.kdV[0],3))
+        write_conf = True
       if self.conf['Kf'] == "-1":
         self.conf['Kf'] = str('{:f}'.format(CP.lateralTuning.pid.kf))
         write_conf = True
@@ -53,6 +56,7 @@ def read_config(self):
         self.config.update({"tuneGernby":"1"})
         self.config.update({"Kp":"-1"})
         self.config.update({"Ki":"-1"})
+        self.config.update({"Kd":"-1"})
         self.element_updated = True
 
       if "liveParams" not in self.config:
@@ -150,7 +154,7 @@ def read_config(self):
       self.config = {"cameraOffset":"0.06", "lastTrMode":"1", "battChargeMin":"60", "battChargeMax":"70", \
                      "wheelTouchSeconds":"180", "accelerationMode":"1","battPercOff":"25", "carVoltageMinEonShutdown":"11800", \
                      "brakeStoppingTarget":"0.25", "tuneGernby":"1", "AutoHold":"0",\
-                     "Kp":"-1", "Ki":"-1", "liveParams":"1", "leadDistance":"5", "deadzone":"0.0", \
+                     "Kp":"-1", "Ki":"-1", "Kd":"-1", "liveParams":"1", "leadDistance":"5", "deadzone":"0.0", \
                      "1barBP0":"-0.1", "1barBP1":"2.25", "2barBP0":"-0.1", "2barBP1":"2.5", "3barBP0":"0.0", \
                      "3barBP1":"3.0", "1barMax":"2.1", "2barMax":"2.1", "3barMax":"2.1", \
                      "1barHwy":"0.4", "2barHwy":"0.3", "3barHwy":"0.1", \