Merge pull request commaai#588 from arne182/074-clean

074 clean

selfdrive/controls/lib/lane_planner.py

@@ -18,12 +18,22 @@ def compute_path_pinv(l=50):
 def model_polyfit(points, path_pinv):
   return np.dot(path_pinv, [float(x) for x in points])
 
+def eval_poly(poly, x):
+  return poly[3] + poly[2]*x + poly[1]*x**2 + poly[0]*x**3
 
-def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
+
+def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width, v_ego):
   # This will improve behaviour when lanes suddenly widen
+  # these numbers were tested on 2000segments and found to work well
   lane_width = min(4.0, lane_width)
-  l_prob = l_prob * interp(abs(l_poly[3]), [2, 2.5], [1.0, 0.0])
-  r_prob = r_prob * interp(abs(r_poly[3]), [2, 2.5], [1.0, 0.0])
+  width_poly = l_poly - r_poly
+  prob_mods = []
+  for t_check in [0.0, 1.5, 3.0]:
+    width_at_t = eval_poly(width_poly, t_check * (v_ego + 7))
+    prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
+  mod = min(prob_mods)
+  l_prob = mod * l_prob
+  r_prob = mod * r_prob
 
   path_from_left_lane = l_poly.copy()
   path_from_left_lane[3] -= lane_width / 2.0
@@ -86,7 +96,7 @@ def update_d_poly(self, v_ego):
     self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
                       (1 - self.lane_width_certainty) * speed_lane_width
 
-    self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width)
+    self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width, v_ego)
 
   def update(self, v_ego, md):
     self.parse_model(md)

selfdrive/controls/lib/long_mpc.py

@@ -1,6 +1,5 @@
 import os
 import math
-import time
 
 import cereal.messaging as messaging
 import cereal.messaging_arne as messaging_arne
@@ -12,7 +11,6 @@
 from common.op_params import opParams
 from common.numpy_fast import interp, clip
 from common.travis_checker import travis
-from selfdrive.config import Conversions as CV
 
 LOG_MPC = os.environ.get('LOG_MPC', False)
 
@@ -37,9 +35,6 @@ def __init__(self, mpc_id):
       self.pm = messaging_arne.PubMaster(['smiskolData'])
     else:
       self.pm = None
-    self.car_data = {'v_ego': 0.0, 'a_ego': 0.0}
-    self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False}
-    self.df_data = {"v_leads": [], "v_egos": []}  # dynamic follow data
     self.last_cost = 0.0
     self.df_profile = self.op_params.get('dynamic_follow', 'relaxed').strip().lower()
     self.sng = False
@@ -74,31 +69,13 @@ def set_cur_state(self, v, a):
     self.cur_state[0].v_ego = v
     self.cur_state[0].a_ego = a
 
-  def get_TR(self, CS):
-    if not self.lead_data['status'] or travis:
+  def get_TR(self, CS, lead):
+    if not lead.status or travis:
       TR = 1.8
     elif CS.vEgo < 5.0:
-      TRs = [2.0, 1.8, 1.75, 1.6]
-      vEgos = [2.0, 3.0, 4.0, 5.0]
-      #TRs = [1.8, 1.6]
-      #vEgos =[4.0, 5.0]
-      TR = interp(CS.vEgo, vEgos, TRs)
-      p_mod_pos = 1.0
-      p_mod_neg = 1.0
-      TR_mod = []
-      x = [-20.0383, -15.6978, -11.2053, -7.8781, -5.0407, -3.2167, -1.6122, 0.0]  # relative velocity values
-      y = [0.641, 0.506, 0.418, 0.334, 0.24, 0.115, 0.055, 0.01]  # modification values
-      TR_mod.append(interp(self.lead_data['v_lead'] - self.car_data['v_ego'], x, y))
-
-      x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0]  # lead acceleration values
-      y = [0.265, 0.187, 0.096, 0.057, 0.033, 0.024, 0.0]  # modification values
-      TR_mod.append(interp(self.calculate_lead_accel(), x, y))
-
-      self.TR_Mod = sum([mod * p_mod_neg if mod < 0 else mod * p_mod_pos for mod in TR_mod]) # calculate TR_Mod so that the cost function works correctly
-
+      TR = 1.8
     else:
