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 - Conditional Experimental Mode - Curve Detected Ahead

Browse Source 
Switch to 'Experimental Mode' when a curve is detected.
This commit is contained in:
FrogAi
2024-07-31 17:25:38 -07:00
parent a1749cd8db
commit 723d2e9c14
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
@@ -9,6 +9,9 @@ class ConditionalExperimentalMode:
self.frogpilot_planner = FrogPilotPlanner
self.curvature_mac = MovingAverageCalculator()
self.curve_detected = False
self.experimental_mode = False
def update(self, carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles):
@@ -26,6 +29,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, tracking_lead, v_ego, v_lead, frogpilot_toggles):
self.curve_detection(v_ego, frogpilot_toggles)
def curve_detection(self, v_ego, frogpilot_toggles):
curve_detected = (1 / self.frogpilot_planner.road_curvature)**0.5 < v_ego
curve_active = (0.9 / self.frogpilot_planner.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