From f4726bf0953fb773b2ea3d9b5e01db580c5d783e 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          |  8 ++++++++
 selfdrive/frogpilot/ui/visual_settings.cc |  1 +
 selfdrive/frogpilot/ui/visual_settings.h  |  2 +-
 6 files changed, 29 insertions(+), 1 deletion(-)
 mode change 100755 => 100644 selfdrive/controls/lib/events.py

diff --git a/cereal/car.capnp b/cereal/car.capnp
index 2f1c8a5..45ae48e 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -121,6 +121,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     frogSteerSaturated @122;
     greenLight @123;
     holidayActive @124;
+    leadDeparting @126;
 
     radarCanErrorDEPRECATED @15;
     communityFeatureDisallowedDEPRECATED @62;
diff --git a/common/params.cc b/common/params.cc
index 844ef21..7ef390a 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -280,6 +280,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"HolidayThemes", 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 6e5e1f7..2fad25a 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -182,6 +182,8 @@ class Controls:
     self.previously_enabled = False
     self.stopped_for_light_previously = False
 
+    self.previous_lead_distance = 0
+
     ignore = self.sensor_packets + ['testJoystick']
     if SIMULATION:
       ignore += ['driverCameraState', 'managerState']
@@ -554,6 +556,20 @@ class Controls:
       if green_light:
         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"""
 
@@ -1042,6 +1058,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
old mode 100755
new mode 100644
index 6dd2fe7..741243b
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -1002,6 +1002,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
   EventName.holidayActive: {
     ET.PERMANENT: holiday_alert,
   },
+
+  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 5de3f83..bd6dbf8 100644
--- a/selfdrive/frogpilot/ui/visual_settings.cc
+++ b/selfdrive/frogpilot/ui/visual_settings.cc
@@ -22,6 +22,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 41bfb2f..9cc34dc 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 = {"HolidayThemes", "CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"};
   std::set<QString> modelUIKeys = {"DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};