Lead departing alert

Added toggle to enable an alert when the lead vehicle starts to depart.

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

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.),
+  },
 }