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

Aggressive acceleration with a lead

Browse Source 
Added toggle to make the acceleration more aggressive when following a lead.
This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent 7383ee0ddc
commit c1a7f1e3b0
5 changed files with 12 additions and 3 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

@@ -330,7 +330,7 @@ class LongitudinalMpc:
self.cruise_min_a = min_a
self.max_a = max_a
def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, personality=log.LongitudinalPersonality.standard):
t_follow = get_T_FOLLOW(personality)
v_ego = self.x0[1]
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
@@ -338,6 +338,12 @@ class LongitudinalMpc:
lead_xv_0 = self.process_lead(radarstate.leadOne)
lead_xv_1 = self.process_lead(radarstate.leadTwo)
# 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 = t_follow / t_follow_offset
# 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.

2
selfdrive/controls/lib/longitudinal_planner.py
View File

@@ -139,7 +139,7 @@ class LongitudinalPlanner:
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=self.personality)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, frogpilot_planner.aggressive_acceleration, personality=self.personality)
self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)