From d2b73b7660d71ed58d84020b4031afab51159776 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Tue, 27 Feb 2024 16:34:47 -0700
Subject: [PATCH] Lead departing alert

Added toggle to enable an alert when the lead vehicle starts to depart.
---
 cereal/car.capnp                          |  1 +
 common/params.cc                          |  1 +
 selfdrive/controls/controlsd.py           | 17 +++++++++++++++++
 selfdrive/controls/lib/events.py          |  7 +++++++
 selfdrive/frogpilot/ui/visual_settings.cc |  1 +
 selfdrive/frogpilot/ui/visual_settings.h  |  2 +-
 6 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/cereal/car.capnp b/cereal/car.capnp
index 08d24baf8..42652875f 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -120,6 +120,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     # FrogPilot events
     frogSteerSaturated @121;
     greenLight @122;
+    leadDeparting @124;
 
     radarCanErrorDEPRECATED @15;
     communityFeatureDisallowedDEPRECATED @62;
diff --git a/common/params.cc b/common/params.cc
index bb0fd12b8..15ec994ed 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -275,6 +275,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"HigherBitrate", PERSISTENT},
     {"LaneLinesWidth", PERSISTENT},
     {"LateralTune", PERSISTENT},
+    {"LeadDepartingAlert", PERSISTENT},
     {"LeadInfo", PERSISTENT},
     {"LongitudinalTune", PERSISTENT},
     {"LongPitch", PERSISTENT},
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 3a689d6ec..0ed2b2d42 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -181,6 +181,8 @@ class Controls:
     self.previously_enabled = False
     self.stopped_for_light_previously = False
 
+    self.previous_lead_distance = 0
+
     self.green_light_mac = MovingAverageCalculator()
 
     ignore = self.sensor_packets + ['testJoystick']
@@ -551,6 +553,20 @@ class Controls:
       if self.green_light_mac.get_moving_average() >= PROBABILITY:
         self.events.add(EventName.greenLight)
 
+    # Lead departing alert
+    if self.lead_departing_alert and self.sm.frame % 50 == 0:
+      lead = self.sm['radarState'].leadOne
+      lead_distance = lead.dRel
+      lead_departing = lead_distance - self.previous_lead_distance > 0.5 and self.previous_lead_distance != 0 and CS.standstill
+      self.previous_lead_distance = lead_distance
+
+      lead_departing &= not CS.gasPressed
+      lead_departing &= lead.vLead > 1
+      lead_departing &= self.driving_gear
+
+      if lead_departing:
+        self.events.add(EventName.leadDeparting)
+
   def data_sample(self):
     """Receive data from sockets and update carState"""
 
@@ -1039,6 +1055,7 @@ class Controls:
 
     custom_alerts = self.params.get_bool("CustomAlerts")
     self.green_light_alert = custom_alerts and self.params.get_bool("GreenLightAlert")
+    self.lead_departing_alert = custom_alerts and self.params.get_bool("LeadDepartingAlert")
 
     custom_theme = self.params.get_bool("CustomTheme")
     custom_sounds = self.params.get_int("CustomSounds") if custom_theme else 0
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index 599998b42..466d401ea 100755
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -974,6 +974,13 @@ EVENTS: Dict[int, Dict[str, Union[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.),
+  },
 }
 
 
diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc
index d5febe7eb..416ff2bb6 100644
--- a/selfdrive/frogpilot/ui/visual_settings.cc
+++ b/selfdrive/frogpilot/ui/visual_settings.cc
@@ -21,6 +21,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
 
     {"CustomAlerts", "Custom Alerts", "Enable custom alerts for various logic or situational changes.", "../frogpilot/assets/toggle_icons/icon_green_light.png"},
     {"GreenLightAlert", "Green Light Alert", "Get an alert when a traffic light changes from red to green.", ""},
+    {"LeadDepartingAlert", "Lead Departing Alert", "Get an alert when your lead vehicle starts departing when you're at a standstill.", ""},
 
     {"CustomUI", "Custom Onroad UI", "Customize the Onroad UI with some additional visual functions.", "../assets/offroad/icon_road.png"},
     {"AccelerationPath", "Acceleration Path", "Visualize the car's intended acceleration or deceleration with a color-coded path.", ""},
diff --git a/selfdrive/frogpilot/ui/visual_settings.h b/selfdrive/frogpilot/ui/visual_settings.h
index d56ed63a4..727ccf6a8 100644
--- a/selfdrive/frogpilot/ui/visual_settings.h
+++ b/selfdrive/frogpilot/ui/visual_settings.h
@@ -29,7 +29,7 @@ private:
   void updateToggles();
 
   std::set<QString> alertVolumeControlKeys = {"EngageVolume", "DisengageVolume", "RefuseVolume", "PromptVolume", "PromptDistractedVolume", "WarningSoftVolume", "WarningImmediateVolume"};
-  std::set<QString> customAlertsKeys = {"GreenLightAlert"};
+  std::set<QString> customAlertsKeys = {"GreenLightAlert", "LeadDepartingAlert"};
   std::set<QString> customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "FPSCounter", "LeadInfo"};
   std::set<QString> customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"};
   std::set<QString> modelUIKeys = {"DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};