mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-09 01:23:43 +08:00
Code Issues 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-06-26 00:40:29 -07:00
parent 63a3558a20
commit ed0379a245
1 changed files with 15 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
+15
View File
@@ -7,8 +7,11 @@ class ConditionalExperimentalMode:
def __init__(self):
self.params_memory = Params("/dev/shm/params")
self.curve_detected = False
self.experimental_mode = False
self.curvature_mac = MovingAverageCalculator()
def update(self, carState, frogpilotNavigation, lead, modelData, model_length, road_curvature, slower_lead, tracking_lead, v_ego, v_lead, frogpilot_toggles):
if not carState.standstill:
self.update_conditions(lead.dRel, model_length, road_curvature, slower_lead, tracking_lead, v_ego, v_lead, frogpilot_toggles)
@@ -22,6 +25,18 @@ class ConditionalExperimentalMode:
self.status_value = 7 if tracking_lead else 8
return True
if frogpilot_toggles.conditional_curves and self.curve_detected and (frogpilot_toggles.conditional_curves_lead or not tracking_lead):
self.status_value = 12
return True
return False
def update_conditions(self, lead_distance, model_length, road_curvature, slower_lead, tracking_lead, v_ego, v_lead, frogpilot_toggles):
self.road_curvature(road_curvature, v_ego, frogpilot_toggles)
def road_curvature(self, road_curvature, v_ego, frogpilot_toggles):
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