Compare commits

..

16 Commits

Author SHA1 Message Date
Isaac Barham 27a220677a Productionize assisted driving milestones
Assisted-by: OpenAI Codex
2026-08-29 07:52:11 -04:00
Isaac Barham 26e4889fcb Raise comma four alert volume 2026-08-28 19:38:48 -04:00
Isaac Barham 70fa5d0fca Boost comma four alerts and reset milestones 2026-08-28 16:13:14 -04:00
Isaac Barham 2d700cc0d0 Add alert-style milestone scrim 2026-08-28 11:34:46 -04:00
Isaac Barham cc9ae66b22 Persist assisted driving milestones 2026-08-28 09:41:13 -04:00
Isaac Barham 505270420f Refine milestone celebration typography 2026-08-28 08:40:09 -04:00
Isaac Barham bb1a17d2a0 Prototype assisted driving milestones 2026-08-28 07:34:03 -04:00
Isaac Barham 6db807b5a0 ford: narrow lateral path interface
Assisted-by: Codex
2026-08-27 20:06:36 -04:00
Isaac Barham 42e1414bc4 ford: source C2 only from desired curvature
Prevent model-fit curvature jitter from directly modulating the PSCM's slow C2 channel.

Assisted-by: Codex
2026-08-27 19:53:59 -04:00
Isaac Barham 3e020e321f ford: gate curvature rate with maneuver demand
Assisted-by: Codex
2026-08-27 19:44:19 -04:00
Isaac Barham 7e2000e909 ford: make path allocation demand driven
Assisted-by: Codex
2026-08-27 19:08:21 -04:00
Isaac Barham e96055846c ford: distill lateral path controller
Assisted-by: Codex
2026-08-27 16:27:38 -04:00
Isaac Barham d47646b28f Ford: keep LMC2 available through path gaps
Assisted-by: Codex
2026-08-27 15:36:50 -04:00
Isaac Barham e75bc83424 Ford: retain centering through curve exits
Keep a bounded geometric C2 band for lane centering, preserve established rolling arcs during same-direction unwind, and smoothly release old-direction C2 on reversals. Slew-limit the fast C1 command to prevent threshold chatter.

Assisted-by: Codex
2026-08-27 15:13:56 -04:00
Isaac Barham 08e48958b6 Ford: close the loop on path curvature
Use the rolling path for pose and slow geometry while allocating jerk-limited requested curvature and bounded tracking error to the fast heading field. Prevent filtered C2 from reinforcing an unwind or reversal.

