LatControlTorque: add derivative

common/op_params.py

@@ -99,6 +99,7 @@ def __init__(self):
 
     self.fork_params = {
       # 'camera_offset': Param(-0.04 if TICI else 0.06, NUMBER, 'Your camera offset to use in lane_planner.py, live=True),
+      'torque_derivative': Param(5.0, NUMBER, 'Derivative used in the lateral torque controller', live=True),
       'global_df_mod': Param(1.0, NUMBER, 'The multiplier for the current distance used by dynamic follow. The range is limited from 0.85 to 2.5\n'
                                           'Smaller values will get you closer, larger will get you farther\n'
                                           'This is multiplied by any profile that\'s active. Set to 1. to disable', live=True),

selfdrive/car/toyota/tunes.py

@@ -53,13 +53,14 @@ def set_long_tune(tune, name):
 
 
 ###### LAT ######
-def set_lat_tune(tune, name, params, MAX_LAT_ACCEL=2.5, FRICTION=.1, kif=(1.0, 0.1, 1.0)):
+def set_lat_tune(tune, name, params, MAX_LAT_ACCEL=2.5, FRICTION=.1):
   if name == LatTunes.TORQUE:
     tune.init('torque')
     tune.torque.useSteeringAngle = True
-    tune.torque.kp = kif[0] / MAX_LAT_ACCEL
-    tune.torque.kf = kif[2] / MAX_LAT_ACCEL
-    tune.torque.ki = kif[1] / MAX_LAT_ACCEL
+    tune.torque.kp = 1.0 / MAX_LAT_ACCEL
+    tune.torque.kf = 1.0 / MAX_LAT_ACCEL
+    tune.torque.ki = 0.1 / MAX_LAT_ACCEL
+    tune.torque.kd = 5.0 / MAX_LAT_ACCEL
     tune.torque.friction = FRICTION
 
   elif name == LatTunes.INDI_PRIUS:

selfdrive/controls/lib/latcontrol_torque.py

@@ -1,3 +1,4 @@
+from collections import deque
 import math
 
 from cereal import log
@@ -6,6 +7,8 @@
 from selfdrive.controls.lib.pid import PIDController
 from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
 
+from common.op_params import opParams
+
 # At higher speeds (25+mph) we can assume:
 # Lateral acceleration achieved by a specific car correlates to
 # torque applied to the steering rack. It does not correlate to
@@ -31,6 +34,8 @@ def __init__(self, CP, CI):
     self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
     self.friction = CP.lateralTuning.torque.friction
     self.kf = CP.lateralTuning.torque.kf
+    self.op_params = opParams()
+    self.errors = deque([0] * 10, maxlen=10)  # 10 frames
 
   def reset(self):
     super().reset()
@@ -55,19 +60,25 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi
 
       setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature
       measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature
+
       error = setpoint - measurement
-      pid_log.error = error
+      error_rate = (error - self.errors[0]) / len(self.errors)
+      self.errors.append(error)
+      # live tune for now
+      self.pid.k_d = self.op_params.get('torque_derivative')
 
       ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
       # convert friction into lateral accel units for feedforward
       friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction])
       ff += friction_compensation / self.kf
-      output_torque = self.pid.update(error,
+      output_torque = self.pid.update(error, error_rate=error_rate,
                                       override=CS.steeringPressed, feedforward=ff,
                                       speed=CS.vEgo,
                                       freeze_integrator=CS.steeringRateLimited)
 
       pid_log.active = True
+      pid_log.error = error
+      pid_log.errorRate = error_rate
       pid_log.p = self.pid.p
       pid_log.i = self.pid.i
       pid_log.d = self.pid.d

selfdrive/controls/lib/pid.py

@@ -16,6 +16,7 @@ def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308,
       self._k_i = [[0], [self._k_i]]
     if isinstance(self._k_d, Number):
       self._k_d = [[0], [self._k_d]]
+    self.k_d = 0.
 
     self.pos_limit = pos_limit
     self.neg_limit = neg_limit
@@ -34,9 +35,9 @@ 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])
+  # @property
+  # def k_d(self):
+  #   return interp(self.speed, self._k_d[0], self._k_d[1])
 
   @property
   def error_integral(self):