Tb205gti pid long

selfdrive/car/tesla/PCC_module.py

@@ -26,7 +26,7 @@
 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
+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
 
@@ -182,6 +182,11 @@ def load_pid(self):
         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!")
@@ -193,6 +198,7 @@ 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 :
@@ -662,17 +668,17 @@ def _accel_limit_multiplier(CS, lead):
   with other accel limits."""
   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
   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.0)]) # 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)

selfdrive/car/tesla/carcontroller.py

@@ -579,8 +579,8 @@ def update(self, enabled, CS, frame, actuators, \
       if enable_steer_control and op_status == 3:
         op_status = 0x5
       park_brake_request = int(CS.ahbEnabled)
-      if park_brake_request == 1:
-        print("Park Brake Request received")
+#      if park_brake_request == 1:
+#        print("Park Brake Request received")
       adaptive_cruise = 1 if (not self.PCC.pcc_available and self.ACC.adaptive) or self.PCC.pcc_available else 0
       can_sends.append(teslacan.create_fake_DAS_msg(speed_control_enabled,speed_override,self.DAS_206_apUnavailable, collision_warning, op_status, \
             acc_speed_kph, \

selfdrive/car/tesla/interface.py

@@ -146,31 +146,47 @@ def get_params(candidate, fingerprint, vin="", is_panda_black=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.45, 0.4]
+        ret.longitudinalTuning.kiBP = [0., 5., 10., 35.]
+        ret.longitudinalTuning.kiV = [0.01,0.01,0.01,0.01]
+     #   ret.longitudinalTuning.kdBP = [0, 20., 25., 35]
+     #   ret.longitudinalTuning.kdV = [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.kpV = [0.275, 0.275, 0.35, 0.37]
+
+        ret.longitudinalTuning.kiBP = [0., 5., 22., 35.]
+        ret.longitudinalTuning.kiV = [0.07, 0.07, 0.084, 0.087]
+     #   ret.longitudinalTuning.kdBP = [0, 5., 22., 35]
+     #   ret.longitudinalTuning.kdV = [0.010, 0.010, 0.010, 0.01]
+
       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.kpBP = [0., 5., 22., 35.]
+        ret.longitudinalTuning.kpV = [0.50, 0.45, 0.45, 0.4]
+        ret.longitudinalTuning.kiBP = [0., 5., 22., 35.]
         ret.longitudinalTuning.kiV = [0.01,0.01,0.01]
+      #  ret.longitudinalTuning.kdBP = [0, 20., 25., 35]
+      #  ret.longitudinalTuning.kdV = [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.00750,0.00725]
+       # ret.longitudinalTuning.kdBP = [0, 20., 25., 35]
+       # ret.longitudinalTuning.kdV = [0.01, 0.01, 0.01, 0.01]
+
       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]
+   #     ret.longitudinalTuning.kdBP = [0, 20., 25., 35]
+   #     ret.longitudinalTuning.kdV = [0.01, 0.01, 0.01, 0.01]
+
 
 
     else:
@@ -204,8 +220,8 @@ def get_params(candidate, fingerprint, vin="", is_panda_black=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.gasMaxBP = [0., 20.]  # m/s
+    ret.gasMaxV = [0.3, 0.6] #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
 

selfdrive/controls/lib/longcontrol.py

@@ -1,6 +1,8 @@
 from cereal import log
 from common.numpy_fast import clip, interp
-from selfdrive.controls.lib.pid import PIController
+from selfdrive.controls.lib.pid_real import PIController
+import time
+import os
 
 LongCtrlState = log.ControlsState.LongControlState
 
@@ -58,11 +60,18 @@ 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, 5., 22.,35.]
+    kdV = [0.02, 0.02, 0.022, 0.24]
+#TODO: use CP.longitudalTuning.kdBp and CP.longitudalTuning.kdV
+
     self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
+                            (kdBp,kdV),
                             rate=RATE,
                             sat_limit=0.8,
                             convert=compute_gb)
+
     self.v_pid = 0.0
     self.last_output_gb = 0.0
 
@@ -73,6 +82,8 @@ def reset(self, v_pid):
 
   def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
+
+
     # Actuation limits
     gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
     brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
@@ -127,4 +138,4 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
     final_gas = clip(output_gb, 0., gas_max)
     final_brake = -clip(output_gb, -brake_max, 0.)
 
-    return final_gas, final_brake
+    return final_gas, final_brake

selfdrive/controls/lib/pid_real.py

@@ -0,0 +1,117 @@
+import numpy as np
+from common.numpy_fast import clip, interp
+from selfdrive.car.tesla.speed_utils.movingaverage import MovingAverage
+
+
+def apply_deadzone(error, deadzone):
+  if error > deadzone:
+    error -= deadzone
+  elif error < - deadzone:
+    error += deadzone
+  else:
+    error = 0.
+  return error
+
+class PIController():
+  def __init__(self, k_p, k_i, k_d, k_f=0.85, pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
+    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.pos_limit = pos_limit
+    self.neg_limit = neg_limit
+
+    self.sat_count_rate = 1.0 / rate
+    self.i_unwind_rate = 0.3 / rate
+    self.i_rate = 1.0 / rate
+    self.rate = 1.0 / rate
+    self.d_rate = 7.0 / rate
+    self.sat_limit = sat_limit
+    self.convert = convert
+    self.past_errors_avg = 0
+    self.past_errors = MovingAverage(3)
+
+    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, override, error):
+    saturated = (control < self.neg_limit) or (control > self.pos_limit)
+
+    if saturated and not override 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.i = 0.0
+    self.f = 0.0
+    self.d = 0.0
+    self.sat_count = 0.0
+    self.saturated = False
+    self.control = 0
+    self.past_errors_avg = 0
+    self.past_errors.reset()
+
+  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
+
+    clipped_error = clip(error,0,5)
+    self.k_f = clipped_error * 0.2
+    #clip just to be absolutely sure?
+#    self.k_f = clip(self.k_f,0,1)
+
+    self.f = feedforward * self.k_f
+    self.d = 0.0
+
+    if override:
+      self.i -= self.i_unwind_rate * float(np.sign(self.i))
+    else:
+      i = self.i + error * self.k_i * self.i_rate
+      if self.past_errors.no_items == self.past_errors.length:
+        self.d = self.k_d * ((error - self.past_errors_avg) / self.d_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.i = i
+
+    self.past_errors_avg = self.past_errors.add(error)
+
+    control = self.p + self.f + self.i + self.d #adds the derivatoive gain to the control output
+    if self.convert is not None:
+      control = self.convert(control, speed=self.speed)
+
+    if check_saturation:
+      self.saturated = self._check_saturation(control, override, error)
+    else:
+      self.saturated = False
+
+    self.control = clip(control, self.neg_limit, self.pos_limit)
+    return self.control

selfdrive/controls/lib/planner.py

@@ -29,7 +29,7 @@
 # need fast accel at very low speed for stop and go
 # make sure these accelerations are smaller than mpc limits
 
-_A_CRUISE_MAX_V = [2.0, 1.6, 1.0, 0.7]
+_A_CRUISE_MAX_V = [1.775, 1.85, 1.325, 1.25]
 #_A_CRUISE_MAX_V = [2.7, 2.7, 2.0, 2.0] #we had this for tesla before, BB
 _A_CRUISE_MAX_BP = [0.,  6.4, 22.5, 40.]