brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Increase stop distance

Browse Source 
Added toggle to increase the stop distance from lead vehicles.
This commit is contained in:
FrogAi
2024-02-27 16:34:47 -07:00
parent d78099c85b
commit c131087dc0
5 changed files with 17 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -326,10 +326,10 @@ class LongitudinalMpc:
lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
return lead_xv
def process_lead(self, lead):
def process_lead(self, lead, increased_stopping_distance):
v_ego = self.x0[1]
if lead is not None and lead.status:
x_lead = lead.dRel
x_lead = lead.dRel - increased_stopping_distance
v_lead = lead.vLead
a_lead = lead.aLeadK
a_lead_tau = lead.aLeadTau
@@ -361,8 +361,8 @@ class LongitudinalMpc:
v_ego = self.x0[1]
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
lead_xv_0 = self.process_lead(radarstate.leadOne)
lead_xv_1 = self.process_lead(radarstate.leadTwo)
lead_xv_0 = self.process_lead(radarstate.leadOne, frogpilot_planner.increased_stopping_distance)
lead_xv_1 = self.process_lead(radarstate.leadTwo, frogpilot_planner.increased_stopping_distance)
# Offset by FrogAi for FrogPilot for a more natural takeoff with a lead
if frogpilot_planner.aggressive_acceleration: