From 2ff6060fe3c28a6dd36b3a73e5efdef65754a0ed Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 27 Apr 2025 22:10:55 -0400 Subject: [PATCH] sp events --- cereal/custom.capnp | 4 ++ .../lib/speed_limit_controller/__init__.py | 1 - .../speed_limit_controller.py | 18 ++++---- sunnypilot/selfdrive/selfdrived/events.py | 41 +++++++++++++++++++ 4 files changed, 56 insertions(+), 8 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index d5544522b7..ce432db490 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -145,6 +145,10 @@ struct OnroadEventSP @0xda96579883444c35 { hyundaiRadarTracksConfirmed @13; experimentalModeSwitched @14; wrongCarModeAlertOnly @15; + speedLimitPreActive @16; + speedLimitActive @17; + speedLimitConfirmed @18; + speedLimitValueChange @19; } } diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py index 6de4b0235d..c50520ede3 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py @@ -17,4 +17,3 @@ LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and curren LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers. SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState -EventName = car.CarEvent.EventName diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py index d5412ce61a..4dbfcdcb8b 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py @@ -1,17 +1,21 @@ import numpy as np import time +from cereal import custom from openpilot.common.conversions import Conversions as CV from openpilot.common.params import Params from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V, \ - PARAMS_UPDATE_PERIOD, TEMP_INACTIVE_GUARD_PERIOD, LIMIT_SPEED_OFFSET_TH, EventName, SpeedLimitControlState + PARAMS_UPDATE_PERIOD, TEMP_INACTIVE_GUARD_PERIOD, LIMIT_SPEED_OFFSET_TH, SpeedLimitControlState from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.selfdrive.selfdrived.events import ET from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy, Engage, OffsetType from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import description_for_state, debug from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver +from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP from openpilot.selfdrive.modeld.constants import ModelConstants +EventNameSP = custom.OnroadEventSP.EventName + ACTIVE_STATES = (SpeedLimitControlState.active, SpeedLimitControlState.adapting) @@ -279,22 +283,22 @@ class SpeedLimitController: def _update_events(self, events): if self._speed_limit > 0 and self._warning_type_type == 2 and \ self._speed_limit_warning_offsetted_rounded < int(round(self._v_ego * self._ms_to_local)): - events.add(EventName.speedLimitPreActive) + events.add(EventNameSP.speedLimitPreActive) if not self.is_active: if self._state == SpeedLimitControlState.preActive and self._state_prev != SpeedLimitControlState.preActive and \ self._v_cruise_rounded != self._speed_limit_offsetted_rounded: - events.add(EventName.speedLimitPreActive) + events.add(EventNameSP.speedLimitPreActive) else: if self._engage_type == Engage.user_confirm: if self._state_prev == SpeedLimitControlState.preActive: - events.add(EventName.speedLimitConfirmed) - events.add(EventName.speedLimitActive) + events.add(EventNameSP.speedLimitConfirmed) + events.add(EventNameSP.speedLimitActive) elif self._engage_type == Engage.auto: if self._state_prev not in ACTIVE_STATES: - events.add(EventName.speedLimitActive) + events.add(EventNameSP.speedLimitActive) elif self._speed_limit_changed != 0: - events.add(EventName.speedLimitValueChange) + events.add(EventNameSP.speedLimitValueChange) def update(self, enabled, v_ego, a_ego, sm, v_cruise_setpoint, events): _car_state = sm['carState'] diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index 6813542c0e..985161108a 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -1,4 +1,6 @@ +import cereal.messaging as messaging from cereal import log, car, custom +from openpilot.common.conversions import Conversions as CV from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \ NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert @@ -14,6 +16,17 @@ EventNameSP = custom.OnroadEventSP.EventName EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()} +def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + speedLimit = sm['longitudinalPlanSP'].speedLimit + speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)) + message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit' + return Alert( + message, + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 4.) + + class EventsSP(EventsBase): def __init__(self): super().__init__() @@ -134,4 +147,32 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.WARNING: wrong_car_mode_alert, }, + EventNameSP.speedLimitPreActive: { + ET.WARNING: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.MID, VisualAlert.none, AudibleAlert.none, .45), + }, + + EventNameSP.speedLimitActive: { + ET.WARNING: Alert( + "Set speed changed to match posted speed limit", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 3.), + }, + + EventNameSP.speedLimitConfirmed: { + ET.WARNING: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.MID, VisualAlert.none, AudibleAlert.none, .45), + }, + + EventNameSP.speedLimitValueChange: { + ET.WARNING: speed_limit_adjust_alert, + }, + }