TB205GTI Long PID control (commaai#155)

* PID control, not just PI control

WIP!

* Proper PID control for long, SP parameters

Co-authored-by: Comma Device <device@comma.ai>

selfdrive/car/tesla/PCC_module.py

@@ -6,7 +6,7 @@
 from selfdrive.car.tesla.values import CruiseState, CruiseButtons
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.speed_smoother import speed_smoother
-from selfdrive.controls.lib.planner import calc_cruise_accel_limits
+from selfdrive.controls.lib.planner import calc_cruise_accel_limits, limit_accel_in_turns
 import cereal.messaging as messaging
 import time
 import math
@@ -25,8 +25,8 @@
 MAX_RADAR_DISTANCE = 120. #max distance to take in consideration radar reading
 MAX_PEDAL_VALUE = 112.
 PEDAL_HYST_GAP = 1.0  # don't change pedal command for small oscilalitons within this value
-# Cap the pedal to go from 0 to max in 4 seconds
-PEDAL_MAX_UP = MAX_PEDAL_VALUE * _DT / 4
+# Cap the pedal to go from 0 to max in 2 seconds
+PEDAL_MAX_UP = MAX_PEDAL_VALUE * _DT / 2
 # Cap the pedal to go from max to 0 in 0.4 seconds
 PEDAL_MAX_DOWN = MAX_PEDAL_VALUE * _DT / 0.4
 
@@ -173,26 +173,32 @@ def __init__(self,carcontroller):
     self.params = Params()
     average_speed_over_x_suggestions = 6 # 0.3 seconds (20x a second)
     self.fleet_speed = FleetSpeed(average_speed_over_x_suggestions)
-
-  def load_pid(self):
-    try:
-      v_pid_json = open(V_PID_FILE)
-      data = json.load(v_pid_json)
-      if (self.LoC):
-        if self.LoC.pid:
-          self.LoC.pid.p = data['p']
-          self.LoC.pid.i = data['i']
-          self.LoC.pid.f = data['f']
-      else:
-        print("self.LoC not initialized!")
-    except :
-      print("file not present, creating at next reset")
+
+    def load_pid(self):
+      try:
+        v_pid_json = open(V_PID_FILE)
+        data = json.load(v_pid_json)
+        if (self.LoC):
+          if self.LoC.pid:
+            self.LoC.pid.p = data['p']
+            self.LoC.pid.i = data['i']
+            if 'd' not in data:
+              self.Loc.pid.d = 0.01
+            else:
+              self.LoC.pid.d = data['d']
+            self.LoC.pid.f = data['f']
+        else:
+          print("self.LoC not initialized!")
+      except :
+        print("file not present, creating at next reset")
+
 
     #Helper function for saving the PCC pid constants across drives
   def save_pid(self, pid):
     data = {}
     data['p'] = pid.p
     data['i'] = pid.i
+    data['d'] = pid.d
     data['f'] = pid.f
     try:
       with open(V_PID_FILE , 'w') as outfile :
@@ -359,14 +365,17 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s
 
 
     v_ego = CS.v_ego
+
     following = False
     if self.lead_1:
       following = self.lead_1.status and self.lead_1.dRel < MAX_RADAR_DISTANCE and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0
     accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego,following)]
+
+
     accel_limits[1] *= _accel_limit_multiplier(CS, self.lead_1)
     accel_limits[0] = _decel_limit(accel_limits[0], CS.v_ego, self.lead_1, CS, self.pedal_speed_kph)
     jerk_limits = [min(-0.1, accel_limits[0]/2.), max(0.1, accel_limits[1]/2.)]  # TODO: make a separate lookup for jerk tuning
-    #accel_limits = limit_accel_in_turns(v_ego, CS.angle_steers, accel_limits, CS.CP)
+    accel_limits = limit_accel_in_turns(v_ego, CS.angle_steers, accel_limits, CS.CP)
 
     output_gb = 0
     ####################################################################
@@ -379,7 +388,7 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s
       enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [LongCtrlState.pid, LongCtrlState.stopping]
       # determine if pedal is pressed by human
       self.prev_accelerator_pedal_pressed = self.accelerator_pedal_pressed
