Compare commits

..

1 Commits

Author SHA1 Message Date
nayan d68a6cd87d zoom zoom 2026-08-28 21:11:29 -04:00
38 changed files with 67 additions and 1505 deletions
-1
View File
@@ -9,7 +9,6 @@
*.ttf filter=lfs diff=lfs merge=lfs -text
*.otf filter=lfs diff=lfs merge=lfs -text
*.wav filter=lfs diff=lfs merge=lfs -text
openpilot/selfdrive/assets/sounds/milestone.wav -filter -diff -merge -text
openpilot/selfdrive/car/tests/test_models_segs.txt filter=lfs diff=lfs merge=lfs -text
openpilot/common/hardware/comma/updater filter=lfs diff=lfs merge=lfs -text
+1 -33
View File
@@ -382,7 +382,6 @@ struct CarControlSP @0xa5cd762cd951a455 {
leadOne @2 :LeadData;
leadTwo @3 :LeadData;
intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
fordLateralPath @5 :FordLateralPath;
struct Param {
key @0 :Text;
@@ -403,14 +402,6 @@ struct CarControlSP @0xa5cd762cd951a455 {
}
}
struct FordLateralPath {
pathOffset @0 :Float32; # c0 [m]
pathAngle @1 :Float32; # c1 [rad]
curvature @2 :Float32; # c2 [1/m]
curvatureRate @3 :Float32; # c3 [1/m^2]
valid @4 :Bool;
}
struct BackupManagerSP @0xf98d843bfd7004a3 {
backupStatus @0 :Status;
restoreStatus @1 :Status;
@@ -478,30 +469,7 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
}
}
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 CustomReserved10 @0xcb9fd56c7057593a {
}
struct CustomReserved11 @0xc2243c65e0340384 {
+1 -1
View File
@@ -2640,7 +2640,7 @@ struct Event {
carStateSP @114 :Custom.CarStateSP;
liveMapDataSP @115 :Custom.LiveMapDataSP;
modelDataV2SP @116 :Custom.ModelDataV2SP;
assistedDrivingMilestoneState @136 :Custom.AssistedDrivingMilestoneState;
customReserved10 @136 :Custom.CustomReserved10;
customReserved11 @137 :Custom.CustomReserved11;
customReserved12 @138 :Custom.CustomReserved12;
customReserved13 @139 :Custom.CustomReserved13;
-1
View File
@@ -90,7 +90,6 @@ _services: dict[str, tuple] = {
"carParamsSP": (True, 0.02, 1),
"carControlSP": (True, 100., 10),
"carStateSP": (True, 100., 10),
"assistedDrivingMilestoneState": (True, 10., 1),
"liveMapDataSP": (True, 1., 1),
"modelDataV2SP": (True, 20., None, QueueSize.BIG),
"liveLocationKalman": (True, 20.),
-4
View File
@@ -97,10 +97,6 @@ Params::Params(const std::string &path) {
}
Params::~Params() {
flushNonBlockingWrites();
}
void Params::flushNonBlockingWrites() {
if (future.valid()) {
future.wait();
}
-1
View File
@@ -75,7 +75,6 @@ public:
return put(key.c_str(), val ? "1" : "0", 1);
}
void putNonBlocking(const std::string &key, const std::string &val);
void flushNonBlockingWrites();
inline void putBoolNonBlocking(const std::string &key, bool val) {
putNonBlocking(key, val ? "1" : "0");
}
-5
View File
@@ -73,7 +73,6 @@ 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)
@@ -179,10 +178,6 @@ class Params:
def put_bool(self, key, val, block=False):
params_put_bool(self.p, self.check_key(key), val, block)
def flush(self):
"""Wait for all prior nonblocking writes from this Params instance."""
params_flush(self.p)
def remove(self, key):
params_remove(self.p, self.check_key(key))
-6
View File
@@ -133,12 +133,6 @@ 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);
-5
View File
@@ -136,8 +136,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
// --- sunnypilot params --- //
{"ApiCache_DriveStats", {PERSISTENT, JSON}},
{"AssistedDrivingMilestonesEnabled", {PERSISTENT | BACKUP, BOOL, "1"}},
{"AssistedDrivingMilestoneState", {PERSISTENT, JSON, "{}"}},
{"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
{"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
{"BlinkerLateralReengageDelay", {PERSISTENT | BACKUP, INT, "0"}}, // seconds
@@ -158,7 +156,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
{"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
{"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
{"FullAssistDrivenDistanceMeters", {PERSISTENT, FLOAT, "0.0"}},
{"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
{"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
{"HasAcceptedTermsSP", {PERSISTENT, STRING, "0"}},
@@ -168,9 +165,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"}},
{"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}},
{"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}},
-7
View File
@@ -106,13 +106,6 @@ 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()
Binary file not shown.
-1
View File
@@ -63,6 +63,5 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct
struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement(
**remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {}))
)
struct_dataclass.fordLateralPath = structs.FordLateralPath(**remove_deprecated(struct_dict.get('fordLateralPath', {})))
return struct_dataclass
@@ -13,7 +13,6 @@ from openpilot.common.swaglog import cloudlog
from opendbc.car.car_helpers import interfaces
from opendbc.car.vehicle_model import VehicleModel
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
@@ -53,8 +52,6 @@ class Controls(ControlsExt):
self.steer_limited_by_safety = False
self.curvature = 0.0
self.desired_curvature = 0.0
self.ford_path_controller = FordPathController()
self.ford_path = FordPath()
self.pose_calibrator = PoseCalibrator()
self.calibrated_pose: Pose | None = None
@@ -158,11 +155,6 @@ class Controls(ControlsExt):
actuators.curvature = float(lateral_output)
else:
actuators.steeringAngleDeg = float(lateral_output)
if self.CP.brand == "ford":
self.ford_path = self.ford_path_controller.update(model_v2 if self.sm.valid['modelV2'] else None,
self.desired_curvature, current_curvature=self.curvature,
v_ego=CS.vEgo, active=CC.latActive)
actuators.curvature = float(self.ford_path.curvature)
# Ensure no NaNs/Infs
for p in ACTUATOR_FIELDS:
attr = getattr(actuators, p)
@@ -1,134 +0,0 @@
from dataclasses import dataclass
import math
import numpy as np
DBC_OFFSET = (-5.12, 5.11)
DBC_ANGLE = (-0.5, 0.5235)
DBC_CURVATURE = (-0.02, 0.02)
DBC_CURVATURE_RATE = (-0.001024, 0.001023)
_PATH_MIN_LOOKAHEAD = 7.0
_POSE_BLEND_CURVATURE = (0.006, 0.012)
_TRACKING_ERROR_DEADZONE = 0.0005
_TRACKING_ERROR_LIMIT = 0.012
_PATH_OFFSET_RATE = 4.0
_PATH_ANGLE_RATE = 1.0
@dataclass(frozen=True)
class FordPath:
valid: bool = False
path_offset: float = 0.0
path_angle: float = 0.0
curvature: float = 0.0
curvature_rate: float = 0.0
def _finite(value: float) -> float:
return float(value) if math.isfinite(value) else 0.0
def _sample(distance: float, distances: list[float], values: list[float]) -> float:
return float(np.interp(distance, distances, values))
def _blend_share(demand: float) -> float:
lower, upper = _POSE_BLEND_CURVATURE
return float(np.clip((demand - lower) / (upper - lower), 0.0, 1.0))
def _model_path(model) -> tuple[list[float], list[float], list[float]] | None:
try:
x = [float(value) for value in model.position.x]
y = [float(value) for value in model.position.y]
heading = [float(value) for value in model.orientation.z]
except (AttributeError, TypeError, ValueError):
return None
if len(x) < 2 or len(x) != len(y) or len(x) != len(heading):
return None
if not all(math.isfinite(value) for values in (x, y, heading) for value in values):
return None
distance = [0.0]
for i in range(1, len(x)):
distance.append(distance[-1] + math.hypot(x[i] - x[i - 1], y[i] - y[i - 1]))
if distance[-1] <= 0.0:
return None
unwrapped_heading = [heading[0]]
for value in heading[1:]:
delta = (value - unwrapped_heading[-1] + math.pi) % (2.0 * math.pi) - math.pi
unwrapped_heading.append(unwrapped_heading[-1] + delta)
return distance, y, unwrapped_heading
def _encode_path(path: tuple[list[float], list[float], list[float]], desired_curvature: float,
current_curvature: float, v_ego: float) -> FordPath:
distance, offset, heading = path
offset_horizon = min(_PATH_MIN_LOOKAHEAD, distance[-1])
angle_horizon = min(max(v_ego, _PATH_MIN_LOOKAHEAD), distance[-1])
model_offset = _sample(offset_horizon, distance, offset)
model_angle = _sample(angle_horizon, distance, heading)
offset_curvature = 2.0 * model_offset / offset_horizon ** 2
angle_curvature = model_angle / angle_horizon
pose_share = _blend_share(max(abs(offset_curvature), abs(angle_curvature), abs(desired_curvature)))
tracking_error = desired_curvature - current_curvature
if tracking_error * desired_curvature > 0.0:
tracking_error = math.copysign(max(abs(tracking_error) - _TRACKING_ERROR_DEADZONE, 0.0), tracking_error)
tracking_error = float(np.clip(tracking_error, -_TRACKING_ERROR_LIMIT, _TRACKING_ERROR_LIMIT))
else:
tracking_error = 0.0
# C2 owns normal path following. As model pose demand grows, transfer the
# same path continuously to the faster C0/C1 fields. Measured shortfall is
# expressed in those same pose units and cannot initiate the transfer.
path_offset = pose_share * (model_offset + 0.5 * tracking_error * offset_horizon ** 2)
path_angle = pose_share * (model_angle + tracking_error * angle_horizon)
curvature = desired_curvature * (1.0 - pose_share)
return FordPath(
valid=True,
path_offset=float(np.clip(path_offset, *DBC_OFFSET)),
path_angle=float(np.clip(path_angle, *DBC_ANGLE)),
curvature=float(np.clip(curvature, *DBC_CURVATURE)),
curvature_rate=0.0,
)
class FordPathController:
"""Blend normal C2 following into the model's forward C0/C1 pose."""
def __init__(self, dt: float = 0.01):
self.dt = dt
self._last_path = FordPath(valid=True)
def _limit(self, target: FordPath) -> FordPath:
offset_delta = target.path_offset - self._last_path.path_offset
angle_delta = target.path_angle - self._last_path.path_angle
scale = min(
1.0,
_PATH_OFFSET_RATE * self.dt / abs(offset_delta) if offset_delta else 1.0,
_PATH_ANGLE_RATE * self.dt / abs(angle_delta) if angle_delta else 1.0,
)
self._last_path = FordPath(
True,
self._last_path.path_offset + scale * offset_delta,
self._last_path.path_angle + scale * angle_delta,
self._last_path.curvature + scale * (target.curvature - self._last_path.curvature),
0.0,
)
return self._last_path
def update(self, model, desired_curvature: float, *, current_curvature: float = 0.0,
v_ego: float = 0.0, active: bool = True) -> FordPath:
if not active:
self._last_path = FordPath(valid=True)
return FordPath()
path = _model_path(model) if model is not None else None
if path is None:
return self._limit(FordPath(valid=True))
return self._limit(_encode_path(path, _finite(desired_curvature), _finite(current_curvature),
max(_finite(v_ego), 0.0)))
@@ -1,233 +0,0 @@
import math
from types import SimpleNamespace
import numpy as np
from openpilot.cereal import custom
from openpilot.selfdrive.car.helpers import convert_carControlSP
from openpilot.selfdrive.controls.lib.ford_path import DBC_ANGLE, DBC_CURVATURE, DBC_OFFSET, FordPathController
def _path(curvature: float, speed: float = 8.0):
t = np.linspace(0.0, 3.0, 61)
distance = speed * t
heading = curvature * distance
x = np.zeros_like(distance)
y = np.zeros_like(distance)
for i in range(1, len(distance)):
ds = distance[i] - distance[i - 1]
average_heading = 0.5 * (heading[i] + heading[i - 1])
x[i] = x[i - 1] + ds * math.cos(average_heading)
y[i] = y[i - 1] + ds * math.sin(average_heading)
return SimpleNamespace(
position=SimpleNamespace(t=t.tolist(), x=x.tolist(), y=y.tolist()),
orientation=SimpleNamespace(z=heading.tolist()),
)
def _changing_path(start_curvature: float, end_curvature: float, speed: float = 8.0):
t = np.linspace(0.0, 3.0, 61)
distance = speed * t
curvature = np.interp(distance, [distance[0], min(distance[-1], 7.0)], [start_curvature, end_curvature])
heading = np.zeros_like(distance)
x = np.zeros_like(distance)
y = np.zeros_like(distance)
for i in range(1, len(distance)):
ds = distance[i] - distance[i - 1]
heading[i] = heading[i - 1] + 0.5 * (curvature[i] + curvature[i - 1]) * ds
average_heading = 0.5 * (heading[i] + heading[i - 1])
x[i] = x[i - 1] + ds * math.cos(average_heading)
y[i] = y[i - 1] + ds * math.sin(average_heading)
return SimpleNamespace(
position=SimpleNamespace(t=t.tolist(), x=x.tolist(), y=y.tolist()),
orientation=SimpleNamespace(z=heading.tolist()),
)
def _command(model, desired_curvature: float, *, current_curvature: float = 0.0, v_ego: float = 8.0):
return FordPathController(dt=1.0).update(model, desired_curvature, current_curvature=current_curvature, v_ego=v_ego)
def _equivalent_curvature(command) -> float:
return 2.0 * command.path_offset / 7.0 ** 2 + 2.0 * command.path_angle / 7.0 + command.curvature
def test_gentle_path_uses_only_c2():
command = _command(_path(0.004, speed=20.0), 0.004, v_ego=20.0)
assert command.valid
assert command.path_offset == 0.0
assert command.path_angle == 0.0
assert np.isclose(command.curvature, 0.004)
assert command.curvature_rate == 0.0
def test_spatially_growing_path_adds_fast_pose_before_action_becomes_large():
controller = FordPathController(dt=1.0)
command = controller.update(_changing_path(0.0, 0.04), 0.012, current_curvature=0.0, v_ego=8.0)
assert command.path_offset > 0.0
assert command.path_angle > 0.0
assert command.curvature < 0.012
assert command.curvature_rate == 0.0
def test_growing_model_pose_adds_authority_but_c3_is_never_transmitted():
constant = _command(_path(0.012), 0.012)
growing = _command(_changing_path(0.0, 0.04), 0.012)
assert growing.path_offset > constant.path_offset
assert growing.path_angle > constant.path_angle
assert constant.curvature_rate == 0.0
assert growing.curvature_rate == 0.0
def test_large_maneuver_uses_fast_pose_and_zeros_c2():
command = _command(_path(0.04), 0.04)
assert command.path_offset > 0.5
assert command.path_angle > 0.2
assert command.curvature == 0.0
assert command.curvature_rate == 0.0
def test_model_pose_can_trigger_maneuver_when_action_is_late():
command = _command(_path(0.04), 0.002)
assert command.path_offset > 0.5
assert command.path_angle > 0.2
assert command.curvature == 0.0
def test_action_can_trigger_maneuver_before_model_pose_grows():
command = _command(_path(0.002), 0.04)
assert command.path_offset > 0.0
assert command.path_angle > 0.0
assert command.curvature == 0.0
def test_nearby_demands_blend_continuously_without_a_mode_threshold():
low = _command(_path(0.0119), 0.0119)
high = _command(_path(0.0121), 0.0121)
assert abs(high.path_offset - low.path_offset) < 0.05
assert abs(high.path_angle - low.path_angle) < 0.03
assert abs(high.curvature - low.curvature) < 0.001
def test_leaving_c2_normal_band_does_not_drop_total_authority():
normal = _command(_path(0.006), 0.006)
transition = _command(_path(0.0061), 0.0061)
assert transition.curvature <= normal.curvature
assert _equivalent_curvature(transition) >= _equivalent_curvature(normal)
def test_low_speed_still_uses_available_model_pose():
command = _command(_path(0.04, speed=2.0), 0.04, v_ego=2.0)
assert command.path_offset > 0.0
assert command.path_angle > 0.0
def test_higher_speed_extends_heading_horizon_without_moving_offset_horizon():
model = _changing_path(0.0, 0.04, speed=20.0)
slow = _command(model, 0.012, v_ego=7.0)
fast = _command(model, 0.012, v_ego=20.0)
assert np.isclose(fast.path_offset, slow.path_offset)
assert fast.path_angle > slow.path_angle
def test_short_model_uses_available_endpoint():
model = _path(0.04, speed=1.0)
command = _command(model, 0.04, v_ego=1.0)
assert command.valid
assert command.path_offset > 0.0
assert command.path_angle > 0.0
def test_turn_entry_coordinates_c2_release_with_fast_pose_attack():
controller = FordPathController(dt=0.01)
for _ in range(20):
assert controller.update(_path(0.004), 0.004, v_ego=8.0).curvature > 0.0
outputs = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)]
assert 0.0 < outputs[0].curvature < 0.004
assert outputs[0].path_offset > 0.0
assert outputs[0].path_angle > 0.0
assert outputs[-1].curvature == 0.0
def test_turn_exit_allows_c2_to_take_over_while_fast_pose_drains():
controller = FordPathController(dt=0.01)
for _ in range(20):
controller.update(_path(0.04), 0.04, current_curvature=0.02, v_ego=8.0)
outputs = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)]
assert 0.0 < outputs[0].curvature < 0.004
assert outputs[0].path_offset != 0.0 or outputs[0].path_angle != 0.0
assert outputs[-1].path_offset == 0.0
assert outputs[-1].path_angle == 0.0
def test_100hz_handoff_preserves_total_authority_without_entry_drop_or_exit_overshoot():
controller = FordPathController(dt=0.01)
normal = controller.update(_path(0.006), 0.006, current_curvature=0.006, v_ego=8.0)
entries = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)]
entry_authority = np.asarray([_equivalent_curvature(command) for command in entries])
assert np.all(np.diff(entry_authority) >= -1e-9)
assert entry_authority[0] >= _equivalent_curvature(normal)
exits = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)]
exit_authority = np.asarray([_equivalent_curvature(command) for command in exits])
assert np.all(np.diff(exit_authority) <= 1e-9)
assert np.all(exit_authority >= 0.004 - 1e-9)
def test_measured_undertracking_adds_fast_authority_without_overshoot_countersteer():
model = _path(0.04)
under = _command(model, 0.04, current_curvature=0.005)
on_target = _command(model, 0.04, current_curvature=0.04)
over = _command(model, 0.04, current_curvature=0.05)
assert under.path_offset > on_target.path_offset
assert under.path_angle > on_target.path_angle
assert over.path_offset == on_target.path_offset
assert over.path_angle == on_target.path_angle
def test_s_turn_reverses_model_pose_without_slow_c2():
controller = FordPathController(dt=0.05)
for _ in range(10):
controller.update(_path(0.04), 0.04, v_ego=8.0)
outputs = [controller.update(_path(-0.04), -0.04, v_ego=8.0) for _ in range(10)]
assert all(command.curvature == 0.0 for command in outputs)
assert np.all(np.diff([command.path_offset for command in outputs]) < 0.0)
assert np.all(np.diff([command.path_angle for command in outputs]) < 0.0)
assert outputs[-1].path_offset < 0.0
assert outputs[-1].path_angle < 0.0
def test_output_limits_and_rates_are_bounded():
controller = FordPathController()
outputs = [controller.update(_path(0.2), 0.2, v_ego=8.0) for _ in range(100)]
assert all(DBC_OFFSET[0] <= command.path_offset <= DBC_OFFSET[1] for command in outputs)
assert all(DBC_ANGLE[0] <= command.path_angle <= DBC_ANGLE[1] for command in outputs)
assert all(DBC_CURVATURE[0] <= command.curvature <= DBC_CURVATURE[1] for command in outputs)
assert np.max(np.abs(np.diff([command.path_offset for command in outputs]))) <= 0.04 + 1e-9
assert np.max(np.abs(np.diff([command.path_angle for command in outputs]))) <= 0.01 + 1e-9
def test_invalid_model_ramps_pose_to_zero_and_inactive_resets():
controller = FordPathController(dt=0.01)
for _ in range(20):
active = controller.update(_path(0.04), 0.04, v_ego=8.0)
invalid = controller.update(None, 0.0, v_ego=8.0)
assert invalid.valid
assert abs(invalid.path_offset) < abs(active.path_offset)
assert abs(invalid.path_angle) < abs(active.path_angle)
assert not controller.update(_path(0.0), 0.0, v_ego=8.0, active=False).valid
def test_sunnypilot_path_message_round_trip():
message = custom.CarControlSP.new_message()
message.fordLateralPath.pathOffset = 0.3
message.fordLateralPath.pathAngle = -0.2
message.fordLateralPath.curvature = 0.008
message.fordLateralPath.curvatureRate = -0.0004
message.fordLateralPath.valid = True
path = convert_carControlSP(message.as_reader()).fordLateralPath
assert np.isclose(path.pathOffset, 0.3)
assert np.isclose(path.pathAngle, -0.2)
assert np.isclose(path.curvature, 0.008)
assert np.isclose(path.curvatureRate, -0.0004)
assert path.valid
+1 -66
View File
@@ -32,14 +32,7 @@ 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
@@ -95,8 +88,7 @@ class SelfdriveD(CruiseHelper):
self.big_model_ready_t = 0.
# Setup sockets
self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] +
['selfdriveStateSP', 'onroadEventsSP', 'assistedDrivingMilestoneState'])
self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] + ['selfdriveStateSP', 'onroadEventsSP'])
self.gps_location_service = get_gps_location_service(self.params)
self.gps_packets = [self.gps_location_service]
@@ -135,7 +127,6 @@ 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()
@@ -146,11 +137,6 @@ 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
@@ -541,8 +527,6 @@ 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)
@@ -661,31 +645,6 @@ 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)
@@ -695,28 +654,6 @@ 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)
@@ -729,7 +666,6 @@ 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)
@@ -743,7 +679,6 @@ class SelfdriveD(CruiseHelper):
self.step()
self.rk.monitor_time()
finally:
self.assisted_driving_milestones.close()
e.set()
t.join()
+1 -7
View File
@@ -1,8 +1,5 @@
import os
import pyray as rl
import openpilot.cereal.messaging as messaging
from openpilot.common.hardware import PC
from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
from openpilot.selfdrive.ui.mici.layouts.settings.settings import SettingsLayout
from openpilot.selfdrive.ui.mici.layouts.offroad_alerts import MiciOffroadAlerts
@@ -64,8 +61,7 @@ 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_milestone_preview = PC and os.getenv("SP_MILESTONE_PREVIEW") == "1"
if not self._onboarding_window.completed and not skip_onboarding_for_milestone_preview:
if not self._onboarding_window.completed:
gui_app.push_widget(self._onboarding_window)
# initialize correct onroad layout
@@ -123,8 +119,6 @@ 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,7 +47,6 @@ 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)
@@ -58,7 +57,6 @@ class TogglesLayoutMici(NavScroller):
is_metric_toggle,
ldw_toggle,
always_on_dm_toggle,
milestone_celebrations_toggle,
record_front,
record_mic,
enable_openpilot,
@@ -70,7 +68,6 @@ 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),
@@ -20,7 +20,6 @@ AlertSize = log.SelfdriveState.AlertSize
AlertStatus = log.SelfdriveState.AlertStatus
ALERT_MARGIN = 18
ALERT_BACKGROUND_OPACITY = 0.90
ALERT_FONT_SMALL = 66 - 50
ALERT_FONT_BIG = 88 - 40
@@ -280,7 +279,7 @@ class AlertRenderer(Widget, SpeedLimitAlertRenderer):
def _draw_background(self, alert: Alert) -> None:
# draw top gradient for alert text at top
color = ALERT_COLORS.get(alert.status, ALERT_COLORS[AlertStatus.normal])
color = rl.Color(color.r, color.g, color.b, int(255 * ALERT_BACKGROUND_OPACITY * self._alpha_filter.x))
color = rl.Color(color.r, color.g, color.b, int(255 * 0.90 * self._alpha_filter.x))
translucent_color = rl.Color(color.r, color.g, color.b, int(0 * self._alpha_filter.x))
small_alert_height = round(self._rect.height * 0.583) # 140px at mici height
@@ -19,15 +19,10 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCamera
from openpilot.common.transformations.orientation import rot_from_euler
from enum import IntEnum
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_CELEBRATION_ENABLED:
from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_celebration import MilestoneCelebration
OpState = log.SelfdriveState.OpenpilotState
CALIBRATED = log.ExtrinsicsCalibration.Status.calibrated
NARROW_ROAD_CAM = VisionStreamType.VISION_STREAM_NARROW_ROAD
@@ -161,7 +156,6 @@ class AugmentedRoadView(CameraView):
self._alert_renderer = AlertRenderer()
self._driver_state_renderer = DriverStateRenderer()
self._confidence_ball = ConfidenceBall()
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,
@@ -229,12 +223,6 @@ class AugmentedRoadView(CameraView):
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))
@@ -259,6 +247,7 @@ class AugmentedRoadView(CameraView):
self._confidence_ball.render(self.rect)
self._bookmark_icon.render(self.rect)
def _switch_stream_if_needed(self, sm):
if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams:
v_ego = sm['carState'].vEgo
@@ -366,12 +355,10 @@ class AugmentedRoadView(CameraView):
return self._cached_matrix
def show_event(self):
super().show_event()
if gui_app.sunnypilot_ui():
ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.RESUME)
def hide_event(self):
super().hide_event()
if gui_app.sunnypilot_ui():
ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.PAUSE)
+9 -25
View File
@@ -24,8 +24,14 @@ ALERT_RAMP_TIME = 4 # seconds to ramp to max volume for warningImmediate
SELFDRIVE_STATE_TIMEOUT = 5 # 5 seconds
FILTER_DT = 1. / (micd.SAMPLE_RATE / micd.FFT_SAMPLES)
AMBIENT_DB = 26 # DB where MIN_VOLUME is applied
DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied
VOLUME_BASE = 20
if HARDWARE.get_device_type() == "tizi":
AMBIENT_DB = 30
VOLUME_BASE = 10
AudibleAlert = log.SelfdriveState.AudibleAlert
AudibleAlertSP = custom.SelfdriveStateSP.AudibleAlert
@@ -47,7 +53,6 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
AudibleAlert.promptDistracted: ("dm_warning.wav", None, MAX_VOLUME),
AudibleAlert.preAlert: ("pre_alert.wav", 1, MAX_VOLUME),
AudibleAlert.complete: ("milestone.wav", 1, MAX_VOLUME),
AudibleAlert.warningSoft: ("critical.wav", None, MAX_VOLUME),
AudibleAlert.warningImmediate: ("dm_critical.wav", None, MAX_VOLUME),
@@ -55,14 +60,6 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
**sound_list_sp,
}
def calculate_volume_for_device(weighted_db: float, device_type: str) -> float:
ambient_db = 30 if device_type in ("mici", "tizi") else 26
volume_base = 10 if device_type in ("mici", "tizi") else 20
volume_boost = 1.5 if device_type == "mici" else 1.0
volume = ((weighted_db - ambient_db) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME
return min(MAX_VOLUME, volume_boost * math.pow(volume_base, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1)))
def check_selfdrive_timeout_alert(sm):
ss_missing = time.monotonic() - sm.recv_time['selfdriveState']
@@ -77,7 +74,6 @@ class Soundd(QuietMode):
def __init__(self):
super().__init__()
self.device_type = HARDWARE.get_device_type()
self.load_sounds()
self.current_alert = AudibleAlert.none
@@ -89,7 +85,6 @@ 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)
@@ -169,19 +164,9 @@ class Soundd(QuietMode):
self.update_alert(AudibleAlert.none)
self.selfdrive_timeout_alert = False
def update_milestone_alert(self, sm):
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):
return calculate_volume_for_device(weighted_db, self.device_type)
volume = ((weighted_db - AMBIENT_DB) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME
return math.pow(VOLUME_BASE, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1))
@retry(attempts=10, delay=3)
def get_stream(self, sd):
@@ -195,7 +180,7 @@ class Soundd(QuietMode):
import sounddevice as sd
micd.patch_sounddevice(sd)
sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure', 'assistedDrivingMilestoneState'])
sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure'])
with self.get_stream(sd) as stream:
rk = Ratekeeper(20)
@@ -213,7 +198,6 @@ class Soundd(QuietMode):
self.current_volume = self.calculate_volume(float(self.spl_filter_weighted.x))
self.get_audible_alert(sm)
self.update_milestone_alert(sm)
# Ramp up immediate warning sound over 4s
if self.current_alert == AudibleAlert.warningImmediate:
@@ -5,28 +5,14 @@ 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, 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
from openpilot.system.ui.widgets.label import UnifiedLabel
class MiciHomeLayoutSP(MiciHomeLayout):
@@ -40,52 +26,6 @@ class MiciHomeLayoutSP(MiciHomeLayout):
gray_idx = self._status_bar_layout.widgets.index(self._egpu_icon_gray)
self._status_bar_layout.widgets.insert(gray_idx + 1, self._egpu_icon_default)
self._status_bar_layout.widgets.insert(gray_idx + 2, self._egpu_icon_orange)
initial_summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
self._last_summary_id = initial_summary.get("id", 0)
self._summary_wait_until = 0.0
self._summary_visible_until = 0.0
self._drive_summary = {}
def request_drive_summary(self) -> None:
self._summary_wait_until = time.monotonic() + SUMMARY_WAIT_SECONDS
def _render(self, _: rl.Rectangle) -> None:
super()._render(_)
now = time.monotonic()
if now < self._summary_wait_until:
summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
summary_id = summary.get("id", 0)
if summary_id and summary_id != self._last_summary_id:
self._last_summary_id = summary_id
distances = summary.get("distancesMeters", {})
enabled = ui_state.params.get_bool("AssistedDrivingMilestonesEnabled")
if enabled and any(_nonnegative_float(distances.get(category, 0.0)) > 0.0 for category in ("mads", "fullAssist")):
self._drive_summary = summary
self._summary_visible_until = now + SUMMARY_DURATION_SECONDS
self._summary_wait_until = 0.0
if now < self._summary_visible_until:
self._draw_drive_summary(_)
def _draw_drive_summary(self, rect: rl.Rectangle) -> None:
distances = self._drive_summary.get("distancesMeters", {})
metric = self._drive_summary.get("unit") == "metric"
meters_per_unit = METERS_PER_KILOMETER if metric else METERS_PER_MILE
unit = "KM" if metric else "MI"
mads = _nonnegative_float(distances.get("mads", 0.0)) / meters_per_unit
full_assist = _nonnegative_float(distances.get("fullAssist", 0.0)) / meters_per_unit
rl.draw_rectangle_rec(rect, rl.Color(0, 0, 0, 235))
gui_label(rl.Rectangle(rect.x, rect.y + 14, rect.width, 52), tr("DRIVE COMPLETE"), 42,
font_weight=FontWeight.SEMI_BOLD, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
gui_label(rl.Rectangle(rect.x + 20, rect.y + 78, rect.width / 2 - 30, 42), tr("MADS"), 28,
color=rl.Color(255, 255, 255, 184), alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 78, rect.width / 2 - 30, 42), tr("FULL ASSIST"), 28,
color=rl.Color(255, 255, 255, 184), alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
gui_label(rl.Rectangle(rect.x + 20, rect.y + 116, rect.width / 2 - 30, 72), f"{mads:.1f} {unit}", 48,
font_weight=FontWeight.DISPLAY, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 116, rect.width / 2 - 30, 72), f"{full_assist:.1f} {unit}", 48,
font_weight=FontWeight.DISPLAY, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
def _set_egpu_visibility(self):
chestnut = ui_state.sm["deviceState"].chestnutPresent
@@ -1,228 +0,0 @@
"""Render assisted-driving milestone celebrations over the on-road view."""
import math
import random
import time
from collections import deque
from dataclasses import dataclass
import pyray as rl
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.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
METERS_PER_MILE = 1609.344
METERS_PER_KILOMETER = 1000.0
CONFETTI_COLORS = (
rl.Color(255, 55, 95, 255),
rl.Color(255, 183, 3, 255),
rl.Color(48, 209, 88, 255),
rl.Color(36, 179, 255, 255),
rl.Color(112, 72, 232, 255),
rl.Color(255, 45, 196, 255),
)
@dataclass(frozen=True)
class ConfettiParticle:
x: float
y: float
width: float
height: float
speed: float
drift: float
angle: float
spin: float
phase: float
color: rl.Color
@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: CelebrationMilestone | None = None
self._pending_milestones: deque[CelebrationMilestone] = deque()
self._last_event_id = 0
self._particles = self._make_particles()
@staticmethod
def _make_particles() -> list[ConfettiParticle]:
rng = random.Random(20260828)
return [
ConfettiParticle(
x=rng.random(),
y=rng.uniform(-0.25, 0.95),
width=rng.uniform(10, 24),
height=rng.uniform(24, 58),
speed=rng.uniform(0.12, 0.34),
drift=rng.uniform(-0.035, 0.035),
angle=rng.uniform(0, 360),
spin=rng.uniform(-150, 150),
phase=rng.uniform(0, math.tau),
color=CONFETTI_COLORS[rng.randrange(len(CONFETTI_COLORS))],
)
for _ in range(PARTICLE_COUNT)
]
def _render(self, rect: rl.Rectangle, /) -> None:
now = time.monotonic()
if ui_state.started_time != self._drive_started_time:
self._drive_started_time = ui_state.started_time
self._celebration_started_time = None
self._current_milestone = None
self._pending_milestones.clear()
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
if self._celebration_started_time is None or self._current_milestone is None:
return
elapsed = now - self._celebration_started_time
if elapsed >= CELEBRATION_DURATION:
self._celebration_started_time = None
self._current_milestone = None
return
alpha = min(1.0, elapsed / 0.2, (CELEBRATION_DURATION - elapsed) / 0.8)
self._draw_background_scrim(rect, alpha)
self._draw_confetti(rect, elapsed, alpha)
self._draw_milestone(rect, elapsed, alpha, self._current_milestone)
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 _consume_event(self, suppress: bool) -> None:
if not ui_state.sm.updated["assistedDrivingMilestoneState"]:
return
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
compact = rect.height <= 300
particle_scale = rect.height / 1080.0
particles = self._particles[:100] if compact else self._particles
for particle in particles:
x = rect.x + rect.width * (particle.x + particle.drift * elapsed + 0.012 * math.sin(elapsed * 3 + particle.phase))
y = rect.y - rect.height * 0.2 + (particle.y * travel_height + particle.speed * rect.height * elapsed) % travel_height
flip = 0.2 + 0.8 * abs(math.sin(elapsed * 5 + particle.phase))
particle_rect = rl.Rectangle(x, y, particle.width * particle_scale * flip, particle.height * particle_scale)
origin = rl.Vector2(particle_rect.width / 2, particle_rect.height / 2)
color = rl.Color(particle.color.r, particle.color.g, particle.color.b, int(255 * alpha))
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: 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)
number_size = int(FONT_SIZES.set_speed * scale * pulse)
milestone_size = int(FONT_SIZES.max_speed * scale * pulse)
category_size = int(22 * scale * pulse)
unit_size = category_size
display_font = gui_app.font(FontWeight.DISPLAY)
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
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):,}"
else:
number = f"{displayed_distance:,.1f}"
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)
max_number_width = rect.width * 0.72 - unit_bounds.x - 8 * scale
if number_bounds.x > max_number_width:
number_size = max(1, int(number_size * max_number_width / number_bounds.x))
number_bounds = measure_text_cached(display_font, number, number_size)
category_bounds = measure_text_cached(semibold_font, category, category_size)
milestone_bounds = measure_text_cached(semibold_font, milestone_label, milestone_size)
center_x = rect.x + rect.width / 2
center_y = rect.y + rect.height / 2
text_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha))
secondary_color = rl.Color(255, 255, 255, int(255 * 0.72 * alpha))
number_line_width = number_bounds.x + 8 * scale + unit_bounds.x
number_x = center_x - number_line_width / 2
number_y = center_y - 76 * scale
unit_y = center_y + 14 * scale
category_y = center_y - 91 * scale
milestone_y = center_y + 50 * scale
rl.draw_text_ex(semibold_font, category, rl.Vector2(center_x - category_bounds.x / 2, category_y),
category_size, 0, secondary_color)
rl.draw_text_ex(display_font, number, rl.Vector2(number_x, number_y), number_size, 0, text_color)
rl.draw_text_ex(semibold_font, unit, rl.Vector2(number_x + number_bounds.x + 8 * scale, unit_y),
unit_size, 0, secondary_color)
rl.draw_text_ex(semibold_font, milestone_label, rl.Vector2(center_x - milestone_bounds.x / 2, milestone_y),
milestone_size, 0, text_color)
@staticmethod
def _draw_background_scrim(rect: rl.Rectangle, alpha: float) -> None:
# Match the alert background: a mostly opaque black core fading to transparent.
fade_height = round(rect.height * 0.25)
solid_height = round(rect.height * 0.50)
solid_color = rl.Color(0, 0, 0, int(255 * ALERT_BACKGROUND_OPACITY * alpha))
transparent = rl.Color(0, 0, 0, 0)
x = int(rect.x)
y = int(rect.y)
width = int(rect.width)
rl.draw_rectangle_gradient_v(x, y, width, fade_height, transparent, solid_color)
rl.draw_rectangle(x, y + fade_height, width, solid_height, solid_color)
rl.draw_rectangle_gradient_v(x, y + fade_height + solid_height, width, fade_height, solid_color, transparent)
@@ -35,8 +35,7 @@ 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",
"assistedDrivingMilestoneState",
"gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay"
]
self.sunnylink_state = SunnylinkState()
@@ -1,45 +0,0 @@
#!/usr/bin/env python3
"""Generate the assisted-driving milestone celebration chime."""
import math
import wave
from array import array
from pathlib import Path
SAMPLE_RATE = 48_000
DURATION_SECONDS = 0.82
NOTES = (
(0.00, 523.25),
(0.11, 659.25),
(0.22, 783.99),
)
def note_sample(age: float, frequency: float) -> float:
if not 0 <= age <= 0.58:
return 0.0
attack = min(age / 0.008, 1.0)
release = min((0.58 - age) / 0.15, 1.0)
envelope = attack * release * math.exp(-3.8 * age)
tone = math.sin(math.tau * frequency * age) + 0.16 * math.sin(math.tau * frequency * 2 * age)
return envelope * tone
def main() -> None:
output = Path(__file__).parents[4] / "openpilot/selfdrive/assets/sounds/milestone.wav"
samples = array('h')
for frame in range(round(SAMPLE_RATE * DURATION_SECONDS)):
t = frame / SAMPLE_RATE
value = 0.38 * sum(note_sample(t - start, frequency) for start, frequency in NOTES)
samples.append(round(max(-1.0, min(1.0, value)) * 32767))
with wave.open(str(output), "wb") as wav:
wav.setnchannels(1)
wav.setsampwidth(2)
wav.setframerate(SAMPLE_RATE)
wav.writeframes(samples.tobytes())
if __name__ == "__main__":
main()
@@ -1,38 +0,0 @@
#!/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()
@@ -1,27 +0,0 @@
#!/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 -52
View File
@@ -4,63 +4,12 @@ import time
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.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert
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):
comma_three_x_volume = calculate_volume_for_device(weighted_db, "tizi")
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_typed_milestone_event_once(self):
soundd = Soundd()
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'])
pm = PubMaster(['selfdriveState', 'selfdriveStateSP'])
+2 -7
View File
@@ -41,12 +41,7 @@ def _compute_hash(file_path: str) -> str | None:
return None
async def verify_file(file_path: str, expected_hash: str) -> bool:
file_hash = _compute_hash(file_path)
return file_hash == expected_hash.lower() if file_hash else False
def _verify_file(file_path: str, expected_hash: str) -> bool:
def verify_file(file_path: str, expected_hash: str) -> bool:
file_hash = _compute_hash(file_path)
return file_hash == expected_hash.lower() if file_hash else False
@@ -81,7 +76,7 @@ def _bundle_artifacts(bundle: custom.ModelManagerSP.ModelBundle) -> list[tuple[s
def _bundle_is_valid_locally(bundle: custom.ModelManagerSP.ModelBundle) -> bool:
model_root = Paths.model_root()
return all(_verify_file(os.path.join(model_root, file_name), expected_hash)
return all(verify_file(os.path.join(model_root, file_name), expected_hash)
for file_name, expected_hash in _bundle_artifacts(bundle))
+45 -20
View File
@@ -10,6 +10,7 @@ import os
import time
import requests
from openpilot.common.file_chunker import get_chunk_name
from openpilot.common.params import Params
from openpilot.common.realtime import Ratekeeper
from openpilot.common.swaglog import cloudlog
@@ -22,6 +23,8 @@ from openpilot.sunnypilot.models.helpers import (ACTIVE_BUNDLE_KEYS, get_active_
# (connect, read) seconds. read is per-request inactivity, not a total cap
DOWNLOAD_TIMEOUT = (30, 30)
# how many download+verify rounds before giving up on a chunk that won't verify
MAX_CHUNK_VERIFY_ATTEMPTS = 3
class DownloadCancelled(Exception):
@@ -153,6 +156,35 @@ class ModelManagerSP:
os.remove(base_path)
del self._download_start_times[artifact.fileName]
async def _verify_chunks_parallel(self, artifact, full_path: str) -> set[int]:
num_chunks = len(artifact.chunks)
start = time.monotonic()
futures = []
chunk_index: dict[asyncio.Future, int] = {}
for i, chunk in enumerate(artifact.chunks):
fut = asyncio.ensure_future(asyncio.to_thread(verify_file, get_chunk_name(full_path, i, num_chunks), chunk.sha256))
futures.append(fut)
chunk_index[fut] = i
valid_chunks: set[int] = set()
pending = set(futures)
while pending:
if self._download_interrupted():
for t in pending:
t.cancel()
raise DownloadCancelled("Download cancelled")
done, pending = await asyncio.wait(pending, return_when=asyncio.FIRST_COMPLETED)
for fut in done:
if fut.result():
valid_chunks.add(chunk_index[fut])
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.verifying
artifact.downloadProgress.progress = (len(valid_chunks) / num_chunks) * 100
self._sync_artifact_progress(artifact)
self._report_status()
elapsed = time.monotonic() - start
cloudlog.info(f"Verified {len(valid_chunks)}/{num_chunks} chunks of {artifact.fileName} in {elapsed:.2f}s")
return valid_chunks
async def _process_artifact(self, artifact, destination_path: str) -> None:
if not artifact.downloadUri.uri:
return None
@@ -170,20 +202,10 @@ class ModelManagerSP:
is_cached = False
valid_chunks: set[int] = set()
if len(artifact.chunks) > 0:
from openpilot.common.file_chunker import get_chunk_name
num_chunks = len(artifact.chunks)
for i, chunk in enumerate(artifact.chunks):
if self._download_interrupted():
raise DownloadCancelled("Download cancelled")
if await verify_file(get_chunk_name(full_path, i, num_chunks), chunk.sha256):
valid_chunks.add(i)
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.verifying
artifact.downloadProgress.progress = (len(valid_chunks) / num_chunks) * 100
self._sync_artifact_progress(artifact)
self._report_status()
is_cached = len(valid_chunks) == num_chunks
valid_chunks = await self._verify_chunks_parallel(artifact, full_path)
is_cached = len(valid_chunks) == len(artifact.chunks)
else:
if await verify_file(full_path, expected_hash):
if verify_file(full_path, expected_hash):
is_cached = True
if is_cached:
@@ -195,15 +217,18 @@ class ModelManagerSP:
return
if len(artifact.chunks) > 0:
await self._download_chunked(url, full_path, artifact, skip=valid_chunks)
from openpilot.common.file_chunker import get_chunk_name
for i, chunk in enumerate(artifact.chunks):
chunk_path = get_chunk_name(full_path, i, len(artifact.chunks))
if not await verify_file(chunk_path, chunk.sha256):
raise ValueError(f"Hash validation failed for chunk {i+1} of {filename}")
attempts = 0
while len(valid_chunks) < len(artifact.chunks):
attempts += 1
cloudlog.warning(f"Re-downloading {len(artifact.chunks) - len(valid_chunks)} invalid chunk(s) of {filename} (attempt {attempts})")
await self._download_chunked(url, full_path, artifact, skip=valid_chunks)
valid_chunks = await self._verify_chunks_parallel(artifact, full_path)
if len(valid_chunks) < len(artifact.chunks) and attempts >= MAX_CHUNK_VERIFY_ATTEMPTS:
missing = next(i for i in range(len(artifact.chunks)) if i not in valid_chunks)
raise ValueError(f"Hash validation failed for chunk {missing+1} of {filename} after {attempts} attempts")
else:
await self._download_file(url, full_path, artifact)
if not await verify_file(full_path, expected_hash):
if not verify_file(full_path, expected_hash):
raise ValueError(f"Hash validation failed for {filename}")
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.downloaded
@@ -104,14 +104,6 @@ class ControlsExt(ModelStateBase):
CC_SP.intelligentCruiseButtonManagement.sendButton = icbm_src.sendButton
CC_SP.intelligentCruiseButtonManagement.vTarget = icbm_src.vTarget
ford_path = getattr(self, 'ford_path', None)
if ford_path is not None:
CC_SP.fordLateralPath.valid = ford_path.valid
CC_SP.fordLateralPath.pathOffset = ford_path.path_offset
CC_SP.fordLateralPath.pathAngle = ford_path.path_angle
CC_SP.fordLateralPath.curvature = ford_path.curvature
CC_SP.fordLateralPath.curvatureRate = ford_path.curvature_rate
return CC_SP
@staticmethod
@@ -1,260 +0,0 @@
"""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
@@ -1,123 +0,0 @@
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,12 +1383,6 @@
"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,10 +20,6 @@ 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
@@ -99,32 +99,6 @@ def _migrate_model_bundle_slots(_params):
cloudlog.exception(f"Error migrating model bundle slots: {e}")
def _migrate_assisted_driving_milestones(_params):
try:
state = _params.get("AssistedDrivingMilestoneState", return_default=True)
if isinstance(state, dict) and state.get("version") == 1:
return
_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 migrating assisted-driving milestone state: {e}")
def run_migration(_params):
# migrate OnroadScreenOffBrightness
if _params.get("OnroadScreenOffBrightnessMigrated") != ONROAD_BRIGHTNESS_MIGRATION_VERSION:
@@ -164,5 +138,3 @@ def run_migration(_params):
# seed the usbgpu model slot from the pre-split single slot
_migrate_model_bundle_slots(_params)
_migrate_assisted_driving_milestones(_params)
@@ -7,44 +7,7 @@ See the LICENSE.md file in the root directory for more details.
from openpilot.common.params import Params
from openpilot.common.test import OpenpilotTestCase
from openpilot.sunnypilot.system.params_migration import _migrate_model_bundle_slots, run_migration
class TestAssistedDrivingMilestoneMigration(OpenpilotTestCase):
def test_preserves_prototype_distances_once(self):
class ParamsStub:
def __init__(self):
self.values = {
"MadsDrivenDistanceMeters": 123.0,
"FullAssistDrivenDistanceMeters": 456.0,
"OnroadScreenOffBrightness": 0,
"OnroadScreenOffTimer": 15,
"AssistedDrivingMilestoneState": {},
"IsMetric": False,
}
def get(self, key, return_default=False):
return self.values.get(key)
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)
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)
state = params.get("AssistedDrivingMilestoneState")
assert state["distancesMeters"] == {"mads": 123.0, "fullAssist": 456.0}
from openpilot.sunnypilot.system.params_migration import _migrate_model_bundle_slots
class TestModelBundleSlotMigration(OpenpilotTestCase):