-      self.store_df_data()
-      TR = self.dynamic_follow(CS)
+      TR = self.dynamic_follow(CS, lead)
 
     if not travis:
       self.change_cost(TR,CS.vEgo)
@@ -114,135 +91,40 @@ def send_cur_TR(self, TR):
 
   def change_cost(self, TR, vEgo):
     TRs = [0.9, 1.8, 2.7]
-    costs = [0.75, 0.2, 0.075]
+    costs = [1.0, 0.15, 0.05]
     cost = interp(TR, TRs, costs)
-    if vEgo < 5.0:
-      cost = 0.2
-      cost = cost * min(max(1.0 , (6.0 - vEgo)),5.0) 
-    if self.TR_Mod > 0:
-      cost = cost + self.TR_Mod/2.0
     if self.last_cost != cost:
       self.libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
       self.last_cost = cost
 
-  def store_df_data(self):
-    v_lead_retention = 1.9  # keep only last x seconds
-    v_ego_retention = 2.5
-
-    cur_time = time.time()
-    if self.lead_data['status']:
-      self.df_data['v_leads'] = [sample for sample in self.df_data['v_leads'] if
-                                 cur_time - sample['time'] <= v_lead_retention
-                                 and not self.new_lead]  # reset when new lead
-      self.df_data['v_leads'].append({'v_lead': self.lead_data['v_lead'], 'time': cur_time})
-
-    self.df_data['v_egos'] = [sample for sample in self.df_data['v_egos'] if cur_time - sample['time'] <= v_ego_retention]
-    self.df_data['v_egos'].append({'v_ego': self.car_data['v_ego'], 'time': cur_time})
-
-  def calculate_lead_accel(self):
-    min_consider_time = 1.0  # minimum amount of time required to consider calculation
-    a_lead = self.lead_data['a_lead']
-    if len(self.df_data['v_leads']):  # if not empty
-      elapsed = self.df_data['v_leads'][-1]['time'] - self.df_data['v_leads'][0]['time']
-      if elapsed > min_consider_time:  # if greater than min time (not 0)
-        a_calculated = (self.df_data['v_leads'][-1]['v_lead'] - self.df_data['v_leads'][0]['v_lead']) / elapsed  # delta speed / delta time
-        # old version: # if abs(a_calculated) > abs(a_lead) and a_lead < 0.33528:  # if a_lead is greater than calculated accel (over last 1.5s, use that) and if lead accel is not above 0.75 mph/s
-        #   a_lead = a_calculated
-
-        # long version of below: if (a_calculated < 0 and a_lead >= 0 and a_lead < -a_calculated * 0.5) or (a_calculated > 0 and a_lead <= 0 and -a_lead > a_calculated * 0.5) or (a_lead * a_calculated > 0 and abs(a_calculated) > abs(a_lead)):
-        if (a_calculated < 0 <= a_lead < -a_calculated * 0.55) or (a_calculated > 0 >= a_lead and -a_lead < a_calculated * 0.45) or (a_lead * a_calculated > 0 and abs(a_calculated) > abs(a_lead)):  # this is a mess, fix
-          a_lead = a_calculated
-    return a_lead  # if above doesn't execute, we'll return a_lead from radar
-
-  def dynamic_follow(self, CS):
+  def dynamic_follow(self, CS, lead):
     self.df_profile = self.op_params.get('dynamic_follow', 'relaxed').strip().lower()
