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

Increase the stopping distance

Browse Source 
Added toggle to increase the stopping distance.
This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent 72fac6ed9d
commit 54c9cc3195
5 changed files with 20 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -322,10 +322,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
@@ -351,19 +351,19 @@ class LongitudinalMpc:
self.cruise_min_a = min_a
self.max_a = max_a
def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, custom_personalities, aggressive_follow, standard_follow, relaxed_follow, personality=log.LongitudinalPersonality.standard):
def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, custom_personalities, aggressive_follow, standard_follow, relaxed_follow, increased_stopping_distance, personality=log.LongitudinalPersonality.standard):
t_follow = get_T_FOLLOW(custom_personalities, aggressive_follow, standard_follow, relaxed_follow, personality)
self.t_follow = t_follow
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, increased_stopping_distance)
lead_xv_1 = self.process_lead(radarstate.leadTwo, increased_stopping_distance)
# Offset by FrogAi for FrogPilot for a more natural takeoff with a lead
if aggressive_acceleration:
distance_factor = np.maximum(1, lead_xv_0[:,0] - (lead_xv_0[:,1] * t_follow))
t_follow_offset = np.clip((lead_xv_0[:,1] - v_ego) + (STOP_DISTANCE - v_ego), 1, distance_factor)
t_follow_offset = np.clip((lead_xv_0[:,1] - v_ego) + (STOP_DISTANCE + increased_stopping_distance - v_ego), 1, distance_factor)
t_follow = t_follow / t_follow_offset
# LongitudinalPlan variables for onroad driving insights