From 0e9845cf109465d1b58e54614331fd3b8f6240d7 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Sat, 22 Jun 2024 14:31:30 -0700
Subject: [PATCH] Controls - Conditional Experimental Mode - Slower Lead
 Detected Ahead

Switch to 'Experimental Mode' when a slower lead vehicle is detected ahead.
---
 .../frogpilot/controls/frogpilot_planner.py   |  6 ++++++
 .../lib/conditional_experimental_mode.py      | 19 ++++++++++++++++++-
 2 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py
index 37bef95a2..ba196c823 100644
--- a/selfdrive/frogpilot/controls/frogpilot_planner.py
+++ b/selfdrive/frogpilot/controls/frogpilot_planner.py
@@ -24,6 +24,7 @@ class FrogPilotPlanner:
 
     self.cem = ConditionalExperimentalMode()
 
+    self.slower_lead = False
     self.tracking_lead = False
 
     self.acceleration_jerk = 0
@@ -89,6 +90,11 @@ class FrogPilotPlanner:
       self.speed_jerk = self.base_speed_jerk
 
   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, modelData, v_cruise, v_ego, frogpilot_toggles):
     v_cruise_cluster = max(controlsState.vCruiseCluster, v_cruise) * 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 647b8f8e2..81831ff2d 100644
--- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
+++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
@@ -11,6 +11,7 @@ class ConditionalExperimentalMode:
     self.experimental_mode = False
 
     self.curvature_mac = MovingAverageCalculator()
+    self.slow_lead_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:
@@ -26,7 +27,7 @@ class ConditionalExperimentalMode:
       return True
 
     approaching_maneuver = modelData.navEnabled and (frogpilotNavigation.approachingIntersection or frogpilotNavigation.approachingTurn)
-    if frogpilot_toggles.conditional_navigation and approaching_maneuver and (frogpilot_toggles.conditional_navigation_lead or not lead_status):
+    if frogpilot_toggles.conditional_navigation and approaching_maneuver and (frogpilot_toggles.conditional_navigation_lead or not tracking_lead):
       self.status_value = 10 if frogpilotNavigation.approachingIntersection else 11
       return True
 
@@ -34,10 +35,15 @@ class ConditionalExperimentalMode:
       self.status_value = 12
       return True
 
+    if frogpilot_toggles.conditional_lead and self.slow_lead_detected:
+      self.status_value = 13 if v_lead < 1 else 14
+      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)
+    self.slow_lead(slower_lead, tracking_lead, v_lead, frogpilot_toggles)
 
   def road_curvature(self, road_curvature, v_ego, frogpilot_toggles):
     curve_detected = (1 / road_curvature)**0.5 < v_ego
@@ -45,3 +51,14 @@ class ConditionalExperimentalMode:
 
     self.curvature_mac.add_data(curve_detected or curve_active)
     self.curve_detected = self.curvature_mac.get_moving_average() >= PROBABILITY
+
+  def slow_lead(self, slower_lead, tracking_lead, v_lead, frogpilot_toggles):
+    if tracking_lead:
+      slower_lead &= frogpilot_toggles.conditional_slower_lead
+      stopped_lead = frogpilot_toggles.conditional_stopped_lead and v_lead < 1
+
+      self.slow_lead_mac.add_data(slower_lead or stopped_lead)
+      self.slow_lead_detected = self.slow_lead_mac.get_moving_average() >= PROBABILITY
+    else:
+      self.slow_lead_mac.reset_data()
+      self.slow_lead_detected = False