diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc index 95aa4ef26f..5c3b03d2af 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc +++ b/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(SpeedLimitMode::WARNING))); + if (speed_limit_mode_param == SpeedLimitMode::ASSIST) { + params.put("SpeedLimitMode", std::to_string(static_cast(SpeedLimitMode::WARNING))); + } } } else { has_longitudinal_control = false; diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py index 5467639a63..6a868ab85b 100644 --- a/sunnypilot/selfdrive/car/interfaces.py +++ b/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: diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py index 5a92d878d6..1133c65ef2 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.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)