Smoother longitudinal

common/op_params.py

@@ -100,6 +100,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\n'
                                                                                 'If you have a comma three, note that the default camera offset is -0.04!', live=True),
+                        'long_kd': Param(0.5, NUMBER, live=True),
                         'dynamic_follow': Param('stock', str, static=True, hidden=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'

selfdrive/car/toyota/interface.py

@@ -392,15 +392,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
     # intercepting the DSU is a community feature since it requires unofficial hardware
     ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu
 
-    if ret.enableGasInterceptor:
-      # Transitions from original pedal tuning at MIN_ACC_SPEED to default tuning at MIN_ACC_SPEED + hysteresis gap
-      ret.gasMaxBP = [0., MIN_ACC_SPEED]
-      ret.gasMaxV = [0.2, 0.5]
-      ret.longitudinalTuning.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
-      ret.longitudinalTuning.kpV = [1.2, 0.8, 0.765, 2.255, 1.5]
-      ret.longitudinalTuning.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
-      ret.longitudinalTuning.kiV = [0.18, 0.165, 0.489, 0.36]
-    elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]:
+    if candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2] and not ret.enableGasInterceptor:
       # Improved longitudinal tune
       ret.longitudinalTuning.deadzoneBP = [0., 8.05]
       ret.longitudinalTuning.deadzoneV = [.0, .14]
@@ -419,6 +411,11 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       ret.longitudinalTuning.kiBP = [0., 35.]
       ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
       ret.longitudinalTuning.kiV = [0.54, 0.36]
+      ret.longitudinalTuning.kdV = [0.5, 1.2, 2.5]  # TODO: why is this increasing?
+
+      if ret.enableGasInterceptor:
+        # Default longitudinal tune with tweaks for pedal
+        ret.longitudinalTuning.kpV = [3.6 * 0.92, 2.4 * 0.95, 1.5]
 
     return ret
 

selfdrive/controls/controlsd.py

@@ -668,7 +668,7 @@ def publish_logs(self, CS, start_time, actuators, lac_log):
     controlsState.vPid = float(self.LoC.v_pid)
     controlsState.vCruise = float(self.v_cruise_kph)
     controlsState.upAccelCmd = float(self.LoC.pid.p)
-    controlsState.uiAccelCmd = float(self.LoC.pid.id)
+    controlsState.uiAccelCmd = float(self.LoC.pid.i)
     controlsState.ufAccelCmd = float(self.LoC.pid.f)
     controlsState.cumLagMs = -self.rk.remaining * 1000.
     controlsState.startMonoTime = int(start_time * 1e9)

selfdrive/controls/lib/latcontrol_pid.py

@@ -1,16 +1,16 @@
 import math
 
-from selfdrive.controls.lib.pid import LatPIDController
+from selfdrive.controls.lib.pid import PIDController
 from selfdrive.controls.lib.drive_helpers import get_steer_max
 from cereal import log
 
 
 class LatControlPID():
   def __init__(self, CP):
-    self.pid = LatPIDController((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, sat_limit=CP.steerLimitTimer)
+    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, sat_limit=CP.steerLimitTimer, derivative_period=5)
     self.new_kf_tuned = CP.lateralTuning.pid.newKfTuned
 
   def reset(self):

selfdrive/controls/lib/longcontrol.py

@@ -2,7 +2,7 @@
 from common.numpy_fast import clip, interp
 from selfdrive.controls.lib.drive_helpers import CONTROL_N
 from selfdrive.modeld.constants import T_IDXS
-from selfdrive.controls.lib.pid import LongPIDController
+from selfdrive.controls.lib.pid import PIDController
 from selfdrive.controls.lib.dynamic_gas import DynamicGas
 from common.op_params import opParams
 
@@ -58,15 +58,13 @@ 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
-    # kdBP = [0., 16., 35.]
-    # kdV = [0.08, 1.215, 2.51]
-
-    self.pid = LongPIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
-                                 (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
-                                 ([0], [0]),
-                                 rate=RATE,
-                                 sat_limit=0.8,
-                                 convert=compute_gb)
+    self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
+                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
+                             (CP.longitudinalTuning.kdBP, CP.longitudinalTuning.kdV),
+                             rate=RATE,
+                             sat_limit=0.8,
+                             derivative_period=100,  # 1 second
+                             convert=compute_gb)
     self.v_pid = 0.0
     self.last_output_gb = 0.0
 
@@ -86,7 +84,9 @@ def update(self, active, CS, CP, long_plan, extras):
       accel_delay = interp(CS.vEgo, [4.4704, 35.7632], [0.15, 0.3])  # 10 to 80 mph
       v_target = interp(accel_delay, T_IDXS[:CONTROL_N], long_plan.speeds)
       v_target_future = long_plan.speeds[-1]
