diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py index 9781171ed..7d205efd5 100644 --- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py @@ -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