From 763049f06886f59f9fd5975b0e7576551ecb26c0 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sat, 27 Dec 2025 17:49:26 +0100 Subject: [PATCH 1/4] SL: Re enable and validate ingestion of swaglogs (#1580) * Re enable and validate ingestion --- sunnypilot/sunnylink/athena/sunnylinkd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sunnypilot/sunnylink/athena/sunnylinkd.py b/sunnypilot/sunnylink/athena/sunnylinkd.py index 9f70aead5..d1a03778c 100755 --- a/sunnypilot/sunnylink/athena/sunnylinkd.py +++ b/sunnypilot/sunnylink/athena/sunnylinkd.py @@ -62,7 +62,7 @@ def handle_long_poll(ws: WebSocket, exit_event: threading.Event | None) -> None: threading.Thread(target=ws_ping, args=(ws, end_event), name='ws_ping'), threading.Thread(target=ws_queue, args=(end_event,), name='ws_queue'), threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler'), - # threading.Thread(target=sunny_log_handler, args=(end_event, comma_prime_cellular_end_event), name='log_handler'), + threading.Thread(target=sunny_log_handler, args=(end_event, comma_prime_cellular_end_event), name='log_handler'), threading.Thread(target=stat_handler, args=(end_event, Paths.stats_sp_root(), True), name='stat_handler'), ] + [ threading.Thread(target=jsonrpc_handler, args=(end_event, partial(startLocalProxy, end_event),), name=f'worker_{x}') From 9442bc9aeccf979a90fcc7e1e1b291ea630559df Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Mon, 29 Dec 2025 14:55:39 -0700 Subject: [PATCH 2/4] modeld_v2: planplus model tuning (#1620) * modeld: planplus model tuning * little more * final --------- Co-authored-by: Jason Wen --- common/params_keys.h | 1 + sunnypilot/modeld_v2/modeld.py | 6 +- .../modeld_v2/tests/test_recovery_power.py | 61 +++++++++++++++++++ sunnypilot/sunnylink/params_metadata.json | 7 +++ 4 files changed, 73 insertions(+), 2 deletions(-) create mode 100644 sunnypilot/modeld_v2/tests/test_recovery_power.py diff --git a/common/params_keys.h b/common/params_keys.h index 7cfdf7540..89f70471c 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -224,6 +224,7 @@ inline static std::unordered_map keys = { {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}}, {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}}, {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}}, + {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}}, // mapd {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}}, diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index 82eb099e7..b3b1d35fd 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -28,7 +28,6 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle from openpilot.sunnypilot.models.runners.helpers import get_model_runner PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad" -RECOVERY_POWER = 1.0 # The higher this number the more aggressively the model will recover to lanecenter, too high and it will ping-pong class FrameMeta: @@ -63,6 +62,7 @@ class ModelState(ModelStateBase): self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0")) self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0")) self.MIN_LAT_CONTROL_SPEED = 0.3 + self.PLANPLUS_CONTROL: float = 1.0 buffer_length = 5 if self.model_runner.is_20hz else 2 self.frames = {name: DrivingModelFrame(context, buffer_length) for name in self.model_runner.vision_input_names} @@ -158,7 +158,8 @@ class ModelState(ModelStateBase): lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action: plan = model_output['plan'][0] if 'planplus' in model_output: - plan = plan + RECOVERY_POWER*model_output['planplus'][0] + recovery_power = self.PLANPLUS_CONTROL * (0.75 if v_ego > 20.0 else 1.0) + plan = plan + recovery_power * model_output['planplus'][0] desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS, action_t=long_action_t) desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS) @@ -283,6 +284,7 @@ def main(demo=False): v_ego = max(sm["carState"].vEgo, 0.) if sm.frame % 60 == 0: model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay) + model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True) lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) diff --git a/sunnypilot/modeld_v2/tests/test_recovery_power.py b/sunnypilot/modeld_v2/tests/test_recovery_power.py new file mode 100644 index 000000000..e38b2c86d --- /dev/null +++ b/sunnypilot/modeld_v2/tests/test_recovery_power.py @@ -0,0 +1,61 @@ +import numpy as np + +from cereal import log + +from openpilot.sunnypilot.modeld_v2.constants import Plan +from openpilot.sunnypilot.modeld_v2.modeld import ModelState +import openpilot.sunnypilot.modeld_v2.modeld as modeld + + +class MockStruct: + def __init__(self, **kwargs): + for k, v in kwargs.items(): + setattr(self, k, v) + + +def test_recovery_power_scaling(): + state = MockStruct( + PLANPLUS_CONTROL=1.0, + LONG_SMOOTH_SECONDS=0.3, + LAT_SMOOTH_SECONDS=0.1, + MIN_LAT_CONTROL_SPEED=0.3, + mlsim=True, + generation=12, + constants=MockStruct(T_IDXS=np.arange(100), DESIRE_LEN=8) + ) + prev_action = log.ModelDataV2.Action() + recorded_vel: list = [] + + def mock_accel(plan_vel, plan_accel, t_idxs, action_t=0.0): + recorded_vel.append(plan_vel.copy()) + return 0.0, False + + modeld.get_accel_from_plan = mock_accel + modeld.get_curvature_from_output = lambda *args: 0.0 + plan = np.random.rand(1, 100, 15).astype(np.float32) + planplus = np.random.rand(1, 100, 15).astype(np.float32) + + model_output: dict = { + 'plan': plan.copy(), + 'planplus': planplus.copy() + } + + test_cases: list = [ + # (control, v_ego, expected_factor) + (0.55, 20.0, 1.0), + (1.0, 25.0, .75), + (1.5, 25.1, 0.75), + (2.0, 20.0, 1.0), + (0.75, 19.0, 1.0), + (0.8, 25.1, 0.75), + ] + + for control, v_ego, factor in test_cases: + state.PLANPLUS_CONTROL = control + recorded_vel.clear() + ModelState.get_action_from_model(state, model_output, prev_action, 0.0, 0.0, v_ego) + + expected_recovery_power = control * factor + expected_plan_vel = plan[0, :, Plan.VELOCITY][:, 0] + expected_recovery_power * planplus[0, :, Plan.VELOCITY][:, 0] + + np.testing.assert_allclose(recorded_vel[0], expected_plan_vel, rtol=1e-5, atol=1e-6) diff --git a/sunnypilot/sunnylink/params_metadata.json b/sunnypilot/sunnylink/params_metadata.json index bbefc3d0c..f335457fb 100644 --- a/sunnypilot/sunnylink/params_metadata.json +++ b/sunnypilot/sunnylink/params_metadata.json @@ -826,6 +826,13 @@ "title": "Panda Som Reset Triggered", "description": "" }, + "PlanplusControl": { + "title": "Plan Plus Controls", + "description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lanecenter, too high and it will ping-pong", + "min": 0.0, + "max": 2.0, + "step": 0.1 + }, "PrimeType": { "title": "Prime Type", "description": "" From 6df313b974ef2d7a779c23b1540b30c5eaed283c Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Mon, 29 Dec 2025 15:01:58 -0700 Subject: [PATCH 3/4] modeld_v2: remove dead test (#1621) Co-authored-by: Jason Wen --- sunnypilot/modeld_v2/tests/test_modeld.py | 102 ---------------------- 1 file changed, 102 deletions(-) delete mode 100644 sunnypilot/modeld_v2/tests/test_modeld.py diff --git a/sunnypilot/modeld_v2/tests/test_modeld.py b/sunnypilot/modeld_v2/tests/test_modeld.py deleted file mode 100644 index 6927c9e47..000000000 --- a/sunnypilot/modeld_v2/tests/test_modeld.py +++ /dev/null @@ -1,102 +0,0 @@ -import numpy as np -import random - -import cereal.messaging as messaging -from msgq.visionipc import VisionIpcServer, VisionStreamType -from opendbc.car.car_helpers import get_demo_car_params -from openpilot.common.params import Params -from openpilot.common.transformations.camera import DEVICE_CAMERAS -from openpilot.common.realtime import DT_MDL -from openpilot.system.manager.process_config import managed_processes -from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state - -CAM = DEVICE_CAMERAS[("tici", "ar0231")].fcam -IMG = np.zeros(int(CAM.width*CAM.height*(3/2)), dtype=np.uint8) -IMG_BYTES = IMG.flatten().tobytes() - - -class TestModeld: - - def setup_method(self): - self.vipc_server = VisionIpcServer("camerad") - self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, CAM.width, CAM.height) - self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, CAM.width, CAM.height) - self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, CAM.width, CAM.height) - self.vipc_server.start_listener() - Params().put("CarParams", get_demo_car_params().to_bytes()) - - self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry']) - self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'liveCalibration']) - - managed_processes['modeld'].start() - self.pm.wait_for_readers_to_update("roadCameraState", 10) - - def teardown_method(self): - managed_processes['modeld'].stop() - del self.vipc_server - - def _send_frames(self, frame_id, cams=None): - if cams is None: - cams = ('roadCameraState', 'wideRoadCameraState') - - cs = None - for cam in cams: - msg = messaging.new_message(cam) - cs = getattr(msg, cam) - cs.frameId = frame_id - cs.timestampSof = int((frame_id * DT_MDL) * 1e9) - cs.timestampEof = int(cs.timestampSof + (DT_MDL * 1e9)) - cam_meta = meta_from_camera_state(cam) - - self.pm.send(msg.which(), msg) - self.vipc_server.send(cam_meta.stream, IMG_BYTES, cs.frameId, - cs.timestampSof, cs.timestampEof) - return cs - - def _wait(self): - self.sm.update(5000) - if self.sm['modelV2'].frameId != self.sm['cameraOdometry'].frameId: - self.sm.update(1000) - - def test_modeld(self): - for n in range(1, 500): - cs = self._send_frames(n) - self._wait() - - mdl = self.sm['modelV2'] - assert mdl.frameId == n - assert mdl.frameIdExtra == n - assert mdl.timestampEof == cs.timestampEof - assert mdl.frameAge == 0 - assert mdl.frameDropPerc == 0 - - odo = self.sm['cameraOdometry'] - assert odo.frameId == n - assert odo.timestampEof == cs.timestampEof - - def test_dropped_frames(self): - """ - modeld should only run on consecutive road frames - """ - frame_id = -1 - road_frames = list() - for n in range(1, 50): - if (random.random() < 0.1) and n > 3: - cams = random.choice([(), ('wideRoadCameraState', )]) - self._send_frames(n, cams) - else: - self._send_frames(n) - road_frames.append(n) - self._wait() - - if len(road_frames) < 3 or road_frames[-1] - road_frames[-2] == 1: - frame_id = road_frames[-1] - - mdl = self.sm['modelV2'] - odo = self.sm['cameraOdometry'] - assert mdl.frameId == frame_id - assert mdl.frameIdExtra == frame_id - assert odo.frameId == frame_id - if n != frame_id: - assert not self.sm.updated['modelV2'] - assert not self.sm.updated['cameraOdometry'] From edeede5e82282369e0e5f8e302de0863159efb3d Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Mon, 29 Dec 2025 20:03:08 -0700 Subject: [PATCH 4/4] modeld_v2: conditional model compilation for metadrive testing (#1623) * modeld_v2: conditional model compilation for PC * full send * shebang --------- Co-authored-by: Jason Wen --- sunnypilot/modeld_v2/SConscript | 36 +++++++++ sunnypilot/modeld_v2/install_models_pc.py | 89 +++++++++++++++++++++++ sunnypilot/models/runners/helpers.py | 10 +-- 3 files changed, 126 insertions(+), 9 deletions(-) create mode 100755 sunnypilot/modeld_v2/install_models_pc.py diff --git a/sunnypilot/modeld_v2/SConscript b/sunnypilot/modeld_v2/SConscript index 4c04b7382..750a242ad 100644 --- a/sunnypilot/modeld_v2/SConscript +++ b/sunnypilot/modeld_v2/SConscript @@ -1,3 +1,4 @@ +import os import glob Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'visionipc', 'transformations') @@ -28,3 +29,38 @@ for pathdef, fn in {'TRANSFORM': 'transforms/transform.cl', 'LOADYUV': 'transfor cython_libs = envCython["LIBS"] + libs commonmodel_lib = lenv.Library('commonmodel', common_src) lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks) +tinygrad_files = ["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + "/**", recursive=True, root_dir=env.Dir("#").abspath) if 'pycache' not in x] + +# Get model metadata +PC = not os.path.isfile('/TICI') +if PC: + inputs = tinygrad_files + [File(Dir("#sunnypilot/modeld_v2").File("install_models_pc.py").abspath)] + outputs = [] + model_dir = Dir("models").abspath + cmd = f'python3 {Dir("#sunnypilot/modeld_v2").abspath}/install_models_pc.py {model_dir}' + + for model_name in ['supercombo', 'driving_vision', 'driving_policy']: + if File(f"models/{model_name}.onnx").exists(): + inputs.append(File(f"models/{model_name}.onnx")) + inputs.append(File(f"models/{model_name}_tinygrad.pkl")) + outputs.append(File(f"models/{model_name}_metadata.pkl")) + if outputs: + lenv.Command(outputs, inputs, cmd) + +def tg_compile(flags, model_name): + pythonpath_string = 'PYTHONPATH="${PYTHONPATH}:' + env.Dir("#tinygrad_repo").abspath + '"' + fn = File(f"models/{model_name}").abspath + return lenv.Command( + fn + "_tinygrad.pkl", + [fn + ".onnx"] + tinygrad_files, + f'{pythonpath_string} {flags} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {fn}_tinygrad.pkl' + ) + +# Compile small models +for model_name in ['supercombo', 'driving_vision', 'driving_policy']: + if File(f"models/{model_name}.onnx").exists(): + flags = { + 'larch64': 'DEV=QCOM', + 'Darwin': f'DEV=CPU HOME={os.path.expanduser("~")} IMAGE=0', # tinygrad calls brew which needs a $HOME in the env + }.get(arch, 'DEV=CPU CPU_LLVM=1 IMAGE=0') + tg_compile(flags, model_name) diff --git a/sunnypilot/modeld_v2/install_models_pc.py b/sunnypilot/modeld_v2/install_models_pc.py new file mode 100755 index 000000000..3f964dc28 --- /dev/null +++ b/sunnypilot/modeld_v2/install_models_pc.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +import sys +import shutil +import pickle +import codecs +import onnx +from pathlib import Path + +from openpilot.system.hardware.hw import Paths + + +def get_name_and_shape(value_info): + shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim]) + return value_info.name, shape + + +def get_metadata_value_by_name(model, name): + for prop in model.metadata_props: + if prop.key == name: + return prop.value + return None + + +def generate_metadata_pkl(model_path, output_path): + try: + model = onnx.load(str(model_path)) + output_slices = get_metadata_value_by_name(model, 'output_slices') + + if output_slices: + metadata = { + 'model_checkpoint': get_metadata_value_by_name(model, 'model_checkpoint'), + 'output_slices': pickle.loads(codecs.decode(output_slices.encode(), "base64")), + 'input_shapes': dict([get_name_and_shape(x) for x in model.graph.input]), + 'output_shapes': dict([get_name_and_shape(x) for x in model.graph.output]) + } + with open(output_path, 'wb') as f: + pickle.dump(metadata, f) + return True + else: + return False + except Exception: + return False + + +def install_models(model_dir): + model_dir = Path(model_dir) + models = ["driving_policy", "driving_vision"] + found_models = [] + + for model in models: + if (model_dir / f"{model}.onnx").exists(): + found_models.append(model) + + if not found_models: + return + + try: + custom_name = input(f"Found models ({', '.join(found_models)}). Enter model short name (e.g. wmiv4): ").strip() + except EOFError: + return + + if not custom_name: + print("No name provided, skipping installation.") + return + + dest_dir = Path(Paths.model_root()) + dest_dir.mkdir(parents=True, exist_ok=True) + + for model in found_models: + onnx_path = model_dir / f"{model}.onnx" + tinygrad_pkl = model_dir / f"{model}_tinygrad.pkl" + metadata_pkl = model_dir / f"{model}_metadata.pkl" + + if not metadata_pkl.exists(): + generate_metadata_pkl(onnx_path, metadata_pkl) + + dest_tinygrad = dest_dir / f"{model}_{custom_name}_tinygrad.pkl" + dest_metadata = dest_dir / f"{model}_{custom_name}_metadata.pkl" + + if tinygrad_pkl.exists(): + shutil.move(str(tinygrad_pkl), str(dest_tinygrad)) + if metadata_pkl.exists(): + shutil.move(str(metadata_pkl), str(dest_metadata)) + +if __name__ == "__main__": + if len(sys.argv) < 2: + print("Usage: install_models_pc.py ") + sys.exit(1) + install_models(sys.argv[1]) diff --git a/sunnypilot/models/runners/helpers.py b/sunnypilot/models/runners/helpers.py index 6a128b340..8f9d8fc2f 100644 --- a/sunnypilot/models/runners/helpers.py +++ b/sunnypilot/models/runners/helpers.py @@ -2,25 +2,17 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle from openpilot.sunnypilot.models.runners.model_runner import ModelRunner from openpilot.sunnypilot.models.runners.tinygrad.tinygrad_runner import TinygradRunner, TinygradSplitRunner from openpilot.sunnypilot.models.runners.constants import ModelType -from openpilot.system.hardware import TICI -if not TICI: - from openpilot.sunnypilot.models.runners.onnx.onnx_runner import ONNXRunner def get_model_runner() -> ModelRunner: """ Factory function to create and return the appropriate ModelRunner instance. - Selects between ONNXRunner (for non-TICI platforms) and TinygradRunner - (for TICI platforms), choosing TinygradSplitRunner if separate vision/policy + Selects TinygradRunner, choosing TinygradSplitRunner if separate vision/policy models are detected in the active bundle. :return: An instance of a ModelRunner subclass (ONNXRunner, TinygradRunner, or TinygradSplitRunner). """ - if not TICI: - return ONNXRunner() - - # On TICI platforms, use Tinygrad runners bundle = get_active_bundle() if bundle and bundle.models: model_types = {m.type.raw for m in bundle.models}