dragonpilot 0.7.5.4

========================
* Removed Steering monitor timer adjustment in code (timer in settings will no longer work.)
* Updated Dynamic Follow model. (Special Thanks to @ShaneSmiskol for the feature and @cgw1968 for testing)

selfdrive/controls/lib/driver_monitor.py

@@ -117,9 +117,6 @@ class DriverStatus():
     self.is_rhd_region = False
     self.is_rhd_region_checked = False
 
-    # dp
-    self.awareness_time = _AWARENESS_TIME
-
     self._set_timers(active_monitoring=True)
 
   def _set_timers(self, active_monitoring):
@@ -147,9 +144,9 @@ class DriverStatus():
         self.awareness_active = self.awareness
         self.awareness = self.awareness_passive
 
-      self.threshold_pre = _AWARENESS_PRE_TIME_TILL_TERMINAL / self.awareness_time
-      self.threshold_prompt = _AWARENESS_PROMPT_TIME_TILL_TERMINAL / self.awareness_time
-      self.step_change = DT_DMON / self.awareness_time
+      self.threshold_pre = _AWARENESS_PRE_TIME_TILL_TERMINAL / _AWARENESS_TIME
+      self.threshold_prompt = _AWARENESS_PROMPT_TIME_TILL_TERMINAL / _AWARENESS_TIME
+      self.step_change = DT_DMON / _AWARENESS_TIME
       self.active_monitoring_mode = False
 
   def _is_driver_distracted(self, pose, blink):

selfdrive/controls/lib/dynamic_follow/__init__.py

@@ -28,10 +28,10 @@ class DynamicFollow:
 
     # Model variables
     mpc_rate = 1 / 20.
-    self.model_scales = {'v_ego': [-0.06112159043550491, 37.96522521972656], 'v_lead': [0.0, 35.27671432495117], 'x_lead': [2.4600000381469727, 139.52000427246094]}
+    self.model_scales = {'v_ego': [-0.06112159043550491, 37.96522521972656], 'a_lead': [-3.109330892562866, 3.3612186908721924], 'v_lead': [0.0, 35.27671432495117], 'x_lead': [2.4600000381469727, 141.44000244140625]}
     self.predict_rate = 1 / 4.
-    self.skip_every = round(0.2 / mpc_rate)
-    self.model_input_len = round(35 / mpc_rate)  # int: model input time
+    self.skip_every = round(0.25 / mpc_rate)
+    self.model_input_len = round(45 / mpc_rate)
 
     # Dynamic follow variables
     self.default_TR = 1.8
@@ -40,6 +40,9 @@ class DynamicFollow:
     self.v_ego_retention = 2.5
     self.v_rel_retention = 1.5
 
+    self.sng_TR = 1.8  # reacceleration stop and go TR
+    self.sng_speed = 18.0 * CV.MPH_TO_MS
+
     # dp params
     self.last_ts = 0.
     self.dp_df_profile = PROFILE_OFF
@@ -64,6 +67,7 @@ class DynamicFollow:
     self.last_cost = 0.0
     self.last_predict_time = 0.0
     self.auto_df_model_data = []
+    self._get_live_params()  # so they're defined just in case
 
   def update(self, CS, libmpc):
     self._get_live_params()
@@ -135,6 +139,7 @@ class DynamicFollow:
     # Store data for auto-df model
     self.auto_df_model_data.append([self._norm(self.car_data.v_ego, 'v_ego'),
                                     self._norm(self.lead_data.v_lead, 'v_lead'),
+                                    self._norm(self.lead_data.a_lead, 'a_lead'),
                                     self._norm(self.lead_data.x_lead, 'x_lead')])
     while len(self.auto_df_model_data) > self.model_input_len:
       del self.auto_df_model_data[0]
@@ -220,12 +225,27 @@ class DynamicFollow:
         return calc_mod
     return None
 
