zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Controls - Conditional Experimental Mode - Slower Lead Detected Ahead

Browse Source 
Switch to 'Experimental Mode' when a slower lead vehicle is detected ahead.
This commit is contained in:
FrogAi 2024-05-26 23:06:02 -07:00
parent eb64c6837d
commit c13261e391
2 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
selfdrive/frogpilot/controls/frogpilot_planner.py
View File

@ -26,6 +26,8 @@ class FrogPilotPlanner:
self.cem = ConditionalExperimentalMode()
self.slower_lead = False
self.acceleration_jerk = 0
self.frame = 0
self.road_curvature = 0
@ -60,6 +62,11 @@ class FrogPilotPlanner:
self.frame += 1
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 slower lead
if frogpilot_toggles.conditional_experimental_mode and v_lead < v_ego:
distance_factor = max(lead_distance - (v_lead * self.t_follow), 1)
braking_offset = np.clip((v_ego - v_lead) - COMFORT_BRAKE, 1, distance_factor)
self.slower_lead = max(braking_offset, 1) > 1
def update_v_cruise(self, carState, controlsState, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, v_cruise, v_ego, frogpilot_toggles):
v_cruise_cluster = max(controlsState.vCruiseCluster, controlsState.vCruise) * CV.KPH_TO_MS

14
selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
View File

@ -14,6 +14,7 @@ class ConditionalExperimentalMode:
self.curvature_mac = MovingAverageCalculator()
self.lead_detection_mac = MovingAverageCalculator()
self.slow_lead_mac = MovingAverageCalculator()
def update(self, carState, enabled, frogpilotNavigation, lead_distance, lead, modelData, road_curvature, slower_lead, v_ego, v_lead, frogpilot_toggles):
self.update_conditions(lead_distance, lead.status, modelData, road_curvature, slower_lead, carState.standstill, v_ego, v_lead, frogpilot_toggles)
@ -37,6 +38,10 @@ class ConditionalExperimentalMode:
self.status_value = 11 if self.lead_detected else 12
return True
if frogpilot_toggles.conditional_slower_lead and self.slower_lead_detected:
self.status_value = 12 if self.lead_stopped else 13
return True
if frogpilot_toggles.conditional_curves and self.curve_detected:
self.status_value = 15
return True
@ -46,6 +51,7 @@ class ConditionalExperimentalMode:
def update_conditions(self, lead_distance, lead_status, modelData, road_curvature, slower_lead, standstill, v_ego, v_lead, frogpilot_toggles):
self.lead_detection(lead_status)
self.road_curvature(road_curvature, v_ego, frogpilot_toggles)
self.slow_lead(slower_lead)
def lead_detection(self, lead_status):
self.lead_detection_mac.add_data(lead_status)
@ -61,3 +67,11 @@ class ConditionalExperimentalMode:
else:
self.curvature_mac.reset_data()
self.curve_detected = False
def slow_lead(self, slower_lead):
if self.lead_detected:
self.slow_lead_mac.add_data(self.lead_stopped or slower_lead)
self.slower_lead_detected = self.slow_lead_mac.get_moving_average() >= PROBABILITY
else:
self.slow_lead_mac.reset_data()
self.slower_lead_detected = False