diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py index 0135b2806..5a92d878d 100644 --- a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py +++ b/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 diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index 5cd7255bc..5d5424bb1 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/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.), }, }