Assisted-by: Codex
2026-08-27 14:35:59 -04:00
Isaac Barham 25d0d0f1ff Ford: embed model path in rolling reference
Assisted-by: Codex
2026-08-27 13:08:15 -04:00
70 changed files with 1929 additions and 365 deletions
+1
View File
@@ -9,6 +9,7 @@
*.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
@@ -8,13 +8,13 @@ on:
required: true
type: string
target_hardware:
description: 'Hardware target to compile for (qcom or chestnut)'
description: 'Hardware target to compile for (qcom or usbgpu)'
required: true
type: choice
default: 'qcom'
options:
- qcom
- chestnut
- usbgpu
hf_repo:
description: 'Hugging Face dataset repository'
required: false
@@ -59,7 +59,7 @@ jobs:
id: get-json
run: |
cd docs/docs
PREFIX="driving_models_${{ inputs.target_hardware == 'chestnut' && 'chestnut_' || '' }}v"
PREFIX="driving_models_${{ inputs.target_hardware == 'usbgpu' && 'usbgpu_' || '' }}v"
latest=$(ls ${PREFIX}*.json | sed -E "s/${PREFIX}([0-9]+)\.json/\1/" | sort -n | tail -1)
next=$((latest+1))
json_file="${PREFIX}${next}.json"
+1 -1
View File
@@ -176,7 +176,7 @@ jobs:
build_big_model:
needs: resolve
if: ${{ inputs.target == 'big' }}
runs-on: [self-hosted, chestnut]
runs-on: [self-hosted, usbgpu]
env:
BIG_ONNX: openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx
BIG_PKL: openpilot/selfdrive/modeld/models/big_driving_tinygrad.pkl
@@ -30,7 +30,7 @@ on:
type: boolean
default: true
target_hardware:
description: 'Hardware target to compile for (qcom or chestnut)'
description: 'Hardware target to compile for (qcom or usbgpu)'
required: false
type: string
default: 'qcom'
@@ -101,7 +101,7 @@ on:
default: 'qcom'
options:
- qcom
- chestnut
- usbgpu
hf_repo:
description: 'Hugging Face dataset repository'
required: false
@@ -109,7 +109,7 @@ on:
default: 'sunnypilot/sunnypilot_models_v1'
env:
RECOMPILED_DIR: recompiled${{ inputs.recompiled_dir }}
JSON_FILE: docs/docs/driving_models_${{ inputs.target_hardware == 'chestnut' && 'chestnut_v' || 'v' }}${{ inputs.json_version }}.json
JSON_FILE: docs/docs/driving_models_${{ inputs.target_hardware == 'usbgpu' && 'usbgpu_v' || 'v' }}${{ inputs.json_version }}.json
jobs:
build_model:
@@ -31,7 +31,7 @@ on:
type: string
default: ''
target_hardware:
description: 'Hardware target to compile for (qcom or chestnut)'
description: 'Hardware target to compile for (qcom or usbgpu)'
required: false
type: string
default: 'qcom'
@@ -57,7 +57,7 @@ on:
type: choice
options:
- qcom
- chestnut
- usbgpu
default: 'qcom'
@@ -102,7 +102,7 @@ jobs:
cat $GITHUB_OUTPUT
- run: |
cd ${{ github.workspace }}/openpilot/openpilot
if [ "${{ inputs.target_hardware }}" != "chestnut" ]; then
if [ "${{ inputs.target_hardware }}" != "usbgpu" ]; then
git lfs pull -X "**/selfdrive/modeld/models/big_*.onnx,**/selfdrive/modeld/models/dmonitoring_*.onnx"
rm -f selfdrive/modeld/models/big_*.onnx selfdrive/modeld/models/dmonitoring_*.onnx
else
@@ -121,7 +121,7 @@ jobs:
if-no-files-found: error
build_model:
runs-on: [self-hosted, chestnut]
runs-on: [self-hosted, usbgpu]
needs: get_model
env:
MODEL_NAME: ${{ inputs.custom_name || inputs.upstream_branch }} (${{ needs.get_model.outputs.model_date }})
@@ -185,9 +185,9 @@ jobs:
CAMERA_RES=$(python3 -c "from openpilot.common.transformations.camera import _ar_ox_fisheye as a, _os_fisheye as o; print(f'{a.width}x{a.height} {o.width}x{o.height}')")
TG_FLAGS_QCOM="DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
if [ "${{ inputs.target_hardware }}" == "chestnut" ]; then
echo "CHESTNUT build"
export CHESTNUT=1
if [ "${{ inputs.target_hardware }}" == "usbgpu" ]; then
echo "USBGPU build"
export USBGPU=1
TG_FLAGS="DEBUG=1 DEV=USB+AMD:LLVM WARP_DEV=QCOM FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2"
OUTPUT_PKL="${{ env.MODELS_DIR }}/big_driving_tinygrad.pkl"
else
+33 -1
View File
@@ -382,6 +382,7 @@ struct CarControlSP @0xa5cd762cd951a455 {
leadOne @2 :LeadData;
leadTwo @3 :LeadData;
intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
fordLateralPath @5 :FordLateralPath;
struct Param {
key @0 :Text;
@@ -402,6 +403,14 @@ 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;
@@ -469,7 +478,30 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
}
}
struct CustomReserved10 @0xcb9fd56c7057593a {
struct AssistedDrivingMilestoneState @0xcb9fd56c7057593a {
enabled @0 :Bool;
madsDistanceMeters @1 :Float64;
fullAssistDistanceMeters @2 :Float64;
event @3 :Event;
struct Event {
id @0 :UInt64;
category @1 :Category;
distanceMeters @2 :Float64;
previousDistanceMeters @3 :Float64;
unit @4 :Unit;
}
enum Category {
none @0;
mads @1;
fullAssist @2;
}
enum Unit {
imperial @0;
metric @1;
}
}
struct CustomReserved11 @0xc2243c65e0340384 {
+1 -1
View File
@@ -2640,7 +2640,7 @@ struct Event {
carStateSP @114 :Custom.CarStateSP;
liveMapDataSP @115 :Custom.LiveMapDataSP;
modelDataV2SP @116 :Custom.ModelDataV2SP;
customReserved10 @136 :Custom.CustomReserved10;
assistedDrivingMilestoneState @136 :Custom.AssistedDrivingMilestoneState;
customReserved11 @137 :Custom.CustomReserved11;
customReserved12 @138 :Custom.CustomReserved12;
customReserved13 @139 :Custom.CustomReserved13;
+1
View File
@@ -90,6 +90,7 @@ _services: dict[str, tuple] = {
"carParamsSP": (True, 0.02, 1),
"carControlSP": (True, 100., 10),
"carStateSP": (True, 100., 10),
"assistedDrivingMilestoneState": (True, 10., 1),
"liveMapDataSP": (True, 1., 1),
"modelDataV2SP": (True, 20., None, QueueSize.BIG),
"liveLocationKalman": (True, 20.),
+4
View File
@@ -97,6 +97,10 @@ Params::Params(const std::string &path) {
}
Params::~Params() {
flushNonBlockingWrites();
}
void Params::flushNonBlockingWrites() {
if (future.valid()) {
future.wait();
}
+1
View File
@@ -75,6 +75,7 @@ public:
return put(key.c_str(), val ? "1" : "0", 1);
}
void putNonBlocking(const std::string &key, const std::string &val);
void flushNonBlockingWrites();
inline void putBoolNonBlocking(const std::string &key, bool val) {
putNonBlocking(key, val ? "1" : "0");
}
+5
View File
@@ -73,6 +73,7 @@ params_get = _bind("params_get", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool],
params_get_bool = _bind("params_get_bool", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool], ctypes.c_bool)
params_put = _bind("params_put", [ParamsHandle, ctypes.c_char_p, ctypes.c_char_p, ctypes.c_size_t, ctypes.c_bool], ctypes.c_int)
params_put_bool = _bind("params_put_bool", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool, ctypes.c_bool], ctypes.c_int)
params_flush = _bind("params_flush", [ParamsHandle])
params_remove = _bind("params_remove", [ParamsHandle, ctypes.c_char_p], ctypes.c_int)
params_get_path = _bind("params_get_path", [ParamsHandle, ctypes.c_char_p, ctypes.c_size_t], ParamsBuffer)
params_keys_size = _bind("params_keys_size", [ParamsHandle], ctypes.c_size_t)
@@ -178,6 +179,10 @@ class Params:
def put_bool(self, key, val, block=False):
params_put_bool(self.p, self.check_key(key), val, block)
def flush(self):
"""Wait for all prior nonblocking writes from this Params instance."""
params_flush(self.p)
def remove(self, key):
params_remove(self.p, self.check_key(key))
+6
View File
@@ -133,6 +133,12 @@ int params_put_bool(ParamsHandle *handle, const char *key, bool value, bool bloc
});
}
void params_flush(ParamsHandle *handle) noexcept {
translate_exceptions([&]() {
handle->params.flushNonBlockingWrites();
});
}
int params_remove(ParamsHandle *handle, const char *key) noexcept {
return translate_exceptions(-1, [&]() {
return handle->params.remove(key);
+10 -6
View File
@@ -130,12 +130,14 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"UpdaterLastFetchTime", {PERSISTENT, TIME}},
{"UptimeOffroad", {PERSISTENT, FLOAT, "0.0"}},
{"UptimeOnroad", {PERSISTENT, FLOAT, "0.0"}},
{"ChestnutActive", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}},
{"ChestnutLoading", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}},
{"UsbGpuActive", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}},
{"UsbGpuLoading", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}},
{"Version", {PERSISTENT, STRING}},
// --- 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
@@ -156,6 +158,7 @@ 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"}},
@@ -165,7 +168,9 @@ 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}},
@@ -195,16 +200,15 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
// Model Manager params
{"ModelManager_ActiveBundle", {PERSISTENT, JSON}},
{"ModelManager_ActiveBundleUSBGPU", {PERSISTENT, JSON}}, //TODO-SP: kept for migration, remove on next sync?
{"ModelManager_ActiveBundleChestnut", {PERSISTENT, JSON}},
{"ModelManager_ActiveBundleUSBGPU", {PERSISTENT, JSON}},
{"ModelManager_ActiveJson", {CLEAR_ON_MANAGER_START, JSON}},
{"ModelManager_ClearCache", {CLEAR_ON_MANAGER_START, BOOL}},
{"ModelManager_DownloadRef", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, STRING}},
{"ModelManager_Favs", {PERSISTENT | BACKUP, STRING}},
{"ModelManager_LastSyncTime", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}},
{"ModelManager_LastSyncTime_Chestnut", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}},
{"ModelManager_LastSyncTime_USBGPU", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}},
{"ModelManager_ModelsCache", {PERSISTENT | BACKUP, JSON}},
{"ModelManager_ModelsCache_Chestnut", {PERSISTENT | BACKUP, JSON}},
{"ModelManager_ModelsCache_USBGPU", {PERSISTENT | BACKUP, JSON}},
// Neural Network Lateral Control
{"NeuralNetworkLateralControl", {PERSISTENT | BACKUP, BOOL, "0"}},
+7
View File
@@ -106,6 +106,13 @@ class TestParams(OpenpilotTestCase):
assert q.get("CarParams") is None
assert q.get("CarParams", True) == b"1"
def test_flush_non_blocking_writes(self):
self.params.put("DongleId", "first")
self.params.put("DongleId", "last")
self.params.flush()
assert self.params.get("DongleId") == "last"
def test_params_all_keys(self):
keys = Params().all_keys()
@@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:845c40ff0d37612e8f2f482a36845744b5ae91ce2fcfc8117990d7d278b59820
size 13079
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:8a8c5fece2a1c7587feb41cbe04c6aee08e768ecd9b5d00da6af9832a4ccc842
size 2034
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:7409c53d7c72681c24982fd83b56ce70f80797c9c0f936d9296a5c18557ac472
size 7279
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:58bd6155433f623b1f75d134bd8ca4745d9aa71f6767eb807cdbcf7deb3089a1
size 10876
Binary file not shown.
+1
View File
@@ -63,5 +63,6 @@ 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,6 +13,7 @@ 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
@@ -52,6 +53,8 @@ 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
@@ -155,6 +158,11 @@ 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, v_ego=CS.vEgo, active=CC.latActive,
current_curvature=self.curvature)
actuators.curvature = float(self.ford_path.curvature)
# Ensure no NaNs/Infs
for p in ACTUATOR_FIELDS:
attr = getattr(actuators, p)
@@ -0,0 +1,166 @@
from dataclasses import dataclass, fields
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_OFFSET_DISTANCE = 7.0
_PATH_MIN_LOOKAHEAD = 7.0
_CURVATURE_RATE_HORIZONS = (3.5, 5.0, 7.0)
_CENTERING_CURVATURE_BASEBAND = (0.003, 0.006)
_TRACKING_ERROR_DEADZONE = 0.0005
_TRACKING_ERROR_LIMIT = 0.012
_PATH_RATES = (4.0, 1.0, math.inf, 0.002)
@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 _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 _curvature_rate(path: tuple[list[float], list[float], list[float]]) -> float:
distance, _, heading = path
rates = []
for requested_horizon in _CURVATURE_RATE_HORIZONS:
horizon = min(requested_horizon, distance[-1])
start = _sample(0.0, distance, heading)
midpoint = _sample(0.5 * horizon, distance, heading)
end = _sample(horizon, distance, heading)
rates.append(4.0 * (start - 2.0 * midpoint + end) / horizon ** 2)
magnitude = sum(abs(rate) for rate in rates)
if magnitude == 0.0:
return 0.0
return sorted(rates)[1] * abs(sum(rates)) / magnitude
def _curvature(path: tuple[list[float], list[float], list[float]]) -> float:
distance, _, heading = path
horizon = min(_PATH_MIN_LOOKAHEAD, distance[-1])
return (_sample(horizon, distance, heading) - _sample(0.0, distance, heading)) / horizon
def _encode_path(model, desired_curvature: float, v_ego: float, current_curvature: float | None) -> FordPath:
path = _model_path(model)
if path is None:
return FordPath()
distance, offset, heading = path
lookahead = max(_finite(v_ego), _PATH_MIN_LOOKAHEAD)
path_offset = _sample(_PATH_OFFSET_DISTANCE, distance, offset)
path_angle = _sample(lookahead, distance, heading)
model_curvature = _curvature(path)
model_curvature_rate = _curvature_rate(path)
action_curvature = _finite(desired_curvature)
requested_curvature = max((model_curvature, action_curvature), key=abs)
maneuver_residual = requested_curvature - model_curvature
path_offset += 0.5 * maneuver_residual * _PATH_OFFSET_DISTANCE ** 2
path_angle += maneuver_residual * lookahead
correction = 0.0
tracking_demand = 0.0
wheel_beyond_target = False
if current_curvature is not None:
target_curvature = action_curvature
measured_curvature = _finite(current_curvature)
tracking_error = target_curvature - measured_curvature
correction = math.copysign(max(abs(tracking_error) - _TRACKING_ERROR_DEADZONE, 0.0), tracking_error)
tracking_demand = abs(correction)
wheel_beyond_target = target_curvature * measured_curvature > 0.0 and \
abs(target_curvature) + _TRACKING_ERROR_DEADZONE < abs(measured_curvature)
correction_limit = _TRACKING_ERROR_LIMIT
if correction * target_curvature < 0.0:
correction_limit = 0.5 * abs(target_curvature)
correction = float(np.clip(correction, -correction_limit, correction_limit))
correction_offset = 0.5 * correction * _PATH_OFFSET_DISTANCE ** 2
correction_angle = correction * lookahead
if wheel_beyond_target:
path_offset = correction_offset
path_angle = correction_angle
else:
path_offset += correction_offset
path_angle += correction_angle
spatial_demand = abs(model_curvature_rate) * lookahead / 3.0
overflow_demand = max(abs(requested_curvature) - DBC_CURVATURE[1], 0.0)
maneuver_demand = max(spatial_demand, overflow_demand, tracking_demand)
maneuver_share = 1.0 if wheel_beyond_target else \
float(np.interp(maneuver_demand, _CENTERING_CURVATURE_BASEBAND, (0.0, 1.0)))
path_offset *= maneuver_share
path_angle *= maneuver_share
centering_curvature = action_curvature
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(centering_curvature * (1.0 - maneuver_share), *DBC_CURVATURE)),
curvature_rate=float(np.clip(model_curvature_rate * maneuver_share, *DBC_CURVATURE_RATE)),
)
class FordPathController:
"""Convert the model path directly into one vehicle-independent Ford path command."""
def __init__(self, dt: float = 0.01):
self.dt = dt
self._last_path = FordPath(valid=True)
def _limit(self, target: FordPath) -> FordPath:
values = []
for field, rate in zip(fields(FordPath)[1:], _PATH_RATES, strict=True):
previous = getattr(self._last_path, field.name)
value = getattr(target, field.name)
values.append(float(np.clip(value, previous - rate * self.dt, previous + rate * self.dt)))
self._last_path = FordPath(True, *values)
return self._last_path
def update(self, model, desired_curvature: float, *, v_ego: float = 0.0, active: bool = True,
current_curvature: float | None = None) -> FordPath:
if not active:
self._last_path = FordPath(valid=True)
return FordPath()
if model is None:
return self._limit(FordPath(valid=True))
return self._limit(_encode_path(model, desired_curvature, v_ego, current_curvature))
@@ -0,0 +1,290 @@
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_CURVATURE, FordPathController
def _path(curvature: float, curvature_rate: float = 0.0, speed: float = 8.0):
t = np.linspace(0.0, 3.0, 61)
distance = speed * t
heading = curvature * distance + 0.5 * curvature_rate * distance ** 2
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 _equivalent_curvature(path, distance: float = 7.0) -> float:
offset = path.path_offset + path.path_angle * distance + 0.5 * path.curvature * distance ** 2 + \
path.curvature_rate * distance ** 3 / 6.0
return 2.0 * offset / distance ** 2
def _command(model, desired_curvature: float, *, v_ego: float = 0.0, current_curvature: float | None = None):
return FordPathController(dt=1.0).update(model, desired_curvature, v_ego=v_ego, current_curvature=current_curvature)
def test_steady_arc_uses_c2_without_fast_pose_fields():
path = _command(_path(0.008), 0.008, v_ego=8.0)
assert path.valid
assert abs(path.path_offset) < 1e-9
assert abs(path.path_angle) < 1e-9
assert np.isclose(path.curvature, 0.008, atol=5e-5)
assert abs(path.curvature_rate) < 1e-5
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
def test_tight_arc_uses_signed_forward_pose_without_slow_c2():
left = _command(_path(0.04), 0.04, v_ego=8.0)
right = _command(_path(-0.04), -0.04, v_ego=8.0)
assert left.curvature == 0.0
assert right.curvature == 0.0
assert abs(left.curvature_rate) < 1e-4
assert abs(right.curvature_rate) < 1e-4
assert left.path_angle > 0.06
assert right.path_angle < -0.06
assert left.path_offset > 0.5
assert right.path_offset < -0.5
def test_c2_does_not_increase_while_tight_curve_unwinds():
curvatures = (0.04, 0.018, 0.016, 0.014, 0.012, 0.010, 0.008, 0.006, 0.0)
measured = (0.04,) + curvatures[:-1]
commands = [_command(_path(curvature), curvature, v_ego=8.0, current_curvature=actual).curvature
for curvature, actual in zip(curvatures, measured, strict=True)]
assert np.all(np.diff(commands) <= 1e-9)
def test_fresh_model_replaces_previous_path_without_hidden_state():
controller = FordPathController(dt=1.0)
initial = controller.update(_path(0.04), 0.04, v_ego=8.0)
replanned = controller.update(_path(0.0), 0.0, v_ego=8.0)
assert initial.path_offset > 0.5
assert replanned == FordPathController().update(_path(0.0), 0.0, v_ego=8.0)
def test_s_turn_reverses_fast_fields_while_c2_is_bounded():
controller = FordPathController(dt=0.05)
controller.update(_path(0.04), 0.04, v_ego=8.0)
controller.update(_path(0.04), 0.04, v_ego=8.0)
outputs = []
for frame_id in range(5):
model = _path(-0.02)
model.frameId = frame_id + 1
model.timestampEof = frame_id + 1
outputs.append(controller.update(model, -0.02, v_ego=8.0, current_curvature=0.02))
assert all(path.valid for path in outputs)
assert all(DBC_CURVATURE[0] <= path.curvature <= DBC_CURVATURE[1] for path in outputs)
assert all(path.curvature <= 0.0 for path in outputs)
assert outputs[-1].path_angle < -0.03
assert outputs[-1].path_offset < 0.0
def test_reversal_does_not_add_software_persistence_to_centering_c2():
controller = FordPathController()
assert controller.update(_path(0.002), 0.002, v_ego=8.0).curvature > 0.0
reversing = controller.update(_path(-0.02), -0.02, v_ego=8.0)
assert reversing.curvature <= 0.0
def test_requested_turn_is_not_cancelled_by_previous_path():
controller = FordPathController(dt=1.0)
previous = _path(-0.02, speed=3.0)
previous.frameId = 1
previous.timestampEof = 1
controller.update(previous, -0.02, v_ego=3.0, current_curvature=-0.01)
requested = _path(0.02, speed=3.0)
requested.frameId = 2
requested.timestampEof = 2
command = controller.update(requested, 0.02, v_ego=3.0, current_curvature=0.007)
assert command.path_offset >= 0.0
assert command.path_angle >= 0.0
assert command.curvature >= 0.0
assert _equivalent_curvature(command) >= 0.02
def test_short_low_speed_model_uses_available_path_endpoint():
command = FordPathController().update(_path(0.02, speed=1.0), 0.02, v_ego=1.0, current_curvature=0.0)
assert command.valid
assert command.path_offset > 0.0
assert command.path_angle > 0.0
def test_action_demand_exposes_forward_path_authority():
command = FordPathController().update(_path(0.002), 0.0055, v_ego=8.0, current_curvature=0.0005)
assert _equivalent_curvature(command) >= 0.004
def test_minor_curve_uses_c2_when_tracking_is_close():
command = FordPathController(dt=1.0).update(_path(0.005), 0.005, v_ego=8.0, current_curvature=0.0048)
assert abs(command.path_offset) < 1e-9
assert abs(command.path_angle) < 1e-9
assert command.curvature > 0.0049
def test_minor_changing_curve_does_not_emit_ungated_c3():
command = FordPathController(dt=1.0).update(_path(0.005, 0.0003), 0.005, v_ego=8.0, current_curvature=0.0048)
assert abs(command.path_offset) < 1e-9
assert abs(command.path_angle) < 1e-9
assert command.curvature > 0.0049
assert command.curvature_rate == 0.0
def test_c2_uses_stable_action_curvature_not_independent_model_fit():
controller = FordPathController(dt=1.0)
first = controller.update(_path(0.004), 0.002, v_ego=8.0, current_curvature=0.002)
second = controller.update(_path(0.006), 0.002, v_ego=8.0, current_curvature=0.002)
assert np.isclose(first.curvature, 0.002)
assert np.isclose(second.curvature, 0.002)
def test_action_curvature_corrects_stale_opposing_model_at_low_speed():
command = FordPathController().update(_path(-0.001, speed=1.0), 0.005, v_ego=1.0, current_curvature=0.001)
assert command.path_offset > 0.0
assert command.path_angle > 0.0
assert command.curvature >= 0.0
assert _equivalent_curvature(command) >= 0.004
def test_small_action_sign_noise_does_not_reverse_a_strong_model_path():
command = FordPathController(dt=1.0).update(_path(0.04), -0.0005, v_ego=6.0, current_curvature=0.02)
assert command.path_offset > 0.0
assert command.path_angle > 0.0
assert _equivalent_curvature(command) > 0.02
def test_measured_curvature_after_path_exit_commands_countersteer():
command = FordPathController().update(_path(0.0), 0.0, v_ego=8.0, current_curvature=0.006)
assert command.curvature == 0.0
assert command.path_offset < 0.0
assert command.path_angle < 0.0
def test_measured_curvature_countersteers_when_beyond_modeled_arc():
controller = FordPathController(dt=1.0)
command = controller.update(_path(0.004), 0.003, v_ego=8.0, current_curvature=0.012)
tracking = FordPathController(dt=1.0).update(_path(0.004), 0.003, v_ego=8.0, current_curvature=0.004)
assert command.path_offset < 0.0
assert command.path_angle < 0.0
assert command.path_angle < tracking.path_angle
assert command.curvature == 0.0
def test_model_reversal_suppresses_old_c2_and_countersteers():
reversing = FordPathController().update(_path(-0.02), -0.0005, v_ego=8.0, current_curvature=0.01)
assert reversing.curvature <= 0.0
assert reversing.path_angle < 0.0
def test_reversal_noise_band_is_continuous():
inside = FordPathController(dt=1.0).update(_path(0.02), -0.000099, v_ego=8.0, current_curvature=0.01)
outside = FordPathController(dt=1.0).update(_path(0.02), -0.000101, v_ego=8.0, current_curvature=0.01)
assert abs(outside.path_angle - inside.path_angle) < 0.005
def test_curvature_error_increases_forward_pose_command_while_behind():
behind = FordPathController(dt=1.0).update(_path(0.008), 0.008, v_ego=15.0, current_curvature=0.0)
tracking = FordPathController(dt=1.0).update(_path(0.008), 0.008, v_ego=15.0, current_curvature=0.008)
assert behind.path_offset > tracking.path_offset + 0.01
assert behind.path_angle > tracking.path_angle + 0.015
assert behind.curvature == 0.0
assert tracking.curvature > 0.007
assert np.isclose(behind.curvature_rate, tracking.curvature_rate)
def test_measured_wheel_beyond_action_countersteers_model_arc():
controller = FordPathController()
controller.update(_path(0.02), 0.02, v_ego=15.0, current_curvature=0.02)
outputs = [controller.update(_path(0.02), 0.003, v_ego=15.0, current_curvature=0.01) for _ in range(4)]
unwinding = outputs[-1]
assert unwinding.curvature == 0.0
assert unwinding.path_angle < 0.0
def test_action_c2_remains_active_for_centering():
centering = FordPathController(dt=1.0).update(_path(0.002), 0.002, v_ego=15.0, current_curvature=0.002)
assert centering.curvature > 0.001
assert abs(centering.path_offset) < 1e-9
assert abs(centering.path_angle) < 1e-9
def test_tight_turn_from_stop_builds_bounded_forward_pose_authority():
controller = FordPathController()
outputs = [controller.update(_path(0.04), 0.04, v_ego=0.0, current_curvature=0.0) for _ in range(20)]
path = outputs[-1]
assert path.curvature == 0.0
assert path.path_offset > 0.7
assert path.path_angle > 0.15
assert np.max(np.abs(np.diff([output.path_offset for output in outputs]))) <= 0.04 + 1e-9
assert np.max(np.abs(np.diff([output.path_angle for output in outputs]))) <= 0.01 + 1e-9
def test_curvature_feedback_is_bounded_for_bad_measurement():
bounded = FordPathController().update(_path(0.008), 0.008, v_ego=15.0, current_curvature=-0.02)
corrupted = FordPathController().update(_path(0.008), 0.008, v_ego=15.0, current_curvature=-1.0)
assert np.isclose(corrupted.path_angle, bounded.path_angle)
def test_invalid_model_ramps_pose_to_zero_while_remaining_in_extended_mode():
controller = FordPathController()
for _ in range(10):
active = controller.update(_path(0.04), 0.04, v_ego=12.0)
missing = controller.update(None, 0.0, v_ego=12.0)
assert active.path_offset > 0.0
assert missing.valid
assert np.isclose(active.path_offset - missing.path_offset, 0.04)
assert missing.curvature == 0.0
assert not controller.update(_path(0.0), 0.0, v_ego=12.0, active=False).valid
+13 -13
View File
@@ -7,7 +7,7 @@ from openpilot.common.file_chunker import chunk_file, get_chunk_targets, get_exi
from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE, DM_INPUT_SIZE
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.modeld.helpers import TG_INPUT_DEVICES_PATH, chestnut_present, modeld_pkl_path
from openpilot.selfdrive.modeld.helpers import TG_INPUT_DEVICES_PATH, usbgpu_present, modeld_pkl_path
CAMERA_CONFIGS = [
@@ -36,18 +36,18 @@ else:
tg_devices = { # which device to put jit inputs to at runtime
'openpilot.selfdrive.modeld.modeld': {
'default': {'WARP_DEV': tg_backend, 'QUEUE_DEV': tg_backend},
'chestnut': {'WARP_DEV': tg_backend, 'QUEUE_DEV': 'AMD'}
'usbgpu': {'WARP_DEV': tg_backend, 'QUEUE_DEV': 'AMD'}
},
'openpilot.selfdrive.modeld.dmonitoringmodeld': {
'default': {'DEV': tg_backend}
},
}
CHESTNUT = chestnut_present()
if CHESTNUT:
chestnut_tg_flags = f'DEBUG=2 DEV=USB+AMD:LLVM WARP_DEV={tg_backend} FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2'
USBGPU = usbgpu_present()
if USBGPU:
usbgpu_tg_flags = f'DEBUG=2 DEV=USB+AMD:LLVM WARP_DEV={tg_backend} FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2'
# the USB+AMD GPU takes an exclusive flock; serialize all targets that touch it
chestnut_lock = File("models/.chestnut.lock").abspath
usbgpu_lock = File("models/.usb_gpu.lock").abspath
def write_tg_devices(target, source, env):
with open(str(target[0]), "w") as f:
@@ -74,10 +74,10 @@ model_w, model_h = MEDMODEL_INPUT_SIZE
frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
if not os.getenv('SKIP_TINYGRAD_COMPILE'):
for chestnut in [False, True] if CHESTNUT else [False]:
target_pkl_path = File(modeld_pkl_path(chestnut)).abspath
# BIG_INTO_SMALL=1 builds the default target from the big model, e.g. to test it without a chestnut
file_prefix, cmd_flags = ('big_', chestnut_tg_flags) if chestnut else ('big_' if os.getenv('BIG_INTO_SMALL') else '', tg_flags)
for usbgpu in [False, True] if USBGPU else [False]:
target_pkl_path = File(modeld_pkl_path(usbgpu)).abspath
# BIG_INTO_SMALL=1 builds the default target from the big model, e.g. to test it without a USB GPU
file_prefix, cmd_flags = ('big_', usbgpu_tg_flags) if usbgpu else ('big_' if os.getenv('BIG_INTO_SMALL') else '', tg_flags)
driving_onnx_deps = get_existing_chunks(File(f"models/{file_prefix}driving_supercombo.onnx").abspath)
camera_res_args = ' '.join(f'{cw}x{ch}' for cw, ch in CAMERA_CONFIGS)
# CPU 7 is isolated with isolcpus on AGNOS, so explicitly pin the compiler to it.
@@ -104,14 +104,14 @@ if not os.getenv('SKIP_TINYGRAD_COMPILE'):
chunk_file(pkl, chunks)
def do_chunk(target, source, env, pkl=target_pkl_path, chunks=chunk_targets):
chunk_file(pkl, chunks)
actions = Action(do_compile, " [CHESTNUT] $TARGET") if chestnut else [cmd, Action(do_chunk, " [CHUNK] $TARGET")]
actions = Action(do_compile, " [USBGPU] $TARGET") if usbgpu else [cmd, Action(do_chunk, " [CHUNK] $TARGET")]
node = lenv.Command(
chunk_targets,
tinygrad_files + compile_modeld_script + driving_onnx_deps + [Value(chunk_targets), chunker_file],
actions,
)
if chestnut:
lenv.SideEffect(chestnut_lock, node)
if usbgpu:
lenv.SideEffect(usbgpu_lock, node)
# get model metadata
fn = File(f"models/dmonitoring_model").abspath
@@ -29,7 +29,7 @@ class ModelState:
output: np.ndarray
def __init__(self, cam_w: int, cam_h: int):
self.DEV = get_tg_input_devices(PROCESS_NAME, chestnut=False)['DEV']
self.DEV = get_tg_input_devices(PROCESS_NAME, usbgpu=False)['DEV']
with open(METADATA_PATH, 'rb') as f:
model_metadata = pickle.load(f)
self.input_shapes = model_metadata['input_shapes']
+7 -7
View File
@@ -13,12 +13,12 @@ MODELS_DIR = Path(__file__).resolve().parent / 'models'
TG_INPUT_DEVICES_PATH = MODELS_DIR / 'tg_input_devices.json'
def get_tg_input_devices(process_name: str, chestnut: bool):
def get_tg_input_devices(process_name: str, usbgpu: bool):
with open(TG_INPUT_DEVICES_PATH) as f:
return json.load(f)[process_name]['default' if not chestnut else 'chestnut']
return json.load(f)[process_name]['default' if not usbgpu else 'usbgpu']
def modeld_pkl_path(chestnut: bool):
prefix = 'big_' if chestnut else ''
def modeld_pkl_path(usbgpu: bool):
prefix = 'big_' if usbgpu else ''
return MODELS_DIR / f'{prefix}driving_tinygrad.pkl'
def dump_oob(obj, f):
@@ -45,7 +45,7 @@ def load_oob(f):
yield pb
return pickle.load(io.BytesIO(opcodes), buffers=buffers())
def chestnut_present() -> bool:
def usbgpu_present() -> bool:
for d in USB_DEVICES_PATH.glob("*"):
try:
usb_id = (int((d / "idVendor").read_text(), 16), int((d / "idProduct").read_text(), 16))
@@ -56,5 +56,5 @@ def chestnut_present() -> bool:
pass
return False
def chestnut_compiled() -> bool:
return Path(get_manifest_path(modeld_pkl_path(chestnut=True))).is_file()
def usbgpu_compiled() -> bool:
return Path(get_manifest_path(modeld_pkl_path(usbgpu=True))).is_file()
+30 -32
View File
@@ -1,9 +1,7 @@
#!/usr/bin/env python3
from collections.abc import Callable
import ctypes
from functools import cached_property
import os
os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom
os.environ['GMMU'] = '0' # for usbgpu fast loading, noop for qcom
from tinygrad.tensor import Tensor
from tinygrad.device import Device
import struct
@@ -32,7 +30,7 @@ from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, WARP_IN
from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_driving_model_data, fill_pose_msg, PublishState
from openpilot.common.file_chunker import open_file_chunked
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
from openpilot.selfdrive.modeld.helpers import chestnut_present, chestnut_compiled, modeld_pkl_path, get_tg_input_devices, load_oob
from openpilot.selfdrive.modeld.helpers import usbgpu_present, usbgpu_compiled, modeld_pkl_path, get_tg_input_devices, load_oob
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
@@ -96,10 +94,8 @@ class ChestnutState:
if self.big and "AMD" in Device._opened_devices and self.sends % 100 == 1:
try:
smu = Device["AMD"].iface.dev_impl.smu
metrics_t = smu.smu_mod.SmuMetricsExternal_t
smu._send_msg(smu.smu_mod.PPSMC_MSG_TransferTableSmu2Dram, smu.smu_mod.TABLE_SMU_METRICS, timeout=100)
metrics_buf = bytearray(smu.adev.vram.view(smu.driver_table_paddr, ctypes.sizeof(metrics_t))[:])
metrics = metrics_t.from_buffer(metrics_buf).SmuMetrics
metrics = smu.read_table(smu.smu_mod.SmuMetricsExternal_t, smu.smu_mod.TABLE_SMU_METRICS).SmuMetrics
self.metrics = {'tempC': metrics.AvgTemperature[smu.smu_mod.TEMP_HOTSPOT],
'memoryTempC': metrics.AvgTemperature[smu.smu_mod.TEMP_MEM],
'powerDrawW': metrics.AverageSocketPower,
@@ -145,18 +141,18 @@ class FrameMeta:
class ModelState(ModelStateBase):
prev_desire: np.ndarray # for tracking the rising edge of the pulse
def __init__(self, cam_w: int, cam_h: int, chestnut: bool):
def __init__(self, cam_w: int, cam_h: int, usbgpu: bool):
ModelStateBase.__init__(self)
input_devices = get_tg_input_devices(PROCESS_NAME, chestnut)
input_devices = get_tg_input_devices(PROCESS_NAME, usbgpu)
self.WARP_DEV, self.QUEUE_DEV = input_devices['WARP_DEV'], input_devices['QUEUE_DEV']
jits = load_oob(open_file_chunked(modeld_pkl_path(chestnut)))
jits = load_oob(open_file_chunked(modeld_pkl_path(usbgpu)))
metadata = jits['metadata']
self.input_shapes = metadata['input_shapes']
self.vision_input_names = [k for k in self.input_shapes if 'img' in k]
self.output_slices = metadata['output_slices']
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
self.chestnut = chestnut
self.usbgpu = usbgpu
self.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
self.input_queues, self.npy = make_input_queues(self.input_shapes, self.frame_skip, device=self.QUEUE_DEV)
@@ -172,7 +168,7 @@ class ModelState(ModelStateBase):
return parsed_model_outputs
def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
inputs: dict[str, np.ndarray], after_enqueue: Callable[[], None] | None = None) -> dict[str, np.ndarray]:
inputs: dict[str, np.ndarray]) -> dict[str, np.ndarray] | None:
for key in bufs.keys():
ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data
yuv_size = self.frame_buf_params[key][3]
@@ -196,11 +192,11 @@ class ModelState(ModelStateBase):
outs, = self.run_policy(
**{k: self.input_queues[k] for k in POLICY_INPUTS if k in self.input_queues}, warped=warped
)
if after_enqueue is not None:
after_enqueue()
model_output = outs.numpy()[0]
if self.chestnut and not np.all(np.isfinite(model_output)):
raise RuntimeError("model output not finite")
if self.usbgpu and not np.all(np.isfinite(model_output)):
# TODO remove with prev_feat
cloudlog.error("model output not finite, dropping frame")
return None
outputs_dict = self.parser.parse_outputs(self.slice_outputs(model_output, self.output_slices))
self.npy['prev_feat'][:] = model_output[self.output_slices['hidden_state']]
@@ -222,12 +218,12 @@ class ModelState(ModelStateBase):
def main(demo=False):
cloudlog.warning("modeld init")
CHESTNUT = chestnut_present() and chestnut_compiled()
if CHESTNUT:
USBGPU = usbgpu_present() and usbgpu_compiled()
if USBGPU:
os.environ['HCQDEV_WAIT_TIMEOUT_MS'] = '3000'
params = Params()
params.put_bool("ChestnutLoading", CHESTNUT)
params.remove("ChestnutActive")
params.put_bool("UsbGpuLoading", USBGPU)
params.remove("UsbGpuActive")
config_realtime_process(7, 54)
@@ -257,7 +253,7 @@ def main(demo=False):
st = time.monotonic()
cloudlog.warning("loading model")
model = None
if CHESTNUT:
if USBGPU:
big_model = None
def load_big():
nonlocal big_model
@@ -271,23 +267,23 @@ def main(demo=False):
loader.start()
loader.join(BIG_MODEL_TIMEOUT)
model = big_model
params.put_bool("ChestnutActive", model is not None)
params.put_bool("UsbGpuActive", model is not None)
small_model = ModelState(vipc_client_main.width, vipc_client_main.height, False) if model is None or CHESTNUT else None
small_model = ModelState(vipc_client_main.width, vipc_client_main.height, False) if model is None or USBGPU else None
if model is None:
model = small_model
params.put_bool("ChestnutLoading", False)
params.put_bool("UsbGpuLoading", False)
assert model is not None
cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
# messaging
pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutState"] if CHESTNUT else [])
pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutState"] if USBGPU else [])
pm = PubMaster(pub_socks)
sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"])
publish_state = PublishState()
params = Params()
chestnut_state = ChestnutState(pm, model.chestnut) if CHESTNUT else None
chestnut_state = ChestnutState(pm, model.usbgpu) if USBGPU else None
# setup filter to track dropped frames
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ)
@@ -397,15 +393,13 @@ def main(demo=False):
mt1 = time.perf_counter()
try:
send_chestnut = (chestnut_state is not None and
run_count % round(ModelConstants.MODEL_RUN_FREQ / SERVICE_LIST['chestnutState'].frequency) == 0)
model_output = model.run(bufs, transforms, inputs, chestnut_state.send if send_chestnut else None)
model_output = model.run(bufs, transforms, inputs)
except Exception:
if not params.get_bool("ChestnutActive"):
if not params.get_bool("UsbGpuActive"):
raise
# fallback to small model
cloudlog.exception("big model failed, fall back to small")
params.put_bool("ChestnutActive", False)
params.put_bool("UsbGpuActive", False)
assert small_model is not None
model = small_model
if chestnut_state is not None:
@@ -425,7 +419,7 @@ def main(demo=False):
fill_model_msg(modelv2_send, model_output, action,
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, extrinsics_calibration_seen)
modelv2_send.modelV2.big = model.chestnut
modelv2_send.modelV2.big = model.usbgpu
desire_state = modelv2_send.modelV2.meta.desireState
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
@@ -447,6 +441,10 @@ def main(demo=False):
pm.send('modelDataV2SP', mdv2sp_send)
last_vipc_frame_id = meta_main.frame_id
if chestnut_state is not None and run_count % round(ModelConstants.MODEL_RUN_FREQ / SERVICE_LIST['chestnutState'].frequency) == 0:
chestnut_state.send()
if __name__ == "__main__":
try:
import argparse
+70 -5
View File
@@ -32,7 +32,14 @@ from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP
from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper
from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement
from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
from openpilot.sunnypilot.selfdrive.selfdrived.assisted_driving_milestones import (
AssistCategory,
AssistedDrivingMilestones,
MilestoneEvent,
MilestoneStore,
)
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
from openpilot.sunnypilot.system.statsd import statlog
REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
@@ -88,7 +95,8 @@ class SelfdriveD(CruiseHelper):
self.big_model_ready_t = 0.
# Setup sockets
self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] + ['selfdriveStateSP', 'onroadEventsSP'])
self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] +
['selfdriveStateSP', 'onroadEventsSP', 'assistedDrivingMilestoneState'])
self.gps_location_service = get_gps_location_service(self.params)
self.gps_packets = [self.gps_location_service]
@@ -127,6 +135,7 @@ class SelfdriveD(CruiseHelper):
self.params.remove("ExperimentalMode")
self.CS_prev = car.CarState.new_message()
self.car_state_log_mono_time = 0
self.AM = AlertManager()
self.events = Events()
@@ -137,6 +146,11 @@ class SelfdriveD(CruiseHelper):
self.cruise_mismatch_counter = 0
self.last_steering_pressed_frame = 0
self.distance_traveled = 0
self.assisted_driving_milestones = AssistedDrivingMilestones(MilestoneStore(self.params))
self.assisted_driving_milestones_enabled = bool(self.params.get("AssistedDrivingMilestonesEnabled", return_default=True))
self.assisted_driving_milestone_drive_id = ""
self._milestone_event: MilestoneEvent | None = None
self._milestone_event_expires_ns = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.logged_comm_issue = None
@@ -195,17 +209,17 @@ class SelfdriveD(CruiseHelper):
self.events.add(EventName.joystickDebug)
self.startup_event = None
loading = self.params.get_bool("ChestnutLoading")
loading = self.params.get_bool("UsbGpuLoading")
if self.big_model_loading and not loading:
self.big_model_ready_t = time.monotonic()
self.big_model_loading = loading
if self.big_model_loading:
self.events.add(EventName.bigModelLoading)
big_active = self.params.get("ChestnutActive")
chestnut_present = self.sm['deviceState'].chestnutPresent
big_active = self.params.get("UsbGpuActive")
usbgpu_present = self.sm['deviceState'].chestnutPresent
model_unavailable = big_active is True and self.sm.seen['modelV2'] and not self.sm.alive['modelV2']
big_failed = big_active is False or model_unavailable or (self.big_model_active and not chestnut_present)
big_failed = big_active is False or model_unavailable or (self.big_model_active and not usbgpu_present)
if big_failed and not self.big_model_failed:
self.events.add(EventName.bigModelFailed)
self.big_model_failed = big_failed
@@ -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()
+8 -8
View File
@@ -9,7 +9,7 @@ from openpilot.system.ui.widgets.layouts import HBoxLayout
from openpilot.system.ui.widgets.icon_widget import IconWidget
from openpilot.system.ui.widgets.label import UnifiedLabel, gui_label
from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
from openpilot.selfdrive.ui.ui_state import ui_state, ChestnutState
from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.common.version import RELEASE_BRANCHES
HEAD_BUTTON_FONT_SIZE = 40
@@ -139,8 +139,8 @@ class MiciHomeLayout(Widget):
self._version_text = self._get_version_text()
self._experimental_icon = IconWidget("icons_mici/experimental_mode.png", (48, 48))
self._chestnut_icon = IconWidget("icons_mici/chestnut_green.png", (68, 40))
self._chestnut_failed_icon = IconWidget("icons_mici/chestnut_orange.png", (68, 40))
self._egpu_icon = IconWidget("icons_mici/egpu_green.png", (50, 37))
self._egpu_icon_gray = IconWidget("icons_mici/egpu_gray.png", (50, 37))
self._mic_icon = IconWidget("icons_mici/microphone.png", (32, 46))
self._body_icon = IconWidget("icons_mici/body.png", (54, 37))
@@ -150,8 +150,8 @@ class MiciHomeLayout(Widget):
IconWidget("icons_mici/settings.png", (48, 48), opacity=0.9),
NetworkIcon(),
self._experimental_icon,
self._chestnut_icon,
self._chestnut_failed_icon,
self._egpu_icon,
self._egpu_icon_gray,
self._body_icon,
self._mic_icon,
], spacing=18)
@@ -249,10 +249,10 @@ class MiciHomeLayout(Widget):
# ***** Center-aligned bottom section icons *****
self._experimental_icon.set_visible(ui_state.experimental_mode)
if gui_app.sunnypilot_ui():
self._set_chestnut_visibility()
self._set_egpu_visibility()
else:
self._chestnut_icon.set_visible(ui_state.chestnut_state in (ChestnutState.READY, ChestnutState.LOADING, ChestnutState.ACTIVE))
self._chestnut_failed_icon.set_visible(ui_state.chestnut_state in (ChestnutState.UNCOMPILED, ChestnutState.FAILED))
self._egpu_icon.set_visible(ui_state.sm["deviceState"].chestnutPresent and ui_state.usbgpu_compiled)
self._egpu_icon_gray.set_visible(ui_state.sm["deviceState"].chestnutPresent and not ui_state.usbgpu_compiled)
self._mic_icon.set_visible(ui_state.recording_audio)
self._body_icon.set_visible(bool(ui_state.is_body))
+7 -1
View File
@@ -1,5 +1,8 @@
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
@@ -61,7 +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))
if not self._onboarding_window.completed:
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
@@ -119,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),
@@ -20,6 +20,7 @@ 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
@@ -279,7 +280,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 * 0.90 * self._alpha_filter.x))
color = rl.Color(color.r, color.g, color.b, int(255 * ALERT_BACKGROUND_OPACITY * 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,10 +19,15 @@ 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
@@ -156,6 +161,7 @@ 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,
@@ -223,6 +229,12 @@ 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))
@@ -247,7 +259,6 @@ 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
@@ -355,10 +366,12 @@ 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)
@@ -3,7 +3,7 @@ import pyray as rl
from dataclasses import dataclass
from openpilot.common.constants import CV
from openpilot.selfdrive.ui.mici.onroad.torque_bar import TorqueBar
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus, ChestnutState
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
from openpilot.system.ui.lib.application import gui_app, FontWeight
from openpilot.system.ui.lib.multilang import tr
from openpilot.system.ui.lib.text_measure import measure_text_cached
@@ -107,7 +107,8 @@ class HudRenderer(Widget):
self.speed: float = 0.0
self.v_ego_cluster_seen: bool = False
self._engaged: bool = False
self._chestnut_fade_time: float = 0
self._small_model_engaged: bool = False
self._egpu_fade_time: float = 0
self._can_draw_top_icons = True
self._show_wheel_critical = False
@@ -123,15 +124,17 @@ class HudRenderer(Widget):
self._txt_wheel: rl.Texture = gui_app.texture('icons_mici/wheel.png', 50, 50)
self._txt_wheel_critical: rl.Texture = gui_app.texture('icons_mici/wheel_critical.png', 50, 50)
self._txt_exclamation_point: rl.Texture = gui_app.texture('icons_mici/exclamation_point.png', 9, 44)
self._txt_chestnut: rl.Texture = gui_app.texture('icons_mici/chestnut.png', 60, 44)
self._txt_chestnut_green: rl.Texture = gui_app.texture('icons_mici/chestnut_green.png', 60, 44)
self._txt_chestnut_orange: rl.Texture = gui_app.texture('icons_mici/chestnut_orange.png', 75, 44)
self._chestnut_icon: rl.Texture | None = None
self._txt_egpu: rl.Texture = gui_app.texture('icons_mici/egpu.png', 60, 44)
self._txt_egpu_green: rl.Texture = gui_app.texture('icons_mici/egpu_green.png', 60, 44)
self._txt_egpu_orange: rl.Texture = gui_app.texture('icons_mici/egpu_orange.png', 60, 44)
self._txt_egpu_crossed: rl.Texture = gui_app.texture('icons_mici/egpu_crossed.png', 60, 52)
self._egpu_icon: rl.Texture | None = None
self._wheel_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps)
self._wheel_y_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps)
self._set_speed_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
self._chestnut_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
self._egpu_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
def set_wheel_critical_icon(self, critical: bool):
"""Set the wheel icon to critical or normal state."""
@@ -162,10 +165,13 @@ class HudRenderer(Widget):
controls_state.deprecated.vCruise if v_cruise_cluster == 0.0 else v_cruise_cluster
)
engaged = sm['selfdriveState'].enabled
if (engaged and not self._engaged and not ui_state.usbgpu_loading and ui_state.usbgpu_active is not True and
ui_state.sm.recv_frame['modelV2'] > ui_state.started_frame):
self._small_model_engaged = True
if engaged != self._engaged:
self._egpu_fade_time = rl.get_time() if engaged else 0
if (set_speed != self.set_speed and engaged) or (engaged and not self._engaged):
self._set_speed_changed_time = rl.get_time()
if engaged != self._engaged:
self._chestnut_fade_time = rl.get_time() if engaged else 0
self._engaged = engaged
self.set_speed = set_speed
self.is_cruise_set = 0 < self.set_speed < SET_SPEED_NA
@@ -185,7 +191,8 @@ class HudRenderer(Widget):
if self.is_cruise_set:
self._draw_set_speed(rect)
self._draw_model_source(rect)
if ui_state.usbgpu and ui_state.usbgpu_compiled:
self._draw_model_source(rect)
self._draw_steering_wheel(rect)
@@ -193,24 +200,30 @@ class HudRenderer(Widget):
if ui_state.sm.recv_frame['selfdriveState'] < ui_state.started_frame:
return
loading = ui_state.chestnut_state == ChestnutState.LOADING
big_failed = (ui_state.usbgpu_active is False or not ui_state.sm['deviceState'].chestnutPresent or
(ui_state.usbgpu_active is True and ui_state.sm.recv_frame['modelV2'] > ui_state.started_frame and
not ui_state.sm.alive['modelV2']) or
(ui_state.usbgpu_active is None and ui_state.sm.recv_frame['modelV2'] > ui_state.started_frame))
self._small_model_engaged &= big_failed
loading = ui_state.usbgpu_loading or (ui_state.usbgpu_active is None and not big_failed)
if loading:
icon = self._txt_chestnut
opacity = 0.35 + 0.65 * (0.5 - 0.5 * math.cos(rl.get_time() * 6.0))
elif ui_state.chestnut_state in (ChestnutState.UNCOMPILED, ChestnutState.FAILED):
icon = self._txt_chestnut_orange
opacity = 1.0
elif ui_state.chestnut_state == ChestnutState.ACTIVE:
icon = self._txt_chestnut_green
pulse = 0.5 - 0.5 * math.cos(rl.get_time() * 6.0)
icon = self._txt_egpu
opacity = 0.35 + 0.65 * pulse
elif self._small_model_engaged:
icon = self._txt_egpu_crossed
opacity = 0.65
elif big_failed:
icon = self._txt_egpu_orange
opacity = 1.0
else:
return
icon = self._txt_egpu_green
opacity = 1.0
if icon is not self._chestnut_icon:
self._chestnut_fade_time = rl.get_time()
self._chestnut_icon = icon
visible = loading or rl.get_time() - self._chestnut_fade_time < SET_SPEED_PERSISTENCE
alpha = self._chestnut_alpha_filter.update(visible)
if icon is not self._egpu_icon:
self._egpu_fade_time = rl.get_time()
self._egpu_icon = icon
alpha = self._egpu_alpha_filter.update(loading or 0 < rl.get_time() - self._egpu_fade_time < SET_SPEED_PERSISTENCE)
if alpha < 1e-2:
return
+25 -9
View File
@@ -24,14 +24,8 @@ 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
@@ -53,6 +47,7 @@ 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),
@@ -60,6 +55,14 @@ 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']
@@ -74,6 +77,7 @@ class Soundd(QuietMode):
def __init__(self):
super().__init__()
self.device_type = HARDWARE.get_device_type()
self.load_sounds()
self.current_alert = AudibleAlert.none
@@ -85,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)
@@ -164,9 +169,19 @@ 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):
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))
return calculate_volume_for_device(weighted_db, self.device_type)
@retry(attempts=10, delay=3)
def get_stream(self, sd):
@@ -180,7 +195,7 @@ class Soundd(QuietMode):
import sounddevice as sd
micd.patch_sounddevice(sd)
sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure'])
sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure', 'assistedDrivingMilestoneState'])
with self.get_stream(sd) as stream:
rk = Ratekeeper(20)
@@ -198,6 +213,7 @@ 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:
@@ -62,14 +62,14 @@ class ModelsLayout(Widget):
self.big_model_item = ListItemSP(
title=tr("Big Model"),
action_item=ScrollingButtonAction(tr("SELECT")),
callback=lambda: self._open_source_dialog("chestnut")
callback=lambda: self._open_source_dialog("usbgpu")
)
self.download_item = download_status_item(lambda: tr("Download") if self._downloading else tr("Model Status"))
self.refresh_item = button_item(tr("Refresh Model List"), tr("REFRESH"), "",
lambda: (ui_state.params.put("ModelManager_LastSyncTime", 0),
ui_state.params.put("ModelManager_LastSyncTime_Chestnut", 0),
ui_state.params.put("ModelManager_LastSyncTime_USBGPU", 0),
gui_app.push_widget(alert_dialog(tr("Fetching Latest Models")))))
self.clear_cache_item = ListItemSP(
@@ -177,14 +177,14 @@ class ModelsLayout(Widget):
big_state = big_model_state()
carry_source, carry_internal, _ = carrying_model()
segments = []
for source, label in (("qcom", tr("small")), ("chestnut", tr("big"))):
for source, label in (("qcom", tr("small")), ("usbgpu", tr("big"))):
if segments:
segments.append(("|", rl.GRAY, None, None))
bundle = get_selected_bundle(ui_state.params, source)
name = bundle.internalName if bundle else default_model_name(source)
color = ON_COLOR if (source == carry_source and name == carry_internal) else rl.LIGHTGRAY
name = "" + name
if source == "chestnut":
if source == "usbgpu":
if big_state == 'failed':
color = rl.RED
elif big_state == 'loading':
@@ -208,10 +208,10 @@ class ModelsLayout(Widget):
"""The failover story for the Model Status row. One-way big -> small, and the
fallback is runner-matched: a Default big can only fall back to the Default
small (stock modeld), a custom big has no automatic fallback yet."""
if not ui_state.chestnut_present:
if not ui_state.usbgpu:
return ""
big_bundle = get_selected_bundle(ui_state.params, "chestnut")
big_name = big_bundle.internalName if big_bundle else default_model_name("chestnut")
big_bundle = get_selected_bundle(ui_state.params, "usbgpu")
big_name = big_bundle.internalName if big_bundle else default_model_name("usbgpu")
big_is_default = big_bundle is None
fallback_name = default_model_name("qcom")
state = big_model_state()
@@ -225,7 +225,7 @@ class ModelsLayout(Widget):
return tr("Getting the big model ready.")
if big_is_default:
return tr("{} will drive. If it fails during a drive, {} takes over until the next drive.").format(big_name, fallback_name)
return tr("{} will drive when the chestnut is ready.").format(big_name)
return tr("{} will drive when the eGPU is ready.").format(big_name)
@staticmethod
def _download_row_state(progresses, name: str) -> dict:
@@ -261,7 +261,7 @@ class ModelsLayout(Widget):
ui_state.params.put("ModelManager_DownloadRef", selected_bundle.ref)
def _resolve_selected_bundle(self, ref):
source_bundles = {source: bundles_for_source(source) for source in ("qcom", "chestnut")}
source_bundles = {source: bundles_for_source(source) for source in ("qcom", "usbgpu")}
resolved = resolve_bundle_by_ref(ref, source_bundles)
return resolved[0] if resolved else None
@@ -329,7 +329,7 @@ class ModelsLayout(Widget):
self._handle_bundle_download_progress()
carry_source, _, carry_display = carrying_model()
for item, item_source in ((self.small_model_item, "qcom"), (self.big_model_item, "chestnut")):
for item, item_source in ((self.small_model_item, "qcom"), (self.big_model_item, "usbgpu")):
bundle = get_selected_bundle(ui_state.params, item_source)
name = bundle.displayName if bundle else default_model_name(item_source)
color = ON_COLOR if (item_source == carry_source and name == carry_display) else style.ITEM_TEXT_VALUE_COLOR
@@ -9,7 +9,7 @@ import math
import pyray as rl
import time
from dataclasses import dataclass
from openpilot.selfdrive.ui.ui_state import ui_state, ChestnutState
from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.sunnypilot.sunnylink.api import UNREGISTERED_SUNNYLINK_DONGLE_ID
from openpilot.system.ui.lib.application import gui_app
from openpilot.system.ui.lib.multilang import tr_noop
@@ -21,8 +21,8 @@ METRIC_MARGIN = 30
METRIC_START_Y = 300
HOME_BTN = rl.Rectangle(60, 860, 180, 180)
CHESTNUT_ICON_WIDTH = 180
CHESTNUT_ICON_HEIGHT = 133
EGPU_ICON_WIDTH = 180
EGPU_ICON_HEIGHT = 133
# Color scheme
@@ -59,9 +59,10 @@ class MetricData:
class SidebarSP:
def __init__(self):
self._sunnylink_status = MetricData(tr_noop("SUNNYLINK"), tr_noop("OFFLINE"), Colors.WARNING)
self._chestnut_green_img = gui_app.texture("icons_mici/chestnut_green.png", CHESTNUT_ICON_WIDTH, CHESTNUT_ICON_HEIGHT)
self._chestnut_default_img = gui_app.texture("icons_mici/chestnut.png", CHESTNUT_ICON_WIDTH, CHESTNUT_ICON_HEIGHT)
self._chestnut_orange_img = gui_app.texture("icons_mici/chestnut_orange.png", CHESTNUT_ICON_WIDTH, CHESTNUT_ICON_HEIGHT)
self._egpu_green_img = gui_app.texture("icons_mici/egpu_green.png", EGPU_ICON_WIDTH, EGPU_ICON_HEIGHT)
self._egpu_default_img = gui_app.texture("icons_mici/egpu.png", EGPU_ICON_WIDTH, EGPU_ICON_HEIGHT)
self._egpu_orange_img = gui_app.texture("icons_mici/egpu_orange.png", EGPU_ICON_WIDTH, EGPU_ICON_HEIGHT)
self._egpu_gray_img = gui_app.texture("icons_mici/egpu_gray.png", EGPU_ICON_WIDTH, EGPU_ICON_HEIGHT)
def _update_sunnylink_status(self):
if not ui_state.params.get_bool("SunnylinkEnabled"):
@@ -89,17 +90,22 @@ class SidebarSP:
def _get_home_icon(self, default_img: rl.Texture) -> tuple[rl.Texture, rl.Vector2, float]:
default_pos = rl.Vector2(HOME_BTN.x, HOME_BTN.y)
state = ui_state.chestnut_state
if state == ChestnutState.DISCONNECTED:
if not ui_state.sm["deviceState"].chestnutPresent:
return default_img, default_pos, 1.0
if state == ChestnutState.LOADING:
icon = self._chestnut_default_img
big_model_selected = ui_state.usbgpu_compiled or ui_state.model_runner_tinygrad
big_model_failed = ui_state.started and ui_state.big_model_failed
loading = ui_state.usbgpu_loading or (big_model_selected and ui_state.started and ui_state.usbgpu_active is None)
if loading:
icon = self._egpu_default_img
opacity = 0.35 + 0.65 * (0.5 - 0.5 * math.cos(rl.get_time() * 6.0))
elif state in (ChestnutState.UNCOMPILED, ChestnutState.FAILED):
icon, opacity = self._chestnut_orange_img, 1.0
elif big_model_selected and big_model_failed:
icon, opacity = self._egpu_orange_img, 1.0
elif big_model_selected:
icon, opacity = self._egpu_green_img, 1.0
else:
icon, opacity = self._chestnut_green_img, 1.0
icon, opacity = self._egpu_gray_img, 1.0
x = HOME_BTN.x + (HOME_BTN.width - icon.width) / 2
y = HOME_BTN.y + (HOME_BTN.height - icon.height) / 2
@@ -5,30 +5,109 @@ 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, ChestnutState
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):
def __init__(self):
super().__init__()
self._openpilot_label = UnifiedLabel("sunnypilot", font_size=88, font_weight=FontWeight.AUDIOWIDE, max_width=480, wrap_text=False)
self._chestnut_loading_icon = IconWidget("icons_mici/chestnut.png", (68, 40))
self._chestnut_loading_icon.set_visible(False)
failed_idx = self._status_bar_layout.widgets.index(self._chestnut_failed_icon)
self._status_bar_layout.widgets.insert(failed_idx + 1, self._chestnut_loading_icon)
self._egpu_icon_default = IconWidget("icons_mici/egpu.png", (50, 37))
self._egpu_icon_default.set_visible(False)
self._egpu_icon_orange = IconWidget("icons_mici/egpu_orange.png", (50, 37))
self._egpu_icon_orange.set_visible(False)
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 _set_chestnut_visibility(self):
# stock has no loading tier: it shows green from the moment a big model is available. keep the
# pulse so the status bar and the onroad HUD agree on what loading looks like.
loading = ui_state.chestnut_state == ChestnutState.LOADING
self._chestnut_loading_icon._opacity = 0.35 + 0.65 * (0.5 - 0.5 * math.cos(rl.get_time() * 6.0))
self._chestnut_loading_icon.set_visible(loading)
self._chestnut_icon.set_visible(not loading and ui_state.chestnut_state in (ChestnutState.READY, ChestnutState.ACTIVE))
self._chestnut_failed_icon.set_visible(ui_state.chestnut_state in (ChestnutState.UNCOMPILED, ChestnutState.FAILED))
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
if not chestnut:
self._egpu_icon.set_visible(False)
self._egpu_icon_default.set_visible(False)
self._egpu_icon_orange.set_visible(False)
self._egpu_icon_gray.set_visible(False)
return
big_model_selected = ui_state.usbgpu_compiled or ui_state.model_runner_tinygrad
big_model_failed = ui_state.started and ui_state.big_model_failed
loading = ui_state.usbgpu_loading or (big_model_selected and ui_state.started and ui_state.usbgpu_active is None)
if loading:
self._egpu_icon_default._opacity = 0.35 + 0.65 * (0.5 - 0.5 * math.cos(rl.get_time() * 6.0))
self._egpu_icon_default.set_visible(True)
self._egpu_icon.set_visible(False)
self._egpu_icon_orange.set_visible(False)
self._egpu_icon_gray.set_visible(False)
else:
self._egpu_icon_default.set_visible(False)
self._egpu_icon.set_visible(big_model_selected and not big_model_failed)
self._egpu_icon_orange.set_visible(big_model_selected and big_model_failed)
self._egpu_icon_gray.set_visible(not big_model_selected)
@@ -27,14 +27,14 @@ def _model_info() -> tuple[str, str, str]:
state = big_model_state()
_, _, carry_display = carrying_model()
if carry_display is None:
big = get_selected_bundle(ui_state.params, "chestnut")
carry_display = big.displayName if big else default_model_name("chestnut")
big = get_selected_bundle(ui_state.params, "usbgpu")
carry_display = big.displayName if big else default_model_name("usbgpu")
active_text = (carry_display or active_name).lower()
if state == 'failed':
return active_text, tr("big model"), tr("unavailable")
if state == 'loading':
return active_text, tr("big model"), tr("getting ready")
header = tr("small model") if source == "chestnut" else tr("big model")
header = tr("small model") if source == "usbgpu" else tr("big model")
return active_text, header, other_name.lower()
@@ -113,7 +113,7 @@ class ModelsLayoutMici(NavScroller):
hardware_btns = []
active = active_source()
for source, label in (("qcom", tr("small models")), ("chestnut", tr("big models"))):
for source, label in (("qcom", tr("small models")), ("usbgpu", tr("big models"))):
bundle = get_selected_bundle(ui_state.params, source)
value = (bundle.internalName if bundle else default_model_name(source)).lower()
if source == active:
@@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details.
import pyray as rl
from openpilot.selfdrive.ui.mici.onroad.hud_renderer import HudRenderer
from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.selfdrive.ui.sunnypilot.onroad.blind_spot_indicators import BlindSpotIndicators
@@ -21,6 +22,8 @@ class HudRendererSP(HudRenderer):
def _render(self, rect: rl.Rectangle) -> None:
super()._render(rect)
if ui_state.usbgpu and not ui_state.usbgpu_compiled and ui_state.model_runner_tinygrad:
self._draw_model_source(rect)
self.blind_spot_indicators.render(rect)
def _has_blind_spot_detected(self) -> bool:
+20 -17
View File
@@ -4,15 +4,15 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
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.
"""
from openpilot.selfdrive.ui.ui_state import ui_state, ChestnutState
from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.sunnypilot.models.fetcher import get_cached_bundles
from openpilot.sunnypilot.models.helpers import get_active_source, get_selected_bundle, resolve_bundle_by_ref
from openpilot.sunnypilot.models.model_name import DEFAULT_BIG_MODEL, DEFAULT_MODEL
def active_source() -> str:
return get_active_source(chestnut=ui_state.chestnut_present,
chestnut_active=ui_state.chestnut_active, chestnut_loading=ui_state.chestnut_loading,
return get_active_source(usbgpu=ui_state.usbgpu,
usbgpu_active=ui_state.usbgpu_active, usbgpu_loading=ui_state.usbgpu_loading,
offroad=ui_state.is_offroad())
@@ -23,7 +23,7 @@ def bundles_for_source(source: str):
def default_model(source: str) -> str:
return DEFAULT_BIG_MODEL if source == 'chestnut' else DEFAULT_MODEL
return DEFAULT_BIG_MODEL if source == 'usbgpu' else DEFAULT_MODEL
def default_model_name(source: str) -> str:
@@ -31,10 +31,13 @@ def default_model_name(source: str) -> str:
def big_model_state() -> str | None:
"""'failed' | 'loading' | None, from the same state the icons render."""
return {ChestnutState.UNCOMPILED: 'failed',
ChestnutState.FAILED: 'failed',
ChestnutState.LOADING: 'loading'}.get(ui_state.chestnut_state)
"""'failed' | 'loading' | None, mirroring the sidebar's detection (#1969)."""
if ui_state.started and ui_state.usbgpu and ui_state.big_model_failed:
return 'failed'
big_selected = ui_state.usbgpu_compiled or ui_state.model_runner_tinygrad
if ui_state.usbgpu_loading or (big_selected and ui_state.started and ui_state.usbgpu_active is None):
return 'loading'
return None
def carrying_model() -> tuple[str | None, str | None, str | None]:
@@ -42,14 +45,14 @@ def carrying_model() -> tuple[str | None, str | None, str | None]:
when a Default big cannot carry, stock modeld runs the Default small, never the
small slot's pick; a custom big has no automatic fallback yet -> (None, None, None)."""
source = active_source()
if source == "chestnut":
bundle = get_selected_bundle(ui_state.params, "chestnut")
if source == "usbgpu":
bundle = get_selected_bundle(ui_state.params, "usbgpu")
if bundle:
return "chestnut", bundle.internalName, bundle.displayName
name = default_model_name("chestnut")
return "chestnut", name, name
if ui_state.chestnut_present:
if get_selected_bundle(ui_state.params, "chestnut") is None:
return "usbgpu", bundle.internalName, bundle.displayName
name = default_model_name("usbgpu")
return "usbgpu", name, name
if ui_state.usbgpu:
if get_selected_bundle(ui_state.params, "usbgpu") is None:
name = default_model_name("qcom")
return "qcom", name, name
return None, None, None
@@ -63,7 +66,7 @@ def carrying_model() -> tuple[str | None, str | None, str | None]:
def queued_name(current_ref) -> str | None:
ref = ui_state.params.get("ModelManager_DownloadRef")
if ref and ref != current_ref:
source_bundles = {source: bundles_for_source(source) for source in ("qcom", "chestnut")}
source_bundles = {source: bundles_for_source(source) for source in ("qcom", "usbgpu")}
if resolved := resolve_bundle_by_ref(ref, source_bundles):
return resolved[0].internalName
return None
@@ -76,7 +79,7 @@ def model_info() -> tuple[str, str, str]:
manager republishes a tick after a chestnut change, so the stale bundle
would flash the wrong model."""
source = active_source()
other = "qcom" if source == "chestnut" else "chestnut"
other = "qcom" if source == "usbgpu" else "usbgpu"
active_bundle = get_selected_bundle(ui_state.params, source)
other_bundle = get_selected_bundle(ui_state.params, other)
@@ -0,0 +1,228 @@
"""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,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()
@@ -152,13 +153,10 @@ class UIStateSP:
self.has_icbm = self.CP_SP.intelligentCruiseButtonManagementAvailable and self.params.get_bool("IntelligentCruiseButtonManagement")
self._enforce_constraints()
source = get_active_source(chestnut=self.chestnut_present, chestnut_active=self.chestnut_active,
chestnut_loading=self.chestnut_loading, offroad=self.is_offroad())
source = get_active_source(usbgpu=self.usbgpu, usbgpu_active=self.usbgpu_active,
usbgpu_loading=self.usbgpu_loading, offroad=self.is_offroad())
self.active_bundle = self.params.get(ACTIVE_BUNDLE_KEYS[source])
self.model_runner_tinygrad = self.active_bundle is not None and self.active_bundle.get("runner") == "tinygrad"
# stock only counts the default big model's compiled pkl. a downloaded big bundle runs on the
# chestnut just the same, so ChestnutState has to see it as available too.
self.chestnut_compiled = self.chestnut_compiled or self.model_runner_tinygrad
self.blindspot = self.params.get_bool("BlindSpot")
self.chevron_metrics = self.params.get("ChevronInfo")
self.custom_interactive_timeout = self.params.get("InteractivityTimeout", return_default=True)
@@ -0,0 +1,45 @@
#!/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()
+38
View File
@@ -0,0 +1,38 @@
#!/usr/bin/env python3
"""Publish deterministic milestone events for the local comma-four UI preview."""
import itertools
import time
from openpilot.cereal import messaging
def main() -> None:
pm = messaging.PubMaster(["assistedDrivingMilestoneState"])
milestones = itertools.cycle(((1, 0, "mads"), (2, 1, "fullAssist"), (5, 2, "mads"), (10, 5, "fullAssist")))
event_id = 0
milestone, previous_milestone, category = 0, 0, "mads"
next_event_time = time.monotonic() + 1.0
while True:
now = time.monotonic()
if now >= next_event_time:
event_id += 1
milestone, previous_milestone, category = next(milestones)
next_event_time = now + 6.0
msg = messaging.new_message("assistedDrivingMilestoneState")
state = msg.assistedDrivingMilestoneState
state.enabled = True
if event_id:
state.event.id = event_id
state.event.category = category
state.event.distanceMeters = milestone * 1609.344
state.event.previousDistanceMeters = previous_milestone * 1609.344
state.event.unit = "imperial"
pm.send("assistedDrivingMilestoneState", msg)
time.sleep(0.1)
if __name__ == "__main__":
main()
+27
View File
@@ -0,0 +1,27 @@
#!/usr/bin/env bash
set -e
repo_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../../.." && pwd)"
replay_pid=""
preview_pid=""
cleanup() {
for pid in "$preview_pid" "$replay_pid"; do
if [[ -n "$pid" ]]; then
kill "$pid" 2>/dev/null || true
wait "$pid" 2>/dev/null || true
fi
done
}
trap cleanup EXIT INT TERM
export PATH="$repo_root/.venv/bin:$PATH"
export SP_MILESTONE_PREVIEW=1
playback="${SP_MILESTONE_PLAYBACK:-1}"
"$repo_root/openpilot/tools/replay/replay" --demo --playback "$playback" &
replay_pid=$!
"$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/tests/milestone_preview.py" &
preview_pid=$!
"$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py"
+52 -1
View File
@@ -4,12 +4,63 @@ 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, check_selfdrive_timeout_alert
from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, Soundd, calculate_volume_for_device, 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'])
+20 -43
View File
@@ -12,7 +12,7 @@ from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.ui.lib.prime_state import PrimeState
from openpilot.system.ui.lib.application import gui_app
from openpilot.common.hardware import HARDWARE, PC
from openpilot.selfdrive.modeld.helpers import chestnut_compiled
from openpilot.selfdrive.modeld.helpers import usbgpu_compiled
from openpilot.selfdrive.ui.sunnypilot.ui_state import UIStateSP, DeviceSP
@@ -28,15 +28,6 @@ class UIStatus(Enum):
LONG_ONLY = "long_only"
class ChestnutState(Enum):
DISCONNECTED = "disconnected"
UNCOMPILED = "uncompiled"
READY = "ready"
LOADING = "loading"
ACTIVE = "active"
FAILED = "failed"
class UIState(UIStateSP):
_instance: 'UIState | None' = None
@@ -91,11 +82,10 @@ class UIState(UIStateSP):
self.always_on_dm: bool = self.params.get_bool("AlwaysOnDM")
self.experimental_mode: bool = self.params.get_bool("ExperimentalMode")
self.experimental_mode_confirmed: bool = self.params.get_bool("ExperimentalModeConfirmed")
self.chestnut_present: bool = False
self.chestnut_compiled: bool = chestnut_compiled()
self.chestnut_active: bool | None = None
self.chestnut_loading: bool = False
self.chestnut_state = ChestnutState.DISCONNECTED
self.usbgpu: bool = False
self.usbgpu_compiled: bool = usbgpu_compiled()
self.usbgpu_active: bool | None = self.params.get("UsbGpuActive")
self.usbgpu_loading: bool = self.params.get_bool("UsbGpuLoading")
self.started: bool = False
self.ignition: bool = False
self.recording_audio: bool = False
@@ -122,6 +112,15 @@ class UIState(UIStateSP):
def add_on_body_changed_callbacks(self, callback: Callable[[], None]):
self._on_body_changed_callbacks.append(callback)
@property
def big_model_failed(self) -> bool:
# Mirrors the onroad HUD's four-condition check so sidebar and home icons reflect the same failure states
return (self.usbgpu_active is False or
not self.sm['deviceState'].chestnutPresent or
(self.usbgpu_active is True and self.sm.recv_frame['modelV2'] > self.started_frame and
not self.sm.alive['modelV2']) or
(self.usbgpu_active is None and self.sm.recv_frame['modelV2'] > self.started_frame))
@property
def engaged(self) -> bool:
return self.started and (self.sm["selfdriveState"].enabled or self.sm["selfdriveStateSP"].mads.enabled)
@@ -141,7 +140,6 @@ class UIState(UIStateSP):
self.sm.update(0)
self._update_state()
self._update_status()
self._update_chestnut_state()
device.update()
UIStateSP.update(self)
@@ -205,35 +203,12 @@ class UIState(UIStateSP):
self.status = UIStatus.DISENGAGED
self.started_frame = self.sm.frame
self.started_time = time.monotonic()
self.chestnut_present = self.sm["deviceState"].chestnutPresent
for callback in self._offroad_transition_callbacks:
callback()
self._started_prev = self.started
def _update_chestnut_state(self) -> None:
detected = self.sm["deviceState"].chestnutPresent
if not self.started:
self.chestnut_present = detected
self.chestnut_state = (ChestnutState.READY if detected and self.chestnut_compiled else
ChestnutState.UNCOMPILED if detected else ChestnutState.DISCONNECTED)
return
model_seen = self.sm.recv_frame["modelV2"] > self.started_frame
if not self.chestnut_present:
self.chestnut_state = ChestnutState.DISCONNECTED
elif not self.chestnut_compiled:
self.chestnut_state = ChestnutState.UNCOMPILED
elif self.chestnut_state == ChestnutState.FAILED or not detected or (model_seen and (not self.sm.alive["modelV2"] or not self.sm["modelV2"].big)):
self.chestnut_state = ChestnutState.FAILED
elif self.chestnut_loading or not model_seen:
self.chestnut_state = ChestnutState.LOADING
elif self.chestnut_active is False:
self.chestnut_state = ChestnutState.FAILED
else:
self.chestnut_state = ChestnutState.ACTIVE
def update_params(self) -> None:
# For slower operations
# Update longitudinal control state
@@ -250,10 +225,12 @@ class UIState(UIStateSP):
self.always_on_dm = self.params.get_bool("AlwaysOnDM")
self.experimental_mode = self.params.get_bool("ExperimentalMode")
self.experimental_mode_confirmed = self.params.get_bool("ExperimentalModeConfirmed")
if not self.chestnut_compiled:
self.chestnut_compiled = chestnut_compiled()
self.chestnut_active = self.params.get("ChestnutActive")
self.chestnut_loading = self.params.get_bool("ChestnutLoading")
# keep usbgpu UI active until offroad transition when gpu disappears
self.usbgpu = self.sm["deviceState"].chestnutPresent or (self.usbgpu and self.started)
if not self.usbgpu_compiled:
self.usbgpu_compiled = usbgpu_compiled()
self.usbgpu_active = self.params.get("UsbGpuActive")
self.usbgpu_loading = self.params.get_bool("UsbGpuLoading")
UIStateSP.update_params(self)
@@ -298,7 +298,7 @@ def _load_policy_runners(args: argparse.Namespace) -> tuple[list, list]:
if __name__ == "__main__":
if 'USB' in os.getenv('DEV', '') or os.getenv('CHESTNUT'):
if 'USB' in os.getenv('DEV', '') or os.getenv('USBGPU'):
from openpilot.system.hardware.chestnut.flash import link_up
for _ in range(10):
if link_up():
+21 -21
View File
@@ -9,7 +9,7 @@ See the LICENSE.md file in the root directory for more details.
import os
os.environ['GMMU'] = '0'
from openpilot.common.hardware import COMMA_HARDWARE
from openpilot.selfdrive.modeld.helpers import chestnut_present, load_oob
from openpilot.selfdrive.modeld.helpers import usbgpu_present, load_oob
import time
import numpy as np
import openpilot.cereal.messaging as messaging
@@ -84,14 +84,14 @@ class ModelState(ModelStateBase):
inputs: dict[str, np.ndarray]
prev_desire: np.ndarray
def __init__(self, cam_w: int, cam_h: int, chestnut: bool = False):
def __init__(self, cam_w: int, cam_h: int, usbgpu: bool = False):
ModelStateBase.__init__(self)
env_pkl = os.environ.get('COMBINED_MODEL_PKL')
if env_pkl and os.path.exists(env_pkl):
model_bundle = None
else:
model_bundle = get_active_bundle(chestnut=chestnut)
model_bundle = get_active_bundle(usbgpu=usbgpu)
self.generation = model_bundle.generation if model_bundle is not None else None
overrides = {override.key: override.value for override in model_bundle.overrides} if model_bundle else {}
@@ -99,7 +99,7 @@ class ModelState(ModelStateBase):
self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0"))
self.MIN_LAT_CONTROL_SPEED = 0.3
self.PLANPLUS_CONTROL: float = 1.0
self.chestnut = chestnut
self.usbgpu = usbgpu
pkl_path = _find_driving_pkl(model_bundle)
assert pkl_path is not None, "No driving pkl found — all models must be compiled with compile_modeld.py"
@@ -110,7 +110,7 @@ class ModelState(ModelStateBase):
jits = load_oob(open_file_chunked(pkl_path))
self.WARP_DEV = 'QCOM' if COMMA_HARDWARE else 'CPU'
self.DEV = 'AMD' if self.chestnut else self.WARP_DEV
self.DEV = 'AMD' if self.usbgpu else self.WARP_DEV
self.QUEUE_DEV = self.DEV
metadata = jits['metadata']
@@ -185,7 +185,7 @@ class ModelState(ModelStateBase):
else:
self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=frame_tensor, big_frame=big_frame_tensor)
if self.chestnut:
if self.usbgpu:
self.warmup()
def warmup(self) -> None:
@@ -287,7 +287,7 @@ class ModelState(ModelStateBase):
buf[0, :-1] = buf[0, 1:]
buf[0, -1, :] = outputs['desired_curvature'][0, :] if not self.mlsim else 0
if self.chestnut and not np.all(np.isfinite(outputs.get('plan', np.array([0.])))):
if self.usbgpu and not np.all(np.isfinite(outputs.get('plan', np.array([0.])))):
cloudlog.error("model output not finite, dropping frame")
return None
@@ -327,13 +327,13 @@ def main(demo=False):
setproctitle(PROCESS_NAME)
config_realtime_process(7, 54)
CHESTNUT = chestnut_present()
if CHESTNUT:
USBGPU = usbgpu_present()
if USBGPU:
os.environ['HCQDEV_WAIT_TIMEOUT_MS'] = '3000'
params = Params()
params.put_bool("ChestnutLoading", CHESTNUT)
params.remove("ChestnutActive")
params.put_bool("UsbGpuLoading", USBGPU)
params.remove("UsbGpuActive")
# visionipc clients
while True:
@@ -362,31 +362,31 @@ def main(demo=False):
st = time.monotonic()
model = None
if CHESTNUT:
if USBGPU:
import threading
def load():
nonlocal model
model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, chestnut=True)
model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, usbgpu=True)
t = threading.Thread(target=load, daemon=True)
t.start()
t.join(60)
if model is None:
params.put_bool("ChestnutActive", False)
raise RuntimeError("chestnut model load failed or timed out (60s)")
params.put_bool("ChestnutActive", True)
params.put_bool("UsbGpuActive", False)
raise RuntimeError("eGPU model load failed or timed out (60s)")
params.put_bool("UsbGpuActive", True)
else:
model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, chestnut=False)
model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, usbgpu=False)
params.put_bool("ChestnutLoading", False)
params.put_bool("UsbGpuLoading", False)
cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
# messaging
pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutState"] if CHESTNUT else [])
pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutState"] if USBGPU else [])
pm = PubMaster(pub_socks)
sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"])
publish_state = PublishState()
chestnut_state = ChestnutState(pm, CHESTNUT) if CHESTNUT else None
chestnut_state = ChestnutState(pm, USBGPU) if USBGPU else None
# setup filter to track dropped frames
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / model.constants.MODEL_FREQ)
@@ -524,7 +524,7 @@ def main(demo=False):
fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, meta_constants)
modelv2_send.modelV2.big = model.chestnut
modelv2_send.modelV2.big = model.usbgpu
desire_state = modelv2_send.modelV2.meta.desireState
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
@@ -190,8 +190,8 @@ def tmp_path():
def patch_modeld(monkeypatch):
def _patch(bundle):
monkeypatch.setattr(helpers, 'get_active_bundle', lambda params=None, *, chestnut=None: bundle)
monkeypatch.setattr(modeld_module, 'get_active_bundle', lambda params=None, *, chestnut=None: bundle)
monkeypatch.setattr(helpers, 'get_active_bundle', lambda params=None, *, usbgpu=None: bundle)
monkeypatch.setattr(modeld_module, 'get_active_bundle', lambda params=None, *, usbgpu=None: bundle)
return _patch
@@ -59,8 +59,8 @@ class TestFindDrivingPkl(OpenpilotTestCase):
class TestModelStateCombinedInit(OpenpilotTestCase):
def test_asserts_when_no_pkl(self, monkeypatch):
bundle = DummyBundle(models=[], is_20hz=True)
monkeypatch.setattr(helpers, 'get_active_bundle', lambda params=None, *, chestnut=None: bundle)
monkeypatch.setattr(modeld_module, 'get_active_bundle', lambda params=None, *, chestnut=None: bundle)
monkeypatch.setattr(helpers, 'get_active_bundle', lambda params=None, *, usbgpu=None: bundle)
monkeypatch.setattr(modeld_module, 'get_active_bundle', lambda params=None, *, usbgpu=None: bundle)
with self.assertRaisesRegex(AssertionError, "No driving pkl found"):
ModelState(cam_w=CAM_W, cam_h=CAM_H)
+2 -2
View File
@@ -9,8 +9,8 @@ from openpilot.sunnypilot.models.model_name import DEFAULT_MODEL, DEFAULT_BIG_MO
def get_default_model() -> str:
show_big_model = (ui_state.chestnut_present
and (ui_state.chestnut_active or ui_state.chestnut_loading or ui_state.is_offroad()))
show_big_model = (ui_state.usbgpu
and (ui_state.usbgpu_active or ui_state.usbgpu_loading or ui_state.is_offroad()))
return DEFAULT_BIG_MODEL if show_big_model else DEFAULT_MODEL
+7 -7
View File
@@ -139,11 +139,11 @@ class ModelCache:
class ModelFetcher:
"""Handles fetching and caching of model data from remote source"""
MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v21.json"
MODEL_URL_CHESTNUT = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_chestnut_v22.json"
MODEL_URL_USBGPU = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_usbgpu_v22.json"
MODEL_SOURCES = {
"qcom": (MODEL_URL, ""),
"chestnut": (MODEL_URL_CHESTNUT, "_Chestnut"),
"usbgpu": (MODEL_URL_USBGPU, "_USBGPU"),
}
def __init__(self, params: Params):
@@ -156,12 +156,12 @@ class ModelFetcher:
self._refetched: set[str] = set()
self.params.put("ModelManager_ActiveJson", {
"qcom": self.MODEL_URL,
"chestnut": self.MODEL_URL_CHESTNUT,
"usbgpu": self.MODEL_URL_USBGPU,
}, block=True)
@staticmethod
def active_source(chestnut_present: bool) -> str:
return "chestnut" if chestnut_present else "qcom"
return "usbgpu" if chestnut_present else "qcom"
def _fetch_and_cache_models(self, source: str) -> list[custom.ModelManagerSP.ModelBundle] | None:
"""Fetches fresh model data from remote and updates cache.
@@ -200,7 +200,7 @@ class ModelFetcher:
@staticmethod
def _cache_matches_source(source: str, cached_data: dict) -> bool:
bundles = cached_data.get("bundles", [])
if source == "chestnut":
if source == "usbgpu":
return any(bundle.get("is_big") is True for bundle in bundles)
return not any(bundle.get("is_big") is True for bundle in bundles)
@@ -261,10 +261,10 @@ def get_cached_bundles(params: Params, source: str) -> list[custom.ModelManagerS
if __name__ == "__main__":
from openpilot.selfdrive.modeld.helpers import chestnut_present
from openpilot.selfdrive.modeld.helpers import usbgpu_present
params = Params()
model_fetcher = ModelFetcher(params)
bundles = model_fetcher.get_bundles_for_source(ModelFetcher.active_source(chestnut_present()))
bundles = model_fetcher.get_bundles_for_source(ModelFetcher.active_source(usbgpu_present()))
for bundle in bundles:
for model in bundle.models:
model_overrides = {override.key: override.value for override in bundle.overrides}
+11 -11
View File
@@ -16,7 +16,7 @@ from openpilot.common.params import Params
from openpilot.common.swaglog import cloudlog
from openpilot.sunnypilot.models.constants import Meta, MetaSimPose, MetaTombRaider
from openpilot.common.hardware.hw import Paths
from openpilot.selfdrive.modeld.helpers import chestnut_present
from openpilot.selfdrive.modeld.helpers import usbgpu_present
# SET ME TO THE EXACT JSON VERSION WE SET IN SUNNYPILOT_MODELS REPO
REQUIRED_JSON_VERSION = 18
@@ -27,7 +27,7 @@ ModelManager = custom.ModelManagerSP
ACTIVE_BUNDLE_KEYS = {
"qcom": "ModelManager_ActiveBundle",
"chestnut": "ModelManager_ActiveBundleChestnut",
"usbgpu": "ModelManager_ActiveBundleUSBGPU",
}
_LAST_VALIDATED_RAW: dict[str, dict | None] = {}
@@ -126,20 +126,20 @@ def get_selected_bundle(params: Params | None = None, source: str = "qcom") -> "
return _parse_active_bundle(params.get(ACTIVE_BUNDLE_KEYS[source]))
def get_active_source(chestnut: bool | None = None, chestnut_active: bool | None = None,
chestnut_loading: bool | None = None, offroad: bool | None = None) -> str:
if chestnut is None:
chestnut = chestnut_present()
state_valid = chestnut_active is not None or chestnut_loading is not None or offroad is not None
big_active = chestnut and (not state_valid or chestnut_active or chestnut_loading or offroad)
return "chestnut" if big_active else "qcom"
def get_active_source(usbgpu: bool | None = None, usbgpu_active: bool | None = None,
usbgpu_loading: bool | None = None, offroad: bool | None = None) -> str:
if usbgpu is None:
usbgpu = usbgpu_present()
state_valid = usbgpu_active is not None or usbgpu_loading is not None or offroad is not None
big_active = usbgpu and (not state_valid or usbgpu_active or usbgpu_loading or offroad)
return "usbgpu" if big_active else "qcom"
def get_active_bundle(params: Params | None = None, *, chestnut: bool | None = None) -> "custom.ModelManagerSP.ModelBundle | None":
def get_active_bundle(params: Params | None = None, *, usbgpu: bool | None = None) -> "custom.ModelManagerSP.ModelBundle | None":
# no cross-slot fallback: an empty active slot means the hardware default, which
# only stock modeld can run - modeld_v2 requires a real bundle
params = params or Params()
return get_selected_bundle(params, get_active_source(chestnut=chestnut))
return get_selected_bundle(params, get_active_source(usbgpu=usbgpu))
def resolve_bundle_by_ref(
+3 -3
View File
@@ -40,7 +40,7 @@ class ModelManagerSP:
self.available_models: list[custom.ModelManagerSP.ModelBundle] = []
self.source_models: dict[str, list[custom.ModelManagerSP.ModelBundle]] = {}
self.selected_bundle: custom.ModelManagerSP.ModelBundle = None
self.active_bundle: custom.ModelManagerSP.ModelBundle = get_active_bundle(self.params, chestnut=self.chestnut_present)
self.active_bundle: custom.ModelManagerSP.ModelBundle = get_active_bundle(self.params, usbgpu=self.chestnut_present)
self._chunk_size = 128 * 1000 # 128 KB chunks
self._download_start_times: dict[str, float] = {} # Track start time per model
self._download_ref: bytes | str | None = None
@@ -280,7 +280,7 @@ class ModelManagerSP:
raise DownloadCancelled("Download cancelled")
self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.downloaded
self.params.put(ACTIVE_BUNDLE_KEYS[source], model_bundle.to_dict(), block=True)
self.active_bundle = get_active_bundle(self.params, chestnut=self.chestnut_present)
self.active_bundle = get_active_bundle(self.params, usbgpu=self.chestnut_present)
except Exception:
if self.selected_bundle is not None:
@@ -326,7 +326,7 @@ class ModelManagerSP:
self.source_models = {source: self.model_fetcher.get_bundles_for_source(source) for source in ModelFetcher.MODEL_SOURCES}
self.available_models = self.source_models[ModelFetcher.active_source(self.chestnut_present)]
validate_active_bundles(self.params, self.source_models)
self.active_bundle = get_active_bundle(self.params, chestnut=self.chestnut_present)
self.active_bundle = get_active_bundle(self.params, usbgpu=self.chestnut_present)
self._process_download_requests()
@@ -392,7 +392,7 @@ class TestManagerDownload(ManagerDownloadTestBase):
asyncio.run(self.manager._download_bundle(self._bundle, self.dest, "qcom"))
assert "ModelManager_ActiveBundle" in store, "qcom download must write the qcom slot"
assert "ModelManager_ActiveBundleChestnut" not in store, "qcom download must not touch the chestnut slot"
assert "ModelManager_ActiveBundleUSBGPU" not in store, "qcom download must not touch the usbgpu slot"
assert self.manager.selected_bundle.status == custom.ModelManagerSP.DownloadStatus.downloaded
assert self.manager.active_bundle is not None and self.manager.active_bundle.ref == "test-ref"
assert self.manager.active_bundle.status == custom.ModelManagerSP.DownloadStatus.downloaded
@@ -401,18 +401,18 @@ class TestManagerDownload(ManagerDownloadTestBase):
assert missing == [], f"chunks missing from the cache: {missing}"
self.run_with_server(body)
def test_download_writes_chestnut_slot(self):
"""A download resolved to the chestnut source writes the chestnut active bundle slot only."""
def test_download_writes_usbgpu_slot(self):
"""A download resolved to the usbgpu source writes the usbgpu active bundle slot only."""
def body():
self.make_artifact(chunked=True)
self._bundle.ref = "big-ref"
self._bundle.minimumSelectorVersion = 18
params, store = self._make_params_with_store()
self.manager.params = params
asyncio.run(self.manager._download_bundle(self._bundle, self.dest, "chestnut"))
asyncio.run(self.manager._download_bundle(self._bundle, self.dest, "usbgpu"))
assert "ModelManager_ActiveBundleChestnut" in store, "chestnut download must write the chestnut slot"
assert "ModelManager_ActiveBundle" not in store, "chestnut download must not touch the qcom slot"
assert "ModelManager_ActiveBundleUSBGPU" in store, "usbgpu download must write the usbgpu slot"
assert "ModelManager_ActiveBundle" not in store, "usbgpu download must not touch the qcom slot"
assert self.manager.selected_bundle.status == custom.ModelManagerSP.DownloadStatus.downloaded
self.run_with_server(body)
@@ -447,20 +447,20 @@ class TestResolveBundleByRef(OpenpilotTestCase):
def test_qcom_ref_resolves_to_qcom_slot(self):
small = self._bundle("small")
assert resolve_bundle_by_ref("small", {"qcom": [small], "chestnut": []}) == (small, "qcom")
assert resolve_bundle_by_ref("small", {"qcom": [small], "usbgpu": []}) == (small, "qcom")
def test_chestnut_ref_resolves_to_chestnut_slot(self):
def test_usbgpu_ref_resolves_to_usbgpu_slot(self):
big = self._bundle("big")
assert resolve_bundle_by_ref("big", {"qcom": [], "chestnut": [big]}) == (big, "chestnut")
assert resolve_bundle_by_ref("big", {"qcom": [], "usbgpu": [big]}) == (big, "usbgpu")
def test_unknown_ref_returns_none(self):
source_bundles = {"qcom": [self._bundle("small")], "chestnut": []}
source_bundles = {"qcom": [self._bundle("small")], "usbgpu": []}
assert resolve_bundle_by_ref("nope", source_bundles) is None
def manifest_bundle(short_name: str, ref: str, index: int = 0, is_big: bool = False) -> dict:
"""Minimal manifest bundle dict, version-compatible (no chunks to avoid disk side effects).
Big (chestnut) bundles carry `is_big: true` in the manifest JSON."""
Big (usbgpu) bundles carry `is_big: true` in the manifest JSON."""
return {
"index": index,
"short_name": short_name,
@@ -489,15 +489,15 @@ class TestModelFetcherSources(OpenpilotTestCase):
"""Both manifests are always maintained: get_bundles_for_source exposes either
source by name, and active_source picks which one matches the attached hardware."""
def _make_params(self, qcom_manifest, chestnut_manifest):
def _make_params(self, qcom_manifest, usbgpu_manifest):
params = mock.MagicMock()
def get(key):
if key == "ModelManager_ModelsCache":
return qcom_manifest
if key == "ModelManager_ModelsCache_Chestnut":
return chestnut_manifest
if key in ("ModelManager_LastSyncTime", "ModelManager_LastSyncTime_Chestnut"):
if key == "ModelManager_ModelsCache_USBGPU":
return usbgpu_manifest
if key in ("ModelManager_LastSyncTime", "ModelManager_LastSyncTime_USBGPU"):
return fresh_sync_time()
return None
@@ -506,14 +506,14 @@ class TestModelFetcherSources(OpenpilotTestCase):
def test_active_source_follows_chestnut_presence(self):
assert ModelFetcher.active_source(False) == "qcom"
assert ModelFetcher.active_source(True) == "chestnut"
assert ModelFetcher.active_source(True) == "usbgpu"
def test_get_bundles_for_source_returns_each_source(self):
params = self._make_params({"bundles": [manifest_bundle("small", "aaa")]},
{"bundles": [manifest_bundle("big", "bbb", is_big=True)]})
fetcher = ModelFetcher(params)
assert [bundle.ref for bundle in fetcher.get_bundles_for_source("qcom")] == ["aaa"]
assert [bundle.ref for bundle in fetcher.get_bundles_for_source("chestnut")] == ["bbb"]
assert [bundle.ref for bundle in fetcher.get_bundles_for_source("usbgpu")] == ["bbb"]
def test_get_bundles_for_source_unknown(self):
assert ModelFetcher(mock.MagicMock()).get_bundles_for_source("bogus") == []
@@ -522,16 +522,16 @@ class TestModelFetcherSources(OpenpilotTestCase):
params = self._make_params({"bundles": [manifest_bundle("small", "aaa")]},
{"bundles": [manifest_bundle("big", "bbb", is_big=True)]})
qcom_bundles = get_cached_bundles(params, "qcom")
chestnut_bundles = get_cached_bundles(params, "chestnut")
usbgpu_bundles = get_cached_bundles(params, "usbgpu")
assert [b.ref for b in qcom_bundles] == ["aaa"]
assert [b.ref for b in chestnut_bundles] == ["bbb"]
assert [b.ref for b in usbgpu_bundles] == ["bbb"]
assert qcom_bundles[0].displayName == "SMALL"
def test_get_cached_bundles_empty_when_missing(self):
params = mock.MagicMock()
params.get.return_value = None
assert get_cached_bundles(params, "qcom") == []
assert get_cached_bundles(params, "chestnut") == []
assert get_cached_bundles(params, "usbgpu") == []
def test_get_cached_bundles_unknown_source(self):
assert get_cached_bundles(mock.MagicMock(), "bogus") == []
@@ -543,27 +543,27 @@ class TestModelFetcherSources(OpenpilotTestCase):
assert active_json_calls, "expected ModelManager_ActiveJson to be written"
assert active_json_calls[-1].args[1] == {
"qcom": ModelFetcher.MODEL_URL,
"chestnut": ModelFetcher.MODEL_URL_CHESTNUT,
"usbgpu": ModelFetcher.MODEL_URL_USBGPU,
}
class TestSourceCacheIntegrity(OpenpilotTestCase):
"""Each source's cached manifest must contain only that source's models; the
`is_big` flag in the JSON marks the big (chestnut) models. A mismatched cache is
`is_big` flag in the JSON marks the big (usbgpu) models. A mismatched cache is
legacy data from before the per-source split (the active manifest was cached
under the unsuffixed key regardless of hardware) and is refetched. This
replaces the old one-time bundle migration."""
def _make_params(self, qcom_manifest, chestnut_manifest):
def _make_params(self, qcom_manifest, usbgpu_manifest):
params = mock.MagicMock()
def get(key):
if key == "ModelManager_ModelsCache":
return qcom_manifest
if key == "ModelManager_ModelsCache_Chestnut":
return chestnut_manifest
if key in ("ModelManager_LastSyncTime", "ModelManager_LastSyncTime_Chestnut"):
if key == "ModelManager_ModelsCache_USBGPU":
return usbgpu_manifest
if key in ("ModelManager_LastSyncTime", "ModelManager_LastSyncTime_USBGPU"):
return fresh_sync_time()
return None
@@ -584,13 +584,13 @@ class TestSourceCacheIntegrity(OpenpilotTestCase):
bundles = fetcher.get_bundles_for_source("qcom")
assert [bundle.ref for bundle in bundles] == ["aaa"]
def test_chestnut_cache_without_big_models_is_refetched(self):
def test_usbgpu_cache_without_big_models_is_refetched(self):
params = self._make_params({"bundles": [manifest_bundle("small", "aaa")]},
{"bundles": [manifest_bundle("big2", "ccc")]})
fetcher = ModelFetcher(params)
fetched = self._fetched(manifest_bundle("big", "bbb", is_big=True))
with mock.patch.object(fetcher, "_fetch_and_cache_models", return_value=fetched):
bundles = fetcher.get_bundles_for_source("chestnut")
bundles = fetcher.get_bundles_for_source("usbgpu")
assert [bundle.ref for bundle in bundles] == ["bbb"]
def test_matching_caches_are_used_without_fetch(self):
@@ -599,7 +599,7 @@ class TestSourceCacheIntegrity(OpenpilotTestCase):
fetcher = ModelFetcher(params)
with mock.patch.object(fetcher, "_fetch_and_cache_models", side_effect=AssertionError("cache should be used")):
assert [bundle.ref for bundle in fetcher.get_bundles_for_source("qcom")] == ["aaa"]
assert [bundle.ref for bundle in fetcher.get_bundles_for_source("chestnut")] == ["bbb"]
assert [bundle.ref for bundle in fetcher.get_bundles_for_source("usbgpu")] == ["bbb"]
def test_stale_version_cache_is_refetched(self):
"""A source-matching cache whose bundles are all filtered by the selector version
@@ -660,28 +660,28 @@ class TestActiveBundleValidation(OpenpilotTestCase):
bundle.runner = runner
return bundle.to_dict()
def _params(self, qcom=None, chestnut=None):
def _params(self, qcom=None, usbgpu=None):
params = mock.MagicMock()
def get(key, *args, **kwargs):
return {"ModelManager_ActiveBundle": qcom, "ModelManager_ActiveBundleChestnut": chestnut}.get(key)
return {"ModelManager_ActiveBundle": qcom, "ModelManager_ActiveBundleUSBGPU": usbgpu}.get(key)
params.get.side_effect = get
return params
def test_empty_catalog_does_not_reset_slot(self):
params = self._params(qcom=self._raw_bundle("small"))
with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=False):
validate_active_bundles(params, {"qcom": [], "chestnut": []})
with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=False):
validate_active_bundles(params, {"qcom": [], "usbgpu": []})
params.remove.assert_not_called()
def test_reset_recomputes_runner_from_surviving_slot(self):
tinygrad = int(custom.ModelManagerSP.Runner.tinygrad)
big_raw = self._raw_bundle("big", runner=tinygrad)
params = self._params(qcom=self._raw_bundle("gone"), chestnut=big_raw)
params = self._params(qcom=self._raw_bundle("gone"), usbgpu=big_raw)
catalog = {"qcom": [custom.ModelManagerSP.ModelBundle(**self._raw_bundle("other"))],
"chestnut": [custom.ModelManagerSP.ModelBundle(**big_raw)]}
with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=True):
"usbgpu": [custom.ModelManagerSP.ModelBundle(**big_raw)]}
with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=True):
validate_active_bundles(params, catalog)
params.remove.assert_called_once_with("ModelManager_ActiveBundle")
runner_puts = [call for call in params.put.call_args_list if call.args[0] == "ModelRunnerTypeCache"]
@@ -689,7 +689,7 @@ class TestActiveBundleValidation(OpenpilotTestCase):
class TestActiveBundleSelection(OpenpilotTestCase):
"""The effective active bundle is the active source's slot: chestnut when a GPU is
"""The effective active bundle is the active source's slot: usbgpu when a GPU is
present, qcom otherwise. An empty active slot means the hardware default (stock
runner), never the other slot's pick - modeld_v2 requires a real bundle."""
@@ -700,37 +700,37 @@ class TestActiveBundleSelection(OpenpilotTestCase):
bundle.minimumSelectorVersion = 18
return bundle.to_dict()
def _params(self, qcom=None, chestnut=None):
def _params(self, qcom=None, usbgpu=None):
params = mock.MagicMock()
def get(key, *args, **kwargs):
if key == "ModelManager_ActiveBundle":
return qcom
if key == "ModelManager_ActiveBundleChestnut":
return chestnut
if key == "ModelManager_ActiveBundleUSBGPU":
return usbgpu
return None
params.get.side_effect = get
return params
def test_selected_bundle_is_per_slot(self):
params = self._params(qcom=self._raw_bundle("small"), chestnut=self._raw_bundle("big"))
params = self._params(qcom=self._raw_bundle("small"), usbgpu=self._raw_bundle("big"))
assert get_selected_bundle(params, "qcom").ref == "small"
assert get_selected_bundle(params, "chestnut").ref == "big"
assert get_selected_bundle(params, "usbgpu").ref == "big"
def test_no_gpu_uses_qcom_slot(self):
params = self._params(qcom=self._raw_bundle("small"), chestnut=self._raw_bundle("big"))
with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=False):
params = self._params(qcom=self._raw_bundle("small"), usbgpu=self._raw_bundle("big"))
with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=False):
assert get_active_bundle(params).ref == "small"
def test_gpu_uses_chestnut_slot(self):
params = self._params(qcom=self._raw_bundle("small"), chestnut=self._raw_bundle("big"))
with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=True):
def test_gpu_uses_usbgpu_slot(self):
params = self._params(qcom=self._raw_bundle("small"), usbgpu=self._raw_bundle("big"))
with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=True):
assert get_active_bundle(params).ref == "big"
def test_gpu_without_big_selection_is_hardware_default(self):
params = self._params(qcom=self._raw_bundle("small"), chestnut=None)
with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=True):
params = self._params(qcom=self._raw_bundle("small"), usbgpu=None)
with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=True):
assert get_active_bundle(params) is None
@@ -748,36 +748,36 @@ class TestEffectiveSource(OpenpilotTestCase):
return bundle.to_dict()
def test_runtime_no_gpu(self):
with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=False):
with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=False):
assert get_active_source() == "qcom"
def test_runtime_gpu_present(self):
with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=True):
assert get_active_source() == "chestnut"
with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=True):
assert get_active_source() == "usbgpu"
def test_display_offroad_gpu_present_shows_big(self):
assert get_active_source(chestnut=True, chestnut_active=False, chestnut_loading=False, offroad=True) == "chestnut"
assert get_active_source(usbgpu=True, usbgpu_active=False, usbgpu_loading=False, offroad=True) == "usbgpu"
def test_display_onroad_gpu_loading_shows_big(self):
assert get_active_source(chestnut=True, chestnut_active=False, chestnut_loading=True, offroad=False) == "chestnut"
assert get_active_source(usbgpu=True, usbgpu_active=False, usbgpu_loading=True, offroad=False) == "usbgpu"
def test_display_onroad_gpu_active_shows_big(self):
assert get_active_source(chestnut=True, chestnut_active=True, chestnut_loading=False, offroad=False) == "chestnut"
assert get_active_source(usbgpu=True, usbgpu_active=True, usbgpu_loading=False, offroad=False) == "usbgpu"
def test_display_onroad_gpu_idle_shows_small(self):
assert get_active_source(chestnut=True, chestnut_active=False, chestnut_loading=False, offroad=False) == "qcom"
assert get_active_source(usbgpu=True, usbgpu_active=False, usbgpu_loading=False, offroad=False) == "qcom"
def test_display_active_none_is_idle(self):
assert get_active_source(chestnut=True, chestnut_active=None, chestnut_loading=False, offroad=False) == "qcom"
assert get_active_source(usbgpu=True, usbgpu_active=None, usbgpu_loading=False, offroad=False) == "qcom"
def test_active_bundle_follows_source(self):
params = mock.MagicMock()
params.get.side_effect = lambda key: {"ModelManager_ActiveBundle": self._raw_bundle("small"),
"ModelManager_ActiveBundleChestnut": self._raw_bundle("big")}.get(key)
with mock.patch("openpilot.sunnypilot.models.helpers.chestnut_present", return_value=False):
"ModelManager_ActiveBundleUSBGPU": self._raw_bundle("big")}.get(key)
with mock.patch("openpilot.sunnypilot.models.helpers.usbgpu_present", return_value=False):
assert get_active_bundle(params).ref == "small"
assert get_selected_bundle(params, get_active_source(chestnut=True, chestnut_active=False,
chestnut_loading=False, offroad=True)).ref == "big"
assert get_selected_bundle(params, get_active_source(usbgpu=True, usbgpu_active=False,
usbgpu_loading=False, offroad=True)).ref == "big"
@unittest.skipUnless(os.environ.get('RUN_INTEGRATION_TESTS'), 'requires external network')
@@ -104,6 +104,14 @@ 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
@@ -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()
@@ -183,7 +183,7 @@ def getParamsMetadata() -> str:
schema["capability_labels"] = CAPABILITY_LABELS
schema["default_model"] = DEFAULT_MODEL
schema["default_big_model"] = DEFAULT_BIG_MODEL
schema["chestnut_active"] = params.get_bool("ChestnutActive")
schema["usbgpu_active"] = params.get_bool("UsbGpuActive")
raw = json.dumps(schema, separators=(",", ":")).encode("utf-8")
return base64.b64encode(gzip.compress(raw)).decode("utf-8")
except Exception:
@@ -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
+1 -1
View File
@@ -65,7 +65,7 @@ def sp_stats(end_event):
'MadsSteeringMode',
'MadsUnifiedEngagementMode',
'ModelManager_ActiveBundle',
'ModelManager_ActiveBundleChestnut',
'ModelManager_ActiveBundleUSBGPU',
'ModelManager_Favs',
'EnableSunnylinkUploader',
'SunnylinkEnabled',
@@ -89,20 +89,42 @@ def _migrate_model_bundle_slots(_params):
# ActiveBundle. Seed both slots; validation drops whichever does not match
# its own manifest.
try:
if _params.get("ModelManager_ActiveBundleChestnut") is not None:
return
if (chestnut_bundle := _params.get("ModelManager_ActiveBundleUSBGPU")) is not None:
_params.put("ModelManager_ActiveBundleChestnut", chestnut_bundle, block=True)
cloudlog.info("params_migration: seeded ModelManager_ActiveBundleChestnut from ModelManager_ActiveBundleUSBGPU")
if _params.get("ModelManager_ActiveBundleUSBGPU") is not None:
return
if (bundle := _params.get("ModelManager_ActiveBundle")) is None:
return
_params.put("ModelManager_ActiveBundleChestnut", bundle, block=True)
cloudlog.info("params_migration: seeded ModelManager_ActiveBundleChestnut from ModelManager_ActiveBundle")
_params.put("ModelManager_ActiveBundleUSBGPU", bundle, block=True)
cloudlog.info("params_migration: seeded ModelManager_ActiveBundleUSBGPU from ModelManager_ActiveBundle")
except Exception as e:
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:
@@ -140,5 +162,7 @@ def run_migration(_params):
# seed TeslaMadsScreenButton for existing Tesla installs
_migrate_tesla_mads_screen_button(_params)
# seed the chestnut model slot from the pre-split single slot
# seed the usbgpu model slot from the pre-split single slot
_migrate_model_bundle_slots(_params)
_migrate_assisted_driving_milestones(_params)
@@ -7,7 +7,44 @@ 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
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}
class TestModelBundleSlotMigration(OpenpilotTestCase):
@@ -15,22 +52,22 @@ class TestModelBundleSlotMigration(OpenpilotTestCase):
The migration seeds both slots; per-source validation later drops whichever does not
match its own manifest."""
def test_seeds_chestnut_slot_from_active_bundle(self):
def test_seeds_usbgpu_slot_from_active_bundle(self):
params = Params()
bundle = {"ref": "big", "minimumSelectorVersion": 18}
params.put("ModelManager_ActiveBundle", bundle, block=True)
_migrate_model_bundle_slots(params)
assert params.get("ModelManager_ActiveBundleChestnut") == bundle
assert params.get("ModelManager_ActiveBundleUSBGPU") == bundle
assert params.get("ModelManager_ActiveBundle") == bundle
def test_noop_when_chestnut_slot_already_set(self):
def test_noop_when_usbgpu_slot_already_set(self):
params = Params()
params.put("ModelManager_ActiveBundle", {"ref": "small"}, block=True)
params.put("ModelManager_ActiveBundleChestnut", {"ref": "big"}, block=True)
params.put("ModelManager_ActiveBundleUSBGPU", {"ref": "big"}, block=True)
_migrate_model_bundle_slots(params)
assert params.get("ModelManager_ActiveBundleChestnut") == {"ref": "big"}
assert params.get("ModelManager_ActiveBundleUSBGPU") == {"ref": "big"}
def test_noop_when_no_selection(self):
params = Params()
_migrate_model_bundle_slots(params)
assert params.get("ModelManager_ActiveBundleChestnut") is None
assert params.get("ModelManager_ActiveBundleUSBGPU") is None
+2 -2
View File
@@ -16,7 +16,7 @@ from openpilot.common.utils import strip_deprecated_keys
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params
from openpilot.common.realtime import DT_HW
from openpilot.selfdrive.modeld.helpers import MODELS_DIR, chestnut_compiled
from openpilot.selfdrive.modeld.helpers import MODELS_DIR, usbgpu_compiled
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
from openpilot.common.hardware import HARDWARE, COMMA_HARDWARE
from openpilot.common.basedir import BASEDIR
@@ -239,7 +239,7 @@ def hardware_thread(end_event, hw_queue) -> None:
fan_controller = FanController(int(1./DT_HW))
chestnut = Chestnut()
big_model_available = (MODELS_DIR / 'big_driving_supercombo.onnx').is_file() or chestnut_compiled()
big_model_available = (MODELS_DIR / 'big_driving_supercombo.onnx').is_file() or usbgpu_compiled()
while not end_event.is_set():
sm.update(PANDA_STATES_TIMEOUT)