From 723d2e9c148bbafe15109c73b09ebf1abfe67634 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Wed, 31 Jul 2024 17:25:38 -0700
Subject: [PATCH] Controls - Conditional Experimental Mode - Curve Detected
 Ahead

Switch to 'Experimental Mode' when a curve is detected.
---
 .../controls/lib/conditional_experimental_mode.py | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

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