Fix sharp braking when transitioning 4 to 1 bar

edukinara · Apr 27, 2019 · 13b8477 · 13b8477
1 parent bcb4ef7
commit 13b8477
Showing 1 changed file with 13 additions and 13 deletions.
diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
@@ -133,39 +133,39 @@ def update(self, CS, lead, v_cruise_setpoint):
         TR = interp(-self.v_rel, ONE_BAR_PROFILE_BP, ONE_BAR_PROFILE)  
       else:
         TR = ONE_BAR_DISTANCE 
-      if CS.carState.readdistancelines != self.lastTR:
-        self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
-        self.lastTR = CS.carState.readdistancelines  
+      #if CS.carState.readdistancelines != self.lastTR:
+      #  self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+      #  self.lastTR = CS.carState.readdistancelines  
 
     elif CS.carState.readdistancelines == 2:
       #if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
       if self.street_speed:
         TR = interp(-self.v_rel, TWO_BAR_PROFILE_BP, TWO_BAR_PROFILE)
       else:
         TR = TWO_BAR_DISTANCE 
-      if CS.carState.readdistancelines != self.lastTR:
-        self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
-        self.lastTR = CS.carState.readdistancelines  
+      #if CS.carState.readdistancelines != self.lastTR:
+      #  self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+      #  self.lastTR = CS.carState.readdistancelines  
 
     elif CS.carState.readdistancelines == 3:
       if self.street_speed:
       #if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
         TR = interp(-self.v_rel, THREE_BAR_PROFILE_BP, THREE_BAR_PROFILE)
       else:
         TR = THREE_BAR_DISTANCE 
-      if CS.carState.readdistancelines != self.lastTR:
-        self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
-        self.lastTR = CS.carState.readdistancelines   
+      #if CS.carState.readdistancelines != self.lastTR:
+      #  self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+      #  self.lastTR = CS.carState.readdistancelines   
 
     elif CS.carState.readdistancelines == 4:
       TR = FOUR_BAR_DISTANCE
-      if CS.carState.readdistancelines != self.lastTR:
-        self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) 
-        self.lastTR = CS.carState.readdistancelines      
+      #if CS.carState.readdistancelines != self.lastTR:
+      #  self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) 
+      #  self.lastTR = CS.carState.readdistancelines      
 
     else:
      TR = TWO_BAR_DISTANCE # if readdistancelines != 1,2,3,4
-     self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+     #self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)