Merge pull request commaai#84 from ShaneSmiskol/dynamic-follow

Changed dynamic following distance to fourth profile and restored 2.7…
springermac · Feb 16, 2019 · c93c24d · c93c24d
2 parents 6358c07 + 2aa7902
commit c93c24d
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 9 deletions.
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
@@ -197,7 +197,7 @@ def __init__(self, CP):
     self.CL_WAIT_BEFORE_START = 1
     #END OF ALCA PARAMS
 
-    self.read_distance_lines_prev = 3
+    self.read_distance_lines_prev = 4
 
     self.CP = CP
     self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
@@ -432,6 +432,8 @@ def update(self, cp, cp_cam):
       if self.read_distance_lines == 2:
         self.UE.custom_alert_message(2,"Following distance set to 1.8s",200,3)
       if self.read_distance_lines == 3:
+        self.UE.custom_alert_message(2,"Following distance set to 2.7s",200,3)
+      if self.read_distance_lines == 4:
         self.UE.custom_alert_message(2,"Dynamic following distance",200,3)
       self.read_distance_lines_prev = self.read_distance_lines
 

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
@@ -196,7 +196,7 @@ def __init__(self, CP):
     self.includeradius = 22
     self.blind_spot_on = bool(0)
     self.distance_toggle_prev = 2
-    self.read_distance_lines_prev = 3
+    self.read_distance_lines_prev = 4
     self.lane_departure_toggle_on_prev = True
     self.acc_slow_on_prev = False
     #BB UIEvents
@@ -374,6 +374,8 @@ def update(self, cp, cp_cam):
       if self.read_distance_lines == 2:
         self.UE.custom_alert_message(2,"Following distance set to 1.8s",200,3)
       if self.read_distance_lines == 3:
+        self.UE.custom_alert_message(2,"Following distance set to 2.7s",200,3)
+      if self.read_distance_lines == 4:
         self.UE.custom_alert_message(2,"Dynamic following distance",200,3)
       self.read_distance_lines_prev = self.read_distance_lines
 

diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
@@ -271,27 +271,34 @@ def update(self, CS, lead, v_cruise_setpoint):
 
     # Calculate mpc
     t = sec_since_boot()
-    if CS.vEgo < 2:
+    if CS.vEgo < 2.0:
       TR=1.8 # under 41km/hr use a TR of 1.8 seconds
       #if self.lastTR > 0:
         #self.libmpc.init(MPC_COST_LONG.TTC, 0.1, PC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
         #self.lastTR = 0
     else:
-      if CS.readdistancelines == 2:
+      if CS.readdistancelines == 1:
+        if CS.readdistancelines == self.lastTR:
+          TR=0.9 # 10m at 40km/hr
+        else:
+          TR=0.9
+          self.libmpc.init(MPC_COST_LONG.TTC, 1.0, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+          self.lastTR = CS.readdistancelines
+      elif CS.readdistancelines == 2:
         if CS.readdistancelines == self.lastTR:
           TR=1.8 # 20m at 40km/hr
         else:
           TR=1.8
           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:
+      elif CS.readdistancelines == 3:
         if CS.readdistancelines == self.lastTR:
-          TR=0.9 # 10m at 40km/hr
+          TR = 2.7
         else:
-          TR=0.9
-          self.libmpc.init(MPC_COST_LONG.TTC, 1.0, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+          TR = 2.7  # 30m at 40km/hr
+          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:
+      elif CS.readdistancelines == 4:
         if len(self.speed_list) > 400 and len(self.speed_list) != 0:
           self.speed_list.pop(0)
         self.speed_list.append(CS.vEgo * 2.236936)