-    x_vel = [5.0, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocities
-    p_mod_x = [3, 20, 35]  # profile mod speeds
+    x_vel = [5.0, 55.0]  # velocities
     if self.df_profile == 'roadtrip':
-      y_dist = [1.6, 1.4507, 1.4837, 1.5327, 1.553, 1.581, 1.617, 1.653, 1.687, 1.74]  # TRs
-      p_mod_pos = [0.99, 0.815, 0.57]
-      p_mod_neg = [1.0, 1.27, 1.675]
+      y_dist = [1.8, 2.1]  # TRs
     elif self.df_profile == 'traffic':  # for in congested traffic
-      x_vel = [5.0, 7.4507, 9.3133, 11.5598, 13.645, 17.8816, 22.407, 28.8833, 34.8691, 40.3906]
-      y_dist = [1.6, 1.437, 1.468, 1.501, 1.506, 1.38, 1.2216, 1.085, 1.0516, 1.016]
-      p_mod_pos = [1.015, 2.175, 3.65]
-      p_mod_neg = [0.98, 0.08, 0.0]
-      # y_dist = [1.384, 1.391, 1.403, 1.415, 1.437, 1.3506, 1.3959, 1.4156, 1.38, 1.1899, 1.026, 0.9859, 0.9432]  # from 071-2 (need to fix FCW)
-      # p_mod_pos = [1.015, 2.2, 3.95]
-      # p_mod_neg = [0.98, 0.1, 0.0]
+      x_vel = [5.0, 35.0]
+      y_dist = [1.8, 1.1]
     else:  # default to relaxed/stock
-      y_dist = [1.6, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614]
-      p_mod_pos = [1.0, 1.0, 1.0]
-      p_mod_neg = [1.0, 1.0, 1.0]
-
-    p_mod_pos = interp(self.car_data['v_ego'], p_mod_x, p_mod_pos)
-    p_mod_neg = interp(self.car_data['v_ego'], p_mod_x, p_mod_neg)
-
-    sng_TR = 1.7  # reacceleration stop and go TR
-    sng_speed = 15.0 * CV.MPH_TO_MS
-
-    if self.car_data['v_ego'] > sng_speed:  # keep sng distance until we're above sng speed again
-      self.sng = False
-
-    if (self.car_data['v_ego'] >= sng_speed or self.df_data['v_egos'][0]['v_ego'] >= self.car_data['v_ego']) and not self.sng:  # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease
-      TR = interp(self.car_data['v_ego'], x_vel, y_dist)
-    else:  # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating
-      self.sng = True
-      x = [sng_speed / 3.0, sng_speed]  # decrease TR between 5 and 15 mph from 1.8s to defined TR above at 15mph while accelerating
-      y = [sng_TR, interp(sng_speed, x_vel, y_dist)]
-      TR = interp(self.car_data['v_ego'], x, y)
-
-    TR_mod = []
+      y_dist = [1.8, 1.8]
+    TR = interp(CS.vEgo, x_vel, y_dist)
+
     # Dynamic follow modifications (the secret sauce)
-    x = [-20.0383, -15.6978, -11.2053, -7.8781, -5.0407, -3.2167, -1.6122, 0.0, 0.6847, 1.3772, 1.9007, 2.7452]  # relative velocity values
-    y = [0.641, 0.506, 0.418, 0.334, 0.24, 0.115, 0.055, 0.0, -0.03, -0.068, -0.142, -0.221]  # modification values
-    TR_mod.append(interp(self.lead_data['v_lead'] - self.car_data['v_ego'], x, y))
-
-    x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466, 0.5144, 0.6903, 0.9302]  # lead acceleration values
-    y = [0.265, 0.187, 0.096, 0.057, 0.033, 0.024, 0.0, -0.009, -0.042, -0.053, -0.059]  # modification values
-    TR_mod.append(interp(self.calculate_lead_accel(), x, y))
-
-    # x = [4.4704, 22.352]  # 10 to 50 mph  #todo: remove if uneeded/unsafe
-    # y = [0.94, 1.0]
-    # TR_mod *= interp(self.car_data['v_ego'], x, y)  # modify TR less at lower speeds
-
-    self.TR_Mod = sum([mod * p_mod_neg if mod < 0 else mod * p_mod_pos for mod in TR_mod])  # alter TR modification according to profile
+    x = [-5.0, 0.0, 5.0]  # relative velocity values
+    y = [0.2, 0.0, -0.2]  # modification values
+
+    self.TR_Mod = interp(lead.vRel, x, y)
     TR += self.TR_Mod
 
