From cee67b6fcaf140b1f05b0de3d6e96a6bf3e23420 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Wed, 12 Jun 2019 08:19:50 -0400 Subject: [PATCH] reduce phantom brakes when lead car is detected --- selfdrive/car/tesla/PCC_module.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index 61447ce7dbb503..58803e90313815 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -165,6 +165,7 @@ def __init__(self,carcontroller): self.a_acc_sol = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 + self.had_lead = False #Long Control self.LoC = None #when was radar data last updated? @@ -497,7 +498,10 @@ def calc_follow_speed_ms(self, CS): else: lead_dist_m = _visual_radar_adjusted_dist_m(self.lead_1.dRel, CS) # Grab the relative speed. - rel_speed_kph = self.lead_1.vRel * CV.MS_TO_KPH + rel_speed_kph = 0. + if self.had_lead: + #avoid inital break when lead just detected + rel_speed_kph = self.lead_1.vRel * CV.MS_TO_KPH # v_ego is in m/s, so safe_distance is in meters. safe_dist_m = _safe_distance_m(CS.v_ego) # Current speed in kph @@ -509,7 +513,9 @@ def calc_follow_speed_ms(self, CS): # If no lead is present, accel up to max speed if lead_dist_m == 0: new_speed_kph = self.pedal_speed_kph + self.had_lead = False elif lead_dist_m > 0: + self.had_lead = True lead_absolute_speed_kph = actual_speed_kph + rel_speed_kph if lead_dist_m < MIN_SAFE_DIST_M: new_speed_kph = MIN_PCC_V_KPH @@ -543,7 +549,7 @@ def calc_follow_speed_ms(self, CS): # still an acceptable speed, accept it. This could happen if the # driver manually accelerates, or if we roll down a hill. In either # case, don't fight the extra velocity unless necessary. - if actual_speed_kph > new_speed_kph and min_kph < actual_speed_kph < max_kph: + if (actual_speed_kph > new_speed_kph) and (min_kph < actual_speed_kph < max_kph): new_speed_kph = actual_speed_kph new_speed_kph = clip(new_speed_kph, min_kph, max_kph) @@ -594,7 +600,7 @@ def _min_safe_vrel_kph(m): # (meters, safe relative velocity in kph) # Remember, a negative relative velocity means we are closing the distance. (MIN_SAFE_DIST_M, 1), # If lead is close, it better be pulling away. - (200, -15)]) # if lead is far, it's ok if we're closing. + (100, -15)]) # if lead is far, it's ok if we're closing. return _interp_map(m, min_vrel_by_distance) def _is_present(lead):