-      a_target = interp(accel_delay, T_IDXS[:CONTROL_N], long_plan.accels)
+      print('old a_target: {}'.format(round(interp(accel_delay, T_IDXS[:CONTROL_N], long_plan.accels), 2)))
+      a_target = 2 * (v_target - long_plan.speeds[0]) / DEFAULT_LONG_LAG - long_plan.accels[0]
+      print('new a_target: {}'.format(round(a_target, 2)))
     else:
       v_target = 0.0
       v_target_future = 0.0

selfdrive/controls/lib/pid.py

@@ -13,8 +13,9 @@ def apply_deadzone(error, deadzone):
     error = 0.
   return error
 
-class LatPIDController():
-  def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
+class PIDController:
+  def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, derivative_period=100, convert=None):
+    self.op_params = opParams()
     self._k_p = k_p  # proportional gain
     self._k_i = k_i  # integral gain
     self._k_d = k_d  # derivative gain
@@ -27,6 +28,8 @@ def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=1
     self.i_unwind_rate = 0.3 / rate
     self.i_rate = 1.0 / rate
     self.sat_limit = sat_limit
+    self.derivative_period = derivative_period  # frames in time for derivative calculation
+    assert self.derivative_period >= 0
     self.convert = convert
 
     self.reset()
@@ -41,6 +44,7 @@ def k_i(self):
 
   @property
   def k_d(self):
+    return self.op_params.get('long_kd')
     return interp(self.speed, self._k_d[0], self._k_d[1])
 
   def _check_saturation(self, control, check_saturation, error):
@@ -72,8 +76,8 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri
     self.f = feedforward * self.k_f
 
     d = 0
-    if len(self.errors) >= 5:  # makes sure list is long enough
-      d = (error - self.errors[-5]) / 5  # get deriv in terms of 100hz (tune scale doesn't change)
+    if len(self.errors) >= self.derivative_period:  # makes sure list is long enough
+      d = (error - self.errors[-self.derivative_period]) / self.derivative_period  # get deriv in terms of 100hz (tune scale doesn't change)
       d *= self.k_d
 
     if override:
@@ -99,106 +103,8 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri
     self.saturated = self._check_saturation(control, check_saturation, error)
 
     self.errors.append(float(error))
-    while len(self.errors) > 5:
+    while len(self.errors) > self.derivative_period:
       self.errors.pop(0)
 
     self.control = clip(control, self.neg_limit, self.pos_limit)
     return self.control
-
-
-class LongPIDController:
-  def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
-    self.op_params = opParams()
-    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
-
-    self.max_accel_d = 0.4 * CV.MPH_TO_MS
-
-    self.pos_limit = pos_limit
-    self.neg_limit = neg_limit
-
-    self.sat_count_rate = 1.0 / rate
-    self.i_unwind_rate = 0.3 / rate
-    self.rate = 1.0 / rate
-    self.sat_limit = sat_limit
-    self.convert = convert
-
-    self.reset()
-
-  @property
-  def k_p(self):
-    return interp(self.speed, self._k_p[0], self._k_p[1])
-
-  @property
-  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)
-
-    if saturated and check_saturation and abs(error) > 0.1:
-      self.sat_count += self.sat_count_rate
-    else:
-      self.sat_count -= self.sat_count_rate
-
-    self.sat_count = clip(self.sat_count, 0.0, 1.0)
-
-    return self.sat_count > self.sat_limit
-
-  def reset(self):
-    self.p = 0.0
-    self.id = 0.0
-    self.f = 0.0
-    self.sat_count = 0.0
-    self.saturated = False
-    self.control = 0
-    self.last_setpoint = 0.0
-    self.last_error = 0.0
-
-  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
-
-    if override:
-      self.id -= self.i_unwind_rate * float(np.sign(self.id))
-    else:
-      i = self.id + error * self.k_i * self.rate
-      control = self.p + self.f + i
-
-      if self.convert is not None:
-        control = self.convert(control, speed=self.speed)
-
-      # Update when changing i will move the control away from the limits
-      # or when i will move towards the sign of the error
-      if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or \
-          (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \
-              not freeze_integrator:
-        self.id = i
-
-    # if self.op_params.get('enable_long_derivative'):
-    #   if abs(setpoint - self.last_setpoint) / self.rate < self.max_accel_d:  # if setpoint isn't changing much
-    #     d = self.k_d * (error - self.last_error)
-    #     if (self.id > 0 and self.id + d >= 0) or (self.id < 0 and self.id + d <= 0):  # if changing integral doesn't make it cross zero
-    #       self.id += d
-
-    control = self.p + self.f + self.id
-    if self.convert is not None:
-      control = self.convert(control, speed=self.speed)
-
-    self.saturated = self._check_saturation(control, check_saturation, error)
-
-    self.last_setpoint = float(setpoint)
-    self.last_error = float(error)
-
-    self.control = clip(control, self.neg_limit, self.pos_limit)
-    return self.control