Merge branch 'e2e-alert-state-machine' into hkg-angle-steering-2025

selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc

@@ -124,7 +124,9 @@ void SpeedLimitSettings::refresh() {
     intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable();
 
     if (!has_longitudinal_control && CP_SP.getPcmCruiseSpeed()) {
-      params.put("SpeedLimitMode", std::to_string(static_cast<int>(SpeedLimitMode::WARNING)));
+      if (speed_limit_mode_param == SpeedLimitMode::ASSIST) {
+        params.put("SpeedLimitMode", std::to_string(static_cast<int>(SpeedLimitMode::WARNING)));
+      }
     }
   } else {
     has_longitudinal_control = false;

sunnypilot/selfdrive/car/interfaces.py

@@ -86,7 +86,9 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS
     params.remove("CustomAccIncrementsEnabled")
     params.remove("SmartCruiseControlVision")
     params.remove("SmartCruiseControlMap")
-    params.put("SpeedLimitMode", int(SpeedLimitMode.warning))
+
+    if params.get("SpeedLimitMode", return_default=True) == SpeedLimitMode.assist:
+      params.put("SpeedLimitMode", int(SpeedLimitMode.warning))
 
 
 def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:

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.5
+
+
+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)