-  def global_profile_mod(self, TR, profile_mod_pos, profile_mod_neg):
-    if self.global_df_mod is not None:  # only apply when not in sng
-      TR *= self.global_df_mod
-      profile_mod_pos *= (1 - self.global_df_mod) + 1
-      profile_mod_neg *= self.global_df_mod
-    return TR, profile_mod_pos, profile_mod_neg
+  def global_profile_mod(self, profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist):
+    """
+    This function modifies the y_dist list used by dynamic follow in accordance with global_df_mod
+    It also intelligently adjusts the profile mods at each breakpoint based on the change in TR
+    """
+    if self.global_df_mod is None:
+      return profile_mod_pos, profile_mod_neg, y_dist
+    global_df_mod = 1 - self.global_df_mod
+
+    # Calculate new TRs
+    speeds = [0, self.sng_speed, 18, x_vel[-1]]  # [0, 18 mph, ~40 mph, highest profile mod speed (~78 mph)]
+    mods = [0, 0.1, 0.7, 1]  # how much to limit global_df_mod at each speed, 1 is full effect
+    y_dist_new = [y - (y * global_df_mod * np.interp(x, speeds, mods)) for x, y in zip(x_vel, y_dist)]
+
+    # Calculate how to change profile mods based on change in TR
+    # eg. if df mod is 0.7, then increase positive mod and decrease negative mod
+    calc_profile_mods = [(np.interp(mod_x, x_vel, y_dist) - np.interp(mod_x, x_vel, y_dist_new) + 1) for mod_x in profile_mod_x]
+    profile_mod_pos = [mod_pos * mod for mod_pos, mod in zip(profile_mod_pos, calc_profile_mods)]
+    profile_mod_neg = [mod_neg * ((1 - mod) + 1) for mod_neg, mod in zip(profile_mod_neg, calc_profile_mods)]
+
+    return profile_mod_pos, profile_mod_neg, y_dist_new
 
   def _get_TR(self):
     x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocities
@@ -261,24 +281,23 @@ class DynamicFollow:
     else:
       raise Exception('Unknown profile type: {}'.format(df_profile))
 
+    # Global df mod
+    profile_mod_pos, profile_mod_neg, y_dist = self.global_profile_mod(profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist)
+
     # Profile modifications - Designed so that each profile reacts similarly to changing lead dynamics
     profile_mod_pos = interp(self.car_data.v_ego, profile_mod_x, profile_mod_pos)
     profile_mod_neg = interp(self.car_data.v_ego, profile_mod_x, profile_mod_neg)
 
-    sng_TR = 1.8  # reacceleration stop and go TR
-    sng_speed = 18.0 * CV.MPH_TO_MS
-
-    if self.car_data.v_ego > sng_speed:  # keep sng distance until we're above sng speed again
+    if self.car_data.v_ego > self.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 (self.car_data.v_ego >= self.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)
-      TR, profile_mod_pos, profile_mod_neg = self.global_profile_mod(TR, profile_mod_pos, profile_mod_neg)  # only within normal driving conditions
     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 * 0.7, sng_speed]  # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating
-      y = [sng_TR, interp(sng_speed, x_vel, y_dist)]
+      x = [self.sng_speed * 0.7, self.sng_speed]  # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating
+      y = [self.sng_TR, interp(self.sng_speed, x_vel, y_dist)]
       TR = interp(self.car_data.v_ego, x, y)
 
     TR_mods = []
@@ -297,7 +316,7 @@ class DynamicFollow:
       if self.lead_data.v_lead - deadzone > self.car_data.v_ego:
        TR_mods.append(rel_accel_mod)
 
-    x = [sng_speed / 5.0, sng_speed]  # as we approach 0, apply x% more distance
+    x = [self.sng_speed / 5.0, self.sng_speed]  # as we approach 0, apply x% more distance
     y = [1.05, 1.0]
     profile_mod_pos *= interp(self.car_data.v_ego, x, y)  # but only for currently positive mods
 
@@ -306,9 +325,10 @@ class DynamicFollow:
 
     if self.car_data.left_blinker or self.car_data.right_blinker and df_profile != self.df_profiles.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)
-    return clip(TR, 0.9, 2.7)
+      y = [1.0, .75, .65]
+      TR *= interp(self.car_data.v_ego, x, y)  # reduce TR when changing lanes
+
+    return float(clip(TR, self.min_TR, 2.7))
 
   def update_lead(self, v_lead=None, a_lead=None, x_lead=None, status=False, new_lead=False):
     self.lead_data.v_lead = v_lead
@@ -329,4 +349,5 @@ class DynamicFollow:
   def _get_live_params(self):
     self.global_df_mod = None #self.op_params.get('global_df_mod', None)
     if self.global_df_mod is not None:
-      self.global_df_mod = np.clip(self.global_df_mod, 0.7, 1.1)
+      self.global_df_mod = clip(self.global_df_mod, 0.85, 1.2)
+    self.min_TR = 0.9  # default

selfdrive/controls/lib/dynamic_follow/auto_df.py

@@ -16,7 +16,5 @@ def predict(x):
   l1 = np.dot(l0, w[1]) + b[1]
   l1 = np.maximum(0, l1)
   l2 = np.dot(l1, w[2]) + b[2]
-  l2 = np.maximum(0, l2)
-  l3 = np.dot(l2, w[3]) + b[3]
-  l3 = softmax(l3)
-  return l3
+  l2 = softmax(l2)
+  return l2