From 2aa790276998b2d99644121edd29364d0238fe99 Mon Sep 17 00:00:00 2001 From: Shane Date: Fri, 15 Feb 2019 19:17:34 -0600 Subject: [PATCH] Changed dynamic following distance to fourth profile and restored 2.7s profile --- selfdrive/car/honda/carstate.py | 4 +++- selfdrive/car/toyota/carstate.py | 4 +++- selfdrive/controls/lib/planner.py | 21 ++++++++++++++------- 3 files changed, 20 insertions(+), 9 deletions(-) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index f332542e7cea12..019d20df192a08 100644 --- 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 index f788a3a5ef4519..0dfc6f981f244e 100644 --- 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 index fb119b5ad70a5b..041f3dfee2dd42 100755 --- 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)