-      self.accelerator_pedal_pressed = CS.pedal_interceptor_value > 10
+      self.accelerator_pedal_pressed = CS.pedal_interceptor_value > 5
       #reset PID if we just lifted foot of accelerator
       if (not self.accelerator_pedal_pressed) and self.prev_accelerator_pedal_pressed:
         self.reset(CS.v_ego)
@@ -396,13 +405,12 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s
         # cruise speed can't be negative even if user is distracted
         self.v_pid = max(self.v_pid, 0.)
 
-        jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1, self.v_pid * CV.MS_TO_KPH, self.lead_last_seen_time_ms, CS)
         self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
                                                       self.v_pid,
                                                       accel_limits[1], accel_limits[0],
-                                                      jerk_limits[1], jerk_limits[0], #jerk_max, jerk_min,
+                                                      jerk_limits[1], jerk_limits[0],
                                                       _DT_MPC)
-        
+
         # cruise speed can't be negative even is user is distracted
         self.v_cruise = max(self.v_cruise, 0.)
 
@@ -459,7 +467,7 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s
 
     self.last_output_gb = output_gb
     # accel and brake
-    apply_accel = clip(output_gb, 0., _accel_pedal_max(CS.v_ego, self.v_pid, self.lead_1, self.prev_tesla_accel, CS))
+    apply_accel = clip(output_gb, 0., 1) #_accel_pedal_max(CS.v_ego, self.v_pid, self.lead_1, self.prev_tesla_accel, CS))
     MPC_BRAKE_MULTIPLIER = 6.
     apply_brake = -clip(output_gb * MPC_BRAKE_MULTIPLIER, _brake_pedal_min(CS.v_ego, self.v_pid, self.lead_1, CS, self.pedal_speed_kph), 0.)
 
@@ -674,10 +682,10 @@ def _accel_limit_multiplier(CS, lead):
   if CS.teslaModel in ["SP","SPD"]:
       accel_by_speed = OrderedDict([
         # (speed m/s, decel)
-        (0.,  0.95),  #   0 kmh
-        (10., 0.95),  #  35 kmh
-        (20., 0.925),  #  72 kmh
-        (30., 0.875)]) # 107 kmh
+        (0.,  0.985),  #   0 kmh
+        (10., 0.975),  #  35 kmh
+        (20., 0.95),  #  72 kmh
+        (30., 0.9)]) # 107 kmh
   accel_mult = _interp_map(CS.v_ego, accel_by_speed)
   if _is_present(lead):
     safe_dist_m = _safe_distance_m(CS.v_ego,CS)
@@ -736,35 +744,6 @@ def _decel_limit(accel_min,v_ego, lead, CS, max_speed_kph):
     #BB: if we don't have a lead, don't do full regen to slow down smoother
     return accel_min * 0.5 * max_speed_mult
 
-def _accel_pedal_max(v_ego, v_target, lead, prev_tesla_accel,CS):
-  pedal_max = prev_tesla_accel
-  if _is_present(lead):
-    #we have lead, base on speed and distance
-    safe_dist_m = _safe_distance_m(CS.v_ego,CS)
-    v_rel = lead.vLeadK - v_ego
-    accel_speed_map = OrderedDict([
-      # (speed m/s, decel) change in accel (0..1) per second
-      (0.,  0.01),  #  0 MPH 
-      (1., 0.1),   # 4 MPH
-      (5., 0.15),  # 11 MPH
-      (30., 0.20)]) # 67 MPH
-    accel_distance_map = OrderedDict([
-      # (distance in m, acceleration fraction)
-      (0.6 * safe_dist_m, 0.3),
-      (1.0 * safe_dist_m, 1.0),
-      (3.0 * safe_dist_m, 2.0)])
-    pedal_max = prev_tesla_accel + _interp_map(safe_dist_m, accel_distance_map) * _interp_map(v_rel, accel_speed_map) * _DT
-  else:
-    #no lead, do just based on speed
-    accel_speed_map = OrderedDict([
-      # (speed m/s, decel) change in accel (0..1) per second
-      (0.,  0.25),  #  0 MPH 
-      (10., 0.15),  # 22 MPH
-      (20., 0.12),  # 45 MPH
-      (30., 0.10)]) # 67 MPH
-    pedal_max = prev_tesla_accel +  _interp_map(v_ego, accel_speed_map) * _DT
-  return 1. #pedal_max
-
 def _brake_pedal_min(v_ego, v_target, lead, CS, max_speed_kph):
   #if less than 7 MPH we don't have much left till 5MPH to brake, so full regen
   if v_ego <= 7 * CV.MPH_TO_MS: 
