diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index d4cf0331b55e6e..d6de53c26ae62a 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -235,7 +235,7 @@ def update(self, CS, lead, v_cruise_setpoint): # 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 or (x_lead < 17.5 and self.v_rel < 0.5): + if x_lead < 9 or (x_lead < 17.5 and self.v_rel < 0.5): self.tailgating = 1 else: self.tailgating = 0 @@ -252,39 +252,39 @@ def update(self, CS, lead, v_cruise_setpoint): else: self.lead_car_gap_shrinking = 0 - # Is the gap from the lead car shrinking FAST? + # Is the gap from the lead car shrinking FAST (or approaching a nearly stopped vehicle)? if self.v_rel < -7.5: self.lead_car_gap_shrinking_fast = 1 else: self.lead_car_gap_shrinking_fast = 0 + + # Auto Switch to 3 bar distance to create a more conservative stopping under specific conditions + if self.lead_car_gap_shrinking_fast: + TR=2.7 + if self.lastTR != -3: + self.libmpc.init(MPC_COST_LONG.TTC, 0.05, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.lastTR = -3 + + elif self.street_speed and (self.lead_car_gap_shrinking or self.tailgating): + # More conservative braking than comma default + TR=2.2 + if self.lastTR != -2: + self.libmpc.init(MPC_COST_LONG.TTC, 0.0875, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.lastTR = -2 # 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.lead_car_gap_shrinking_fast or (self.street_speed and (self.lead_car_gap_shrinking or self.tailgating)): - TR=2.1 - if self.lastTR != -1: - self.libmpc.init(MPC_COST_LONG.TTC, 0.0825, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) - self.lastTR = -1 - 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 + elif CS.readdistancelines == 1: + TR=1.2 + 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 elif CS.readdistancelines == 2: - # More conservative braking than comma default - if self.lead_car_gap_shrinking_fast or (self.street_speed and (self.lead_car_gap_shrinking or self.tailgating)): - TR=2.0 - if self.lastTR != -2: - self.libmpc.init(MPC_COST_LONG.TTC, 0.0875, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) - self.lastTR = -2 - else: - 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 + 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