diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index 5db7ba1814..2fc9eb98fc 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/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; diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py index 5a92d878d6..944bf617e9 100644 --- a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py +++ b/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)