Smoother long tuning for pedal

SA_RELEASES.md

@@ -1,6 +1,7 @@
 Stock Additions Update 2 - 2021-08-15 (0.8.8)
 ===
  * Update SA to 0.8.8 with new model from upstream!
+ * Experimental model cruise control button is back with a new model
 
 Stock Additions Update 1 - 2021-08-15 (0.8.7)
 ===

selfdrive/car/toyota/interface.py

@@ -392,14 +392,24 @@ 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]
     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]
+      # Default longitudinal tune with tweaks
+      ret.longitudinalTuning.deadzoneBP = [0., 9.]
+      ret.longitudinalTuning.deadzoneV = [0., .15]
+      ret.longitudinalTuning.kpBP = [0., 5., 35.]
+      ret.longitudinalTuning.kiBP = [0., 35.]
+      ret.longitudinalTuning.kdBP = [0., 5., 35.]
+      ret.longitudinalTuning.kpV = [3.6 * 0.9, 2.4 * 0.9, 1.5 * 0.9]
+      ret.longitudinalTuning.kiV = [0.54 * 0.8, 0.36 * 0.8]
+      ret.longitudinalTuning.kdV = [0.05, 0.1, 0.3]
     elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]:
       # Improved longitudinal tune
       ret.longitudinalTuning.deadzoneBP = [0., 8.05]

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)
     self.new_kf_tuned = CP.lateralTuning.pid.newKfTuned
 
   def reset(self):

selfdrive/controls/lib/lead_mpc.py

@@ -49,7 +49,7 @@ def set_cur_state(self, v, a):
     self.cur_state[0].v_ego = v_safe
     self.cur_state[0].a_ego = a_safe
 
-  def update(self, CS, radarstate, v_cruise):
+  def update(self, CS, radarstate, modelstate, v_cruise):
     v_ego = CS.vEgo
     if self.lead_id == 0:
       lead = radarstate.leadOne

selfdrive/controls/lib/long_mpc.py

@@ -9,21 +9,26 @@
 
 
 class LongitudinalMpc():
-  def __init__(self):
+  def __init__(self, mpc_id):
+    self.mpc_id = mpc_id
+
     self.reset_mpc()
     self.last_cloudlog_t = 0.0
     self.ts = list(range(10))
     self.status = True
-    self.min_a = -1.2
+    self.min_a = -1.2 if self.mpc_id == 0 else -3.5
     self.max_a = 1.2
 
 
   def reset_mpc(self):
-    self.libmpc = libmpc_py.libmpc
-    self.libmpc.init(0.0, 1.0, 0.0, 50.0, 10000.0)
+    ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
+    if self.mpc_id == 0:
+      self.libmpc.init(0.0, 1.0, 0.0, 50.0, 10000.0)
+    else:
+      self.libmpc.init(1.0, 1.0, 0.0, 5.0, 10000.0)
 
-    self.mpc_solution = libmpc_py.ffi.new("log_t *")
-    self.cur_state = libmpc_py.ffi.new("state_t *")
+    self.mpc_solution = ffi.new("log_t *")
+    self.cur_state = ffi.new("state_t *")
 
     self.cur_state[0].x_ego = 0
     self.cur_state[0].v_ego = 0
@@ -45,10 +50,14 @@ def set_cur_state(self, v, a):
     self.cur_state[0].v_ego = v_safe
     self.cur_state[0].a_ego = a_safe
 
-  def update(self, carstate, radarstate, v_cruise):
+  def update(self, carstate, radarstate, modelstate, v_cruise):
     v_cruise_clipped = np.clip(v_cruise, self.cur_state[0].v_ego - 10., self.cur_state[0].v_ego + 10.0)
-    poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1])
-    speeds = v_cruise_clipped * np.ones(LON_MPC_N+1)
+    if self.mpc_id == 0:
+      poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1])
+      speeds = v_cruise_clipped * np.ones(LON_MPC_N+1)
+    else:
+      poss = np.minimum(np.array(modelstate.position.x)[:LON_MPC_N+1], v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1]))
+      speeds = np.minimum(np.array(modelstate.velocity.x)[:LON_MPC_N+1], v_cruise_clipped)
     accels = np.zeros(LON_MPC_N+1)
     self.update_with_xva(poss, speeds, accels)
 

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,12 @@ 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,
+                             convert=compute_gb)
     self.v_pid = 0.0
     self.last_output_gb = 0.0
 
@@ -83,8 +80,9 @@ def update(self, active, CS, CP, long_plan, extras):
     # Interp control trajectory
     # TODO estimate car specific lag, use .15s for now
     if len(long_plan.speeds) == CONTROL_N:
-      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)
+      accel_delay = interp(CS.vEgo, [4.4704, 35.7632], [0.2, 0.4])  # 10 to 80 mph
+      speed_delay = interp(CS.vEgo, [4.4704, 35.7632], [0.05, 0.1])
+      v_target = interp(speed_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)
     else:

selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -44,4 +44,5 @@ if GetOption('mpc_generate'):
 
 
 mpc_files = ["longitudinal_mpc.c"] + generated_c
