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

Display openpilot logics for following lead vehicles

Browse Source 
Added toggle to show openpilot's logic in regards to following lead vehicles.
This commit is contained in:
FrogAi
2024-02-27 16:34:47 -07:00
parent 480efba54c
commit f59a61cc8f
10 changed files with 196 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -373,6 +373,16 @@ class LongitudinalMpc:
else:
self.acceleration_offset = 1
# LongitudinalPlan variables for onroad driving insights
if self.status:
self.safe_obstacle_distance = int(np.mean(get_safe_obstacle_distance(v_ego, t_follow)))
self.safe_obstacle_distance_stock = int(np.mean(get_safe_obstacle_distance(v_ego, self.t_follow)))
self.stopped_equivalence_factor = int(np.mean(get_stopped_equivalence_factor(lead_xv_0[:,1])))
else:
self.safe_obstacle_distance = 0
self.safe_obstacle_distance_stock = 0
self.stopped_equivalence_factor = 0
# To estimate a safe distance from a moving lead, we calculate how much stopping
# distance that lead needs as a minimum. We can add that to the current distance
# and then treat that as a stopped car/obstacle at this new distance.