E2E Alert: universal state machine (#1395)

* E2E Helper: universal state machine

* not used

* rename

* 10 frames for both

* time based

* magic

* lead depart: only arm if we have a confirmed close lead for over a second after allowing alert

* less

* shorter trigger

* lol

* always update

selfdrive/ui/sunnypilot/qt/onroad/hud.cc

@@ -67,9 +67,9 @@ void HudRendererSP::updateState(const UIState &s) {
     smartCruiseControlVisionActive = lp_sp.getSmartCruiseControl().getVision().getActive();
     smartCruiseControlMapEnabled = lp_sp.getSmartCruiseControl().getMap().getEnabled();
     smartCruiseControlMapActive = lp_sp.getSmartCruiseControl().getMap().getActive();
-    greenLightAlert = lp_sp.getE2eAlerts().getGreenLightAlert();
-    leadDepartAlert = lp_sp.getE2eAlerts().getLeadDepartAlert();
   }
+  greenLightAlert = lp_sp.getE2eAlerts().getGreenLightAlert();
+  leadDepartAlert = lp_sp.getE2eAlerts().getLeadDepartAlert();
 
   if (sm.updated("liveMapDataSP")) {
     roadNameStr = QString::fromStdString(lmd.getRoadName());
@@ -138,7 +138,7 @@ void HudRendererSP::updateState(const UIState &s) {
   steeringTorqueEps = car_state.getSteeringTorqueEps();
 
   isStandstill = car_state.getStandstill();
-  if (not s.scene.started) standstillElapsedTime = 0.0;
+  if (!s.scene.started) standstillElapsedTime = 0.0;
 
   // override stock current speed values
   float v_ego = (v_ego_cluster_seen && !s.scene.trueVEgoUI) ? car_state.getVEgoCluster() : car_state.getVEgo();
@@ -246,7 +246,7 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
     drawRoadName(p, surface_rect);
 
     // Green Light & Lead Depart Alerts
-    if (greenLightAlert or leadDepartAlert) {
+    if (greenLightAlert || leadDepartAlert) {
       e2eAlertDisplayTimer = 3 * UI_FREQ;
       // reset onroad sleep timer for e2e alerts
       uiStateSP()->reset_onroad_sleep_timer();
@@ -278,7 +278,7 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
     // No Alerts displayed
     else {
       e2eAlertFrame = 0;
-      if (not isStandstill) standstillElapsedTime = 0.0;
+      if (!isStandstill) standstillElapsedTime = 0.0;
     }
 
     // Blinker
@@ -748,7 +748,7 @@ void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect, const Q
   // Alert Circle
   QPoint center = alertRect.center();
   QColor frameColor;
-  if (not alert_alt_text.isEmpty()) frameColor = QColor(255, 255, 255, 75);
+  if (!alert_alt_text.isEmpty()) frameColor = QColor(255, 255, 255, 75);
   else frameColor = pulseElement(e2eAlertFrame) ? QColor(255, 255, 255, 75) : QColor(0, 255, 0, 75);
   p.setPen(QPen(frameColor, 15));
   p.setBrush(QColor(0, 0, 0, 190));
@@ -758,7 +758,7 @@ void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect, const Q
   QColor txtColor;
   QFont font;
   int alert_bottom_adjustment;
-  if (not alert_alt_text.isEmpty()) {
+  if (!alert_alt_text.isEmpty()) {
     font = InterFont(100, QFont::Bold);
     alert_bottom_adjustment = 5;
     txtColor = QColor(255, 255, 255, 255);
@@ -775,7 +775,7 @@ void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect, const Q
   textRect.moveBottom(alertRect.bottom() - alertRect.height() / alert_bottom_adjustment);
   p.drawText(textRect, Qt::AlignCenter, alert_text);
 
-  if (not alert_alt_text.isEmpty()) {
+  if (!alert_alt_text.isEmpty()) {
     // Alert Alternate Text
     p.setFont(InterFont(80, QFont::Bold));
     p.setPen(QColor(255, 175, 3, 240));
@@ -804,7 +804,7 @@ void HudRendererSP::drawCurrentSpeedSP(QPainter &p, const QRect &surface_rect) {
 
 void HudRendererSP::drawBlinker(QPainter &p, const QRect &surface_rect) {
   const bool hazard = leftBlinkerOn && rightBlinkerOn;
-  int blinkerStatus = hazard ? 2 : (leftBlinkerOn or rightBlinkerOn) ? 1 : 0;
+  int blinkerStatus = hazard ? 2 : (leftBlinkerOn || rightBlinkerOn) ? 1 : 0;
 
   if (!leftBlinkerOn && !rightBlinkerOn) {
     blinkerFrameCounter = 0;

sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py

@@ -13,80 +13,156 @@ from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 
 GREEN_LIGHT_X_THRESHOLD = 30
+LEAD_DEPART_DIST_THRESHOLD = 1.0
+TRIGGER_TIMER_THRESHOLD = 0.3
+
+
+class E2EStates:
+  INACTIVE = 0
+  ARMED = 1
+  CONSUMED = 2
 
 
 class E2EAlertsHelper:
   def __init__(self):
     self._params = Params()
     self.frame = -1
+    self.green_light_state = E2EStates.INACTIVE
+    self.prev_green_light_state = E2EStates.INACTIVE
+    self.lead_depart_state = E2EStates.INACTIVE
+    self.prev_lead_depart_state = E2EStates.INACTIVE
 
     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.green_light_trigger_timer = 0
+    self.lead_depart_trigger_timer = 0
     self.last_lead_distance = -1
     self.last_moving_frame = -1
 
+    self.allowed = False
+    self.last_allowed = False
+    self.has_lead = False
+
+    self.lead_depart_arm_timer = 0
+    self.lead_depart_confirmed_lead = False
+    self.lead_depart_armed = False
+
   def _read_params(self) -> None:
     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")
 
-  def update(self, sm: messaging.SubMaster, events_sp: EventsSP) -> None:
-    self._read_params()
-
+  def update_alert_trigger(self, sm: messaging.SubMaster):
     CS = sm['carState']
     CC = sm['carControl']
 
     model_x = sm['modelV2'].position.x
     max_idx = len(model_x) - 1
-    has_lead = sm['radarState'].leadOne.status
+    self.has_lead = sm['radarState'].leadOne.status
     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
 
     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
+    self.allowed = not moving and not CS.gasPressed and not CC.enabled and not recent_moving
 
     # 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
+    green_light_trigger = False
+    if self.green_light_state == E2EStates.ARMED:
+      if model_x[max_idx] > GREEN_LIGHT_X_THRESHOLD:
+        self.green_light_trigger_timer += 1
       else:
-        self.green_light_alert_count = 0
+        self.green_light_trigger_timer = 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
+      if self.green_light_trigger_timer * DT_MDL > TRIGGER_TIMER_THRESHOLD:
+        green_light_trigger = True
+    elif self.green_light_state != E2EStates.ARMED:
+      self.green_light_trigger_timer = 0
 
     # Lead Departure Alert
-    _lead_depart_alert = False
-    if self.lead_depart_alert_enabled and _allowed and has_lead:
+    close_lead_valid = self.has_lead and lead_dRel < 8.0
+    if self.allowed and not self.last_allowed and close_lead_valid:
+      self.lead_depart_confirmed_lead = True
+    elif not self.allowed:
+      self.lead_depart_confirmed_lead = False
+
+    if self.allowed and self.lead_depart_confirmed_lead and close_lead_valid:
+      self.lead_depart_arm_timer += 1
+
+      if self.lead_depart_arm_timer * DT_MDL >= 1.0:
+        self.lead_depart_armed = True
+    else:
+      self.lead_depart_arm_timer = 0
+      self.lead_depart_armed = False
+
+    lead_depart_trigger = False
+    if self.lead_depart_state == E2EStates.ARMED:
       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
+      if self.last_lead_distance != -1 and (lead_dRel - self.last_lead_distance > LEAD_DEPART_DIST_THRESHOLD):
+        self.lead_depart_trigger_timer += 1
+      else:
+        self.lead_depart_trigger_timer = 0
 
-    self.lead_depart_alert = _lead_depart_alert
+      if self.lead_depart_trigger_timer * DT_MDL > TRIGGER_TIMER_THRESHOLD:
+        lead_depart_trigger = True
+    elif self.lead_depart_state != E2EStates.ARMED:
+      self.last_lead_distance = -1
+      self.lead_depart_trigger_timer = 0
+
+    self.last_allowed = self.allowed
+
+    return green_light_trigger, lead_depart_trigger
+
+  @staticmethod
+  def update_state_machine(state: int, enabled: bool, allowed: bool, triggered: bool) -> tuple[int, bool]:
+    if state != E2EStates.INACTIVE:
+      if not allowed or not enabled:
+        state = E2EStates.INACTIVE
+
+      else:
+        if state == E2EStates.ARMED:
+          if triggered:
+            state = E2EStates.CONSUMED
+
+        elif state == E2EStates.CONSUMED:
+          pass
+
+    elif state == E2EStates.INACTIVE:
+      if allowed and enabled:
+        state = E2EStates.ARMED
+
+    return state, triggered
+
+  def update(self, sm: messaging.SubMaster, events_sp: EventsSP) -> None:
+    self._read_params()
+
+    green_light_trigger, lead_depart_trigger = self.update_alert_trigger(sm)
+
+    self.prev_green_light_state = self.green_light_state
+    self.prev_lead_depart_state = self.lead_depart_state
+
+    self.green_light_state, self.green_light_alert = self.update_state_machine(
+      self.green_light_state,
+      self.green_light_alert_enabled,
+      self.allowed and not self.has_lead,
+      green_light_trigger
+    )
+
+    self.lead_depart_state, self.lead_depart_alert = self.update_state_machine(
+      self.lead_depart_state,
+      self.lead_depart_alert_enabled,
+      self.allowed and self.lead_depart_armed,
+      lead_depart_trigger
+    )
 
     if self.green_light_alert or self.lead_depart_alert:
       events_sp.add(custom.OnroadEventSP.EventName.e2eChime)