-env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
+env.SharedLibrary('mpc0', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
+env.SharedLibrary('mpc1', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)

selfdrive/controls/lib/longitudinal_mpc_lib/libmpc_py.py

@@ -4,31 +4,38 @@
 from common.ffi_wrapper import suffix
 
 mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
-libmpc_fn = os.path.join(mpc_dir, "libmpc"+suffix())
 
-ffi = FFI()
-ffi.cdef("""
-const int MPC_N = 32;
+def _get_libmpc(mpc_id):
+  libmpc_fn = os.path.join(mpc_dir, "libmpc%d%s" % (mpc_id, suffix()))
 
-typedef struct {
-double x_ego, v_ego, a_ego;
-} state_t;
+  ffi = FFI()
+  ffi.cdef("""
+  const int MPC_N = 32;
 
+  typedef struct {
+  double x_ego, v_ego, a_ego;
+  } state_t;
 
-typedef struct {
-double x_ego[MPC_N+1];
-double v_ego[MPC_N+1];
-double a_ego[MPC_N+1];
-double t[MPC_N+1];
-double j_ego[MPC_N];
-double cost;
-} log_t;
 
+  typedef struct {
+  double x_ego[MPC_N+1];
+  double v_ego[MPC_N+1];
+  double a_ego[MPC_N+1];
+  double t[MPC_N+1];
+  double j_ego[MPC_N];
+  double cost;
+  } log_t;
 
-void init(double xCost, double vCost, double aCost, double jerkCost, double constraintCost);
-int run_mpc(state_t * x0, log_t * solution,
-            double target_x[MPC_N+1], double target_v[MPC_N+1], double target_a[MPC_N+1],
-            double min_a, double max_a);
-""")
 
-libmpc = ffi.dlopen(libmpc_fn)
+  void init(double xCost, double vCost, double aCost, double jerkCost, double constraintCost);
+  int run_mpc(state_t * x0, log_t * solution,
+              double target_x[MPC_N+1], double target_v[MPC_N+1], double target_a[MPC_N+1],
+              double min_a, double max_a);
+  """)
+
+  return (ffi, ffi.dlopen(libmpc_fn))
+
+mpcs = [_get_libmpc(0), _get_libmpc(1)]
+
+def get_libmpc(mpc_id):
+    return mpcs[mpc_id]

selfdrive/controls/lib/longitudinal_planner.py

@@ -15,8 +15,6 @@
 from selfdrive.controls.lib.long_mpc import LongitudinalMpc
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
 from selfdrive.swaglog import cloudlog
-# from selfdrive.controls.lib.long_mpc_model import LongitudinalMpcModel
-# from common.op_params import opParams
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
 AWARENESS_DECEL = -0.2     # car smoothly decel at .2m/s^2 when user is distracted
@@ -46,44 +44,17 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   return [a_target[0], min(a_target[1], a_x_allowed)]
 
 
-class ModelMpcHelper:
-  def __init__(self):
-    self.model_t = [i ** 2 / 102.4 for i in range(33)]  # the timesteps of the model predictions
-    self.mpc_t = list(range(10))  # the timesteps of what the LongMpcModel class takes in, 1 sec intervels to 10
-    self.model_t_idx = [sorted(range(len(self.model_t)), key=[abs(idx - t) for t in self.model_t].__getitem__)[0] for idx in self.mpc_t]  # matches 0 to 9 interval to idx from t
-    assert len(self.model_t_idx) == 10, 'Needs to be length 10 for mpc'
-
-  def convert_data(self, sm):
-    modelV2 = sm['modelV2']
-    distances, speeds, accelerations = [], [], []
-    if not sm.alive['modelV2'] or len(modelV2.position.x) == 0:
-      return distances, speeds, accelerations
-
-    speeds = [modelV2.velocity.x[t] for t in self.model_t_idx]
-    distances = [modelV2.position.x[t] for t in self.model_t_idx]
-    for t in self.mpc_t:  # todo these three in one loop
-      if 0 < t < 9:
-        accelerations.append((speeds[t + 1] - speeds[t - 1]) / 2)
-
-    # Extrapolate forward and backward at edges
-    accelerations.append(accelerations[-1] - (accelerations[-2] - accelerations[-1]))
-    accelerations.insert(0, accelerations[0] - (accelerations[1] - accelerations[0]))
-    return distances, speeds, accelerations
-
-
 class Planner():
   def __init__(self, CP):
     self.CP = CP
     self.mpcs = {}
     self.mpcs['lead0'] = LeadMpc(0)
     self.mpcs['lead1'] = LeadMpc(1)
-    self.mpcs['cruise'] = LongitudinalMpc()
-    # self.mpcs['model'] = LongitudinalMpcModel()
+    self.mpcs['cruise'] = LongitudinalMpc(0)
+    self.mpcs['e2e'] = LongitudinalMpc(1)
 
     self.fcw = False
     self.fcw_checker = FCWChecker()
-    # self.op_params = opParams()
-    self.model_mpc_helper = ModelMpcHelper()
 
     self.v_desired = 0.0
     self.a_desired = 0.0
@@ -134,9 +105,9 @@ def update(self, sm, CP):
     next_a = np.inf
     for key in self.mpcs:
       self.mpcs[key].set_cur_state(self.v_desired, self.a_desired)
-      self.mpcs[key].update(sm['carState'], sm['radarState'], v_cruise)
-      # TODO: handle model long enabled check
-      if self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a:  # picks slowest solution from accel in ~0.2 seconds
+      self.mpcs[key].update(sm['carState'], sm['radarState'], sm['modelV2'], v_cruise)
+      if (self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a and  # picks slowest solution from accel in ~0.2 seconds
+              ((key == 'e2e' and sm['modelLongButton'].enabled) or key != 'e2e')):
         self.longitudinalPlanSource = key
         self.v_desired_trajectory = self.mpcs[key].v_solution[:CONTROL_N]
         self.a_desired_trajectory = self.mpcs[key].a_solution[:CONTROL_N]