Productionize assisted driving milestones

Assisted-by: OpenAI Codex
This commit is contained in:
Isaac Barham
2026-08-29 07:45:40 -04:00
parent 26e4889fcb
commit 27a220677a
30 changed files with 793 additions and 390 deletions
+24 -1
View File
@@ -478,7 +478,30 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
}
}
struct CustomReserved10 @0xcb9fd56c7057593a {
struct AssistedDrivingMilestoneState @0xcb9fd56c7057593a {
enabled @0 :Bool;
madsDistanceMeters @1 :Float64;
fullAssistDistanceMeters @2 :Float64;
event @3 :Event;
struct Event {
id @0 :UInt64;
category @1 :Category;
distanceMeters @2 :Float64;
previousDistanceMeters @3 :Float64;
unit @4 :Unit;
}
enum Category {
none @0;
mads @1;
fullAssist @2;
}
enum Unit {
imperial @0;
metric @1;
}
}
struct CustomReserved11 @0xc2243c65e0340384 {
+1 -1
View File
@@ -2640,7 +2640,7 @@ struct Event {
carStateSP @114 :Custom.CarStateSP;
liveMapDataSP @115 :Custom.LiveMapDataSP;
modelDataV2SP @116 :Custom.ModelDataV2SP;
customReserved10 @136 :Custom.CustomReserved10;
assistedDrivingMilestoneState @136 :Custom.AssistedDrivingMilestoneState;
customReserved11 @137 :Custom.CustomReserved11;
customReserved12 @138 :Custom.CustomReserved12;
customReserved13 @139 :Custom.CustomReserved13;
+1
View File
@@ -90,6 +90,7 @@ _services: dict[str, tuple] = {
"carParamsSP": (True, 0.02, 1),
"carControlSP": (True, 100., 10),
"carStateSP": (True, 100., 10),
"assistedDrivingMilestoneState": (True, 10., 1),
"liveMapDataSP": (True, 1., 1),
"modelDataV2SP": (True, 20., None, QueueSize.BIG),
"liveLocationKalman": (True, 20.),
+4
View File
@@ -97,6 +97,10 @@ Params::Params(const std::string &path) {
}
Params::~Params() {
flushNonBlockingWrites();
}
void Params::flushNonBlockingWrites() {
if (future.valid()) {
future.wait();
}
+1
View File
@@ -75,6 +75,7 @@ public:
return put(key.c_str(), val ? "1" : "0", 1);
}
void putNonBlocking(const std::string &key, const std::string &val);
void flushNonBlockingWrites();
inline void putBoolNonBlocking(const std::string &key, bool val) {
putNonBlocking(key, val ? "1" : "0");
}
+5
View File
@@ -73,6 +73,7 @@ params_get = _bind("params_get", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool],
params_get_bool = _bind("params_get_bool", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool], ctypes.c_bool)
params_put = _bind("params_put", [ParamsHandle, ctypes.c_char_p, ctypes.c_char_p, ctypes.c_size_t, ctypes.c_bool], ctypes.c_int)
params_put_bool = _bind("params_put_bool", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool, ctypes.c_bool], ctypes.c_int)
params_flush = _bind("params_flush", [ParamsHandle])
params_remove = _bind("params_remove", [ParamsHandle, ctypes.c_char_p], ctypes.c_int)
params_get_path = _bind("params_get_path", [ParamsHandle, ctypes.c_char_p, ctypes.c_size_t], ParamsBuffer)
params_keys_size = _bind("params_keys_size", [ParamsHandle], ctypes.c_size_t)
@@ -178,6 +179,10 @@ class Params:
def put_bool(self, key, val, block=False):
params_put_bool(self.p, self.check_key(key), val, block)
def flush(self):
"""Wait for all prior nonblocking writes from this Params instance."""
params_flush(self.p)
def remove(self, key):
params_remove(self.p, self.check_key(key))
+6
View File
@@ -133,6 +133,12 @@ int params_put_bool(ParamsHandle *handle, const char *key, bool value, bool bloc
});
}
void params_flush(ParamsHandle *handle) noexcept {
translate_exceptions([&]() {
handle->params.flushNonBlockingWrites();
});
}
int params_remove(ParamsHandle *handle, const char *key) noexcept {
return translate_exceptions(-1, [&]() {
return handle->params.remove(key);
+3 -1
View File
@@ -136,7 +136,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
// --- sunnypilot params --- //
{"ApiCache_DriveStats", {PERSISTENT, JSON}},
{"AssistedDistanceMilestoneResetVersion", {PERSISTENT, STRING, "0"}},
{"AssistedDrivingMilestonesEnabled", {PERSISTENT | BACKUP, BOOL, "1"}},
{"AssistedDrivingMilestoneState", {PERSISTENT, JSON, "{}"}},
{"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
{"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
{"BlinkerLateralReengageDelay", {PERSISTENT | BACKUP, INT, "0"}}, // seconds
@@ -167,6 +168,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
{"IsReleaseSpBranch", {CLEAR_ON_MANAGER_START, BOOL}},
{"LastGPSPositionLLK", {PERSISTENT, STRING}},
{"LastDriveAssistedDrivingSummary", {PERSISTENT, JSON, "{}"}},
{"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
{"MadsDrivenDistanceMeters", {PERSISTENT, FLOAT, "0.0"}},
{"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
+7
View File
@@ -106,6 +106,13 @@ class TestParams(OpenpilotTestCase):
assert q.get("CarParams") is None
assert q.get("CarParams", True) == b"1"
def test_flush_non_blocking_writes(self):
self.params.put("DongleId", "first")
self.params.put("DongleId", "last")
self.params.flush()
assert self.params.get("DongleId") == "last"
def test_params_all_keys(self):
keys = Params().all_keys()
+66 -1
View File
@@ -32,7 +32,14 @@ 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.button_state_tracker import ButtonStateTracker
from openpilot.sunnypilot.selfdrive.selfdrived.assisted_driving_milestones import (
AssistCategory,
AssistedDrivingMilestones,
MilestoneEvent,
MilestoneStore,
)
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
from openpilot.sunnypilot.system.statsd import statlog
REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
@@ -88,7 +95,8 @@ class SelfdriveD(CruiseHelper):
self.big_model_ready_t = 0.
# Setup sockets
self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] + ['selfdriveStateSP', 'onroadEventsSP'])
self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] +
['selfdriveStateSP', 'onroadEventsSP', 'assistedDrivingMilestoneState'])
self.gps_location_service = get_gps_location_service(self.params)
self.gps_packets = [self.gps_location_service]
@@ -127,6 +135,7 @@ class SelfdriveD(CruiseHelper):
self.params.remove("ExperimentalMode")
self.CS_prev = car.CarState.new_message()
self.car_state_log_mono_time = 0
self.AM = AlertManager()
self.events = Events()
@@ -137,6 +146,11 @@ class SelfdriveD(CruiseHelper):
self.cruise_mismatch_counter = 0
self.last_steering_pressed_frame = 0
self.distance_traveled = 0
self.assisted_driving_milestones = AssistedDrivingMilestones(MilestoneStore(self.params))
self.assisted_driving_milestones_enabled = bool(self.params.get("AssistedDrivingMilestonesEnabled", return_default=True))
self.assisted_driving_milestone_drive_id = ""
self._milestone_event: MilestoneEvent | None = None
self._milestone_event_expires_ns = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.logged_comm_issue = None
@@ -527,6 +541,8 @@ class SelfdriveD(CruiseHelper):
def data_sample(self):
_car_state = messaging.recv_one(self.car_state_sock)
CS = _car_state.carState if _car_state else self.CS_prev
if _car_state is not None:
self.car_state_log_mono_time = _car_state.logMonoTime
self.sm.update(0)
@@ -645,6 +661,31 @@ class SelfdriveD(CruiseHelper):
self.pm.send('onroadEventsSP', ce_send_sp)
self.events_sp_prev = self.events_sp.names.copy()
def publish_assisted_driving_milestones(self, now_ns: int, event: MilestoneEvent | None) -> None:
if event is not None:
self._milestone_event = event
self._milestone_event_expires_ns = now_ns + 1_000_000_000
elif now_ns >= self._milestone_event_expires_ns:
self._milestone_event = None
if event is None and self.sm.frame % 10 != 0:
return
snapshot = self.assisted_driving_milestones.snapshot()
msg = messaging.new_message("assistedDrivingMilestoneState")
msg.valid = True
state = msg.assistedDrivingMilestoneState
state.enabled = self.assisted_driving_milestones_enabled
state.madsDistanceMeters = snapshot.distances_meters[AssistCategory.MADS]
state.fullAssistDistanceMeters = snapshot.distances_meters[AssistCategory.FULL_ASSIST]
if self._milestone_event is not None:
state.event.id = self._milestone_event.event_id
state.event.category = self._milestone_event.category.value
state.event.distanceMeters = self._milestone_event.distance_meters
state.event.previousDistanceMeters = self._milestone_event.previous_distance_meters
state.event.unit = self._milestone_event.unit.value
self.pm.send("assistedDrivingMilestoneState", msg)
def step(self):
CS = self.data_sample()
self.update_events(CS)
@@ -654,6 +695,28 @@ class SelfdriveD(CruiseHelper):
self.mads.update(CS)
self.update_alerts(CS)
now_ns = time.monotonic_ns()
if not self.assisted_driving_milestone_drive_id:
self.assisted_driving_milestone_drive_id = self.params.get("CurrentRoute") or ""
self.assisted_driving_milestones.set_drive_id(self.assisted_driving_milestone_drive_id)
car_control = self.sm['carControl']
milestone_event = self.assisted_driving_milestones.update(
self.car_state_log_mono_time,
CS.vEgo,
lat_active=car_control.latActive,
long_active=car_control.longActive,
is_metric=self.is_metric,
enabled=self.assisted_driving_milestones_enabled,
)
if milestone_event is not None:
cloudlog.event("assisted_driving_milestone_reached",
event_id=milestone_event.event_id,
category=milestone_event.category.value,
distance_meters=milestone_event.distance_meters)
statlog.gauge(f"assisted_driving_milestone.{milestone_event.category.value}.meters",
milestone_event.distance_meters)
self.publish_assisted_driving_milestones(now_ns, milestone_event)
self.button_state_tracker.update(CS)
self.publish_selfdriveState(CS)
@@ -666,6 +729,7 @@ class SelfdriveD(CruiseHelper):
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.assisted_driving_milestones_enabled = bool(self.params.get("AssistedDrivingMilestonesEnabled", return_default=True))
self.mads.read_params()
time.sleep(0.1)
@@ -679,6 +743,7 @@ class SelfdriveD(CruiseHelper):
self.step()
self.rk.monitor_time()
finally:
self.assisted_driving_milestones.close()
e.set()
t.join()
+4 -2
View File
@@ -64,8 +64,8 @@ class MiciMainLayout(Scroller):
# Start onboarding if terms or training not completed, make sure to push after self
self._onboarding_window = OnboardingWindow(lambda: gui_app.pop_widgets_to(self))
skip_onboarding_for_local_prototype = PC and os.getenv("SP_MILESTONE_PROTOTYPE") == "1"
if not self._onboarding_window.completed and not skip_onboarding_for_local_prototype:
skip_onboarding_for_milestone_preview = PC and os.getenv("SP_MILESTONE_PREVIEW") == "1"
if not self._onboarding_window.completed and not skip_onboarding_for_milestone_preview:
gui_app.push_widget(self._onboarding_window)
# initialize correct onroad layout
@@ -123,6 +123,8 @@ class MiciMainLayout(Scroller):
self._onroad_time_delay = rl.get_time()
else:
self._scroll_to(self._home_layout)
if hasattr(self._home_layout, "request_drive_summary"):
self._home_layout.request_drive_summary()
# FIXME: these two pops can interrupt user interacting in the settings
if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY:
@@ -47,6 +47,7 @@ class TogglesLayoutMici(NavScroller):
is_metric_toggle = BigParamControl("use metric units", "IsMetric")
ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled")
always_on_dm_toggle = BigParamControl("always-on driver monitor", "AlwaysOnDM")
milestone_celebrations_toggle = BigParamControl("assisted driving milestones", "AssistedDrivingMilestonesEnabled")
record_front = BigParamControl("record & upload cabin camera", "RecordFront", toggle_callback=restart_needed_callback)
record_mic = BigParamControl("record & upload mic audio", "RecordAudio", toggle_callback=restart_needed_callback)
enable_openpilot = BigParamControl("enable sunnypilot", "OpenpilotEnabledToggle", toggle_callback=restart_needed_callback)
@@ -57,6 +58,7 @@ class TogglesLayoutMici(NavScroller):
is_metric_toggle,
ldw_toggle,
always_on_dm_toggle,
milestone_celebrations_toggle,
record_front,
record_mic,
enable_openpilot,
@@ -68,6 +70,7 @@ class TogglesLayoutMici(NavScroller):
("IsMetric", is_metric_toggle),
("IsLdwEnabled", ldw_toggle),
("AlwaysOnDM", always_on_dm_toggle),
("AssistedDrivingMilestonesEnabled", milestone_celebrations_toggle),
("RecordFront", record_front),
("RecordAudio", record_mic),
("OpenpilotEnabledToggle", enable_openpilot),
@@ -19,14 +19,14 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCamera
from openpilot.common.transformations.orientation import rot_from_euler
from enum import IntEnum
MILESTONE_PROTOTYPE_ENABLED = gui_app.sunnypilot_ui()
MILESTONE_CELEBRATION_ENABLED = gui_app.sunnypilot_ui()
if gui_app.sunnypilot_ui():
from openpilot.selfdrive.ui.sunnypilot.mici.onroad.hud_renderer import HudRendererSP as HudRenderer
from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus
if MILESTONE_PROTOTYPE_ENABLED:
from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_celebration_prototype import MilestoneCelebrationPrototype
if MILESTONE_CELEBRATION_ENABLED:
from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_celebration import MilestoneCelebration
OpState = log.SelfdriveState.OpenpilotState
CALIBRATED = log.ExtrinsicsCalibration.Status.calibrated
@@ -161,7 +161,7 @@ class AugmentedRoadView(CameraView):
self._alert_renderer = AlertRenderer()
self._driver_state_renderer = DriverStateRenderer()
self._confidence_ball = ConfidenceBall()
self._milestone_celebration = self._child(MilestoneCelebrationPrototype()) if MILESTONE_PROTOTYPE_ENABLED else None
self._milestone_celebration = self._child(MilestoneCelebration()) if MILESTONE_CELEBRATION_ENABLED else None
self._offroad_label = UnifiedLabel("start the car to\nuse sunnypilot", 54, FontWeight.DISPLAY,
text_color=rl.Color(255, 255, 255, int(255 * 0.9)),
alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
@@ -227,11 +227,14 @@ class AugmentedRoadView(CameraView):
# Fade out bottom of overlays for looks
rl.draw_texture_ex(self._fade_texture, rl.Vector2(self._content_rect.x, self._content_rect.y), 0.0, 1.0, rl.WHITE)
if self._milestone_celebration is not None:
self._milestone_celebration.render(self._content_rect)
alert_to_render, not_animating_out = self._alert_renderer.will_render()
if self._milestone_celebration is not None:
if alert_to_render is not None:
self._milestone_celebration.cancel_for_alert()
else:
self._milestone_celebration.render(self._content_rect)
# Hide DMoji when disengaged unless AlwaysOnDM is enabled
should_draw_dmoji = (not self._hud_renderer.drawing_top_icons() and
(ui_state.status != UIStatus.DISENGAGED or ui_state.always_on_dm))
@@ -256,9 +259,6 @@ class AugmentedRoadView(CameraView):
self._confidence_ball.render(self.rect)
self._bookmark_icon.render(self.rect)
if self._milestone_celebration is not None:
self._milestone_celebration.capture_screenshot()
def _switch_stream_if_needed(self, sm):
if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams:
v_ego = sm['carState'].vEgo
+10 -4
View File
@@ -15,7 +15,6 @@ from openpilot.system import micd
from openpilot.common.hardware import HARDWARE
from openpilot.sunnypilot.selfdrive.ui.quiet_mode import QuietMode
from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_tracker_prototype import MILESTONE_EVENT_PAYLOAD
SAMPLE_RATE = 48000
SAMPLE_BUFFER = 4096 # (approx 100ms)
@@ -90,6 +89,7 @@ class Soundd(QuietMode):
self.selfdrive_timeout_alert = False
self.pending_stop = False
self.last_milestone_event_id = 0
self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
@@ -170,8 +170,14 @@ class Soundd(QuietMode):
self.selfdrive_timeout_alert = False
def update_milestone_alert(self, sm):
milestone_event = sm.updated['customReservedRawData0'] and bytes(sm['customReservedRawData0']) == MILESTONE_EVENT_PAYLOAD
if milestone_event and self.current_alert == AudibleAlert.none and not self.enabled:
if not sm.updated['assistedDrivingMilestoneState']:
return
milestone_state = sm['assistedDrivingMilestoneState']
event_id = milestone_state.event.id
if not milestone_state.enabled or event_id == 0 or event_id == self.last_milestone_event_id:
return
self.last_milestone_event_id = event_id
if self.current_alert == AudibleAlert.none and not self.enabled:
self.update_alert(AudibleAlert.complete)
def calculate_volume(self, weighted_db):
@@ -189,7 +195,7 @@ class Soundd(QuietMode):
import sounddevice as sd
micd.patch_sounddevice(sd)
sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure', 'customReservedRawData0'])
sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure', 'assistedDrivingMilestoneState'])
with self.get_stream(sd) as stream:
rk = Ratekeeper(20)
@@ -5,14 +5,28 @@ 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 math
import time
import pyray as rl
from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.system.ui.lib.application import FontWeight
from openpilot.system.ui.lib.multilang import tr
from openpilot.system.ui.widgets.icon_widget import IconWidget
from openpilot.system.ui.widgets.label import UnifiedLabel
from openpilot.system.ui.widgets.label import UnifiedLabel, gui_label
METERS_PER_MILE = 1609.344
METERS_PER_KILOMETER = 1000.0
SUMMARY_DURATION_SECONDS = 10.0
SUMMARY_WAIT_SECONDS = 3.0
def _nonnegative_float(value) -> float:
try:
return max(0.0, float(value))
except (TypeError, ValueError):
return 0.0
class MiciHomeLayoutSP(MiciHomeLayout):
@@ -26,6 +40,52 @@ class MiciHomeLayoutSP(MiciHomeLayout):
gray_idx = self._status_bar_layout.widgets.index(self._egpu_icon_gray)
self._status_bar_layout.widgets.insert(gray_idx + 1, self._egpu_icon_default)
self._status_bar_layout.widgets.insert(gray_idx + 2, self._egpu_icon_orange)
initial_summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
self._last_summary_id = initial_summary.get("id", 0)
self._summary_wait_until = 0.0
self._summary_visible_until = 0.0
self._drive_summary = {}
def request_drive_summary(self) -> None:
self._summary_wait_until = time.monotonic() + SUMMARY_WAIT_SECONDS
def _render(self, _: rl.Rectangle) -> None:
super()._render(_)
now = time.monotonic()
if now < self._summary_wait_until:
summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
summary_id = summary.get("id", 0)
if summary_id and summary_id != self._last_summary_id:
self._last_summary_id = summary_id
distances = summary.get("distancesMeters", {})
enabled = ui_state.params.get_bool("AssistedDrivingMilestonesEnabled")
if enabled and any(_nonnegative_float(distances.get(category, 0.0)) > 0.0 for category in ("mads", "fullAssist")):
self._drive_summary = summary
self._summary_visible_until = now + SUMMARY_DURATION_SECONDS
self._summary_wait_until = 0.0
if now < self._summary_visible_until:
self._draw_drive_summary(_)
def _draw_drive_summary(self, rect: rl.Rectangle) -> None:
distances = self._drive_summary.get("distancesMeters", {})
metric = self._drive_summary.get("unit") == "metric"
meters_per_unit = METERS_PER_KILOMETER if metric else METERS_PER_MILE
unit = "KM" if metric else "MI"
mads = _nonnegative_float(distances.get("mads", 0.0)) / meters_per_unit
full_assist = _nonnegative_float(distances.get("fullAssist", 0.0)) / meters_per_unit
rl.draw_rectangle_rec(rect, rl.Color(0, 0, 0, 235))
gui_label(rl.Rectangle(rect.x, rect.y + 14, rect.width, 52), tr("DRIVE COMPLETE"), 42,
font_weight=FontWeight.SEMI_BOLD, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
gui_label(rl.Rectangle(rect.x + 20, rect.y + 78, rect.width / 2 - 30, 42), tr("MADS"), 28,
color=rl.Color(255, 255, 255, 184), alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 78, rect.width / 2 - 30, 42), tr("FULL ASSIST"), 28,
color=rl.Color(255, 255, 255, 184), alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
gui_label(rl.Rectangle(rect.x + 20, rect.y + 116, rect.width / 2 - 30, 72), f"{mads:.1f} {unit}", 48,
font_weight=FontWeight.DISPLAY, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 116, rect.width / 2 - 30, 72), f"{full_assist:.1f} {unit}", 48,
font_weight=FontWeight.DISPLAY, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
def _set_egpu_visibility(self):
chestnut = ui_state.sm["deviceState"].chestnutPresent
@@ -1,7 +1,6 @@
"""Tesla-style persistent assisted-distance milestones over the on-road view."""
"""Render assisted-driving milestone celebrations over the on-road view."""
import math
import os
import random
import time
from collections import deque
@@ -9,27 +8,20 @@ from dataclasses import dataclass
import pyray as rl
from openpilot.cereal import messaging
from openpilot.common.hardware import PC
from openpilot.cereal import custom
from openpilot.selfdrive.ui.mici.onroad.alert_renderer import ALERT_BACKGROUND_OPACITY
from openpilot.selfdrive.ui.mici.onroad.hud_renderer import FONT_SIZES
from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_tracker_prototype import (
AssistCategory,
AssistedDistanceMilestoneTracker,
DistanceMilestone,
MILESTONE_EVENT_PAYLOAD,
MilestoneStore,
assist_category,
)
from openpilot.system.ui.lib.application import FontWeight, gui_app
from openpilot.system.ui.lib.multilang import tr
from openpilot.system.ui.lib.text_measure import measure_text_cached
from openpilot.system.ui.widgets import Widget
CELEBRATION_DURATION = 4.5
PARTICLE_COUNT = 150
PERSIST_INTERVAL_SECONDS = 60.0
METERS_PER_MILE = 1609.344
METERS_PER_KILOMETER = 1000.0
CONFETTI_COLORS = (
rl.Color(255, 55, 95, 255),
@@ -55,23 +47,25 @@ class ConfettiParticle:
color: rl.Color
class MilestoneCelebrationPrototype(Widget):
"""Throwaway visual spike enabled on the sunnypilot comma four UI."""
@dataclass(frozen=True)
class CelebrationMilestone:
event_id: int
full_assist: bool
distance_meters: float
previous_distance_meters: float
metric: bool
class MilestoneCelebration(Widget):
"""Pure renderer for typed assisted-driving milestone events."""
def __init__(self):
super().__init__()
self._drive_started_time = -1.0
self._celebration_started_time: float | None = None
self._current_milestone: DistanceMilestone | None = None
self._pending_milestones: deque[DistanceMilestone] = deque()
self._store = MilestoneStore()
stored_distances = self._store.reset() if PC and os.getenv("SP_MILESTONE_RESET") == "1" else self._store.load()
self._tracker = AssistedDistanceMilestoneTracker(stored_distances)
self._pm = messaging.PubMaster(["customReservedRawData0"])
self._last_persisted_distances = stored_distances
self._last_persist_time = time.monotonic()
self._screenshot_taken = False
self._screenshot_ready = False
self._current_milestone: CelebrationMilestone | None = None
self._pending_milestones: deque[CelebrationMilestone] = deque()
self._last_event_id = 0
self._particles = self._make_particles()
@staticmethod
@@ -96,32 +90,16 @@ class MilestoneCelebrationPrototype(Widget):
def _render(self, rect: rl.Rectangle, /) -> None:
now = time.monotonic()
if ui_state.started_time != self._drive_started_time:
self._persist_distances(force=True)
self._drive_started_time = ui_state.started_time
self._celebration_started_time = None
self._current_milestone = None
self._pending_milestones.clear()
self._tracker.reset_sampling()
self._screenshot_taken = False
self._screenshot_ready = False
car_control = ui_state.sm["carControl"]
category = assist_category(car_control.latActive, car_control.longActive)
milestones = self._tracker.update(
ui_state.sm.logMonoTime["carState"],
ui_state.sm["carState"].vEgo,
category,
)
self._pending_milestones.extend(milestones)
self._persist_distances(force=bool(milestones))
self._consume_event(suppress=False)
if self._current_milestone is None and self._pending_milestones:
self._current_milestone = self._pending_milestones.popleft()
self._celebration_started_time = now
milestone_event = messaging.new_message("customReservedRawData0", size=len(MILESTONE_EVENT_PAYLOAD), valid=True)
milestone_event.customReservedRawData0 = MILESTONE_EVENT_PAYLOAD
self._pm.send("customReservedRawData0", milestone_event)
if self._celebration_started_time is None or self._current_milestone is None:
return
@@ -136,28 +114,35 @@ class MilestoneCelebrationPrototype(Widget):
self._draw_background_scrim(rect, alpha)
self._draw_confetti(rect, elapsed, alpha)
self._draw_milestone(rect, elapsed, alpha, self._current_milestone)
self._screenshot_ready = elapsed >= 1.0
def hide_event(self) -> None:
self._persist_distances(force=True)
super().hide_event()
def cancel_for_alert(self) -> None:
self._consume_event(suppress=True)
self._celebration_started_time = None
self._current_milestone = None
self._pending_milestones.clear()
def _persist_distances(self, force: bool = False) -> None:
distances = self._tracker.distances_meters()
if distances == self._last_persisted_distances:
def _consume_event(self, suppress: bool) -> None:
if not ui_state.sm.updated["assistedDrivingMilestoneState"]:
return
now = time.monotonic()
if force or now - self._last_persist_time >= PERSIST_INTERVAL_SECONDS:
self._store.save(distances)
self._last_persisted_distances = distances
self._last_persist_time = now
def capture_screenshot(self) -> None:
screenshot_path = os.getenv("SP_MILESTONE_SCREENSHOT")
if screenshot_path and self._screenshot_ready and not self._screenshot_taken:
rl.rl_draw_render_batch_active()
rl.take_screenshot(screenshot_path)
self._screenshot_taken = True
state = ui_state.sm["assistedDrivingMilestoneState"]
event = state.event
if not state.enabled:
self._celebration_started_time = None
self._current_milestone = None
self._pending_milestones.clear()
return
if event.id == 0 or event.id == self._last_event_id:
return
self._last_event_id = event.id
if suppress:
return
self._pending_milestones.append(CelebrationMilestone(
event_id=event.id,
full_assist=event.category == custom.AssistedDrivingMilestoneState.Category.fullAssist,
distance_meters=event.distanceMeters,
previous_distance_meters=event.previousDistanceMeters,
metric=event.unit == custom.AssistedDrivingMilestoneState.Unit.metric,
))
def _draw_confetti(self, rect: rl.Rectangle, elapsed: float, alpha: float) -> None:
travel_height = rect.height * 1.45
@@ -174,7 +159,7 @@ class MilestoneCelebrationPrototype(Widget):
rl.draw_rectangle_pro(particle_rect, origin, particle.angle + particle.spin * elapsed, color)
@staticmethod
def _draw_milestone(rect: rl.Rectangle, elapsed: float, alpha: float, milestone: DistanceMilestone) -> None:
def _draw_milestone(rect: rl.Rectangle, elapsed: float, alpha: float, milestone: CelebrationMilestone) -> None:
# Match the comma four set-speed hierarchy: DISPLAY number with a MAX-sized label.
scale = rect.height / 240.0
pulse = 1.0 + 0.025 * math.sin(min(elapsed, 0.6) / 0.6 * math.pi)
@@ -187,15 +172,17 @@ class MilestoneCelebrationPrototype(Widget):
semibold_font = gui_app.font(FontWeight.SEMI_BOLD)
tween_progress = min(elapsed / 0.85, 1.0)
tween_progress = 1.0 - (1.0 - tween_progress) ** 3
previous_distance = milestone.previous_distance_miles
displayed_distance = previous_distance + (milestone.distance_miles - previous_distance) * tween_progress
meters_per_unit = METERS_PER_KILOMETER if milestone.metric else METERS_PER_MILE
previous_distance = milestone.previous_distance_meters / meters_per_unit
milestone_distance = milestone.distance_meters / meters_per_unit
displayed_distance = previous_distance + (milestone_distance - previous_distance) * tween_progress
if tween_progress >= 1.0:
number = f"{round(milestone.distance_miles):,}"
number = f"{round(milestone_distance):,}"
else:
number = f"{displayed_distance:,.1f}"
unit = "MI"
category = "FULL ASSIST" if milestone.category == AssistCategory.FULL_ASSIST else "MADS"
milestone_label = "MILESTONE"
unit = tr("KM") if milestone.metric else tr("MI")
category = tr("FULL ASSIST") if milestone.full_assist else tr("MADS")
milestone_label = tr("MILESTONE")
unit_bounds = measure_text_cached(semibold_font, unit, unit_size)
number_bounds = measure_text_cached(display_font, number, number_size)
@@ -1,140 +0,0 @@
"""Persistent assisted-distance milestone tracking."""
import math
from collections.abc import Mapping
from dataclasses import dataclass
from enum import StrEnum
from openpilot.common.params import Params
METERS_PER_MILE = 1609.344
MAX_SAMPLE_INTERVAL_SECONDS = 0.5
MILESTONE_EVENT_PAYLOAD = b"sunnypilot-milestone-v1"
class AssistCategory(StrEnum):
MADS = "mads"
FULL_ASSIST = "full_assist"
PARAM_KEYS = {
AssistCategory.MADS: "MadsDrivenDistanceMeters",
AssistCategory.FULL_ASSIST: "FullAssistDrivenDistanceMeters",
}
def assist_category(lat_active: bool, long_active: bool) -> AssistCategory | None:
if not lat_active:
return None
return AssistCategory.FULL_ASSIST if long_active else AssistCategory.MADS
def next_milestone_miles(distance_miles: float) -> float:
"""Return the next value in the 1, 2, 5 × 10ⁿ milestone ladder."""
distance_miles = max(0.0, distance_miles)
magnitude = 10.0 ** math.floor(math.log10(max(1.0, distance_miles)))
for multiplier in (1.0, 2.0, 5.0):
candidate = multiplier * magnitude
if candidate > distance_miles + 1e-9:
return candidate
return 10.0 * magnitude
def previous_milestone_miles(milestone_miles: float) -> float:
if milestone_miles <= 1.0:
return 0.0
magnitude = 10.0 ** math.floor(math.log10(milestone_miles))
normalized = milestone_miles / magnitude
if normalized <= 1.0 + 1e-9:
return 5.0 * magnitude / 10.0
if normalized <= 2.0 + 1e-9:
return magnitude
return 2.0 * magnitude
@dataclass(frozen=True)
class DistanceMilestone:
category: AssistCategory
distance_meters: float
previous_distance_meters: float
@property
def distance_miles(self) -> float:
return self.distance_meters / METERS_PER_MILE
@property
def previous_distance_miles(self) -> float:
return self.previous_distance_meters / METERS_PER_MILE
class MilestoneStore:
def __init__(self, params: Params | None = None):
self._params = params or Params()
def load(self) -> dict[AssistCategory, float]:
return {
category: max(0.0, self._params.get(key, return_default=True) or 0.0)
for category, key in PARAM_KEYS.items()
}
def save(self, distances_meters: Mapping[AssistCategory, float]) -> None:
for category, key in PARAM_KEYS.items():
self._params.put(key, max(0.0, distances_meters.get(category, 0.0)))
def reset(self) -> dict[AssistCategory, float]:
distances = dict.fromkeys(AssistCategory, 0.0)
self.save(distances)
return distances
class AssistedDistanceMilestoneTracker:
def __init__(self, initial_distances_meters: Mapping[AssistCategory, float] | None = None):
initial_distances_meters = initial_distances_meters or {}
self._distance_meters = {
category: max(0.0, initial_distances_meters.get(category, 0.0))
for category in AssistCategory
}
self._next_milestone_meters = {
category: next_milestone_miles(distance / METERS_PER_MILE) * METERS_PER_MILE
for category, distance in self._distance_meters.items()
}
self.reset_sampling()
def reset_sampling(self) -> None:
self._last_timestamp_ns: int | None = None
self._last_speed_mps = 0.0
self._last_category: AssistCategory | None = None
def distance_meters(self, category: AssistCategory) -> float:
return self._distance_meters[category]
def distances_meters(self) -> dict[AssistCategory, float]:
return self._distance_meters.copy()
def update(self, timestamp_ns: int, speed_mps: float, category: AssistCategory | None) -> list[DistanceMilestone]:
milestones: list[DistanceMilestone] = []
speed_mps = max(0.0, speed_mps)
if self._last_timestamp_ns is not None and timestamp_ns != self._last_timestamp_ns:
dt = (timestamp_ns - self._last_timestamp_ns) / 1e9
if 0 < dt <= MAX_SAMPLE_INTERVAL_SECONDS and self._last_category is not None:
delta_meters = (self._last_speed_mps + speed_mps) / 2.0 * dt
active_category = self._last_category
self._distance_meters[active_category] += delta_meters
next_milestone = self._next_milestone_meters[active_category]
while self._distance_meters[active_category] >= next_milestone:
milestone_miles = next_milestone / METERS_PER_MILE
milestones.append(DistanceMilestone(
active_category,
next_milestone,
previous_milestone_miles(milestone_miles) * METERS_PER_MILE,
))
next_milestone = next_milestone_miles(milestone_miles) * METERS_PER_MILE
self._next_milestone_meters[active_category] = next_milestone
self._last_timestamp_ns = timestamp_ns
self._last_speed_mps = speed_mps
self._last_category = category
return milestones
@@ -35,7 +35,8 @@ class UIStateSP:
self.is_sp_release: bool = self.params.get_bool("IsReleaseSpBranch")
self.sm_services_ext = [
"modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
"gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay"
"gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay",
"assistedDrivingMilestoneState",
]
self.sunnylink_state = SunnylinkState()
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Generate the temporary milestone celebration chime."""
"""Generate the assisted-driving milestone celebration chime."""
import math
import wave
+38
View File
@@ -0,0 +1,38 @@
#!/usr/bin/env python3
"""Publish deterministic milestone events for the local comma-four UI preview."""
import itertools
import time
from openpilot.cereal import messaging
def main() -> None:
pm = messaging.PubMaster(["assistedDrivingMilestoneState"])
milestones = itertools.cycle(((1, 0, "mads"), (2, 1, "fullAssist"), (5, 2, "mads"), (10, 5, "fullAssist")))
event_id = 0
milestone, previous_milestone, category = 0, 0, "mads"
next_event_time = time.monotonic() + 1.0
while True:
now = time.monotonic()
if now >= next_event_time:
event_id += 1
milestone, previous_milestone, category = next(milestones)
next_event_time = now + 6.0
msg = messaging.new_message("assistedDrivingMilestoneState")
state = msg.assistedDrivingMilestoneState
state.enabled = True
if event_id:
state.event.id = event_id
state.event.category = category
state.event.distanceMeters = milestone * 1609.344
state.event.previousDistanceMeters = previous_milestone * 1609.344
state.event.unit = "imperial"
pm.send("assistedDrivingMilestoneState", msg)
time.sleep(0.1)
if __name__ == "__main__":
main()
+27
View File
@@ -0,0 +1,27 @@
#!/usr/bin/env bash
set -e
repo_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../../.." && pwd)"
replay_pid=""
preview_pid=""
cleanup() {
for pid in "$preview_pid" "$replay_pid"; do
if [[ -n "$pid" ]]; then
kill "$pid" 2>/dev/null || true
wait "$pid" 2>/dev/null || true
fi
done
}
trap cleanup EXIT INT TERM
export PATH="$repo_root/.venv/bin:$PATH"
export SP_MILESTONE_PREVIEW=1
playback="${SP_MILESTONE_PLAYBACK:-1}"
"$repo_root/openpilot/tools/replay/replay" --demo --playback "$playback" &
replay_pid=$!
"$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/tests/milestone_preview.py" &
preview_pid=$!
"$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py"
@@ -1,24 +0,0 @@
#!/usr/bin/env bash
# PROTOTYPE: launch the demo replay and comma four milestone celebration together.
set -e
prototype_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../../.." && pwd)"
prototype_replay_pid=""
cleanup_prototype() {
if [[ -n "$prototype_replay_pid" ]]; then
kill "$prototype_replay_pid" 2>/dev/null || true
wait "$prototype_replay_pid" 2>/dev/null || true
fi
}
trap cleanup_prototype EXIT INT TERM
export PATH="$prototype_root/.venv/bin:$PATH"
export SP_MILESTONE_PROTOTYPE=1
export SP_MILESTONE_RESET=1
prototype_playback="${SP_MILESTONE_PLAYBACK:-1}"
"$prototype_root/openpilot/tools/replay/replay" --demo --playback "$prototype_playback" &
prototype_replay_pid=$!
"$prototype_root/.venv/bin/python" "$prototype_root/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py"
@@ -1,106 +0,0 @@
import unittest
from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_tracker_prototype import (
METERS_PER_MILE,
PARAM_KEYS,
AssistCategory,
AssistedDistanceMilestoneTracker,
MilestoneStore,
assist_category,
next_milestone_miles,
)
class TestAssistedDistanceMilestoneTracker(unittest.TestCase):
def test_store_round_trips_each_category(self):
class ParamsStub:
def __init__(self):
self.values = {
PARAM_KEYS[AssistCategory.MADS]: 123.0,
PARAM_KEYS[AssistCategory.FULL_ASSIST]: 456.0,
}
def get(self, key, return_default=False):
return self.values.get(key, 0.0 if return_default else None)
def put(self, key, value):
self.values[key] = value
params = ParamsStub()
store = MilestoneStore(params) # type: ignore[arg-type]
self.assertEqual(store.load(), {
AssistCategory.MADS: 123.0,
AssistCategory.FULL_ASSIST: 456.0,
})
store.save({AssistCategory.MADS: 789.0, AssistCategory.FULL_ASSIST: 987.0})
self.assertEqual(store.load(), {
AssistCategory.MADS: 789.0,
AssistCategory.FULL_ASSIST: 987.0,
})
def test_classifies_actual_actuation(self):
self.assertIsNone(assist_category(False, False))
self.assertIsNone(assist_category(False, True))
self.assertEqual(assist_category(True, False), AssistCategory.MADS)
self.assertEqual(assist_category(True, True), AssistCategory.FULL_ASSIST)
def test_uses_a_one_two_five_milestone_ladder(self):
cases = (
(0.0, 1.0),
(1.0, 2.0),
(2.0, 5.0),
(5.0, 10.0),
(10.0, 20.0),
(49.9, 50.0),
(50.0, 100.0),
(999.0, 1000.0),
(1000.0, 2000.0),
)
for distance, expected in cases:
with self.subTest(distance=distance):
self.assertEqual(next_milestone_miles(distance), expected)
def test_tracks_categories_and_emits_dynamic_milestones(self):
tracker = AssistedDistanceMilestoneTracker({
AssistCategory.MADS: METERS_PER_MILE - 5.0,
AssistCategory.FULL_ASSIST: 2 * METERS_PER_MILE - 5.0,
})
self.assertEqual(tracker.update(0, 10.0, AssistCategory.MADS), [])
milestones = tracker.update(500_000_000, 10.0, AssistCategory.FULL_ASSIST)
self.assertEqual(len(milestones), 1)
self.assertEqual(milestones[0].category, AssistCategory.MADS)
self.assertAlmostEqual(milestones[0].previous_distance_miles, 0.0)
self.assertAlmostEqual(milestones[0].distance_miles, 1.0)
milestones = tracker.update(1_000_000_000, 10.0, AssistCategory.FULL_ASSIST)
self.assertEqual(len(milestones), 1)
self.assertEqual(milestones[0].category, AssistCategory.FULL_ASSIST)
self.assertAlmostEqual(milestones[0].previous_distance_miles, 1.0)
self.assertAlmostEqual(milestones[0].distance_miles, 2.0)
def test_does_not_count_unassisted_time_or_timestamp_gaps(self):
tracker = AssistedDistanceMilestoneTracker()
tracker.update(0, 20.0, None)
tracker.update(500_000_000, 20.0, AssistCategory.MADS)
self.assertEqual(tracker.distance_meters(AssistCategory.MADS), 0.0)
tracker.update(2_000_000_000, 20.0, AssistCategory.MADS)
self.assertEqual(tracker.distance_meters(AssistCategory.MADS), 0.0)
def test_drive_reset_preserves_persistent_distance(self):
tracker = AssistedDistanceMilestoneTracker()
tracker.update(0, 10.0, AssistCategory.MADS)
tracker.update(500_000_000, 10.0, AssistCategory.MADS)
distance_before_reset = tracker.distance_meters(AssistCategory.MADS)
tracker.reset_sampling()
self.assertEqual(tracker.distance_meters(AssistCategory.MADS), distance_before_reset)
self.assertEqual(tracker.update(1_000_000_000, 10.0, AssistCategory.MADS), [])
if __name__ == "__main__":
unittest.main()
+39 -12
View File
@@ -5,12 +5,26 @@ from openpilot.common.test import OpenpilotTestCase
from openpilot.cereal import log, messaging
from openpilot.cereal.messaging import SubMaster, PubMaster
from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, Soundd, calculate_volume_for_device, check_selfdrive_timeout_alert
from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_tracker_prototype import MILESTONE_EVENT_PAYLOAD
AudibleAlert = log.SelfdriveState.AudibleAlert
class TestSoundd(OpenpilotTestCase):
@staticmethod
def milestone_submaster(event_id=42):
class SubMasterStub:
def __init__(self):
self.updated = {'assistedDrivingMilestoneState': True}
msg = messaging.new_message('assistedDrivingMilestoneState')
msg.assistedDrivingMilestoneState.enabled = True
msg.assistedDrivingMilestoneState.event.id = event_id
self.data = {'assistedDrivingMilestoneState': msg.assistedDrivingMilestoneState}
def __getitem__(self, service):
return self.data[service]
return SubMasterStub()
def test_comma_four_volume_is_50_percent_louder_than_comma_three_x(self):
for weighted_db in (20.0, 30.0, 40.0, 50.0):
with self.subTest(weighted_db=weighted_db):
@@ -18,21 +32,34 @@ class TestSoundd(OpenpilotTestCase):
comma_four_volume = calculate_volume_for_device(weighted_db, "mici")
assert comma_four_volume == min(1.0, comma_three_x_volume * 1.5)
def test_milestone_chime_uses_ui_milestone_event(self):
def test_milestone_chime_uses_typed_milestone_event_once(self):
soundd = Soundd()
class SubMasterStub:
def __init__(self):
self.updated = {'customReservedRawData0': True}
self.data = {'customReservedRawData0': MILESTONE_EVENT_PAYLOAD}
def __getitem__(self, service):
return self.data[service]
sm = SubMasterStub()
sm = self.milestone_submaster()
soundd.update_milestone_alert(sm)
assert soundd.current_alert == AudibleAlert.complete
soundd.current_alert = AudibleAlert.none
soundd.update_milestone_alert(sm)
assert soundd.current_alert == AudibleAlert.none
def test_safety_alert_consumes_milestone_without_replaying_it(self):
soundd = Soundd()
sm = self.milestone_submaster()
soundd.current_alert = AudibleAlert.warningImmediate
soundd.update_milestone_alert(sm)
soundd.current_alert = AudibleAlert.none
soundd.update_milestone_alert(sm)
assert soundd.current_alert == AudibleAlert.none
def test_quiet_mode_consumes_milestone_without_playing_it(self):
soundd = Soundd()
soundd.enabled = True
soundd.update_milestone_alert(self.milestone_submaster())
assert soundd.current_alert == AudibleAlert.none
def test_check_selfdrive_timeout_alert(self, mocker):
sm = SubMaster(['selfdriveState', 'selfdriveStateSP'])
@@ -0,0 +1,260 @@
"""Authoritative assisted-driving distance and milestone tracking."""
import math
from collections.abc import Mapping
from dataclasses import dataclass
from enum import StrEnum
from openpilot.common.params import Params
METERS_PER_MILE = 1609.344
METERS_PER_KILOMETER = 1000.0
MAX_SAMPLE_INTERVAL_SECONDS = 0.5
PERSIST_INTERVAL_NS = 10_000_000_000
STATE_VERSION = 1
STATE_PARAM = "AssistedDrivingMilestoneState"
LAST_DRIVE_SUMMARY_PARAM = "LastDriveAssistedDrivingSummary"
class AssistCategory(StrEnum):
MADS = "mads"
FULL_ASSIST = "fullAssist"
class MilestoneUnit(StrEnum):
IMPERIAL = "imperial"
METRIC = "metric"
@dataclass(frozen=True)
class MilestoneEvent:
event_id: int
category: AssistCategory
distance_meters: float
previous_distance_meters: float
unit: MilestoneUnit
@dataclass(frozen=True)
class MilestoneSnapshot:
distances_meters: dict[AssistCategory, float]
drive_start_distances_meters: dict[AssistCategory, float]
next_event_id: int
next_summary_id: int
unit: MilestoneUnit
active_drive_id: str
def assist_category(lat_active: bool, long_active: bool) -> AssistCategory | None:
if not lat_active:
return None
return AssistCategory.FULL_ASSIST if long_active else AssistCategory.MADS
def _meters_per_unit(unit: MilestoneUnit) -> float:
return METERS_PER_KILOMETER if unit == MilestoneUnit.METRIC else METERS_PER_MILE
def _next_ladder_value(value: float) -> float:
value = max(0.0, value)
magnitude = 10.0 ** math.floor(math.log10(max(1.0, value)))
for multiplier in (1.0, 2.0, 5.0):
candidate = multiplier * magnitude
if candidate > value + 1e-9:
return candidate
return 10.0 * magnitude
def _previous_ladder_value(value: float) -> float:
if value <= 1.0:
return 0.0
magnitude = 10.0 ** math.floor(math.log10(value))
normalized = value / magnitude
if normalized <= 1.0 + 1e-9:
return 5.0 * magnitude / 10.0
if normalized <= 2.0 + 1e-9:
return magnitude
return 2.0 * magnitude
def next_milestone_meters(distance_meters: float, unit: MilestoneUnit) -> float:
meters_per_unit = _meters_per_unit(unit)
return _next_ladder_value(distance_meters / meters_per_unit) * meters_per_unit
class MilestoneStore:
def __init__(self, params: Params | None = None):
self._params = params or Params()
def load(self) -> MilestoneSnapshot:
raw = self._params.get(STATE_PARAM, return_default=True)
raw = raw if isinstance(raw, dict) else {}
raw_distances = raw.get("distancesMeters", {})
raw_distances = raw_distances if isinstance(raw_distances, dict) else {}
try:
unit = MilestoneUnit(raw.get("unit", MilestoneUnit.IMPERIAL))
except ValueError:
unit = MilestoneUnit.IMPERIAL
def distance(category: AssistCategory) -> float:
try:
return max(0.0, float(raw_distances.get(category.value, 0.0)))
except (TypeError, ValueError):
return 0.0
distances = {category: distance(category) for category in AssistCategory}
raw_drive_start = raw.get("driveStartDistancesMeters", {})
raw_drive_start = raw_drive_start if isinstance(raw_drive_start, dict) else {}
def drive_start_distance(category: AssistCategory) -> float:
try:
return max(0.0, min(float(raw_drive_start.get(category.value, distances[category])), distances[category]))
except (TypeError, ValueError):
return distances[category]
try:
next_event_id = max(1, int(raw.get("nextEventId", 1)))
except (TypeError, ValueError):
next_event_id = 1
try:
next_summary_id = max(1, int(raw.get("nextSummaryId", 1)))
except (TypeError, ValueError):
next_summary_id = 1
return MilestoneSnapshot(
distances_meters=distances,
drive_start_distances_meters={category: drive_start_distance(category) for category in AssistCategory},
next_event_id=next_event_id,
next_summary_id=next_summary_id,
unit=unit,
active_drive_id=str(raw.get("activeDriveId", "")),
)
def save(self, snapshot: MilestoneSnapshot, block: bool = False) -> None:
if block:
self._params.flush()
self._params.put(STATE_PARAM, {
"version": STATE_VERSION,
"distancesMeters": {category.value: max(0.0, snapshot.distances_meters.get(category, 0.0)) for category in AssistCategory},
"driveStartDistancesMeters": {
category.value: max(0.0, snapshot.drive_start_distances_meters.get(category, 0.0)) for category in AssistCategory
},
"nextEventId": max(1, snapshot.next_event_id),
"nextSummaryId": max(1, snapshot.next_summary_id),
"unit": snapshot.unit.value,
"activeDriveId": snapshot.active_drive_id,
}, block=block)
def save_drive_summary(self, summary_id: int, distances_meters: Mapping[AssistCategory, float], unit: MilestoneUnit) -> None:
self._params.put(LAST_DRIVE_SUMMARY_PARAM, {
"version": STATE_VERSION,
"id": summary_id,
"distancesMeters": {category.value: max(0.0, distances_meters.get(category, 0.0)) for category in AssistCategory},
"unit": unit.value,
}, block=True)
class AssistedDrivingMilestones:
"""Tracks, persists, and emits milestones through one small interface."""
def __init__(self, store: MilestoneStore | None = None):
self._store = store or MilestoneStore()
snapshot = self._store.load()
self._distances_meters = snapshot.distances_meters
self._drive_start_distances_meters = snapshot.drive_start_distances_meters
self._next_event_id = snapshot.next_event_id
self._next_summary_id = snapshot.next_summary_id
self._unit = snapshot.unit
self._active_drive_id = snapshot.active_drive_id
self._next_milestone_meters = {
category: next_milestone_meters(distance, self._unit)
for category, distance in self._distances_meters.items()
}
self._last_timestamp_ns: int | None = None
self._last_persist_timestamp_ns: int | None = None
self._last_speed_mps = 0.0
self._last_category: AssistCategory | None = None
self._enabled = False
self._closed = False
def snapshot(self) -> MilestoneSnapshot:
return MilestoneSnapshot(
self._distances_meters.copy(),
self._drive_start_distances_meters.copy(),
self._next_event_id,
self._next_summary_id,
self._unit,
self._active_drive_id,
)
def set_drive_id(self, drive_id: str) -> None:
if not drive_id or drive_id == self._active_drive_id:
return
self._active_drive_id = drive_id
self._drive_start_distances_meters = self._distances_meters.copy()
self._persist()
def update(self, timestamp_ns: int, speed_mps: float, *, lat_active: bool, long_active: bool,
is_metric: bool, enabled: bool) -> MilestoneEvent | None:
self._enabled = enabled
unit = MilestoneUnit.METRIC if is_metric else MilestoneUnit.IMPERIAL
if unit != self._unit:
self._unit = unit
self._next_milestone_meters = {
category: next_milestone_meters(distance, unit)
for category, distance in self._distances_meters.items()
}
speed_mps = max(0.0, speed_mps)
category = assist_category(lat_active, long_active) if enabled else None
event = None
if self._last_timestamp_ns is not None and timestamp_ns != self._last_timestamp_ns:
dt = (timestamp_ns - self._last_timestamp_ns) / 1e9
if 0 < dt <= MAX_SAMPLE_INTERVAL_SECONDS and self._last_category is not None:
active_category = self._last_category
self._distances_meters[active_category] += (self._last_speed_mps + speed_mps) / 2.0 * dt
threshold_meters = self._next_milestone_meters[active_category]
if self._distances_meters[active_category] >= threshold_meters:
meters_per_unit = _meters_per_unit(self._unit)
threshold_units = threshold_meters / meters_per_unit
event = MilestoneEvent(
event_id=self._next_event_id,
category=active_category,
distance_meters=threshold_meters,
previous_distance_meters=_previous_ladder_value(threshold_units) * meters_per_unit,
unit=self._unit,
)
self._next_event_id += 1
self._next_milestone_meters[active_category] = next_milestone_meters(threshold_meters, self._unit)
self._persist(timestamp_ns=timestamp_ns)
self._last_timestamp_ns = timestamp_ns
self._last_speed_mps = speed_mps
self._last_category = category
if self._last_persist_timestamp_ns is None:
self._last_persist_timestamp_ns = timestamp_ns
elif timestamp_ns - self._last_persist_timestamp_ns >= PERSIST_INTERVAL_NS:
self._persist(timestamp_ns=timestamp_ns)
return event
def close(self) -> None:
if self._closed:
return
self._closed = True
drive_distances = {
category: self._distances_meters[category] - self._drive_start_distances_meters[category]
for category in AssistCategory
}
summary_id = self._next_summary_id
self._next_summary_id += 1
self._persist(block=True)
if self._enabled:
self._store.save_drive_summary(summary_id, drive_distances, self._unit)
def _persist(self, block: bool = False, timestamp_ns: int | None = None) -> None:
self._store.save(self.snapshot(), block=block)
self._last_persist_timestamp_ns = self._last_timestamp_ns if timestamp_ns is None else timestamp_ns
@@ -0,0 +1,123 @@
import unittest
from openpilot.sunnypilot.selfdrive.selfdrived.assisted_driving_milestones import (
METERS_PER_MILE,
AssistCategory,
AssistedDrivingMilestones,
MilestoneStore,
MilestoneUnit,
)
class ParamsStub:
def __init__(self, state=None):
self.values = {"AssistedDrivingMilestoneState": state or {}}
self.writes = []
def get(self, key, return_default=False):
return self.values.get(key, {} if return_default else None)
def put(self, key, value, block=False):
self.values[key] = value
self.writes.append((key, value, block))
def flush(self):
pass
class TestAssistedDrivingMilestones(unittest.TestCase):
def test_emits_and_asynchronously_persists_first_imperial_milestone(self):
params = ParamsStub({
"version": 1,
"distancesMeters": {"mads": METERS_PER_MILE - 5.0, "fullAssist": 0.0},
"nextEventId": 7,
"unit": "imperial",
})
milestones = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
self.assertIsNone(milestones.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True))
event = milestones.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
self.assertIsNotNone(event)
assert event is not None
self.assertEqual(event.event_id, 7)
self.assertEqual(event.category, AssistCategory.MADS)
self.assertEqual(event.unit, MilestoneUnit.IMPERIAL)
self.assertAlmostEqual(event.distance_meters, METERS_PER_MILE)
self.assertFalse(params.writes[-1][2])
def test_switching_units_schedules_only_a_future_milestone(self):
params = ParamsStub({
"version": 1,
"distancesMeters": {"mads": 9_500.0, "fullAssist": 0.0},
"nextEventId": 2,
"unit": "imperial",
})
milestones = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
self.assertIsNone(milestones.update(0, 1_000.0, lat_active=True, long_active=False, is_metric=True, enabled=True))
event = milestones.update(500_000_000, 1_000.0, lat_active=True, long_active=False, is_metric=True, enabled=True)
self.assertIsNotNone(event)
assert event is not None
self.assertEqual(event.unit, MilestoneUnit.METRIC)
self.assertAlmostEqual(event.distance_meters, 10_000.0)
def test_ignores_disabled_reverse_and_timestamp_gaps(self):
params = ParamsStub()
milestones = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
milestones.update(0, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
milestones.update(500_000_000, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
milestones.update(1_000_000_000, -20.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
milestones.update(2_000_000_000, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
self.assertEqual(milestones.snapshot().distances_meters[AssistCategory.MADS], 0.0)
def test_close_persists_totals_and_last_drive_summary(self):
params = ParamsStub()
milestones = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
milestones.update(0, 10.0, lat_active=True, long_active=True, is_metric=False, enabled=True)
milestones.update(500_000_000, 10.0, lat_active=True, long_active=True, is_metric=False, enabled=True)
milestones.close()
summary = params.values["LastDriveAssistedDrivingSummary"]
self.assertAlmostEqual(summary["distancesMeters"]["fullAssist"], 5.0)
self.assertTrue(params.writes[-1][2])
write_count = len(params.writes)
milestones.close()
self.assertEqual(len(params.writes), write_count)
def test_process_restart_preserves_the_current_drive_start(self):
params = ParamsStub()
first_process = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
first_process.set_drive_id("route-1")
first_process.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
first_process.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
first_process.close()
second_process = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
second_process.set_drive_id("route-1")
second_process.update(1_000_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
second_process.update(1_500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
second_process.close()
summary = params.values["LastDriveAssistedDrivingSummary"]
self.assertAlmostEqual(summary["distancesMeters"]["mads"], 10.0)
def test_disabled_feature_does_not_publish_drive_summary(self):
params = ParamsStub()
milestones = AssistedDrivingMilestones(MilestoneStore(params)) # type: ignore[arg-type]
milestones.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
milestones.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
milestones.update(1_000_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
milestones.close()
self.assertNotIn("LastDriveAssistedDrivingSummary", params.values)
if __name__ == "__main__":
unittest.main()
@@ -1383,6 +1383,12 @@
"title": "Steering Arc",
"description": "Display steering arc on the driving screen when lateral control is enabled."
},
{
"key": "AssistedDrivingMilestonesEnabled",
"widget": "toggle",
"title": "Assisted Driving Milestones",
"description": "Celebrate cumulative MADS and full-assist distance milestones while driving."
},
{
"key": "ShowTurnSignals",
"widget": "toggle",
@@ -20,6 +20,10 @@ sections:
widget: toggle
title: Steering Arc
description: Display steering arc on the driving screen when lateral control is enabled.
- key: AssistedDrivingMilestonesEnabled
widget: toggle
title: Assisted Driving Milestones
description: Celebrate cumulative MADS and full-assist distance milestones while driving.
- key: ShowTurnSignals
widget: toggle
title: Display Turn Signals
+21 -10
View File
@@ -11,7 +11,6 @@ from openpilot.sunnypilot.selfdrive.car.sync_sunnylink_params import CAR_LIST_JS
ONROAD_BRIGHTNESS_MIGRATION_VERSION: str = "1.0"
ONROAD_BRIGHTNESS_TIMER_MIGRATION_VERSION: str = "1.0"
ASSISTED_DISTANCE_MILESTONE_RESET_VERSION: str = "1"
# index → seconds mapping for OnroadScreenOffTimer (SSoT)
ONROAD_BRIGHTNESS_TIMER_VALUES = {0: 3, 1: 5, 2: 7, 3: 10, 4: 15, 5: 30, **{i: (i - 5) * 60 for i in range(6, 16)}}
@@ -100,17 +99,30 @@ def _migrate_model_bundle_slots(_params):
cloudlog.exception(f"Error migrating model bundle slots: {e}")
def _reset_assisted_distance_milestones(_params):
def _migrate_assisted_driving_milestones(_params):
try:
if _params.get("AssistedDistanceMilestoneResetVersion", return_default=True) == ASSISTED_DISTANCE_MILESTONE_RESET_VERSION:
state = _params.get("AssistedDrivingMilestoneState", return_default=True)
if isinstance(state, dict) and state.get("version") == 1:
return
_params.put("MadsDrivenDistanceMeters", 0.0, block=True)
_params.put("FullAssistDrivenDistanceMeters", 0.0, block=True)
_params.put("AssistedDistanceMilestoneResetVersion", ASSISTED_DISTANCE_MILESTONE_RESET_VERSION, block=True)
cloudlog.info("params_migration: reset assisted-distance milestone counters")
_params.put("AssistedDrivingMilestoneState", {
"version": 1,
"distancesMeters": {
"mads": max(0.0, _params.get("MadsDrivenDistanceMeters", return_default=True) or 0.0),
"fullAssist": max(0.0, _params.get("FullAssistDrivenDistanceMeters", return_default=True) or 0.0),
},
"driveStartDistancesMeters": {
"mads": max(0.0, _params.get("MadsDrivenDistanceMeters", return_default=True) or 0.0),
"fullAssist": max(0.0, _params.get("FullAssistDrivenDistanceMeters", return_default=True) or 0.0),
},
"nextEventId": 1,
"nextSummaryId": 1,
"unit": "metric" if _params.get_bool("IsMetric") else "imperial",
"activeDriveId": "",
}, block=True)
cloudlog.info("params_migration: migrated assisted-driving milestone state")
except Exception as e:
cloudlog.exception(f"Error resetting assisted-distance milestone counters: {e}")
cloudlog.exception(f"Error migrating assisted-driving milestone state: {e}")
def run_migration(_params):
@@ -153,5 +165,4 @@ def run_migration(_params):
# seed the usbgpu model slot from the pre-split single slot
_migrate_model_bundle_slots(_params)
# reset prototype milestone counters once for the next test cycle
_reset_assisted_distance_milestones(_params)
_migrate_assisted_driving_milestones(_params)
@@ -10,8 +10,8 @@ from openpilot.common.test import OpenpilotTestCase
from openpilot.sunnypilot.system.params_migration import _migrate_model_bundle_slots, run_migration
class TestAssistedDistanceMilestoneReset(OpenpilotTestCase):
def test_resets_existing_distances_once(self):
class TestAssistedDrivingMilestoneMigration(OpenpilotTestCase):
def test_preserves_prototype_distances_once(self):
class ParamsStub:
def __init__(self):
self.values = {
@@ -19,7 +19,8 @@ class TestAssistedDistanceMilestoneReset(OpenpilotTestCase):
"FullAssistDrivenDistanceMeters": 456.0,
"OnroadScreenOffBrightness": 0,
"OnroadScreenOffTimer": 15,
"AssistedDistanceMilestoneResetVersion": "0",
"AssistedDrivingMilestoneState": {},
"IsMetric": False,
}
def get(self, key, return_default=False):
@@ -28,19 +29,22 @@ class TestAssistedDistanceMilestoneReset(OpenpilotTestCase):
def put(self, key, value, block=False):
self.values[key] = value
def get_bool(self, key):
return bool(self.values.get(key, False))
params = ParamsStub()
run_migration(params)
assert params.get("MadsDrivenDistanceMeters") == 0.0
assert params.get("FullAssistDrivenDistanceMeters") == 0.0
state = params.get("AssistedDrivingMilestoneState")
assert state["distancesMeters"] == {"mads": 123.0, "fullAssist": 456.0}
params.put("MadsDrivenDistanceMeters", 12.0, block=True)
params.put("FullAssistDrivenDistanceMeters", 34.0, block=True)
run_migration(params)
assert params.get("MadsDrivenDistanceMeters") == 12.0
assert params.get("FullAssistDrivenDistanceMeters") == 34.0
state = params.get("AssistedDrivingMilestoneState")
assert state["distancesMeters"] == {"mads": 123.0, "fullAssist": 456.0}
class TestModelBundleSlotMigration(OpenpilotTestCase):