-    if CS.leftBlinker or CS.rightBlinker and self.df_profile != 'traffic':
-      x = [8.9408, 22.352, 31.2928]  # 20, 50, 70 mph
-      y = [1.0, .75, .65]  # reduce TR when changing lanes
-      TR *= interp(self.car_data['v_ego'], x, y)
-
-    # TR *= self.get_traffic_level()  # modify TR based on last minute of traffic data  # todo: look at getting this to work, a model could be used
+    if CS.leftBlinker or CS.rightBlinker:
+      x = [9.0, 55.0]  #
+      y = [1.0, 0.65]  # reduce TR when changing lanes
+      TR *= interp(CS.vEgo, x, y)
 
     return clip(TR, 0.9, 2.7)
 
-  def process_lead(self, v_lead, a_lead, x_lead, status):
-    self.lead_data['v_lead'] = v_lead
-    self.lead_data['a_lead'] = a_lead
-    self.lead_data['x_lead'] = x_lead
-    self.lead_data['status'] = status
-
-  # def get_traffic_level(self, lead_vels):  # generate a value to modify TR by based on fluctuations in lead speed
-  #   if len(lead_vels) < 60:
-  #     return 1.0  # if less than 30 seconds of traffic data do nothing to TR
-  #   lead_vel_diffs = []
-  #   for idx, vel in enumerate(lead_vels):
-  #     try:
-  #       lead_vel_diffs.append(abs(vel - lead_vels[idx - 1]))
-  #     except:
-  #       pass
-  #
-  #   x = [0, len(lead_vels)]
-  #   y = [1.15, .9]  # min and max values to modify TR by, need to tune
-  #   traffic = interp(sum(lead_vel_diffs), x, y)
-  #
-  #   return traffic
-
   def update(self, pm, CS, lead, v_cruise_setpoint):
     v_ego = CS.vEgo
-    self.car_data = {'v_ego': CS.vEgo, 'a_ego': CS.aEgo}
     # Setup current mpc state
     self.cur_state[0].x_ego = 0.0
 
@@ -254,7 +136,6 @@ def update(self, pm, CS, lead, v_cruise_setpoint):
       if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
         v_lead = 0.0
         a_lead = 0.0
-      self.process_lead(v_lead, a_lead, x_lead, lead.status)
       self.a_lead_tau = lead.aLeadTau
       self.new_lead = False
       if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
@@ -266,7 +147,6 @@ def update(self, pm, CS, lead, v_cruise_setpoint):
       self.cur_state[0].x_l = x_lead
       self.cur_state[0].v_l = v_lead
     else:
-      self.process_lead(None, None, None, False)
       self.prev_lead_status = False
       # Fake a fast lead car, so mpc keeps running
       self.cur_state[0].x_l = 50.0
@@ -276,7 +156,7 @@ def update(self, pm, CS, lead, v_cruise_setpoint):
 
     # Calculate mpc
     t = sec_since_boot()
-    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, self.get_TR(CS))
+    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, self.get_TR(CS, lead))
     duration = int((sec_since_boot() - t) * 1e9)
 
     if LOG_MPC:

selfdrive/controls/lib/longcontrol.py

@@ -13,7 +13,7 @@
 
 STOPPING_BRAKE_RATE = 0.2  # brake_travel/s while trying to stop
 STARTING_BRAKE_RATE = 0.8  # brake_travel/s while releasing on restart
-BRAKE_STOPPING_TARGET = 0.75  # apply at least this amount of brake to maintain the vehicle stationary
+BRAKE_STOPPING_TARGET = 0.7  # apply at least this amount of brake to maintain the vehicle stationary
 
 _MAX_SPEED_ERROR_BP = [0., 30.]  # speed breakpoints
 _MAX_SPEED_ERROR_V = [1.5, .8]  # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp
