zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Controls - Longitudinal Tuning - Increase Stop Distance Behind Lead

Browse Source 
Increase the stopping distance for a more comfortable stop from lead vehicles.
This commit is contained in:
FrogAi 2024-05-14 11:18:27 -07:00
parent f37f6ecd4e
commit 48169cfba3
2 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@ -323,10 +323,10 @@ class LongitudinalMpc:
lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
return lead_xv return lead_xv
def process_lead(self, lead): def process_lead(self, lead, increased_stopping_distance=0):
v_ego = self.x0[1] v_ego = self.x0[1]
if lead is not None and lead.status: if lead is not None and lead.status:
x_lead = lead.dRel x_lead = lead.dRel - increased_stopping_distance
v_lead = lead.vLead v_lead = lead.vLead
a_lead = lead.aLeadK a_lead = lead.aLeadK
a_lead_tau = lead.aLeadTau a_lead_tau = lead.aLeadTau
@ -355,8 +355,9 @@ class LongitudinalMpc:
def update(self, radarstate, v_cruise, x, v, a, j, t_follow, frogpilot_toggles, personality=log.LongitudinalPersonality.standard): def update(self, radarstate, v_cruise, x, v, a, j, t_follow, frogpilot_toggles, personality=log.LongitudinalPersonality.standard):
v_ego = self.x0[1] v_ego = self.x0[1]
self.status = radarstate.leadOne.status or radarstate.leadTwo.status self.status = radarstate.leadOne.status or radarstate.leadTwo.status
increased_distance = frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0)
lead_xv_0 = self.process_lead(radarstate.leadOne) lead_xv_0 = self.process_lead(radarstate.leadOne, increased_distance)
lead_xv_1 = self.process_lead(radarstate.leadTwo) lead_xv_1 = self.process_lead(radarstate.leadTwo)
# To estimate a safe distance from a moving lead, we calculate how much stopping # To estimate a safe distance from a moving lead, we calculate how much stopping

4
selfdrive/frogpilot/controls/frogpilot_planner.py
View File

@ -66,7 +66,9 @@ class FrogPilotPlanner:
v_ego = max(carState.vEgo, 0) v_ego = max(carState.vEgo, 0)
v_lead = self.lead_one.vLead v_lead = self.lead_one.vLead
lead_distance = self.lead_one.dRel distance_offset = max(frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0), 0)
lead_distance = self.lead_one.dRel - distance_offset
stopping_distance = STOP_DISTANCE + distance_offset
if frogpilot_toggles.acceleration_profile == 1: if frogpilot_toggles.acceleration_profile == 1:
self.max_accel = get_max_accel_eco(v_ego) self.max_accel = get_max_accel_eco(v_ego)