mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-01 05:33:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Controls - Longitudinal Tuning - Increase Acceleration Behind Lead

Browse Source 
Increase aggressiveness when following a faster lead.
This commit is contained in:
FrogAi
2024-06-26 01:24:27 -07:00
parent 7696618573
commit 20465ffe42
1 changed files with 13 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/frogpilot/controls/frogpilot_planner.py
+13 -1
View File
@@ -82,7 +82,9 @@ class FrogPilotPlanner:
self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles)
def set_acceleration(self, controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles):
if frogpilot_toggles.acceleration_profile == 1:
if self.lead_one.status and frogpilot_toggles.aggressive_acceleration:
self.max_accel = float(np.clip(self.lead_one.aLeadK, get_max_accel_sport(v_ego), 2.0 if v_ego >= 20 else 4.0))
elif frogpilot_toggles.acceleration_profile == 1:
self.max_accel = get_max_accel_eco(v_ego)
elif frogpilot_toggles.acceleration_profile in (2, 3):
self.max_accel = get_max_accel_sport(v_ego)
@@ -124,6 +126,16 @@ class FrogPilotPlanner:
self.speed_jerk = self.base_speed_jerk
def update_follow_values(self, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles):
# Offset by FrogAi for FrogPilot for a more natural approach to a faster lead
if frogpilot_toggles.aggressive_acceleration and v_lead > v_ego:
distance_factor = max(lead_distance - (v_ego * self.t_follow), 1)
standstill_offset = max(stopping_distance - v_ego, 0) * max(v_lead - v_ego, 0)
acceleration_offset = np.clip((v_lead - v_ego) + standstill_offset - COMFORT_BRAKE, 1, distance_factor)
self.acceleration_jerk = self.base_acceleration_jerk / acceleration_offset
self.danger_jerk = self.base_danger_jerk / acceleration_offset
self.speed_jerk = self.base_speed_jerk / acceleration_offset
self.t_follow /= acceleration_offset
# Offset by FrogAi for FrogPilot for a more natural approach to a slower lead
if frogpilot_toggles.conditional_experimental_mode and v_lead < v_ego:
distance_factor = max(lead_distance - (v_lead * self.t_follow), 1)