Green Light and Lead Departure alerts improvements (#1381)

sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py

@@ -12,47 +12,83 @@ from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 
-TRIGGER_THRESHOLD = 30
+GREEN_LIGHT_X_THRESHOLD = 30
 
 
 class E2EAlertsHelper:
   def __init__(self):
     self._params = Params()
-    self._frame = -1
+    self.frame = -1
 
     self.green_light_alert = False
     self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert")
     self.lead_depart_alert = False
     self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert")
 
+    self.alert_allowed = False
+    self.green_light_alert_count = 0
+    self.last_lead_distance = -1
+    self.last_moving_frame = -1
+
   def _read_params(self) -> None:
-    if self._frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
       self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert")
       self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert")
 
-    self._frame += 1
-
   def update(self, sm: messaging.SubMaster, events_sp: EventsSP) -> None:
     self._read_params()
 
-    if not (self.green_light_alert_enabled or self.lead_depart_alert_enabled):
-      return
-
     CS = sm['carState']
     CC = sm['carControl']
 
     model_x = sm['modelV2'].position.x
     max_idx = len(model_x) - 1
     has_lead = sm['radarState'].leadOne.status
-    lead_vRel: float = sm['radarState'].leadOne.vRel
+    lead_dRel = sm['radarState'].leadOne.dRel
+    standstill = CS.standstill
+    moving = not standstill and CS.vEgo > 0.1
+    _allowed = standstill and not CS.gasPressed and not CC.enabled
 
-    # Green light alert
-    self.green_light_alert = (self.green_light_alert_enabled and model_x[max_idx] > TRIGGER_THRESHOLD
-                              and not has_lead and CS.standstill and not CS.gasPressed and not CC.enabled)
+    if moving:
+      self.last_moving_frame = self.frame
+    recent_moving = self.last_moving_frame == -1 or (self.frame - self.last_moving_frame) * DT_MDL < 2.0
+
+    if standstill and not recent_moving:
+      self.alert_allowed = True
+    elif not standstill:
+      self.alert_allowed = False
+      self.green_light_alert_count = 0
+      self.last_lead_distance = -1
+
+    # Green Light Alert
+    _green_light_alert = False
+    if self.green_light_alert_enabled and _allowed and not has_lead and model_x[max_idx] > GREEN_LIGHT_X_THRESHOLD:
+      if self.alert_allowed:
+        self.green_light_alert_count += 1
+      else:
+        self.green_light_alert_count = 0
+
+      if self.green_light_alert_count > 2 and self.alert_allowed:
+        _green_light_alert = True
+        self.alert_allowed = False
+    else:
+      self.green_light_alert_count = 0
+
+    self.green_light_alert = _green_light_alert
 
     # Lead Departure Alert
-    self.lead_depart_alert = (self.lead_depart_alert_enabled and CS.standstill and model_x[max_idx] > 30
-                              and has_lead and lead_vRel > 1 and not CS.gasPressed)
+    _lead_depart_alert = False
+    if self.lead_depart_alert_enabled and _allowed and has_lead:
+      if self.last_lead_distance == -1 or lead_dRel < self.last_lead_distance:
+        self.last_lead_distance = lead_dRel
+
+      if self.last_lead_distance != -1 and (lead_dRel - self.last_lead_distance > 1.0) and self.alert_allowed:
+        _lead_depart_alert = True
+        self.alert_allowed = False
+
+    self.lead_depart_alert = _lead_depart_alert
 
     if self.green_light_alert or self.lead_depart_alert:
       events_sp.add(custom.OnroadEventSP.EventName.e2eChime)
+
+    self.frame += 1

sunnypilot/selfdrive/selfdrived/events.py

@@ -231,6 +231,6 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       "",
       "",
       AlertStatus.normal, AlertSize.none,
-      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 0.1),
+      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
   },
 }