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

Smoother braking behind lead

Browse Source 
Added toggle to smooth out the braking behavior when approaching a slower lead vehicle.
This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent ce3caf5a11
commit 830077af74
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

@@ -351,7 +351,7 @@ 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, increased_stopping_distance, personality=log.LongitudinalPersonality.standard):
def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, smoother_braking, 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]
@@ -366,6 +366,12 @@ class LongitudinalMpc:
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
# Offset by FrogAi for FrogPilot for a more natural approach to a slower lead
if smoother_braking:
distance_factor = np.maximum(1, lead_xv_0[:,0] - (lead_xv_0[:,1] * t_follow))
t_follow_offset = np.clip((v_ego - lead_xv_0[:,1]) - COMFORT_BRAKE, 1, distance_factor)
t_follow = t_follow / t_follow_offset
# LongitudinalPlan variables for onroad driving insights
if self.status:
self.safe_obstacle_distance = int(np.mean(get_safe_obstacle_distance(v_ego, t_follow)))

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'], frogpilot_planner.v_cruise, x, v, a, j, frogpilot_planner.aggressive_acceleration, frogpilot_planner.increased_stopping_distance,
self.mpc.update(sm['radarState'], frogpilot_planner.v_cruise, x, v, a, j, frogpilot_planner.aggressive_acceleration, frogpilot_planner.increased_stopping_distance, frogpilot_planner.smoother_braking,
frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_follow, frogpilot_planner.standard_follow, frogpilot_planner.relaxed_follow, personality=self.personality)
self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)