From 27a220677a2aecaf419d85a007c9bc4fc1ab01a5 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Sat, 29 Aug 2026 07:45:40 -0400 Subject: [PATCH] Productionize assisted driving milestones Assisted-by: OpenAI Codex --- openpilot/cereal/custom.capnp | 25 +- openpilot/cereal/log.capnp | 2 +- openpilot/cereal/services.py | 1 + openpilot/common/params.cc | 4 + openpilot/common/params.h | 1 + openpilot/common/params.py | 5 + openpilot/common/params_c.cc | 6 + openpilot/common/params_keys.h | 4 +- openpilot/common/tests/test_params.py | 7 + openpilot/selfdrive/selfdrived/selfdrived.py | 67 ++++- openpilot/selfdrive/ui/mici/layouts/main.py | 6 +- .../ui/mici/layouts/settings/toggles.py | 3 + .../ui/mici/onroad/augmented_road_view.py | 20 +- openpilot/selfdrive/ui/soundd.py | 14 +- .../ui/sunnypilot/mici/layouts/home.py | 62 ++++- ..._prototype.py => milestone_celebration.py} | 123 ++++----- .../onroad/milestone_tracker_prototype.py | 140 ---------- openpilot/selfdrive/ui/sunnypilot/ui_state.py | 3 +- .../ui/tests/generate_milestone_sound.py | 2 +- .../selfdrive/ui/tests/milestone_preview.py | 38 +++ .../ui/tests/run_milestone_celebration.sh | 27 ++ .../run_milestone_celebration_prototype.sh | 24 -- .../tests/test_milestone_tracker_prototype.py | 106 ------- openpilot/selfdrive/ui/tests/test_soundd.py | 51 +++- .../selfdrived/assisted_driving_milestones.py | 260 ++++++++++++++++++ .../tests/test_assisted_driving_milestones.py | 123 +++++++++ .../sunnypilot/sunnylink/settings_ui.json | 6 + .../settings_ui_src/pages/visuals.yaml | 4 + .../sunnypilot/system/params_migration.py | 31 ++- .../system/tests/test_params_migration.py | 18 +- 30 files changed, 793 insertions(+), 390 deletions(-) rename openpilot/selfdrive/ui/sunnypilot/onroad/{milestone_celebration_prototype.py => milestone_celebration.py} (68%) delete mode 100644 openpilot/selfdrive/ui/sunnypilot/onroad/milestone_tracker_prototype.py create mode 100755 openpilot/selfdrive/ui/tests/milestone_preview.py create mode 100755 openpilot/selfdrive/ui/tests/run_milestone_celebration.sh delete mode 100644 openpilot/selfdrive/ui/tests/run_milestone_celebration_prototype.sh delete mode 100644 openpilot/selfdrive/ui/tests/test_milestone_tracker_prototype.py create mode 100644 openpilot/sunnypilot/selfdrive/selfdrived/assisted_driving_milestones.py create mode 100644 openpilot/sunnypilot/selfdrive/selfdrived/tests/test_assisted_driving_milestones.py diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index 26165962f5..b670651edc 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -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 { diff --git a/openpilot/cereal/log.capnp b/openpilot/cereal/log.capnp index b87499806f..36bd45a0be 100644 --- a/openpilot/cereal/log.capnp +++ b/openpilot/cereal/log.capnp @@ -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; diff --git a/openpilot/cereal/services.py b/openpilot/cereal/services.py index ed718506f3..a6a2b93a8d 100755 --- a/openpilot/cereal/services.py +++ b/openpilot/cereal/services.py @@ -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.), diff --git a/openpilot/common/params.cc b/openpilot/common/params.cc index bed6827e5a..1a42a96871 100644 --- a/openpilot/common/params.cc +++ b/openpilot/common/params.cc @@ -97,6 +97,10 @@ Params::Params(const std::string &path) { } Params::~Params() { + flushNonBlockingWrites(); +} + +void Params::flushNonBlockingWrites() { if (future.valid()) { future.wait(); } diff --git a/openpilot/common/params.h b/openpilot/common/params.h index de4f9b435f..eeda7a9de1 100644 --- a/openpilot/common/params.h +++ b/openpilot/common/params.h @@ -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"); } diff --git a/openpilot/common/params.py b/openpilot/common/params.py index e14937207a..98c9032089 100644 --- a/openpilot/common/params.py +++ b/openpilot/common/params.py @@ -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)) diff --git a/openpilot/common/params_c.cc b/openpilot/common/params_c.cc index a52adb09da..5cf8815dcc 100644 --- a/openpilot/common/params_c.cc +++ b/openpilot/common/params_c.cc @@ -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); diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 0d6078b30f..e76d2890d3 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -136,7 +136,8 @@ inline static std::unordered_map 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 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"}}, diff --git a/openpilot/common/tests/test_params.py b/openpilot/common/tests/test_params.py index a81d346b06..c388060574 100644 --- a/openpilot/common/tests/test_params.py +++ b/openpilot/common/tests/test_params.py @@ -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() diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index d1bc57c856..b612fd8669 100755 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -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() diff --git a/openpilot/selfdrive/ui/mici/layouts/main.py b/openpilot/selfdrive/ui/mici/layouts/main.py index cb116469d7..3c07ea02c0 100644 --- a/openpilot/selfdrive/ui/mici/layouts/main.py +++ b/openpilot/selfdrive/ui/mici/layouts/main.py @@ -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: diff --git a/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py b/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py index 2dba124df5..4d901057fc 100644 --- a/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py +++ b/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py @@ -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), diff --git a/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py b/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py index 607978a73e..ba7e9dd886 100644 --- a/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -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 diff --git a/openpilot/selfdrive/ui/soundd.py b/openpilot/selfdrive/ui/soundd.py index d173fb97ff..8404e9af98 100644 --- a/openpilot/selfdrive/ui/soundd.py +++ b/openpilot/selfdrive/ui/soundd.py @@ -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) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/home.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/home.py index b261373947..52dec62638 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/home.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/home.py @@ -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 diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/milestone_celebration_prototype.py b/openpilot/selfdrive/ui/sunnypilot/onroad/milestone_celebration.py similarity index 68% rename from openpilot/selfdrive/ui/sunnypilot/onroad/milestone_celebration_prototype.py rename to openpilot/selfdrive/ui/sunnypilot/onroad/milestone_celebration.py index 08cf3a8fd1..de5de4b6a6 100644 --- a/openpilot/selfdrive/ui/sunnypilot/onroad/milestone_celebration_prototype.py +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/milestone_celebration.py @@ -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) diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/milestone_tracker_prototype.py b/openpilot/selfdrive/ui/sunnypilot/onroad/milestone_tracker_prototype.py deleted file mode 100644 index 62d2ce0bb5..0000000000 --- a/openpilot/selfdrive/ui/sunnypilot/onroad/milestone_tracker_prototype.py +++ /dev/null @@ -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 diff --git a/openpilot/selfdrive/ui/sunnypilot/ui_state.py b/openpilot/selfdrive/ui/sunnypilot/ui_state.py index 9bed533d3f..2d20e1e8d9 100644 --- a/openpilot/selfdrive/ui/sunnypilot/ui_state.py +++ b/openpilot/selfdrive/ui/sunnypilot/ui_state.py @@ -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() diff --git a/openpilot/selfdrive/ui/tests/generate_milestone_sound.py b/openpilot/selfdrive/ui/tests/generate_milestone_sound.py index 5d7fc7b86c..103032e9b0 100644 --- a/openpilot/selfdrive/ui/tests/generate_milestone_sound.py +++ b/openpilot/selfdrive/ui/tests/generate_milestone_sound.py @@ -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 diff --git a/openpilot/selfdrive/ui/tests/milestone_preview.py b/openpilot/selfdrive/ui/tests/milestone_preview.py new file mode 100755 index 0000000000..b55b3cb76c --- /dev/null +++ b/openpilot/selfdrive/ui/tests/milestone_preview.py @@ -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() diff --git a/openpilot/selfdrive/ui/tests/run_milestone_celebration.sh b/openpilot/selfdrive/ui/tests/run_milestone_celebration.sh new file mode 100755 index 0000000000..b2493cdebe --- /dev/null +++ b/openpilot/selfdrive/ui/tests/run_milestone_celebration.sh @@ -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" diff --git a/openpilot/selfdrive/ui/tests/run_milestone_celebration_prototype.sh b/openpilot/selfdrive/ui/tests/run_milestone_celebration_prototype.sh deleted file mode 100644 index 54cb7dedf7..0000000000 --- a/openpilot/selfdrive/ui/tests/run_milestone_celebration_prototype.sh +++ /dev/null @@ -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" diff --git a/openpilot/selfdrive/ui/tests/test_milestone_tracker_prototype.py b/openpilot/selfdrive/ui/tests/test_milestone_tracker_prototype.py deleted file mode 100644 index b368951b9a..0000000000 --- a/openpilot/selfdrive/ui/tests/test_milestone_tracker_prototype.py +++ /dev/null @@ -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() diff --git a/openpilot/selfdrive/ui/tests/test_soundd.py b/openpilot/selfdrive/ui/tests/test_soundd.py index aa15010c52..34783c0d1e 100644 --- a/openpilot/selfdrive/ui/tests/test_soundd.py +++ b/openpilot/selfdrive/ui/tests/test_soundd.py @@ -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']) diff --git a/openpilot/sunnypilot/selfdrive/selfdrived/assisted_driving_milestones.py b/openpilot/sunnypilot/selfdrive/selfdrived/assisted_driving_milestones.py new file mode 100644 index 0000000000..6578512ba2 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/selfdrived/assisted_driving_milestones.py @@ -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 diff --git a/openpilot/sunnypilot/selfdrive/selfdrived/tests/test_assisted_driving_milestones.py b/openpilot/sunnypilot/selfdrive/selfdrived/tests/test_assisted_driving_milestones.py new file mode 100644 index 0000000000..8b37d81def --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/selfdrived/tests/test_assisted_driving_milestones.py @@ -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() diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 63bf342fcb..01f8c0de66 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -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", diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml index 3170991157..fd2038f2a2 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml @@ -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 diff --git a/openpilot/sunnypilot/system/params_migration.py b/openpilot/sunnypilot/system/params_migration.py index 2143e13289..297c7b6ab5 100644 --- a/openpilot/sunnypilot/system/params_migration.py +++ b/openpilot/sunnypilot/system/params_migration.py @@ -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) diff --git a/openpilot/sunnypilot/system/tests/test_params_migration.py b/openpilot/sunnypilot/system/tests/test_params_migration.py index 380a075370..6a8cb78899 100644 --- a/openpilot/sunnypilot/system/tests/test_params_migration.py +++ b/openpilot/sunnypilot/system/tests/test_params_migration.py @@ -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):