@@ -792,51 +771,4 @@ def _brake_pedal_min(v_ego, v_target, lead, CS, max_speed_kph):
     brake_mult2 = _interp_map(lead.dRel, brake_distance_map)
   brake_mult = max(brake_mult1, brake_mult2)
   return -brake_mult
-
-def _jerk_limits(v_ego, lead, max_speed_kph, lead_last_seen_time_ms, CS):
-  # Allow higher accel jerk at low speed, to get started
-  accel_jerk_by_speed = OrderedDict([
-    # (Speed in m/s, accel jerk)
-    (0, 0.18),
-    (9, 0.10)])
-  accel_jerk = _interp_map(v_ego, accel_jerk_by_speed)
-
-  # prevent high accel jerk near max speed
-  near_max_speed_multipliers = OrderedDict([
-    # (kph under max speed, accel jerk multiplier)
-    (0, 0.01),
-    (4, 1.0)])
-  near_max_speed_multiplier = _interp_map(max_speed_kph - v_ego * CV.MS_TO_KPH, near_max_speed_multipliers)
-  accel_jerk *= near_max_speed_multiplier
 
-  if _is_present(lead):
-    # pick decel jerk based on how much time we have til collision
-    decel_jerk_map = OrderedDict([
-      # (sec to collision, decel jerk)
-      (0, -1.00),
-      (2, -0.25),
-      (4, -0.01),
-      (80, -0.001)])
-    decel_jerk = _interp_map(_sec_til_collision(lead, CS), decel_jerk_map)
-    safe_dist_m = _safe_distance_m(v_ego,CS) 
-    distance_multipliers  = OrderedDict([
-      # (distance in m, accel jerk)
-      (0.8 * safe_dist_m, 0.01),
-      (2.8 * safe_dist_m, 1.00)])
-    distance_multiplier = _interp_map(lead.dRel, distance_multipliers)
-    accel_jerk *= distance_multiplier
-    return decel_jerk, accel_jerk
-  else:
-    # In the absence of a lead car
-    decel_jerk = -0.15
-    # Limit accel jerk if the lead was only recently lost, to prevent
-    # bucking as a lead is intermittently detected.
-    time_since_lead_seen_ms = _current_time_millis() - lead_last_seen_time_ms
-    time_since_lead_seen_multipliers = OrderedDict([
-      # (ms since last lead sighting, accel jerk multiplier)
-      (0,    0.1),
-      (2000, 1.0)])
-    time_since_lead_seen_multiplier = _interp_map(time_since_lead_seen_ms, time_since_lead_seen_multipliers)
-    accel_jerk *= time_since_lead_seen_multiplier
-
-    return decel_jerk, accel_jerk

selfdrive/car/tesla/interface.py

@@ -9,8 +9,6 @@
 from common.params import read_db
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
 from selfdrive.car.tesla.readconfig import CarSettings
-import cereal.messaging as messaging
-from cereal.services import service_list
 from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V
 from selfdrive.car.interfaces import CarInterfaceBase
 
@@ -149,31 +147,31 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
 
       # Kp and Ki for the longitudinal control
       if teslaModel == "S":
-        ret.longitudinalTuning.kpBP = [0., 5., 35.]
-        ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4]
-        ret.longitudinalTuning.kiBP = [0., 5., 35.]
-        ret.longitudinalTuning.kiV = [0.01,0.01,0.01]
+        ret.longitudinalTuning.kpBP = [0., 5., 22.,  35.]
+        ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4, 0.4]
+        ret.longitudinalTuning.kiBP = [0., 5., 22., 35.]
+        ret.longitudinalTuning.kiV = [0.01,0.01,0.01,0.01]
       elif teslaModel == "SP":
