From e0a97132e56dacd32b36ca0d12bb23342904dff6 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Sun, 26 May 2024 22:52:10 -0700
Subject: [PATCH] Controls - Conditional Experimental Mode - Curve Detected
 Ahead

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

diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
index f8af525..68c70bd 100644
--- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
+++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
@@ -9,8 +9,10 @@ class ConditionalExperimentalMode:
   def __init__(self):
     self.params_memory = Params("/dev/shm/params")
 
+    self.curve_detected = False
     self.experimental_mode = False
 
+    self.curvature_mac = MovingAverageCalculator()
     self.lead_detection_mac = MovingAverageCalculator()
 
   def update(self, carState, enabled, frogpilotNavigation, lead_distance, lead, modelData, road_curvature, slower_lead, v_ego, v_lead, frogpilot_toggles):
@@ -30,11 +32,27 @@ class ConditionalExperimentalMode:
       self.status_value = 11 if self.lead_detected else 12
       return True
 
+    if frogpilot_toggles.conditional_curves and self.curve_detected:
+      self.status_value = 15
+      return True
+
     return False
 
   def update_conditions(self, lead_distance, lead_status, modelData, road_curvature, slower_lead, standstill, v_ego, v_lead, frogpilot_toggles):
     self.lead_detection(lead_status)
+    self.road_curvature(road_curvature, v_ego, frogpilot_toggles)
 
   def lead_detection(self, lead_status):
     self.lead_detection_mac.add_data(lead_status)
     self.lead_detected = self.lead_detection_mac.get_moving_average() >= PROBABILITY
+
+  def road_curvature(self, road_curvature, v_ego, frogpilot_toggles):
+    if frogpilot_toggles.conditional_curves_lead or not self.lead_detected:
+      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
+    else:
+      self.curvature_mac.reset_data()
+      self.curve_detected = False