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

Controls - Conditional Experimental Mode - Curve Detected Ahead

Browse Source 
Switch to 'Experimental Mode' when a curve is detected.
This commit is contained in:
FrogAi 2024-05-26 22:52:10 -07:00
parent 2405523492
commit e0a97132e5
1 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -9,8 +9,10 @@ class ConditionalExperimentalMode:
def __init__(self): def __init__(self):
self.params_memory = Params("/dev/shm/params") self.params_memory = Params("/dev/shm/params")
self.curve_detected = False
self.experimental_mode = False self.experimental_mode = False
self.curvature_mac = MovingAverageCalculator()
self.lead_detection_mac = MovingAverageCalculator() self.lead_detection_mac = MovingAverageCalculator()
def update(self, carState, enabled, frogpilotNavigation, lead_distance, lead, modelData, road_curvature, slower_lead, v_ego, v_lead, frogpilot_toggles): def update(self, carState, enabled, frogpilotNavigation, lead_distance, lead, modelData, road_curvature, slower_lead, v_ego, v_lead, frogpilot_toggles):
@ -30,11 +32,27 @@ class ConditionalExperimentalMode:
self.status_value = 11 if self.lead_detected else 12 self.status_value = 11 if self.lead_detected else 12
return True return True
if frogpilot_toggles.conditional_curves and self.curve_detected:
self.status_value = 15
return True
return False return False
def update_conditions(self, lead_distance, lead_status, modelData, road_curvature, slower_lead, standstill, v_ego, v_lead, frogpilot_toggles): 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.lead_detection(lead_status)
self.road_curvature(road_curvature, v_ego, frogpilot_toggles)
def lead_detection(self, lead_status): def lead_detection(self, lead_status):
self.lead_detection_mac.add_data(lead_status) self.lead_detection_mac.add_data(lead_status)
self.lead_detected = self.lead_detection_mac.get_moving_average() >= PROBABILITY self.lead_detected = self.lead_detection_mac.get_moving_average() >= PROBABILITY
def road_curvature(self, road_curvature, v_ego, frogpilot_toggles):
if frogpilot_toggles.conditional_curves_lead or not self.lead_detected:
curve_detected = (1 / road_curvature)**0.5 < v_ego
curve_active = (0.9 / road_curvature)**0.5 < v_ego and self.curve_detected
self.curvature_mac.add_data(curve_detected or curve_active)
self.curve_detected = self.curvature_mac.get_moving_average() >= PROBABILITY
else:
self.curvature_mac.reset_data()
self.curve_detected = False