sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
github-actions[bot] committed 2026-07-21 07:44:02 +00:00
commit de77c89cf6
3995 files changed
+1260688

No files matched your search

@@ -0,0 +1,68 @@
import copy
import os
import json
from collections import defaultdict
from dataclasses import dataclass
from openpilot.common.basedir import BASEDIR
from openpilot.common.params import Params
from openpilot.selfdrive.selfdrived.events import Alert
from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EmptyAlert
with open(os.path.join(BASEDIR, "openpilot/selfdrive/selfdrived/alerts_offroad.json")) as f:
OFFROAD_ALERTS = json.load(f)
def set_offroad_alert(alert: str, show_alert: bool, extra_text: str | None = None) -> None:
if show_alert:
a = copy.copy(OFFROAD_ALERTS[alert])
a['extra'] = extra_text or ''
Params().put(alert, a, block=True)
else:
Params().remove(alert)
@dataclass
class AlertEntry:
alert: Alert | None = None
start_frame: int = -1
end_frame: int = -1
added_frame: int = -1
def active(self, frame: int) -> bool:
return frame <= self.end_frame
def just_added(self, frame: int) -> bool:
return self.active(frame) and frame == (self.added_frame + 1)
class AlertManager:
def __init__(self):
self.alerts: dict[str, AlertEntry] = defaultdict(AlertEntry)
self.current_alert = EmptyAlert
def add_many(self, frame: int, alerts: list[Alert]) -> None:
for alert in alerts:
entry = self.alerts[alert.alert_type]
entry.alert = alert
if not entry.just_added(frame):
entry.start_frame = frame
min_end_frame = entry.start_frame + alert.duration
entry.end_frame = max(frame + 1, min_end_frame)
entry.added_frame = frame
def process_alerts(self, frame: int, clear_event_types: set):
ae = AlertEntry()
for v in self.alerts.values():
if not v.alert:
continue
if v.alert.event_type in clear_event_types:
v.end_frame = -1
# sort by priority first and then by start_frame
greater = ae.alert is None or (v.alert.priority, v.start_frame) > (ae.alert.priority, ae.start_frame)
if v.active(frame) and greater:
ae = v
self.current_alert = ae.alert if ae.alert is not None else EmptyAlert
@@ -0,0 +1,54 @@
{
"Offroad_TemperatureTooHigh": {
"text": "Device temperature too high. System cooling down before starting. Current internal component temperature: %1",
"severity": 0
},
"Offroad_ConnectivityNeededPrompt": {
"text": "Immediately connect to the internet to check for updates. If you do not connect to the internet, sunnypilot won't engage in %1",
"severity": 0,
"_comment": "Set extra field to number of days"
},
"Offroad_ConnectivityNeeded": {
"text": "Connect to internet to check for updates. sunnypilot won't automatically start until it connects to internet to check for updates.",
"severity": 1
},
"Offroad_UpdateFailed": {
"text": "Unable to download updates\n%1",
"severity": 1,
"_comment": "Set extra field to the failed reason."
},
"Offroad_NeosUpdate": {
"text": "An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install.",
"severity": 0
},
"Offroad_UnregisteredHardware": {
"text": "Failed to register with comma.ai backend. It will not connect or upload to comma.ai servers, and receives no support from comma.ai. If this is a device purchased at comma.ai/shop, open a ticket at https://comma.ai/support.",
"severity": 1
},
"Offroad_CarUnrecognized": {
"text": "sunnypilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai.",
"severity": 0
},
"Offroad_Recalibration": {
"text": "sunnypilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.",
"severity": 0
},
"Offroad_OSMUpdateRequired": {
"text": "OpenStreetMap database is out of date. New maps must be downloaded if you wish to continue using OpenStreetMap data for Enhanced Speed Control and road name display.\n\n%1",
"severity": 0
},
"Offroad_DriverMonitoringUncertain": {
"text": "Poor visibility detected for driver monitoring. Ensure the device has a clear view of the driver. This can be checked in the device settings. Extreme lighting conditions and/or unconventional mounting positions may also trigger this alert.",
"severity": 0
},
"Offroad_ExcessiveActuation": {
"text": "Excessive %1 actuation detected on your last drive. Please contact support at https://comma.ai/support and share your device's Dongle ID for troubleshooting.",
"severity": 1,
"_comment": "Set extra field to lateral or longitudinal."
},
"Offroad_TiciSupport": {
"text": "<b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> in the branch name for the comma three.",
"severity": 1,
"_comment": "Set extra field to the current branch name."
}
}
+963
View File
@@ -0,0 +1,963 @@
#!/usr/bin/env python3
import math
import os
from openpilot.cereal import log
from opendbc.car.structs import car
import openpilot.cereal.messaging as messaging
from openpilot.common.constants import CV
from openpilot.common.git import get_short_branch
from openpilot.common.realtime import DT_CTRL, DT_DMON
from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER
from openpilot.selfdrive.monitoring.policy import DRIVER_MONITOR_SETTINGS
from openpilot.system.micd import SAMPLE_RATE, SAMPLE_BUFFER
from openpilot.selfdrive.ui.feedback.feedbackd import FEEDBACK_MAX_DURATION
from openpilot.common.hardware import HARDWARE
from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
NoEntryAlert, SoftDisableAlert, UserSoftDisableAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, \
StartupAlert, AlertCallbackType, wrong_car_mode_alert
AlertSize = log.SelfdriveState.AlertSize
AlertStatus = log.SelfdriveState.AlertStatus
VisualAlert = car.CarControl.HUDControl.VisualAlert
AudibleAlert = log.SelfdriveState.AudibleAlert
EventName = log.OnroadEvent.EventName
DMON_LOCKOUT_TIME = DRIVER_MONITOR_SETTINGS()._LOCKOUT_TIME
# get event name from enum
EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()}
class Events(EventsBase):
def __init__(self):
super().__init__()
self.event_counters = dict.fromkeys(EVENTS.keys(), 0)
def get_events_mapping(self) -> dict[int, dict[str, Alert | AlertCallbackType]]:
return EVENTS
def get_event_name(self, event: int):
return EVENT_NAME[event]
def get_event_msg_type(self):
return log.OnroadEvent
# ********** helper functions **********
def get_display_speed(speed_ms: float, metric: bool) -> str:
speed = int(round(speed_ms * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)))
unit = 'km/h' if metric else 'mph'
return f"{speed} {unit}"
# ********** alert callback functions **********
def soft_disable_alert(alert_text_2: str) -> AlertCallbackType:
def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
if soft_disable_time < int(0.5 / DT_CTRL):
return ImmediateDisableAlert(alert_text_2)
return SoftDisableAlert(alert_text_2)
return func
def user_soft_disable_alert(alert_text_2: str) -> AlertCallbackType:
def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
if soft_disable_time < int(0.5 / DT_CTRL):
return ImmediateDisableAlert(alert_text_2)
return UserSoftDisableAlert(alert_text_2)
return func
def startup_master_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
branch = get_short_branch() # Ensure get_short_branch is cached to avoid lags on startup
if "REPLAY" in os.environ:
branch = "replay"
return StartupAlert("WARNING: This branch is not tested", branch, alert_status=AlertStatus.userPrompt)
def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
return NoEntryAlert(f"Drive above {get_display_speed(CP.minEnableSpeed, metric)} to engage")
def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
return Alert(
f"Steer Assist Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}",
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4)
def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
first_word = 'Recalibrating' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibrating'
return Alert(
f"{first_word}: {sm['liveCalibration'].calPerc:.0f}%",
f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}",
AlertStatus.normal, AlertSize.mid,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2)
def too_distracted_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
if sm['driverMonitoringState'].lockout:
mins_left = max(1, round((100 - sm['driverMonitoringState'].lockoutRecoveryPercent) / 100 * DMON_LOCKOUT_TIME * DT_DMON / 60.))
return NoEntryAlert("Too Distracted", f"{mins_left} minute{'s' if mins_left != 1 else ''} Left", priority=Priority.HIGH)
return NoEntryAlert("Pay Attention to Engage", priority=Priority.HIGH)
def audio_feedback_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
duration = FEEDBACK_MAX_DURATION - ((sm['audioFeedback'].blockNum + 1) * SAMPLE_BUFFER / SAMPLE_RATE)
return NormalPermanentAlert(
"Recording Audio Feedback",
f"{round(duration)} second{'s' if round(duration) != 1 else ''} remaining. Press again to save early.",
priority=Priority.LOW)
# *** debug alerts ***
def out_of_space_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
full_perc = round(100. - sm['deviceState'].freeSpacePercent)
return NormalPermanentAlert("Out of Storage", f"{full_perc}% full")
def posenet_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
mdl = sm['modelV2'].velocity.x[0] if len(sm['modelV2'].velocity.x) else math.nan
err = CS.vEgo - mdl
msg = f"Speed Error: {err:.1f} m/s"
return NoEntryAlert(msg, alert_text_1="Posenet Speed Invalid")
def process_not_running_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning]
msg = ', '.join(not_running)
return NoEntryAlert(msg, alert_text_1="Process Not Running")
def comm_issue_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
bs = [s for s in sm.data.keys() if not sm.all_checks([s, ])]
msg = ', '.join(bs[:4]) # can't fit too many on one line
return NoEntryAlert(msg, alert_text_1="Communication Issue Between Processes")
def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
all_cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState')
bad_cams = [s.replace('State', '') for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])]
return NormalPermanentAlert("Camera Malfunction", ', '.join(bad_cams))
def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
rpy = sm['liveCalibration'].rpyCalib
yaw = math.degrees(rpy[2] if len(rpy) == 3 else math.nan)
pitch = math.degrees(rpy[1] if len(rpy) == 3 else math.nan)
angles = f"Remount Device (Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°)"
return NormalPermanentAlert("Calibration Invalid", angles)
def paramsd_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
if not sm['liveParameters'].angleOffsetValid:
angle_offset_deg = sm['liveParameters'].angleOffsetDeg
title = "Steering misalignment detected"
text = f"Angle offset too high (Offset: {angle_offset_deg:.1f}°)"
elif not sm['liveParameters'].steerRatioValid:
steer_ratio = sm['liveParameters'].steerRatio
title = "Steer ratio mismatch"
text = f"Steering rack geometry may be off (Ratio: {steer_ratio:.1f})"
elif not sm['liveParameters'].stiffnessFactorValid:
stiffness_factor = sm['liveParameters'].stiffnessFactor
title = "Abnormal tire stiffness"
text = f"Check tires, pressure, or alignment (Factor: {stiffness_factor:.1f})"
else:
return NoEntryAlert("paramsd Temporary Error")
return NoEntryAlert(alert_text_1=title, alert_text_2=text)
def overheat_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
cpu = max(sm['deviceState'].cpuTempC, default=0.)
gpu = max(sm['deviceState'].gpuTempC, default=0.)
temp = max((cpu, gpu, sm['deviceState'].memoryTempC))
return NormalPermanentAlert("System Overheated", f"{temp:.0f} °C")
def low_memory_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
return NormalPermanentAlert("Low Memory", f"{sm['deviceState'].memoryUsagePercent}% used")
def high_cpu_usage_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
x = max(sm['deviceState'].cpuUsagePercent, default=0.)
return NormalPermanentAlert("High CPU Usage", f"{x}% used")
def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
return NormalPermanentAlert("Driving Model Lagging", f"{sm['modelV2'].frameDropPerc:.1f}% frames dropped")
def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
gb = sm['carControl'].actuators.accel / 4.
steer = sm['carControl'].actuators.torque
vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
return NormalPermanentAlert("Joystick Mode", vals)
def longitudinal_maneuver_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
ad = sm['alertDebug']
audible_alert = AudibleAlert.prompt if 'Active' in ad.alertText1 else AudibleAlert.none
alert_status = AlertStatus.userPrompt if 'Active' in ad.alertText1 else AlertStatus.normal
alert_size = AlertSize.mid if ad.alertText2 else AlertSize.small
return Alert(ad.alertText1, ad.alertText2,
alert_status, alert_size,
Priority.LOW, VisualAlert.none, audible_alert, 0.2)
def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
personality = str(personality).title()
return NormalPermanentAlert(f"Driving Personality: {personality}", duration=1.5)
def invalid_lkas_setting_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
text = "Toggle stock LKAS on or off to engage"
if CP.brand == "tesla":
text = "Switch to Traffic-Aware Cruise Control to engage"
elif CP.brand == "mazda":
text = "Enable your car's LKAS to engage"
elif CP.brand == "nissan":
text = "Disable your car's stock LKAS to engage"
return NormalPermanentAlert("Invalid LKAS setting", text)
EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
# ********** events with no alerts **********
EventName.stockFcw: {},
EventName.actuatorsApiUnavailable: {},
# ********** events only containing alerts displayed in all states **********
EventName.joystickDebug: {
ET.WARNING: joystick_alert,
ET.PERMANENT: NormalPermanentAlert("Joystick Mode"),
},
EventName.longitudinalManeuver: {
ET.WARNING: longitudinal_maneuver_alert,
ET.PERMANENT: NormalPermanentAlert("Longitudinal Maneuver Mode",
"Ensure road ahead is clear"),
},
EventName.lateralManeuver: {
ET.WARNING: longitudinal_maneuver_alert,
ET.PERMANENT: NormalPermanentAlert("Lateral Maneuver Mode"),
},
EventName.selfdriveInitializing: {
ET.NO_ENTRY: NoEntryAlert("System Initializing"),
},
EventName.startup: {
ET.PERMANENT: StartupAlert("Be ready to take over at any time")
},
EventName.startupMaster: {
ET.PERMANENT: startup_master_alert,
},
EventName.startupNoControl: {
ET.PERMANENT: StartupAlert("Dashcam mode"),
ET.NO_ENTRY: NoEntryAlert("Dashcam mode"),
},
EventName.startupNoCar: {
ET.PERMANENT: StartupAlert("Dashcam mode for unsupported car"),
},
EventName.startupNoSecOcKey: {
ET.PERMANENT: NormalPermanentAlert("Dashcam Mode",
"Security Key Not Available",
priority=Priority.HIGH),
},
EventName.dashcamMode: {
ET.PERMANENT: NormalPermanentAlert("Dashcam Mode",
priority=Priority.LOWEST),
},
EventName.invalidLkasSetting: {
ET.PERMANENT: invalid_lkas_setting_alert,
ET.NO_ENTRY: NoEntryAlert("Invalid LKAS setting"),
},
EventName.cruiseMismatch: {
#ET.PERMANENT: ImmediateDisableAlert("openpilot failed to cancel cruise"),
},
# openpilot doesn't recognize the car. This switches openpilot into a
# read-only mode. This can be solved by adding your fingerprint.
# See https://github.com/commaai/openpilot/wiki/Fingerprinting for more information
EventName.carUnrecognized: {
ET.PERMANENT: NormalPermanentAlert("Dashcam Mode",
"Car Unrecognized",
priority=Priority.LOWEST),
},
EventName.aeb: {
ET.PERMANENT: Alert(
"BRAKE!",
"Emergency Braking: Risk of Collision",
AlertStatus.critical, AlertSize.full,
Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 2.),
ET.NO_ENTRY: NoEntryAlert("AEB: Risk of Collision"),
},
EventName.stockAeb: {
ET.PERMANENT: Alert(
"BRAKE!",
"Stock AEB: Risk of Collision",
AlertStatus.critical, AlertSize.full,
Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 2.),
ET.NO_ENTRY: NoEntryAlert("Stock AEB: Risk of Collision"),
},
EventName.stockLkas: {
ET.NO_ENTRY: NoEntryAlert("Stock LKAS: Lane Departure Detected"),
},
EventName.fcw: {
ET.PERMANENT: Alert(
"BRAKE!",
"Risk of Collision",
AlertStatus.critical, AlertSize.full,
Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.warningSoft, 2.),
},
EventName.ldw: {
ET.PERMANENT: Alert(
"Lane Departure Detected",
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.ldw, AudibleAlert.prompt, 3.),
},
# ********** events only containing alerts that display while engaged **********
EventName.steerTempUnavailableSilent: {
ET.WARNING: Alert(
"Steering Assist Temporarily Unavailable",
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8),
},
EventName.driverDistracted1: {
ET.PERMANENT: Alert(
"Pay Attention",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, .1),
},
EventName.driverDistracted2: {
ET.PERMANENT: Alert(
"Pay Attention",
"Driver Distracted",
AlertStatus.userPrompt, AlertSize.mid,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
},
EventName.driverDistracted3: {
ET.PERMANENT: Alert(
"DISENGAGE IMMEDIATELY",
"Driver Distracted",
AlertStatus.critical, AlertSize.full,
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1),
},
EventName.driverUnresponsive1: {
ET.PERMANENT: Alert(
"Touch Steering Wheel: No Face Detected",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1),
},
EventName.driverUnresponsive2: {
ET.PERMANENT: Alert(
"Touch Steering Wheel",
"Driver Unresponsive",
AlertStatus.userPrompt, AlertSize.mid,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
},
EventName.driverUnresponsive3: {
ET.PERMANENT: Alert(
"DISENGAGE IMMEDIATELY",
"Driver Unresponsive",
AlertStatus.critical, AlertSize.full,
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1),
},
EventName.manualRestart: {
ET.WARNING: Alert(
"TAKE CONTROL",
"Resume Driving Manually",
AlertStatus.userPrompt, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
},
EventName.resumeRequired: {
ET.WARNING: Alert(
"Press Resume to Exit Standstill",
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
},
EventName.belowSteerSpeed: {
ET.WARNING: below_steer_speed_alert,
},
EventName.preLaneChangeLeft: {
ET.WARNING: Alert(
"Steer Left to Start Lane Change Once Safe",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
},
EventName.preLaneChangeRight: {
ET.WARNING: Alert(
"Steer Right to Start Lane Change Once Safe",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
},
EventName.laneChangeBlocked: {
ET.WARNING: Alert(
"Car Detected in Blindspot",
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1),
},
EventName.laneChange: {
ET.WARNING: Alert(
"Changing Lanes",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
},
EventName.steerSaturated: {
ET.WARNING: Alert(
"Take Control",
"Turn Exceeds Steering Limit",
AlertStatus.userPrompt, AlertSize.mid,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 2.),
},
# Thrown when the fan is driven at >50% but is not rotating
EventName.fanMalfunction: {
ET.PERMANENT: NormalPermanentAlert("Fan Malfunction", "Likely Hardware Issue"),
},
# Camera is not outputting frames
EventName.cameraMalfunction: {
ET.PERMANENT: camera_malfunction_alert,
ET.SOFT_DISABLE: soft_disable_alert("Camera Malfunction"),
ET.NO_ENTRY: NoEntryAlert("Camera Malfunction: Reboot Your Device"),
},
# Camera framerate too low
EventName.cameraFrameRate: {
ET.PERMANENT: NormalPermanentAlert("Camera Frame Rate Low", "Reboot your Device"),
ET.SOFT_DISABLE: soft_disable_alert("Camera Frame Rate Low"),
ET.NO_ENTRY: NoEntryAlert("Camera Frame Rate Low: Reboot Your Device"),
},
# Unused
EventName.locationdTemporaryError: {
ET.NO_ENTRY: NoEntryAlert("locationd Temporary Error"),
ET.SOFT_DISABLE: soft_disable_alert("locationd Temporary Error"),
},
EventName.locationdPermanentError: {
ET.NO_ENTRY: NoEntryAlert("locationd Permanent Error"),
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("locationd Permanent Error"),
ET.PERMANENT: NormalPermanentAlert("locationd Permanent Error"),
},
# openpilot tries to learn certain parameters about your car by observing
# how the car behaves to steering inputs from both human and openpilot driving.
# This includes:
# - steer ratio: gear ratio of the steering rack. Steering angle divided by tire angle
# - tire stiffness: how much grip your tires have
# - angle offset: most steering angle sensors are offset and measure a non zero angle when driving straight
# This alert is thrown when any of these values exceed a sanity check. This can be caused by
# bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub
EventName.paramsdTemporaryError: {
ET.NO_ENTRY: paramsd_invalid_alert,
ET.SOFT_DISABLE: soft_disable_alert("paramsd Temporary Error"),
},
EventName.paramsdPermanentError: {
ET.NO_ENTRY: NoEntryAlert("paramsd Permanent Error"),
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("paramsd Permanent Error"),
ET.PERMANENT: NormalPermanentAlert("paramsd Permanent Error"),
},
# ********** events that affect controls state transitions **********
EventName.pcmEnable: {
ET.ENABLE: EngagementAlert(AudibleAlert.engage),
},
EventName.buttonEnable: {
ET.ENABLE: EngagementAlert(AudibleAlert.engage),
},
EventName.pcmDisable: {
ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
},
EventName.buttonCancel: {
ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: NoEntryAlert("Cancel Pressed"),
},
EventName.brakeHold: {
ET.WARNING: Alert(
"Press Resume to Exit Brake Hold",
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
},
EventName.parkBrake: {
ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"),
},
EventName.pedalPressed: {
ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: NoEntryAlert("Pedal Pressed",
visual_alert=VisualAlert.brakePressed),
},
EventName.steerDisengage: {
ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: NoEntryAlert("Steering Pressed"),
},
EventName.preEnableStandstill: {
ET.PRE_ENABLE: Alert(
"Release Brake to Engage",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1, creation_delay=1.),
},
EventName.gasPressedOverride: {
ET.OVERRIDE_LONGITUDINAL: Alert(
"",
"",
AlertStatus.normal, AlertSize.none,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1),
},
EventName.steerOverride: {
ET.OVERRIDE_LATERAL: Alert(
"",
"",
AlertStatus.normal, AlertSize.none,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1),
},
EventName.wrongCarMode: {
ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: wrong_car_mode_alert,
},
EventName.resumeBlocked: {
ET.NO_ENTRY: NoEntryAlert("Press Set to Engage"),
},
EventName.wrongCruiseMode: {
ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: NoEntryAlert("Adaptive Cruise Disabled"),
},
EventName.steerTempUnavailable: {
ET.SOFT_DISABLE: soft_disable_alert("Steering Assist Temporarily Unavailable"),
ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable"),
},
EventName.steerTimeLimit: {
ET.SOFT_DISABLE: soft_disable_alert("Vehicle Steering Time Limit"),
ET.NO_ENTRY: NoEntryAlert("Vehicle Steering Time Limit"),
},
EventName.outOfSpace: {
ET.PERMANENT: out_of_space_alert,
ET.NO_ENTRY: NoEntryAlert("Out of Storage"),
},
EventName.belowEngageSpeed: {
ET.NO_ENTRY: below_engage_speed_alert,
},
EventName.sensorDataInvalid: {
ET.PERMANENT: Alert(
"Sensor Data Invalid",
"Possible Hardware Issue",
AlertStatus.normal, AlertSize.mid,
Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=1.),
ET.NO_ENTRY: NoEntryAlert("Sensor Data Invalid"),
ET.SOFT_DISABLE: soft_disable_alert("Sensor Data Invalid"),
},
EventName.noGps: {
},
EventName.tooDistracted: {
ET.NO_ENTRY: too_distracted_alert,
},
EventName.excessiveActuation: {
ET.SOFT_DISABLE: soft_disable_alert("Excessive Actuation"),
ET.NO_ENTRY: NoEntryAlert("Excessive Actuation"),
},
EventName.overheat: {
ET.PERMANENT: overheat_alert,
ET.SOFT_DISABLE: soft_disable_alert("System Overheated"),
ET.NO_ENTRY: NoEntryAlert("System Overheated"),
},
EventName.wrongGear: {
ET.SOFT_DISABLE: user_soft_disable_alert("Gear not D"),
ET.NO_ENTRY: NoEntryAlert("Gear not D"),
},
# This alert is thrown when the calibration angles are outside of the acceptable range.
# For example if the device is pointed too much to the left or the right.
# Usually this can only be solved by removing the mount from the windshield completely,
# and attaching while making sure the device is pointed straight forward and is level.
# See https://comma.ai/setup for more information
EventName.calibrationInvalid: {
ET.PERMANENT: calibration_invalid_alert,
ET.SOFT_DISABLE: soft_disable_alert("Calibration Invalid: Remount Device & Recalibrate"),
ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Remount Device & Recalibrate"),
},
EventName.calibrationIncomplete: {
ET.PERMANENT: calibration_incomplete_alert,
ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"),
ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"),
},
EventName.calibrationRecalibrating: {
ET.PERMANENT: calibration_incomplete_alert,
ET.SOFT_DISABLE: soft_disable_alert("Device Remount Detected: Recalibrating"),
ET.NO_ENTRY: NoEntryAlert("Remount Detected: Recalibrating"),
},
EventName.doorOpen: {
ET.SOFT_DISABLE: user_soft_disable_alert("Door Open"),
ET.NO_ENTRY: NoEntryAlert("Door Open"),
},
EventName.seatbeltNotLatched: {
ET.SOFT_DISABLE: user_soft_disable_alert("Seatbelt Unlatched"),
ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"),
},
EventName.espDisabled: {
ET.SOFT_DISABLE: soft_disable_alert("Electronic Stability Control Disabled"),
ET.NO_ENTRY: NoEntryAlert("Electronic Stability Control Disabled"),
},
EventName.lowBattery: {
ET.SOFT_DISABLE: soft_disable_alert("Low Battery"),
ET.NO_ENTRY: NoEntryAlert("Low Battery"),
},
# Different openpilot services communicate between each other at a certain
# interval. If communication does not follow the regular schedule this alert
# is thrown. This can mean a service crashed, did not broadcast a message for
# ten times the regular interval, or the average interval is more than 10% too high.
EventName.commIssue: {
ET.SOFT_DISABLE: soft_disable_alert("Communication Issue Between Processes"),
ET.NO_ENTRY: comm_issue_alert,
},
EventName.commIssueAvgFreq: {
ET.SOFT_DISABLE: soft_disable_alert("Low Communication Rate Between Processes"),
ET.NO_ENTRY: NoEntryAlert("Low Communication Rate Between Processes"),
},
EventName.selfdrivedLagging: {
ET.SOFT_DISABLE: soft_disable_alert("System Lagging"),
ET.NO_ENTRY: NoEntryAlert("Selfdrive Process Lagging: Reboot Your Device"),
},
# Thrown when manager detects a service exited unexpectedly while driving
EventName.processNotRunning: {
ET.NO_ENTRY: process_not_running_alert,
ET.SOFT_DISABLE: soft_disable_alert("Process Not Running"),
},
EventName.radarFault: {
ET.SOFT_DISABLE: soft_disable_alert("Radar Error: Restart the Car"),
ET.NO_ENTRY: NoEntryAlert("Radar Error: Restart the Car"),
},
EventName.radarTempUnavailable: {
ET.SOFT_DISABLE: soft_disable_alert("Radar Temporarily Unavailable"),
ET.NO_ENTRY: NoEntryAlert("Radar Temporarily Unavailable"),
},
# Every frame from the camera should be processed by the model. If modeld
# is not processing frames fast enough they have to be dropped. This alert is
# thrown when over 20% of frames are dropped.
EventName.modeldLagging: {
ET.SOFT_DISABLE: soft_disable_alert("Driving Model Lagging"),
ET.NO_ENTRY: NoEntryAlert("Driving Model Lagging"),
ET.PERMANENT: modeld_lagging_alert,
},
# Besides predicting the path, lane lines and lead car data the model also
# predicts the current velocity and rotation speed of the car. If the model is
# very uncertain about the current velocity while the car is moving, this
# usually means the model has trouble understanding the scene. This is used
# as a heuristic to warn the driver.
EventName.posenetInvalid: {
ET.SOFT_DISABLE: soft_disable_alert("Posenet Speed Invalid"),
ET.NO_ENTRY: posenet_invalid_alert,
},
# When the localizer detects an acceleration of more than 40 m/s^2 (~4G) we
# alert the driver the device might have fallen from the windshield.
EventName.deviceFalling: {
ET.SOFT_DISABLE: soft_disable_alert("Device Fell Off Mount"),
ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"),
},
EventName.lowMemory: {
ET.SOFT_DISABLE: soft_disable_alert("Low Memory: Reboot Your Device"),
ET.PERMANENT: low_memory_alert,
ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device"),
},
EventName.accFaulted: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Fault: Restart the Car"),
ET.PERMANENT: NormalPermanentAlert("Cruise Fault: Restart the car to engage"),
ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"),
},
EventName.espActive: {
ET.SOFT_DISABLE: soft_disable_alert("Electronic Stability Control Active"),
ET.NO_ENTRY: NoEntryAlert("Electronic Stability Control Active"),
},
EventName.controlsMismatch: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch"),
ET.NO_ENTRY: NoEntryAlert("Controls Mismatch"),
},
# Sometimes the USB stack on the device can get into a bad state
# causing the connection to the panda to be lost
EventName.usbError: {
ET.SOFT_DISABLE: soft_disable_alert("USB Error: Reboot Your Device"),
ET.PERMANENT: NormalPermanentAlert("USB Error: Reboot Your Device"),
ET.NO_ENTRY: NoEntryAlert("USB Error: Reboot Your Device"),
},
# This alert can be thrown for the following reasons:
# - No CAN data received at all
# - CAN data is received, but some message are not received at the right frequency
# If you're not writing a new car port, this is usually cause by faulty wiring
EventName.canError: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Unknown Vehicle Variant"),
ET.PERMANENT: Alert(
"Unknown Vehicle Variant",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.),
ET.NO_ENTRY: NoEntryAlert("Unknown Vehicle Variant"),
},
EventName.canBusMissing: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Bus Disconnected"),
ET.PERMANENT: Alert(
"CAN Bus Disconnected: Likely Faulty Cable",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.),
ET.NO_ENTRY: NoEntryAlert("CAN Bus Disconnected: Check Connections"),
},
EventName.steerUnavailable: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("LKAS Fault: Restart the Car"),
ET.PERMANENT: NormalPermanentAlert("LKAS Fault: Restart the car to engage"),
ET.NO_ENTRY: NoEntryAlert("LKAS Fault: Restart the Car"),
},
EventName.reverseGear: {
ET.PERMANENT: Alert(
"Reverse\nGear",
"",
AlertStatus.normal, AlertSize.full,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
ET.USER_DISABLE: ImmediateDisableAlert("Reverse Gear"),
ET.NO_ENTRY: NoEntryAlert("Reverse Gear"),
},
# On cars that use stock ACC the car can decide to cancel ACC for various reasons.
# When this happens we can no long control the car so the user needs to be warned immediately.
EventName.cruiseDisabled: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Is Off"),
},
# When the relay in the harness box opens the CAN bus between the LKAS camera
# and the rest of the car is separated. When messages from the LKAS camera
# are received on the car side this usually means the relay hasn't opened correctly
# and this alert is thrown.
EventName.relayMalfunction: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Harness Relay Malfunction"),
ET.PERMANENT: NormalPermanentAlert("Harness Relay Malfunction", "Check Hardware"),
ET.NO_ENTRY: NoEntryAlert("Harness Relay Malfunction"),
},
EventName.speedTooLow: {
ET.IMMEDIATE_DISABLE: Alert(
"openpilot Canceled",
"Speed too low",
AlertStatus.normal, AlertSize.mid,
Priority.HIGH, VisualAlert.none, AudibleAlert.disengage, 3.),
},
# When the car is driving faster than most cars in the training data, the model outputs can be unpredictable.
EventName.speedTooHigh: {
ET.WARNING: Alert(
"Speed Too High",
"Model uncertain at this speed",
AlertStatus.userPrompt, AlertSize.mid,
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 4.),
ET.NO_ENTRY: NoEntryAlert("Slow down to engage"),
},
EventName.vehicleSensorsInvalid: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Vehicle Sensors Invalid"),
ET.PERMANENT: NormalPermanentAlert("Vehicle Sensors Calibrating", "Drive to Calibrate"),
ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"),
},
EventName.personalityChanged: {
ET.WARNING: personality_changed_alert,
},
EventName.userBookmark: {
ET.PERMANENT: NormalPermanentAlert("Bookmark Saved", duration=1.5),
},
EventName.audioFeedback: {
ET.PERMANENT: audio_feedback_alert,
},
}
if HARDWARE.get_device_type() == 'mici':
EVENTS.update({
EventName.driverDistracted1: {
ET.PERMANENT: Alert(
"Pay Attention",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, 2),
},
EventName.driverDistracted2: {
ET.PERMANENT: Alert(
"Pay Attention",
"Driver Distracted",
AlertStatus.userPrompt, AlertSize.mid,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, 1),
},
EventName.resumeRequired: {
ET.WARNING: Alert(
"Press Resume",
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
},
EventName.preLaneChangeLeft: {
ET.WARNING: Alert(
"Steer Left",
"Confirm Lane Change",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
},
EventName.preLaneChangeRight: {
ET.WARNING: Alert(
"Steer Right",
"Confirm Lane Change",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
},
EventName.laneChangeBlocked: {
ET.WARNING: Alert(
"Car in Blindspot",
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1),
},
EventName.steerSaturated: {
ET.WARNING: Alert(
"take control",
"turn exceeds limit",
AlertStatus.userPrompt, AlertSize.mid,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 2.),
},
EventName.calibrationIncomplete: {
ET.PERMANENT: calibration_incomplete_alert,
ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"),
ET.NO_ENTRY: NoEntryAlert("Calibrating"),
},
EventName.reverseGear: {
ET.PERMANENT: Alert(
"Reverse",
"",
AlertStatus.normal, AlertSize.full,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
ET.USER_DISABLE: ImmediateDisableAlert("Reverse"),
ET.NO_ENTRY: NoEntryAlert("Reverse"),
},
})
if __name__ == '__main__':
# print all alerts by type and priority
from openpilot.cereal.services import SERVICE_LIST
from collections import defaultdict
event_names = {v: k for k, v in EventName.schema.enumerants.items()}
alerts_by_type: dict[str, dict[Priority, list[str]]] = defaultdict(lambda: defaultdict(list))
CP = car.CarParams.new_message()
CS = car.CarState.new_message()
sm = messaging.SubMaster(list(SERVICE_LIST.keys()))
for i, alerts in EVENTS.items():
for et, alert in alerts.items():
if not isinstance(alert, Alert):
alert = alert(CP, CS, sm, False, 1, log.LongitudinalPersonality.standard)
alerts_by_type[et][alert.priority].append(event_names[i])
all_alerts: dict[str, list[tuple[Priority, list[str]]]] = {}
for et, priority_alerts in alerts_by_type.items():
all_alerts[et] = sorted(priority_alerts.items(), key=lambda x: x[0], reverse=True)
for status, evs in sorted(all_alerts.items(), key=lambda x: x[0]):
print(f"**** {status} ****")
for p, alert_list in evs:
print(f" {repr(p)}:")
print(" ", ', '.join(alert_list), "\n")
+55
View File
@@ -0,0 +1,55 @@
import math
from enum import StrEnum, auto
from openpilot.cereal import messaging
from opendbc.car.structs import car
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.locationd.helpers import Pose
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY
from opendbc.car.lateral import ISO_LATERAL_ACCEL
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
MIN_EXCESSIVE_ACTUATION_COUNT = int(0.25 / DT_CTRL)
MIN_LATERAL_ENGAGE_BUFFER = int(1 / DT_CTRL)
class ExcessiveActuationType(StrEnum):
LONGITUDINAL = auto()
LATERAL = auto()
class ExcessiveActuationCheck:
def __init__(self):
self._excessive_counter = 0
self._engaged_counter = 0
def update(self, sm: messaging.SubMaster, CS: car.CarState, calibrated_pose: Pose) -> ExcessiveActuationType | None:
# CS.aEgo can be noisy to bumps in the road, transitioning from standstill, losing traction, etc.
# longitudinal
accel_calibrated = calibrated_pose.acceleration.x
excessive_long_actuation = sm['carControl'].longActive and (accel_calibrated > ACCEL_MAX * 2 or accel_calibrated < ACCEL_MIN * 2)
# lateral
yaw_rate = calibrated_pose.angular_velocity.yaw
roll = sm['liveParameters'].roll
roll_compensated_lateral_accel = (CS.vEgo * yaw_rate) - (math.sin(roll) * ACCELERATION_DUE_TO_GRAVITY)
# Prevent false positives after overriding
excessive_lat_actuation = False
self._engaged_counter = self._engaged_counter + 1 if sm['carControl'].latActive and not CS.steeringPressed else 0
if self._engaged_counter > MIN_LATERAL_ENGAGE_BUFFER:
if abs(roll_compensated_lateral_accel) > ISO_LATERAL_ACCEL * 2:
excessive_lat_actuation = True
# livePose acceleration can be noisy due to bad mounting or aliased livePose measurements
livepose_valid = abs(CS.aEgo - accel_calibrated) < 2
self._excessive_counter = self._excessive_counter + 1 if livepose_valid and (excessive_long_actuation or excessive_lat_actuation) else 0
excessive_type = None
if self._excessive_counter > MIN_EXCESSIVE_ACTUATION_COUNT:
if excessive_long_actuation:
excessive_type = ExcessiveActuationType.LONGITUDINAL
else:
excessive_type = ExcessiveActuationType.LATERAL
return excessive_type
+653
View File
@@ -0,0 +1,653 @@
#!/usr/bin/env python3
import os
import time
import threading
import openpilot.cereal.messaging as messaging
from openpilot.cereal import log, custom
from opendbc.car.structs import car
from msgq.visionipc import VisionIpcClient, VisionStreamType
from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
from openpilot.common.swaglog import cloudlog
from openpilot.common.gps import get_gps_location_service
from openpilot.selfdrive.car.car_specific import CarSpecificEvents
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
from openpilot.selfdrive.selfdrived.events import Events, ET
from openpilot.selfdrive.selfdrived.helpers import ExcessiveActuationCheck
from openpilot.selfdrive.selfdrived.state import StateMachine
from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert
from openpilot.common.version import get_build_metadata
from openpilot.common.hardware import HARDWARE
from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem
from openpilot.sunnypilot import get_sanitize_int_param
from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP
from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper
from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
TESTING_CLOSET = "TESTING_CLOSET" in os.environ
LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}
ThermalStatus = log.DeviceState.ThermalStatus
State = log.SelfdriveState.OpenpilotState
PandaType = log.PandaState.PandaType
LaneChangeState = log.LaneChangeState
LaneChangeDirection = log.LaneChangeDirection
EventName = log.OnroadEvent.EventName
ButtonType = car.CarState.ButtonEvent.Type
SafetyModel = car.CarParams.SafetyModel
AlertLevel = log.DriverMonitoringState.AlertLevel
MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
TurnDirection = custom.ModelDataV2SP.TurnDirection
IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
class SelfdriveD(CruiseHelper):
def __init__(self, CP=None, CP_SP=None):
self.params = Params()
# Ensure the current branch is cached, otherwise the first cycle lags
build_metadata = get_build_metadata()
if CP is None:
cloudlog.info("selfdrived is waiting for CarParams")
self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
cloudlog.info("selfdrived got CarParams")
else:
self.CP = CP
if CP_SP is None:
cloudlog.info("selfdrived is waiting for CarParamsSP")
self.CP_SP = messaging.log_from_bytes(self.params.get("CarParamsSP", block=True), custom.CarParamsSP)
cloudlog.info("selfdrived got CarParamsSP")
else:
self.CP_SP = CP_SP
self.car_events = CarSpecificEvents(self.CP)
self.pose_calibrator = PoseCalibrator()
self.calibrated_pose: Pose | None = None
self.excessive_actuation_check = ExcessiveActuationCheck()
self.excessive_actuation = self.params.get("Offroad_ExcessiveActuation") is not None
# Setup sockets
self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] + ['selfdriveStateSP', 'onroadEventsSP'])
self.gps_location_service = get_gps_location_service(self.params)
self.gps_packets = [self.gps_location_service]
self.sensor_packets = ["accelerometer", "gyroscope"]
self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
# TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches
self.car_state_sock = messaging.sub_sock('carState', timeout=20)
ignore = self.sensor_packets + self.gps_packets + ['alertDebug', 'lateralManeuverPlan'] + ['modelDataV2SP']
if SIMULATION:
ignore += ['driverCameraState', 'managerState']
if REPLAY:
# no vipc in replay will make them ignored anyways
ignore += ['roadCameraState', 'wideRoadCameraState']
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
'lateralManeuverPlan', 'modelDataV2SP', 'longitudinalPlanSP'] + \
self.camera_packets + self.sensor_packets + self.gps_packets,
ignore_alive=ignore, ignore_avg_freq=ignore,
ignore_valid=ignore, frequency=int(1/DT_CTRL))
# read params
self.is_metric = self.params.get_bool("IsMetric")
self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
car_recognized = self.CP.brand != 'mock'
# cleanup old params
if not self.CP.alphaLongitudinalAvailable:
self.params.remove("AlphaLongitudinalEnabled")
if not self.CP.openpilotLongitudinalControl:
self.params.remove("ExperimentalMode")
self.CS_prev = car.CarState.new_message()
self.AM = AlertManager()
self.events = Events()
self.initialized = False
self.enabled = False
self.active = False
self.mismatch_counter = 0
self.cruise_mismatch_counter = 0
self.last_steering_pressed_frame = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.logged_comm_issue = None
self.not_running_prev = None
self.experimental_mode = False
self.personality = get_sanitize_int_param(
"LongitudinalPersonality",
min(log.LongitudinalPersonality.schema.enumerants.values()),
max(log.LongitudinalPersonality.schema.enumerants.values()),
self.params
)
self.recalibrating_seen = False
self.dm_lockout_set = False
self.dm_uncertain_alerted = False
self.state_machine = StateMachine()
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.ignored_processes = {'mapd', }
# Determine startup event
is_remote = build_metadata.openpilot.comma_remote or build_metadata.openpilot.sunnypilot_remote
self.startup_event = EventName.startup if is_remote and build_metadata.tested_channel else EventName.startupMaster
if HARDWARE.get_device_type() == 'mici':
self.startup_event = None
if not car_recognized:
self.startup_event = EventName.startupNoCar
elif car_recognized and self.CP.passive:
self.startup_event = EventName.startupNoControl
elif self.CP.secOcRequired and not self.CP.secOcKeyAvailable:
self.startup_event = EventName.startupNoSecOcKey
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
set_offroad_alert("Offroad_CarUnrecognized", True)
elif self.CP.passive:
self.events.add(EventName.dashcamMode, static=True)
self.events_sp = EventsSP()
self.events_sp_prev = []
self.mads = ModularAssistiveDrivingSystem(self)
self.icbm = IntelligentCruiseButtonManagement(self.CP, self.CP_SP)
self.car_events_sp = CarSpecificEventsSP(self.CP, self.CP_SP)
CruiseHelper.__init__(self, self.CP)
def update_events(self, CS):
"""Compute onroadEvents from carState"""
self.events.clear()
self.events_sp.clear()
if self.sm['controlsState'].lateralControlState.which() == 'debugState':
self.events.add(EventName.joystickDebug)
self.startup_event = None
if self.sm.recv_frame['lateralManeuverPlan'] > 0:
self.events.add(EventName.lateralManeuver)
self.startup_event = None
elif self.sm.recv_frame['alertDebug'] > 0:
self.events.add(EventName.longitudinalManeuver)
self.startup_event = None
# Add startup event
if self.startup_event is not None:
self.events.add(self.startup_event)
self.startup_event = None
# Don't add any more events if not initialized
if not self.initialized:
self.events.add(EventName.selfdriveInitializing)
return
# Check for user bookmark press (bookmark button or end of LKAS button feedback)
if self.sm.updated['userBookmark']:
self.events.add(EventName.userBookmark)
if self.sm.updated['audioFeedback']:
self.events.add(EventName.audioFeedback)
# Don't add any more events while in dashcam mode
if self.CP.passive:
return
# Block resume if cruise never previously enabled
resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed:
self.events.add(EventName.resumeBlocked)
# Handle DM
if not self.CP.notCar:
# Block engaging until lockout times out or ignition reset
if self.sm['driverMonitoringState'].lockout and not self.dm_lockout_set:
self.params.put_bool("DriverTooDistracted", True)
self.dm_lockout_set = True
elif not self.sm['driverMonitoringState'].lockout and self.dm_lockout_set:
self.params.remove("DriverTooDistracted")
self.dm_lockout_set = False
# No entry conditions
if self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout:
self.events.add(EventName.tooDistracted)
# Alerts
vision_dm = self.sm['driverMonitoringState'].activePolicy == MonitoringPolicy.vision
if self.sm['driverMonitoringState'].alertLevel == AlertLevel.one:
self.events.add(EventName.driverDistracted1 if vision_dm else EventName.driverUnresponsive1)
elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.two:
self.events.add(EventName.driverDistracted2 if vision_dm else EventName.driverUnresponsive2)
elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.three:
self.events.add(EventName.driverDistracted3 if vision_dm else EventName.driverUnresponsive3)
# Warn consistent DM uncertainty
if self.sm['driverMonitoringState'].visionPolicyState.uncertainOffroadAlertPercent >= 100 and not self.dm_uncertain_alerted:
set_offroad_alert("Offroad_DriverMonitoringUncertain", True)
self.dm_uncertain_alerted = True
self.events_sp.add_from_msg(self.sm['longitudinalPlanSP'].events)
# Add car events, ignore if CAN isn't valid
if CS.canValid:
car_events = self.car_events.update(CS, self.CS_prev, self.sm['carControl']).to_msg()
self.events.add_from_msg(car_events)
car_events_sp = self.car_events_sp.update(CS, self.events).to_msg()
self.events_sp.add_from_msg(car_events_sp)
if self.CP.notCar:
# wait for everything to init first
if self.sm.frame > int(2. / DT_CTRL) and self.initialized:
# body always wants to enable
self.events.add(EventName.pcmEnable)
# Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0
if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \
(CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \
(CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)):
self.events.add(EventName.pedalPressed)
# Create events for temperature, disk space, and memory
if self.sm['deviceState'].thermalStatus >= ThermalStatus.overheated:
self.events.add(EventName.overheat)
if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
self.events.add(EventName.outOfSpace)
if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION:
self.events.add(EventName.lowMemory)
# Alert if fan isn't spinning for 5 seconds
if self.sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown:
if self.sm['peripheralState'].fanSpeedRpm < 500 and self.sm['deviceState'].fanSpeedPercentDesired > 50:
# allow enough time for the fan controller in the panda to recover from stalls
if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 15.0:
self.events.add(EventName.fanMalfunction)
else:
self.last_functional_fan_frame = self.sm.frame
# Handle calibration status
cal_status = self.sm['liveCalibration'].calStatus
if cal_status != log.LiveCalibrationData.Status.calibrated:
if cal_status == log.LiveCalibrationData.Status.uncalibrated:
self.events.add(EventName.calibrationIncomplete)
elif cal_status == log.LiveCalibrationData.Status.recalibrating:
if not self.recalibrating_seen:
set_offroad_alert("Offroad_Recalibration", True)
self.recalibrating_seen = True
self.events.add(EventName.calibrationRecalibrating)
else:
self.events.add(EventName.calibrationInvalid)
# Lane departure warning
if self.is_ldw_enabled and self.sm.valid['driverAssistance']:
if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture:
self.events.add(EventName.ldw)
# ******************************************************************************************
# NOTE: To fork maintainers.
# Disabling or nerfing safety features will get you and your users banned from our servers.
# We recommend that you do not change these numbers from the defaults.
if self.sm.updated['liveCalibration']:
self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
if self.sm.updated['livePose']:
device_pose = Pose.from_live_pose(self.sm['livePose'])
self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
if self.calibrated_pose is not None:
excessive_actuation = self.excessive_actuation_check.update(self.sm, CS, self.calibrated_pose)
if not self.excessive_actuation and excessive_actuation is not None:
set_offroad_alert("Offroad_ExcessiveActuation", True, extra_text=str(excessive_actuation))
self.excessive_actuation = True
if self.excessive_actuation:
self.events.add(EventName.excessiveActuation)
# ******************************************************************************************
# Handle lane change
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['modelV2'].meta.laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
else:
self.events.add(EventName.preLaneChangeRight)
elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing):
self.events.add(EventName.laneChange)
# Handle lane turn
lane_turn_direction = self.sm['modelDataV2SP'].laneTurnDirection
if lane_turn_direction == TurnDirection.turnLeft:
self.events_sp.add(custom.OnroadEventSP.EventName.laneTurnLeft)
elif lane_turn_direction == TurnDirection.turnRight:
self.events_sp.add(custom.OnroadEventSP.EventName.laneTurnRight)
for i, pandaState in enumerate(self.sm['pandaStates']):
# All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
if i < len(self.CP.safetyConfigs):
safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \
pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \
pandaState.alternativeExperience != self.CP.alternativeExperience
else:
safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
# safety mismatch allows some time for pandad to set the safety mode and publish it back from panda
if (safety_mismatch and self.sm.frame*DT_CTRL > 10.) or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch)
if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
self.events.add(EventName.relayMalfunction)
# Handle HW and system malfunctions
# Order is very intentional here. Be careful when modifying this.
# All events here should at least have NO_ENTRY and SOFT_DISABLE.
num_events = len(self.events)
not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning}
if self.sm.recv_frame['managerState'] and len(not_running):
if not_running != self.not_running_prev:
cloudlog.event("process_not_running", not_running=not_running, error=True)
self.not_running_prev = not_running
if self.sm.recv_frame['managerState'] and (not_running - self.ignored_processes):
self.events.add(EventName.processNotRunning)
else:
if not SIMULATION and not self.rk.lagging:
if not self.sm.all_alive(self.camera_packets):
self.events.add(EventName.cameraMalfunction)
elif not self.sm.all_freq_ok(self.camera_packets):
self.events.add(EventName.cameraFrameRate)
if not REPLAY and self.rk.lagging:
self.events.add(EventName.selfdrivedLagging)
if self.CP.openpilotLongitudinalControl:
if self.sm['radarState'].radarErrors.canError:
self.events.add(EventName.canError)
elif self.sm['radarState'].radarErrors.radarUnavailableTemporary:
self.events.add(EventName.radarTempUnavailable)
elif any(self.sm['radarState'].radarErrors.to_dict().values()):
self.events.add(EventName.radarFault)
if not self.sm.valid['pandaStates']:
self.events.add(EventName.usbError)
if CS.canTimeout:
self.events.add(EventName.canBusMissing)
elif not CS.canValid:
self.events.add(EventName.canError)
# generic catch-all. ideally, a more specific event should be added above instead
has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE))
no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
if not self.sm.all_checks() and no_system_errors:
if not self.sm.all_alive():
self.events.add(EventName.commIssue)
elif not self.sm.all_freq_ok():
self.events.add(EventName.commIssueAvgFreq)
else:
self.events.add(EventName.commIssue)
logs = {
'invalid': [s for s, valid in self.sm.valid.items() if not valid],
'not_alive': [s for s, alive in self.sm.alive.items() if not alive],
'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
}
if logs != self.logged_comm_issue:
cloudlog.event("commIssue", error=True, **logs)
self.logged_comm_issue = logs
else:
self.logged_comm_issue = None
if not self.CP.notCar:
if not self.sm['livePose'].posenetOK:
self.events.add(EventName.posenetInvalid)
if not self.sm['livePose'].inputsOK:
self.events.add(EventName.locationdTemporaryError)
if not self.sm['liveParameters'].valid and cal_status == log.LiveCalibrationData.Status.calibrated and not TESTING_CLOSET and (not SIMULATION or REPLAY):
self.events.add(EventName.paramsdTemporaryError)
# conservative HW alert. if the data or frequency are off, locationd will throw an error
if any((self.sm.frame - self.sm.recv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets):
self.events.add(EventName.sensorDataInvalid)
if not REPLAY:
# Check for mismatch between openpilot and car's PCM
cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
if self.cruise_mismatch_counter > int(6. / DT_CTRL):
self.events.add(EventName.cruiseMismatch)
# Send a "steering required alert" if saturation count has reached the limit
if CS.steeringPressed:
self.last_steering_pressed_frame = self.sm.frame
recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0
controlstate = self.sm['controlsState']
lac = getattr(controlstate.lateralControlState, controlstate.lateralControlState.which())
if lac.active and not recent_steer_pressed and not self.CP.notCar:
clipped_speed = max(CS.vEgo, 0.3)
actual_lateral_accel = controlstate.curvature * (clipped_speed**2)
desired_lateral_accel = self.sm['modelV2'].action.desiredCurvature * (clipped_speed**2)
undershooting = abs(desired_lateral_accel) / abs(1e-3 + actual_lateral_accel) > 1.2
turning = abs(desired_lateral_accel) > 1.0
# TODO: lac.saturated includes speed and other checks, should be pulled out
if undershooting and turning and lac.saturated:
self.events.add(EventName.steerSaturated)
# Check for FCW
stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25
model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
if (planner_fcw or model_fcw) and not self.CP.notCar:
self.events.add(EventName.fcw)
# GPS checks
gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0
if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500):
self.events.add(EventName.noGps)
if gps_ok:
self.distance_traveled = 0
self.distance_traveled += abs(CS.vEgo) * DT_CTRL
# TODO: fix simulator
if not SIMULATION or REPLAY:
if self.sm['modelV2'].frameDropPerc > 1:
self.events.add(EventName.modeldLagging)
# mute canBusMissing event if in Park, as it sometimes may trigger a false alarm with MADS in Paused state
if CS.gearShifter == car.CarState.GearShifter.park and self.mads.enabled:
self.events.remove(EventName.canBusMissing)
CruiseHelper.update(self, CS, self.events_sp, self.experimental_mode)
# decrement personality on distance button press
if self.CP.openpilotLongitudinalControl:
if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
if not self.experimental_mode_switched:
self.personality = (self.personality - 1) % 3
self.params.put('LongitudinalPersonality', self.personality)
self.events.add(EventName.personalityChanged)
self.experimental_mode_switched = False
self.icbm.run(CS, self.sm['carControl'], self.sm['longitudinalPlanSP'], self.is_metric)
def data_sample(self):
_car_state = messaging.recv_one(self.car_state_sock)
CS = _car_state.carState if _car_state else self.CS_prev
self.sm.update(0)
if not self.initialized:
all_valid = CS.canValid and self.sm.all_checks()
timed_out = self.sm.frame * DT_CTRL > 6.
if all_valid or timed_out or (SIMULATION and not REPLAY):
available_streams = VisionIpcClient.available_streams("camerad", block=False)
if VisionStreamType.VISION_STREAM_ROAD not in available_streams:
self.sm.ignore_alive.append('roadCameraState')
self.sm.ignore_valid.append('roadCameraState')
if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams:
self.sm.ignore_alive.append('wideRoadCameraState')
self.sm.ignore_valid.append('wideRoadCameraState')
if REPLAY and any(ps.controlsAllowed for ps in self.sm['pandaStates']):
self.state_machine.state = State.enabled
self.initialized = True
cloudlog.event(
"selfdrived.initialized",
dt=self.sm.frame*DT_CTRL,
timeout=timed_out,
canValid=CS.canValid,
invalid=[s for s, valid in self.sm.valid.items() if not valid],
not_alive=[s for s, alive in self.sm.alive.items() if not alive],
not_freq_ok=[s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
error=True,
)
# When the panda and selfdrived do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter += 1
return CS
def update_alerts(self, CS):
clear_event_types = set()
if ET.WARNING not in self.state_machine.current_alert_types:
clear_event_types.add(ET.WARNING)
if self.enabled:
clear_event_types.add(ET.NO_ENTRY)
pers = LONGITUDINAL_PERSONALITY_MAP[self.personality]
callback_args = [self.CP, CS, self.sm, self.is_metric,
self.state_machine.soft_disable_timer, pers]
alerts = self.events.create_alerts(self.state_machine.current_alert_types, callback_args)
alerts_sp = self.events_sp.create_alerts(self.state_machine.current_alert_types, callback_args)
self.AM.add_many(self.sm.frame, alerts + alerts_sp)
self.AM.process_alerts(self.sm.frame, clear_event_types)
def publish_selfdriveState(self, CS):
# selfdriveState
ss_msg = messaging.new_message('selfdriveState')
ss_msg.valid = True
ss = ss_msg.selfdriveState
ss.enabled = self.enabled
ss.active = self.active
ss.state = self.state_machine.state
ss.engageable = not self.events.contains(ET.NO_ENTRY)
ss.experimentalMode = self.experimental_mode
ss.personality = self.personality
ss.alertText1 = self.AM.current_alert.alert_text_1
ss.alertText2 = self.AM.current_alert.alert_text_2
ss.alertSize = self.AM.current_alert.alert_size
ss.alertStatus = self.AM.current_alert.alert_status
ss.alertType = self.AM.current_alert.alert_type
ss.alertSound = self.AM.current_alert.audible_alert
ss.alertHudVisual = self.AM.current_alert.visual_alert
self.pm.send('selfdriveState', ss_msg)
# onroadEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('onroadEvents', len(self.events))
ce_send.valid = True
ce_send.onroadEvents = self.events.to_msg()
self.pm.send('onroadEvents', ce_send)
self.events_prev = self.events.names.copy()
# selfdriveStateSP
ss_sp_msg = messaging.new_message('selfdriveStateSP')
ss_sp_msg.valid = True
ss_sp = ss_sp_msg.selfdriveStateSP
mads = ss_sp.mads
mads.state = self.mads.state_machine.state
mads.enabled = self.mads.enabled
mads.active = self.mads.active
mads.available = self.mads.enabled_toggle
icbm = ss_sp.intelligentCruiseButtonManagement
icbm.state = self.icbm.state
icbm.sendButton = self.icbm.cruise_button
icbm.vTarget = self.icbm.v_target
self.pm.send('selfdriveStateSP', ss_sp_msg)
# onroadEventsSP - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events_sp.names != self.events_sp_prev):
ce_send_sp = messaging.new_message('onroadEventsSP')
ce_send_sp.valid = True
ce_send_sp.onroadEventsSP.events = self.events_sp.to_msg()
self.pm.send('onroadEventsSP', ce_send_sp)
self.events_sp_prev = self.events_sp.names.copy()
def step(self):
CS = self.data_sample()
self.update_events(CS)
if not self.CP.passive and self.initialized:
self.enabled, self.active = self.state_machine.update(self.events)
if not self.CP.notCar:
self.mads.update(CS)
self.update_alerts(CS)
self.publish_selfdriveState(CS)
self.CS_prev = CS
def params_thread(self, evt):
while not evt.is_set():
self.is_metric = self.params.get_bool("IsMetric")
self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
self.personality = self.params.get("LongitudinalPersonality", return_default=True)
self.mads.read_params()
time.sleep(0.1)
def run(self):
e = threading.Event()
t = threading.Thread(target=self.params_thread, args=(e, ))
try:
t.start()
while True:
self.step()
self.rk.monitor_time()
finally:
e.set()
t.join()
def main():
config_realtime_process(4, Priority.CTRL_HIGH)
s = SelfdriveD()
s.run()
if __name__ == "__main__":
main()
+98
View File
@@ -0,0 +1,98 @@
from openpilot.cereal import log
from openpilot.selfdrive.selfdrived.events import Events, ET
from openpilot.common.realtime import DT_CTRL
State = log.SelfdriveState.OpenpilotState
SOFT_DISABLE_TIME = 3 # seconds
ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding)
ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES)
class StateMachine:
def __init__(self):
self.current_alert_types = [ET.PERMANENT]
self.state = State.disabled
self.soft_disable_timer = 0
def update(self, events: Events):
# decrement the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
self.current_alert_types = [ET.PERMANENT]
# ENABLED, SOFT DISABLING, PRE ENABLING, OVERRIDING
if self.state != State.disabled:
# user and immediate disable always have priority in a non-disabled state
if events.contains(ET.IMMEDIATE_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
elif events.contains(ET.USER_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.USER_DISABLE)
else:
# ENABLED
if self.state == State.enabled:
if events.contains(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
self.current_alert_types.append(ET.SOFT_DISABLE)
elif events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL):
self.state = State.overriding
self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
# SOFT DISABLING
elif self.state == State.softDisabling:
if not events.contains(ET.SOFT_DISABLE):
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
elif self.soft_disable_timer > 0:
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.soft_disable_timer <= 0:
self.state = State.disabled
# PRE ENABLING
elif self.state == State.preEnabled:
if not events.contains(ET.PRE_ENABLE):
self.state = State.enabled
else:
self.current_alert_types.append(ET.PRE_ENABLE)
# OVERRIDING
elif self.state == State.overriding:
if events.contains(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
self.current_alert_types.append(ET.SOFT_DISABLE)
elif not (events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL)):
self.state = State.enabled
else:
self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
# DISABLED
elif self.state == State.disabled:
if events.contains(ET.ENABLE):
if events.contains(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
else:
if events.contains(ET.PRE_ENABLE):
self.state = State.preEnabled
elif events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL):
self.state = State.overriding
else:
self.state = State.enabled
self.current_alert_types.append(ET.ENABLE)
# Check if openpilot is engaged and actuators are enabled
enabled = self.state in ENABLED_STATES
active = self.state in ACTIVE_STATES
if active:
self.current_alert_types.append(ET.WARNING)
return enabled, active
@@ -0,0 +1,60 @@
import random
from openpilot.selfdrive.selfdrived.events import Alert, EVENTS
from openpilot.selfdrive.selfdrived.alertmanager import AlertManager
from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EmptyAlert
class TestAlertManager:
def test_duration(self):
"""
Enforce that an alert lasts for max(alert duration, duration the alert is added)
"""
for duration in range(1, 100):
alert = None
while not isinstance(alert, Alert):
event = random.choice([e for e in EVENTS.values() if len(e)])
alert = random.choice(list(event.values()))
alert.duration = duration
# check two cases:
# - alert is added to AM for <= the alert's duration
# - alert is added to AM for > alert's duration
for greater in (True, False):
if greater:
add_duration = duration + random.randint(1, 10)
else:
add_duration = random.randint(1, duration)
show_duration = max(duration, add_duration)
AM = AlertManager()
for frame in range(duration+10):
if frame < add_duration:
AM.add_many(frame, [alert, ])
AM.process_alerts(frame, set())
shown = AM.current_alert != EmptyAlert
should_show = frame <= show_duration
assert shown == should_show, f"{frame=} {add_duration=} {duration=}"
# check one case:
# - if alert is re-added to AM before it ends the duration is extended
if duration > 1:
AM = AlertManager()
show_duration = duration * 2
for frame in range(duration * 2 + 10):
if frame == 0:
AM.add_many(frame, [alert, ])
if frame == duration:
# add alert one frame before it ends
assert AM.current_alert == alert
AM.add_many(frame, [alert, ])
AM.process_alerts(frame, set())
shown = AM.current_alert != EmptyAlert
should_show = frame <= show_duration
assert shown == should_show, f"{frame=} {duration=}"
@@ -0,0 +1,131 @@
import copy
import json
import os
import random
from PIL import Image, ImageDraw, ImageFont
from openpilot.cereal import log
from opendbc.car.structs import car
from openpilot.cereal.messaging import SubMaster
from openpilot.common.basedir import BASEDIR
from openpilot.common.params import Params
from openpilot.selfdrive.selfdrived.events import Alert, EVENTS, ET
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS
AlertSize = log.SelfdriveState.AlertSize
OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "openpilot/selfdrive/selfdrived/alerts_offroad.json")
# TODO: add callback alerts
ALERTS = []
for event_types in EVENTS.values():
for alert in event_types.values():
ALERTS.append(alert)
class TestAlerts:
@classmethod
def setup_class(cls):
with open(OFFROAD_ALERTS_PATH) as f:
cls.offroad_alerts = json.loads(f.read())
# Create fake objects for callback
cls.CS = car.CarState.new_message()
cls.CP = car.CarParams.new_message()
cfg = [c for c in CONFIGS if c.proc_name == 'selfdrived'][0]
cls.sm = SubMaster(cfg.pubs)
def test_events_defined(self):
# Ensure all events in capnp schema are defined in events.py
events = log.OnroadEvent.EventName.schema.enumerants
for name, e in events.items():
if not name.endswith("DEPRECATED") and not name.startswith("eventReserved"):
fail_msg = f"{name} @{e} not in EVENTS"
assert e in EVENTS.keys(), fail_msg
# ensure alert text doesn't exceed allowed width
def test_alert_text_length(self):
font_path = os.path.join(BASEDIR, "openpilot/selfdrive/assets/fonts")
regular_font_path = os.path.join(font_path, "Inter-SemiBold.ttf")
bold_font_path = os.path.join(font_path, "Inter-Bold.ttf")
semibold_font_path = os.path.join(font_path, "Inter-SemiBold.ttf")
max_text_width = 2160 - 300 # full screen width is usable, minus sidebar
draw = ImageDraw.Draw(Image.new('RGB', (0, 0)))
fonts = {
AlertSize.small: [ImageFont.truetype(semibold_font_path, 74)],
AlertSize.mid: [ImageFont.truetype(bold_font_path, 88),
ImageFont.truetype(regular_font_path, 66)],
}
for alert in ALERTS:
if not isinstance(alert, Alert):
alert = alert(self.CP, self.CS, self.sm, False, 100, log.LongitudinalPersonality.standard)
# for full size alerts, both text fields wrap the text,
# so it's unlikely that they would go past the max width
if alert.alert_size in (AlertSize.none, AlertSize.full):
continue
for i, txt in enumerate([alert.alert_text_1, alert.alert_text_2]):
if i >= len(fonts[alert.alert_size]):
break
font = fonts[alert.alert_size][i]
left, _, right, _ = draw.textbbox((0, 0), txt, font)
width = right - left
msg = f"type: {alert.alert_type} msg: {txt}"
assert width <= max_text_width, msg
def test_alert_sanity_check(self):
for event_types in EVENTS.values():
for event_type, a in event_types.items():
# TODO: add callback alerts
if not isinstance(a, Alert):
continue
if a.alert_size == AlertSize.none:
assert len(a.alert_text_1) == 0
assert len(a.alert_text_2) == 0
elif a.alert_size == AlertSize.small:
assert len(a.alert_text_1) > 0
assert len(a.alert_text_2) == 0
elif a.alert_size == AlertSize.mid:
assert len(a.alert_text_1) > 0
assert len(a.alert_text_2) > 0
else:
assert len(a.alert_text_1) > 0
assert a.duration >= 0.
if event_type not in (ET.WARNING, ET.PERMANENT, ET.PRE_ENABLE):
assert a.creation_delay == 0.
def test_offroad_alerts(self):
params = Params()
for a in self.offroad_alerts:
# set the alert
alert = copy.copy(self.offroad_alerts[a])
set_offroad_alert(a, True)
alert['extra'] = ''
assert alert == params.get(a)
# then delete it
set_offroad_alert(a, False)
assert params.get(a) is None
def test_offroad_alerts_extra_text(self):
params = Params()
for i in range(50):
# set the alert
a = random.choice(list(self.offroad_alerts))
alert = self.offroad_alerts[a]
set_offroad_alert(a, True, extra_text="a"*i)
written_alert = params.get(a)
assert "a"*i == written_alert['extra']
assert alert["text"] == written_alert['text']
@@ -0,0 +1,92 @@
from openpilot.cereal import log
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.selfdrived.state import StateMachine, SOFT_DISABLE_TIME
from openpilot.selfdrive.selfdrived.events import Events, ET, EVENTS, NormalPermanentAlert
State = log.SelfdriveState.OpenpilotState
# The event types that maintain the current state
MAINTAIN_STATES = {State.enabled: (None,), State.disabled: (None,), State.softDisabling: (ET.SOFT_DISABLE,),
State.preEnabled: (ET.PRE_ENABLE,), State.overriding: (ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)}
ALL_STATES = tuple(State.schema.enumerants.values())
# The event types checked in DISABLED section of state machine
ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)
def make_event(event_types):
event = {}
for ev in event_types:
event[ev] = NormalPermanentAlert("alert")
EVENTS[0] = event
return 0
class TestStateMachine:
def setup_method(self):
self.events = Events()
self.state_machine = StateMachine()
self.state_machine.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
def test_immediate_disable(self):
for state in ALL_STATES:
for et in MAINTAIN_STATES[state]:
self.events.add(make_event([et, ET.IMMEDIATE_DISABLE]))
self.state_machine.state = state
self.state_machine.update(self.events)
assert State.disabled == self.state_machine.state
self.events.clear()
def test_user_disable(self):
for state in ALL_STATES:
for et in MAINTAIN_STATES[state]:
self.events.add(make_event([et, ET.USER_DISABLE]))
self.state_machine.state = state
self.state_machine.update(self.events)
assert State.disabled == self.state_machine.state
self.events.clear()
def test_soft_disable(self):
for state in ALL_STATES:
if state == State.preEnabled: # preEnabled considers NO_ENTRY instead
continue
for et in MAINTAIN_STATES[state]:
self.events.add(make_event([et, ET.SOFT_DISABLE]))
self.state_machine.state = state
self.state_machine.update(self.events)
assert self.state_machine.state == State.disabled if state == State.disabled else State.softDisabling
self.events.clear()
def test_soft_disable_timer(self):
self.state_machine.state = State.enabled
self.events.add(make_event([ET.SOFT_DISABLE]))
self.state_machine.update(self.events)
for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)):
assert self.state_machine.state == State.softDisabling
self.state_machine.update(self.events)
assert self.state_machine.state == State.disabled
def test_no_entry(self):
# Make sure noEntry keeps us disabled
for et in ENABLE_EVENT_TYPES:
self.events.add(make_event([ET.NO_ENTRY, et]))
self.state_machine.update(self.events)
assert self.state_machine.state == State.disabled
self.events.clear()
def test_no_entry_pre_enable(self):
# preEnabled with noEntry event
self.state_machine.state = State.preEnabled
self.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE]))
self.state_machine.update(self.events)
assert self.state_machine.state == State.preEnabled
def test_maintain_states(self):
# Given current state's event type, we should maintain state
for state in ALL_STATES:
for et in MAINTAIN_STATES[state]:
self.state_machine.state = state
self.events.add(make_event([et]))
self.state_machine.update(self.events)
assert self.state_machine.state == state
self.events.clear()