-        ret.longitudinalTuning.kpBP = [0., 5., 35.]
-        ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325]
-        ret.longitudinalTuning.kiBP = [0., 5., 35.]
-        ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725]
+        ret.longitudinalTuning.kpBP = [0., 5., 22., 35.] # 0km/h, 18 km/h, 80, 128km/h
+        ret.longitudinalTuning.kiBP = [0., 5., 22., 35.]
+        ret.longitudinalTuning.kpV = [0.3, 0.3, 0.35, 0.37]
+        ret.longitudinalTuning.kiV = [0.07, 0.07, 0.093, 0.092]
       elif teslaModel == "SD":
-        ret.longitudinalTuning.kpBP = [0., 5., 35.]
-        ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4]
-        ret.longitudinalTuning.kiBP = [0., 5., 35.]
-        ret.longitudinalTuning.kiV = [0.01,0.01,0.01]
+        ret.longitudinalTuning.kpBP = [0., 5., 22., 35.]
+        ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4,0.4]
+        ret.longitudinalTuning.kiBP = [0., 5., 22., 35.]
+        ret.longitudinalTuning.kiV = [0.01,0.01,0.01,0.01]
       elif teslaModel == "SPD":
-        ret.longitudinalTuning.kpBP = [0., 5., 35.]
-        ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325]
-        ret.longitudinalTuning.kiBP = [0., 5., 35.]
-        ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725]
+        ret.longitudinalTuning.kpBP = [0., 5., 22., 35.]
+        ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325, 0.325]
+        ret.longitudinalTuning.kiBP = [0., 5., 22.,35.]
+        ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725, 0.00725]
       else:
         #use S numbers if we can't match anything
-        ret.longitudinalTuning.kpBP = [0., 5., 35.]
-        ret.longitudinalTuning.kpV = [0.375, 0.325, 0.3]
-        ret.longitudinalTuning.kiBP = [0., 5., 35.]
-        ret.longitudinalTuning.kiV = [0.08,0.08,0.08]
+        ret.longitudinalTuning.kpBP = [0., 5., 22., 35.]
+        ret.longitudinalTuning.kpV = [0.375, 0.325, 0.3, 0.3]
+        ret.longitudinalTuning.kiBP = [0., 5., 22., 35.]
+        ret.longitudinalTuning.kiV = [0.08,0.08,0.08, 0.08]
 
 
     else:
@@ -207,13 +205,13 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.steerMaxBP = [0.,15.]  # m/s
     ret.steerMaxV = [420.,420.]   # max steer allowed
 
-    ret.gasMaxBP = [0.]  # m/s
-    ret.gasMaxV = [0.3] #if ret.enableGasInterceptor else [0.] # max gas allowed
-    ret.brakeMaxBP = [0., 20.]  # m/s
-    ret.brakeMaxV = [1., 1.]   # max brake allowed - BB: since we are using regen, make this even
+    ret.gasMaxBP = [0., 20.]  # m/s
+    ret.gasMaxV = [0.225, 0.525] #if ret.enableGasInterceptor else [0.] # max gas allowed
+    ret.brakeMaxBP = [0.]  # m/s
+    ret.brakeMaxV = [1.]   # max brake allowed - BB: since we are using regen, make this even
 
-    ret.longitudinalTuning.deadzoneBP = [0., 9.] #BB: added from Toyota to start pedal work; need to tune
-    ret.longitudinalTuning.deadzoneV = [0., 0.] #BB: added from Toyota to start pedal work; need to tune; changed to 0 for now
+    ret.longitudinalTuning.deadzoneBP = [0.] #BB: added from Toyota to start pedal work; need to tune
+    ret.longitudinalTuning.deadzoneV = [0.] #BB: added from Toyota to start pedal work; need to tune; changed to 0 for now
 
     ret.stoppingControl = True
     ret.openpilotLongitudinalControl = True

selfdrive/controls/lib/longcontrol.py

@@ -1,6 +1,6 @@
 from cereal import log
 from common.numpy_fast import clip, interp
-from selfdrive.controls.lib.pid import PIController
+from selfdrive.controls.lib.pid_real