From ed0379a245b26b2bcf0e236159a723cb9363ac6d Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Wed, 26 Jun 2024 00:40:29 -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 62f5066f3..58d247170 100644
--- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
+++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
@@ -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