sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
Whitespace-only changes.
@@ -0,0 +1,247 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.cereal import log, custom
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
|
||||
NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
|
||||
from openpilot.common.hardware import HARDWARE
|
||||
|
||||
AlertSize = log.SelfdriveState.AlertSize
|
||||
AlertStatus = log.SelfdriveState.AlertStatus
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
|
||||
AudibleAlertSP = custom.SelfdriveStateSP.AudibleAlert
|
||||
EventNameSP = custom.OnroadEventSP.EventName
|
||||
|
||||
|
||||
# get event name from enum
|
||||
EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()}
|
||||
|
||||
IS_MICI = HARDWARE.get_device_type() == 'mici'
|
||||
|
||||
|
||||
def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
|
||||
speedLimit = sm['longitudinalPlanSP'].speedLimit.resolver.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.)
|
||||
|
||||
|
||||
def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
|
||||
speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
|
||||
v_cruise_cluster = CS.vCruiseCluster
|
||||
set_speed = sm['controlsState'].deprecated.vCruise if v_cruise_cluster == 0.0 else v_cruise_cluster
|
||||
set_speed_conv = round(set_speed * speed_conv)
|
||||
|
||||
speed_limit_final_last = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast
|
||||
speed_limit_final_last_conv = round(speed_limit_final_last * speed_conv)
|
||||
alert_1_str = ""
|
||||
alert_size = AlertSize.small
|
||||
|
||||
if CP.openpilotLongitudinalControl and CP.pcmCruise:
|
||||
# PCM long
|
||||
cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[metric]
|
||||
pcm_long_required_max = cst_low if speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[metric] else cst_high
|
||||
pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
|
||||
speed_unit = "km/h" if metric else "mph"
|
||||
|
||||
alert_1_str = f"Speed Limit Assist: set to {pcm_long_required_max_set_speed_conv} {speed_unit} to engage"
|
||||
else:
|
||||
if IS_MICI:
|
||||
if set_speed_conv < speed_limit_final_last_conv:
|
||||
alert_1_str = "Press + to confirm speed limit"
|
||||
elif set_speed_conv > speed_limit_final_last_conv:
|
||||
alert_1_str = "Press - to confirm speed limit"
|
||||
else:
|
||||
alert_size = AlertSize.none
|
||||
|
||||
return Alert(
|
||||
alert_1_str,
|
||||
"",
|
||||
AlertStatus.normal, alert_size,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleLow, .1)
|
||||
|
||||
|
||||
class EventsSP(EventsBase):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.event_counters = dict.fromkeys(EVENTS_SP.keys(), 0)
|
||||
|
||||
def get_events_mapping(self) -> dict[int, dict[str, Alert | AlertCallbackType]]:
|
||||
return EVENTS_SP
|
||||
|
||||
def get_event_name(self, event: int):
|
||||
return EVENT_NAME_SP[event]
|
||||
|
||||
def get_event_msg_type(self):
|
||||
return custom.OnroadEventSP.Event
|
||||
|
||||
|
||||
EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
# sunnypilot
|
||||
EventNameSP.lkasEnable: {
|
||||
ET.ENABLE: EngagementAlert(AudibleAlert.engage),
|
||||
},
|
||||
|
||||
EventNameSP.lkasDisable: {
|
||||
ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
|
||||
},
|
||||
|
||||
EventNameSP.manualSteeringRequired: {
|
||||
ET.USER_DISABLE: Alert(
|
||||
"Automatic Lane Centering is OFF",
|
||||
"Manual Steering Required",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.disengage, 1.),
|
||||
},
|
||||
|
||||
EventNameSP.manualLongitudinalRequired: {
|
||||
ET.WARNING: Alert(
|
||||
"Smart/Adaptive Cruise Control: OFF",
|
||||
"Manual Speed Control Required",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
|
||||
},
|
||||
|
||||
EventNameSP.silentLkasEnable: {
|
||||
ET.ENABLE: EngagementAlert(AudibleAlert.none),
|
||||
},
|
||||
|
||||
EventNameSP.silentLkasDisable: {
|
||||
ET.USER_DISABLE: EngagementAlert(AudibleAlert.none),
|
||||
},
|
||||
|
||||
EventNameSP.silentBrakeHold: {
|
||||
ET.WARNING: EngagementAlert(AudibleAlert.none),
|
||||
ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"),
|
||||
},
|
||||
|
||||
EventNameSP.silentWrongGear: {
|
||||
ET.WARNING: Alert(
|
||||
"",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
|
||||
ET.NO_ENTRY: Alert(
|
||||
"Gear not D",
|
||||
"openpilot Unavailable",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 0.),
|
||||
},
|
||||
|
||||
EventNameSP.silentReverseGear: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Reverse\nGear",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
|
||||
ET.NO_ENTRY: NoEntryAlert("Reverse Gear"),
|
||||
},
|
||||
|
||||
EventNameSP.silentDoorOpen: {
|
||||
ET.WARNING: Alert(
|
||||
"",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
|
||||
ET.NO_ENTRY: NoEntryAlert("Door Open"),
|
||||
},
|
||||
|
||||
EventNameSP.silentSeatbeltNotLatched: {
|
||||
ET.WARNING: Alert(
|
||||
"",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
|
||||
ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"),
|
||||
},
|
||||
|
||||
EventNameSP.silentParkBrake: {
|
||||
ET.WARNING: Alert(
|
||||
"",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
|
||||
ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"),
|
||||
},
|
||||
|
||||
EventNameSP.controlsMismatchLateral: {
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch: Lateral"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Controls Mismatch: Lateral"),
|
||||
},
|
||||
|
||||
EventNameSP.experimentalModeSwitched: {
|
||||
ET.WARNING: NormalPermanentAlert("Experimental Mode Switched", duration=1.5)
|
||||
},
|
||||
|
||||
EventNameSP.wrongCarModeAlertOnly: {
|
||||
ET.WARNING: wrong_car_mode_alert,
|
||||
},
|
||||
|
||||
EventNameSP.pedalPressedAlertOnly: {
|
||||
ET.WARNING: NoEntryAlert("Pedal Pressed")
|
||||
},
|
||||
|
||||
EventNameSP.laneTurnLeft: {
|
||||
ET.WARNING: Alert(
|
||||
"Turning Left",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
|
||||
},
|
||||
|
||||
EventNameSP.laneTurnRight: {
|
||||
ET.WARNING: Alert(
|
||||
"Turning Right",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
|
||||
},
|
||||
|
||||
EventNameSP.speedLimitActive: {
|
||||
ET.WARNING: Alert(
|
||||
"Auto adjusting to speed limit",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
|
||||
},
|
||||
|
||||
EventNameSP.speedLimitChanged: {
|
||||
ET.WARNING: Alert(
|
||||
"Set speed changed",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
|
||||
},
|
||||
|
||||
EventNameSP.speedLimitPreActive: {
|
||||
ET.WARNING: speed_limit_pre_active_alert,
|
||||
},
|
||||
|
||||
EventNameSP.speedLimitPending: {
|
||||
ET.WARNING: Alert(
|
||||
"Auto adjusting to last speed limit",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
|
||||
},
|
||||
|
||||
EventNameSP.e2eChime: {
|
||||
ET.PERMANENT: Alert(
|
||||
"",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
|
||||
},
|
||||
}
|
||||
@@ -0,0 +1,245 @@
|
||||
import bisect
|
||||
from enum import IntEnum
|
||||
from abc import abstractmethod
|
||||
from collections.abc import Callable
|
||||
|
||||
from openpilot.cereal import log
|
||||
from opendbc.car.structs import car
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.common.hardware import HARDWARE
|
||||
|
||||
AlertSize = log.SelfdriveState.AlertSize
|
||||
AlertStatus = log.SelfdriveState.AlertStatus
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
AudibleAlert = log.SelfdriveState.AudibleAlert
|
||||
|
||||
|
||||
# Alert priorities
|
||||
class Priority(IntEnum):
|
||||
LOWEST = 0
|
||||
LOWER = 1
|
||||
LOW = 2
|
||||
MID = 3
|
||||
HIGH = 4
|
||||
HIGHEST = 5
|
||||
|
||||
|
||||
# Event types
|
||||
class ET:
|
||||
ENABLE = 'enable'
|
||||
PRE_ENABLE = 'preEnable'
|
||||
OVERRIDE_LATERAL = 'overrideLateral'
|
||||
OVERRIDE_LONGITUDINAL = 'overrideLongitudinal'
|
||||
NO_ENTRY = 'noEntry'
|
||||
WARNING = 'warning'
|
||||
USER_DISABLE = 'userDisable'
|
||||
SOFT_DISABLE = 'softDisable'
|
||||
IMMEDIATE_DISABLE = 'immediateDisable'
|
||||
PERMANENT = 'permanent'
|
||||
|
||||
|
||||
class Alert:
|
||||
def __init__(self,
|
||||
alert_text_1: str,
|
||||
alert_text_2: str,
|
||||
alert_status: log.SelfdriveState.AlertStatus,
|
||||
alert_size: log.SelfdriveState.AlertSize,
|
||||
priority: Priority,
|
||||
visual_alert: car.CarControl.HUDControl.VisualAlert,
|
||||
audible_alert: log.SelfdriveState.AudibleAlert,
|
||||
duration: float,
|
||||
creation_delay: float = 0.):
|
||||
|
||||
self.alert_text_1 = alert_text_1
|
||||
self.alert_text_2 = alert_text_2
|
||||
self.alert_status = alert_status
|
||||
self.alert_size = alert_size
|
||||
self.priority = priority
|
||||
self.visual_alert = visual_alert
|
||||
self.audible_alert = audible_alert
|
||||
|
||||
self.duration = int(duration / DT_CTRL)
|
||||
|
||||
self.creation_delay = creation_delay
|
||||
|
||||
self.alert_type = ""
|
||||
self.event_type: str | None = None
|
||||
|
||||
def __str__(self) -> str:
|
||||
return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}"
|
||||
|
||||
def __gt__(self, alert2) -> bool:
|
||||
if not isinstance(alert2, Alert):
|
||||
return False
|
||||
return self.priority > alert2.priority
|
||||
|
||||
class AlertBase(Alert):
|
||||
def __init__(self, alert_text_1: str, alert_text_2: str, alert_status: log.SelfdriveState.AlertStatus,
|
||||
alert_size: log.SelfdriveState.AlertSize, priority: Priority,
|
||||
visual_alert: car.CarControl.HUDControl.VisualAlert,
|
||||
audible_alert: log.SelfdriveState.AudibleAlert, duration: float):
|
||||
super().__init__(alert_text_1, alert_text_2, alert_status, alert_size, priority, visual_alert, audible_alert, duration)
|
||||
|
||||
|
||||
AlertCallbackType = Callable[[car.CarParams, car.CarState, messaging.SubMaster, bool, int, log.ControlsState], Alert]
|
||||
|
||||
|
||||
# ********** alert callback functions **********
|
||||
|
||||
|
||||
def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
|
||||
text = "Enable Adaptive Cruise to Engage"
|
||||
if CP.brand == "honda":
|
||||
text = "Enable Main Switch to Engage"
|
||||
return NoEntryAlert(text)
|
||||
|
||||
|
||||
class EventsBase:
|
||||
def __init__(self):
|
||||
self.events: list[int] = []
|
||||
self.static_events: list[int] = []
|
||||
self.event_counters = {}
|
||||
|
||||
@property
|
||||
def names(self) -> list[int]:
|
||||
return self.events
|
||||
|
||||
def __len__(self) -> int:
|
||||
return len(self.events)
|
||||
|
||||
def add(self, event_name: int, static: bool = False) -> None:
|
||||
if static:
|
||||
bisect.insort(self.static_events, event_name)
|
||||
bisect.insort(self.events, event_name)
|
||||
|
||||
def clear(self) -> None:
|
||||
self.event_counters = {k: (v + 1 if k in self.events else 0) for k, v in self.event_counters.items()}
|
||||
self.events = self.static_events.copy()
|
||||
|
||||
def contains(self, event_type: str) -> bool:
|
||||
return any(event_type in self.get_events_mapping().get(e, {}) for e in self.events)
|
||||
|
||||
def create_alerts(self, event_types: list[str], callback_args=None):
|
||||
if callback_args is None:
|
||||
callback_args = []
|
||||
|
||||
ret = []
|
||||
for e in self.events:
|
||||
types = self.get_events_mapping()[e].keys()
|
||||
for et in event_types:
|
||||
if et in types:
|
||||
alert = self.get_events_mapping()[e][et]
|
||||
if not isinstance(alert, Alert):
|
||||
alert = alert(*callback_args)
|
||||
|
||||
if DT_CTRL * (self.event_counters[e] + 1) >= alert.creation_delay:
|
||||
alert.alert_type = f"{self.get_event_name(e)}/{et}"
|
||||
alert.event_type = et
|
||||
ret.append(alert)
|
||||
return ret
|
||||
|
||||
def add_from_msg(self, events):
|
||||
for e in events:
|
||||
bisect.insort(self.events, e.name.raw)
|
||||
|
||||
def to_msg(self):
|
||||
ret = []
|
||||
for event_name in self.events:
|
||||
event = self.get_event_msg_type().new_message()
|
||||
event.name = event_name
|
||||
for event_type in self.get_events_mapping().get(event_name, {}):
|
||||
setattr(event, event_type, True)
|
||||
ret.append(event)
|
||||
return ret
|
||||
|
||||
def has(self, event_name: int) -> bool:
|
||||
return event_name in self.events
|
||||
|
||||
def contains_in_list(self, events_list: list[int]) -> bool:
|
||||
return any(event_name in self.events for event_name in events_list)
|
||||
|
||||
def remove(self, event_name: int, static: bool = False) -> None:
|
||||
if static and event_name in self.static_events:
|
||||
self.static_events.remove(event_name)
|
||||
|
||||
if event_name in self.events:
|
||||
self.event_counters[event_name] = self.event_counters[event_name] + 1
|
||||
self.events.remove(event_name)
|
||||
|
||||
@abstractmethod
|
||||
def get_events_mapping(self) -> dict[int, dict[str, Alert | AlertCallbackType]]:
|
||||
raise NotImplementedError
|
||||
|
||||
@abstractmethod
|
||||
def get_event_name(self, event: int) -> str:
|
||||
raise NotImplementedError
|
||||
|
||||
@abstractmethod
|
||||
def get_event_msg_type(self):
|
||||
raise NotImplementedError
|
||||
|
||||
|
||||
EmptyAlert = Alert("" , "", AlertStatus.normal, AlertSize.none, Priority.LOWEST,
|
||||
VisualAlert.none, AudibleAlert.none, 0)
|
||||
|
||||
class NoEntryAlert(Alert):
|
||||
def __init__(self, alert_text_2: str,
|
||||
alert_text_1: str = "openpilot Unavailable",
|
||||
visual_alert: car.CarControl.HUDControl.VisualAlert=VisualAlert.none,
|
||||
priority: Priority = Priority.LOW):
|
||||
if HARDWARE.get_device_type() == 'mici':
|
||||
alert_text_1, alert_text_2 = alert_text_2, alert_text_1
|
||||
super().__init__(alert_text_1, alert_text_2, AlertStatus.normal,
|
||||
AlertSize.mid, priority, visual_alert,
|
||||
AudibleAlert.refuse, 3.)
|
||||
|
||||
|
||||
class SoftDisableAlert(Alert):
|
||||
def __init__(self, alert_text_2: str):
|
||||
super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
|
||||
AlertStatus.userPrompt, AlertSize.full,
|
||||
Priority.MID, VisualAlert.steerRequired,
|
||||
AudibleAlert.warningSoft, 2.),
|
||||
|
||||
|
||||
# less harsh version of SoftDisable, where the condition is user-triggered
|
||||
class UserSoftDisableAlert(SoftDisableAlert):
|
||||
def __init__(self, alert_text_2: str):
|
||||
super().__init__(alert_text_2),
|
||||
self.alert_text_1 = "openpilot will disengage"
|
||||
|
||||
|
||||
class ImmediateDisableAlert(Alert):
|
||||
def __init__(self, alert_text_2: str):
|
||||
super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired,
|
||||
AudibleAlert.warningImmediate, 4.),
|
||||
|
||||
|
||||
class EngagementAlert(Alert):
|
||||
def __init__(self, audible_alert: log.SelfdriveState.AudibleAlert):
|
||||
super().__init__("", "",
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.MID, VisualAlert.none,
|
||||
audible_alert, .2),
|
||||
|
||||
|
||||
class NormalPermanentAlert(Alert):
|
||||
def __init__(self, alert_text_1: str, alert_text_2: str = "", duration: float = 0.2, priority: Priority = Priority.LOWER, creation_delay: float = 0.):
|
||||
super().__init__(alert_text_1, alert_text_2,
|
||||
AlertStatus.normal, AlertSize.mid if len(alert_text_2) else AlertSize.small,
|
||||
priority, VisualAlert.none, AudibleAlert.none, duration, creation_delay=creation_delay),
|
||||
|
||||
|
||||
class StartupAlert(Alert):
|
||||
def __init__(self, alert_text_1: str, alert_text_2: str = "Always keep hands on wheel and eyes on road", alert_status=AlertStatus.normal):
|
||||
alert_size = AlertSize.mid
|
||||
if HARDWARE.get_device_type() == 'mici':
|
||||
if alert_text_2 == "Always keep hands on wheel and eyes on road":
|
||||
alert_text_2 = ""
|
||||
alert_size = AlertSize.small
|
||||
super().__init__(alert_text_1, alert_text_2,
|
||||
alert_status, alert_size,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 5.),
|
||||
Reference in new issue
Block a user