From c13261e3915a987ac27d8448a9fba1caf867b93e Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Sun, 26 May 2024 23:06:02 -0700
Subject: [PATCH] Controls - Conditional Experimental Mode - Slower Lead
 Detected Ahead

Switch to 'Experimental Mode' when a slower lead vehicle is detected ahead.
---
 selfdrive/frogpilot/controls/frogpilot_planner.py  |  7 +++++++
 .../controls/lib/conditional_experimental_mode.py  | 14 ++++++++++++++
 2 files changed, 21 insertions(+)

diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py
index bb1a1c8..a7be82f 100644
--- a/selfdrive/frogpilot/controls/frogpilot_planner.py
+++ b/selfdrive/frogpilot/controls/frogpilot_planner.py
@@ -26,6 +26,8 @@ class FrogPilotPlanner:
 
     self.cem = ConditionalExperimentalMode()
 
+    self.slower_lead = False
+
     self.acceleration_jerk = 0
     self.frame = 0
     self.road_curvature = 0
@@ -60,6 +62,11 @@ class FrogPilotPlanner:
     self.frame += 1
 
   def update_follow_values(self, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles):
+    # Offset by FrogAi for FrogPilot for a more natural approach to a slower lead
+    if frogpilot_toggles.conditional_experimental_mode and v_lead < v_ego:
+      distance_factor = max(lead_distance - (v_lead * self.t_follow), 1)
+      braking_offset = np.clip((v_ego - v_lead) - COMFORT_BRAKE, 1, distance_factor)
+      self.slower_lead = max(braking_offset, 1) > 1
 
   def update_v_cruise(self, carState, controlsState, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, v_cruise, v_ego, frogpilot_toggles):
     v_cruise_cluster = max(controlsState.vCruiseCluster, controlsState.vCruise) * CV.KPH_TO_MS
diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
index f0a5d4b..9328819 100644
--- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
+++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
@@ -14,6 +14,7 @@ class ConditionalExperimentalMode:
 
     self.curvature_mac = MovingAverageCalculator()
     self.lead_detection_mac = MovingAverageCalculator()
+    self.slow_lead_mac = MovingAverageCalculator()
 
   def update(self, carState, enabled, frogpilotNavigation, lead_distance, lead, modelData, road_curvature, slower_lead, v_ego, v_lead, frogpilot_toggles):
     self.update_conditions(lead_distance, lead.status, modelData, road_curvature, slower_lead, carState.standstill, v_ego, v_lead, frogpilot_toggles)
@@ -37,6 +38,10 @@ class ConditionalExperimentalMode:
       self.status_value = 11 if self.lead_detected else 12
       return True
 
+    if frogpilot_toggles.conditional_slower_lead and self.slower_lead_detected:
+      self.status_value = 12 if self.lead_stopped else 13
+      return True
+
     if frogpilot_toggles.conditional_curves and self.curve_detected:
       self.status_value = 15
       return True
@@ -46,6 +51,7 @@ class ConditionalExperimentalMode:
   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)
+    self.slow_lead(slower_lead)
 
   def lead_detection(self, lead_status):
     self.lead_detection_mac.add_data(lead_status)
@@ -61,3 +67,11 @@ class ConditionalExperimentalMode:
     else:
       self.curvature_mac.reset_data()
       self.curve_detected = False
+
+  def slow_lead(self, slower_lead):
+    if self.lead_detected:
+      self.slow_lead_mac.add_data(self.lead_stopped or slower_lead)
+      self.slower_lead_detected = self.slow_lead_mac.get_moving_average() >= PROBABILITY
+    else:
+      self.slow_lead_mac.reset_data()
+      self.slower_lead_detected = False