mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-12 04:33:43 +08:00
Productionize assisted driving milestones
Assisted-by: OpenAI Codex
This commit is contained in:
@@ -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 {
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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.),
|
||||
|
||||
@@ -97,6 +97,10 @@ Params::Params(const std::string &path) {
|
||||
}
|
||||
|
||||
Params::~Params() {
|
||||
flushNonBlockingWrites();
|
||||
}
|
||||
|
||||
void Params::flushNonBlockingWrites() {
|
||||
if (future.valid()) {
|
||||
future.wait();
|
||||
}
|
||||
|
||||
@@ -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");
|
||||
}
|
||||
|
||||
@@ -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))
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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"}},
|
||||
|
||||
@@ -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()
|
||||
|
||||
|
||||
@@ -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()
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
+55
-68
@@ -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
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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
|
||||
|
||||
@@ -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):
|
||||
|
||||
Reference in New Issue
Block a user