@@ -125,16 +125,24 @@ def update(self, active, v_ego, gas_pressed, brake_pressed, standstill, cruise_s
           self.pid._k_i = (CP.longitudinalTuning.kiBP, [x * 0 for x in CP.longitudinalTuning.kiV])
           self.pid.i = 0.0
           self.pid.k_f=1.0
+          self.v_pid = v_ego
+          self.pid.reset()
         if self.lastdecelForTurn and not decelForTurn:
           self.lastdecelForTurn = False
           self.pid._k_p = (CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV)
           self.pid._k_i = (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV)
           self.pid.k_f=1.0
+          self.v_pid = v_ego
+          self.pid.reset()
       else:
+        if self.lastdecelForTurn:
+          self.v_pid = v_ego
+          self.pid.reset()
         self.lastdecelForTurn = False
         self.pid._k_p = (CP.longitudinalTuning.kpBP, [x * 1 for x in CP.longitudinalTuning.kpV])
         self.pid._k_i = (CP.longitudinalTuning.kiBP, [x * 1 for x in CP.longitudinalTuning.kiV])
         self.pid.k_f=1.0
+
 
       output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
 
@@ -146,7 +154,7 @@ def update(self, active, v_ego, gas_pressed, brake_pressed, standstill, cruise_s
       # Keep applying brakes until the car is stopped
       factor = 1
       if hasLead:
-        factor = interp(dRel,[2.0,3.0,4.0,5.0,6.0,7.0,8.0,9.0], [10.0,5.0,2.0,1.0,0.5,0.1,0.0,-0.1])
+        factor = interp(dRel,[2.0,3.0,4.0,5.0,6.0,7.0,8.0,9.0], [5.0,2.5,1.0,0.5,0.25,0.05,0.0,-0.1])
       if not standstill or output_gb > -BRAKE_STOPPING_TARGET:
         output_gb -= STOPPING_BRAKE_RATE / RATE * factor
       output_gb = clip(output_gb, -brake_max, gas_max)

selfdrive/controls/lib/planner.py

@@ -310,7 +310,7 @@ def update(self, sm, pm, CP, VM, PP, arne_sm):
         time_to_turn = max(1.0, sm['liveMapData'].distToTurn / max((v_ego + v_curvature_map)/2, 1.))
         required_decel = min(0, (v_curvature_map - v_ego) / time_to_turn)
         accel_limits[0] = max(accel_limits[0], required_decel)
-      if v_speedlimit_ahead < v_speedlimit and v_ego > v_speedlimit_ahead and sm['liveMapData'].speedLimitAheadDistance > 0.10 and not following:
+      if v_speedlimit_ahead < v_speedlimit and v_ego > v_speedlimit_ahead and sm['liveMapData'].speedLimitAheadDistance > 1.0 and not following:
         required_decel = min(0, (v_speedlimit_ahead*v_speedlimit_ahead - v_ego*v_ego)/(sm['liveMapData'].speedLimitAheadDistance*2))
         required_decel = max(required_decel, -3.0)
         decel_for_turn = True

selfdrive/controls/tests/test_lateral_mpc.py

@@ -25,7 +25,7 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
   p_p = poly_p.copy()
   p_p[3] += poly_shift
 
-  d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width)
+  d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width, v_ref)
 
   CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
   VM = VehicleModel(CP)

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-b98f4b6858a21fd6602fb41c6a21f69b1ca81921
+dfe0776d21e460a8f8e98321f56f421418132620

selfdrive/thermald/thermald.py

@@ -195,8 +195,8 @@ def thermald_thread():
     location = location.gpsLocation if location else None
     msg = read_thermal()
 
-    # clear car params when panda gets disconnected
-    if health is None and health_prev is not None:
+    # clear car params when panda gets connected
+    if health is not None and health_prev is None:
       params.panda_disconnect()
     health_prev = health
 

selfdrive/ui/ui.cc

@@ -95,7 +95,7 @@ static bool handle_df_touch(UIState *s, int touch_x, int touch_y) {
   //dfButton manager  // code below thanks to kumar: https://github.com/arne182/openpilot/commit/71d5aac9f8a3f5942e89634b20cbabf3e19e3e78
   if (s->awake && s->vision_connected && s->active_app == cereal_UiLayoutState_App_home && s->status != STATUS_STOPPED) {
     int padding = 40;
-   if ((1360 - padding <= touch_x) && (855 - padding <= touch_y)) {
+   if (touch_x >= 1212 && touch_x <= 1310 && touch_y >= 902 && touch_y <= 1013) {
       s->scene.uilayout_sidebarcollapsed = true;  // collapse sidebar when tapping df button
       s->scene.dfButtonStatus++;
       if (s->scene.dfButtonStatus > 2) {