From 754c396a3b103524ac2af1df2b6e5e18efb627d3 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Tue, 28 May 2024 02:58:45 -0700
Subject: [PATCH] Visuals - Custom Alerts - Lead Departing Alert

Get an alert when the lead vehicle starts departing when at a standstill.
---
 selfdrive/controls/controlsd.py                   |  3 +++
 selfdrive/controls/lib/events.py                  |  8 ++++++++
 selfdrive/frogpilot/controls/frogpilot_planner.py | 12 ++++++++++++
 3 files changed, 23 insertions(+)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 81945e4..561c411 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -946,6 +946,9 @@ class Controls:
       if self.green_light_mac.get_moving_average() >= PROBABILITY:
         self.events.add(EventName.greenLight)
 
+    if self.frogpilot_toggles.lead_departing_alert and self.sm['frogpilotPlan'].leadDeparting:
+      self.events.add(EventName.leadDeparting)
+
     if self.frogpilot_toggles.speed_limit_alert or self.frogpilot_toggles.speed_limit_confirmation:
       current_speed_limit = self.sm['frogpilotPlan'].slcSpeedLimit
       desired_speed_limit = self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index f7cf0ee..bfd0e16 100755
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -998,6 +998,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
       Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
   },
 
+  EventName.leadDeparting: {
+    ET.PERMANENT: Alert(
+      "Lead departed",
+      "",
+      AlertStatus.frogpilot, AlertSize.small,
+      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
+  },
+
   EventName.noLaneAvailable : {
     ET.PERMANENT: no_lane_available_alert,
   },
diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py
index 29eb94b..35e5c12 100644
--- a/selfdrive/frogpilot/controls/frogpilot_planner.py
+++ b/selfdrive/frogpilot/controls/frogpilot_planner.py
@@ -58,6 +58,7 @@ class FrogPilotPlanner:
     self.lead_one = Lead()
     self.mtsc = MapTurnSpeedController()
 
+    self.lead_departing = False
     self.override_slc = False
     self.slower_lead = False
 
@@ -65,6 +66,7 @@ class FrogPilotPlanner:
     self.frame = 0
     self.mtsc_target = 0
     self.overridden_speed = 0
+    self.previous_lead_distance = 0
     self.road_curvature = 0
     self.slc_target = 0
     self.speed_jerk = 0
@@ -154,6 +156,14 @@ class FrogPilotPlanner:
       self.acceleration_jerk = self.base_acceleration_jerk
       self.speed_jerk = self.base_speed_jerk
 
+    if self.frame % 10 == 0:
+      self.lead_departing = lead_distance - self.previous_lead_distance > 0.5 and self.previous_lead_distance != 0 and carState.standstill
+      self.previous_lead_distance = lead_distance
+
+      self.lead_departing &= not carState.gasPressed
+      self.lead_departing &= v_lead > 1
+      self.lead_departing &= carState.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
+
     self.road_curvature = calculate_road_curvature(modelData, v_ego)
     self.v_cruise = self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, v_cruise, v_ego, frogpilot_toggles)
 
@@ -267,6 +277,8 @@ class FrogPilotPlanner:
     frogpilotPlan.conditionalExperimental = self.cem.experimental_mode
     frogpilotPlan.redLight = self.cem.red_light_detected
 
+    frogpilotPlan.leadDeparting = self.lead_departing
+
     frogpilotPlan.maxAcceleration = self.max_accel
     frogpilotPlan.minAcceleration = self.min_accel