Reduce tailgating for one bar distance at surface street speeds & cod…

…e cleanup (commaai#43)

* Gernby's Lane width filter fix (commaai#41)

* Fix lane width filtering

* Fixed lane width filtering

* CAMERA_OFFSET = 0.08

* Minimum distance for one bar distance setting

* Don't tailgate at slow speeds with 1 bar distance

* x_lead < 5 only

* x_lead < 5 syntax fix

* self.v_rel < -1 or (x_lead < 10 and self.v_rel >= -1 and self.v_rel < 1)

* Tail gating auto distance change

* x_lead < 7.5 and self.v_rel >= -1 and self.v_rel < 0.2

* self.lastTR > 0 re-adding reinitialization

* Revert "Gernby's Lane width filter fix (commaai#41)"

This reverts commit d081cfd8a05d2eb32e0148d477c806e65662dea7.

* Increase min distance to 10m for auto distance

* Auto distance code cleanup

* Add lead car fast approaching autodistance

* Auto distance Code cleanup

* Don't re-initialize MPC if already initialized

* self.lastTR != instead of >

* Initialize TR = 1.8

* include if statements vs just assigning boolean

* switch to 1 and 0 for TRUE AND FALSE

* Revert "switch to 1 and 0 for TRUE AND FALSE"

This reverts commit 58dcf718cdc3fca636229bf29942669670cb4487.

* Revert "include if statements vs just assigning boolean"

This reverts commit da7b5fa5088d62e78ba57d9e4c01e89160e77b9e.

* Revert "Initialize TR = 1.8"

This reverts commit f9c2571faeb0acaab818bcff146a2a397c0f07ff.

* Initialize TR=1.8 inside override loop

* Cleanup TR assignment for override loop

* Code Cleanup

* Syntax error

* Syntax error

* if statements for autodistance vars

* tr-last assignment indent

* CS.readdistancelines assignment to self.tr_last

* remove else MPC init

* Flatten out elif ladder for autodistance

* Removed shrinking_fast stopping for now

Braking is too sudden with 3 bar distance when radar detects car.  Shrinking gap takes care of this with 2 bar distance.

* Code Cleanup

selfdrive/controls/lib/pathplanner.py

@@ -1,7 +1,7 @@
 from common.numpy_fast import interp
 from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv
 
-CAMERA_OFFSET = 0.06  # m from center car to camera
+CAMERA_OFFSET = 0.08  # m from center car to camera
 
 class PathPlanner(object):
   def __init__(self):

selfdrive/controls/lib/planner.py

@@ -151,6 +151,10 @@ def __init__(self, mpc_id, live_longitudinal_mpc):
     self.override = False      # for one bar distance at low speeds - introduce early braking and hysteresis for resume follow
     self.lastTR = 2
     self.last_cloudlog_t = 0.0
+    self.v_rel = 10
+    self.tailgating = 0
+    self.street_speed = 0
+    self.lead_car_gap_shrinking = 0
 
   def send_mpc_solution(self, qp_iterations, calculation_time):
     qp_iterations = max(0, qp_iterations)
@@ -221,38 +225,56 @@ def update(self, CS, lead, v_cruise_setpoint):
     # Calculate mpc
     t = sec_since_boot()
 
-    # Override with 3 bar distance profile if using one bar distance - hysteresis for safety
-    #if self.override and CS.aEgo >= 0:
-      #self.override = False  # When car starts accelerating, resume one bar distance profile
-
-    if CS.readdistancelines == 1 and CS.vEgo < 18.06 and (v_lead - CS.vEgo) < -1:
-      #self.override = True   # If we start braking, then keep 3 bar distance profile until car starts to accelerate
-      TR=1.8
-      #if self.lastTR > 0:
-        #self.libmpc.init(MPC_COST_LONG.TTC, 0.1, PC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
-        #self.lastTR = 0
+    # Calculate conditions
+    self.v_rel = v_lead - CS.vEgo   # calculate relative velocity vs lead car
+
+    # Defining some variables to make the logic more human readable for auto distance override below
+    # Is the car tailgating the lead car?
+    if x_lead < 7.5 and self.v_rel >= -1 and self.v_rel < 0.25:
+      self.tailgating = 1
     else:
-      if CS.readdistancelines == 2:
-        TR=1.8 # 20m at 40km/hr
-        if CS.readdistancelines != self.lastTR:
+      self.tailgating = 0
+
+    # Is the car running surface street speeds?
+    if CS.vEgo < 18.06:
+      self.street_speed = 1
+    else:
+      self.street_speed = 0
+
+    # Is the gap from the lead car shrinking?
+    if self.v_rel < -1:
+      self.lead_car_gap_shrinking = 1
+    else:
+      self.lead_car_gap_shrinking = 0
+
+    # Adjust distance from lead car when distance button pressed 
+    if CS.readdistancelines == 1:
+      # If one bar distance, auto set to 2 bar distance under current conditions to prevent rear ending lead car
+      if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
+        TR=1.8
+        if self.lastTR != 0:
           self.libmpc.init(MPC_COST_LONG.TTC, 0.1, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
-          self.lastTR = CS.readdistancelines
-
-      elif CS.readdistancelines == 1:
+          self.lastTR = 0
+      else:
         TR=0.9 # 10m at 40km/hr
         if CS.readdistancelines != self.lastTR:
           self.libmpc.init(MPC_COST_LONG.TTC, 1.0, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
-          self.lastTR = CS.readdistancelines
+          self.lastTR = CS.readdistancelines  
+
+    elif CS.readdistancelines == 2:
+      TR=1.8 # 20m at 40km/hr
+      if CS.readdistancelines != self.lastTR:
+        self.libmpc.init(MPC_COST_LONG.TTC, 0.1, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+        self.lastTR = CS.readdistancelines  
+
+    elif CS.readdistancelines == 3:
+      TR=2.7 # 30m at 40km/hr
+      if CS.readdistancelines != self.lastTR:
+        self.libmpc.init(MPC_COST_LONG.TTC, 0.05, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) 
+        self.lastTR = CS.readdistancelines  
 
-      elif CS.readdistancelines == 3:
-        TR=2.7 # 30m at 40km/hr
-        if CS.readdistancelines != self.lastTR:
-          self.libmpc.init(MPC_COST_LONG.TTC, 0.05, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
-          self.lastTR = CS.readdistancelines
-
-      else:
-        TR=1.8 # if readdistancelines = 0
-
+    else:
+      TR=1.8 # if readdistancelines = 0
 
     n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, TR)
     duration = int((sec_since_boot() - t) * 1e9)