Update model_replay.py

.github/workflows/model_replay.yaml

@@ -0,0 +1,148 @@
+name: Test Stock vs Sunnypilot Model Equivalence
+
+on:
+  workflow_dispatch:
+    inputs:
+      model_ref:
+        description: 'Upstream openpilot commit ref'
+        required: false
+        default: ''
+  pull_request:
+    paths:
+      - 'openpilot/selfdrive/modeld/**'
+      - 'openpilot/sunnypilot/modeld_v2/**'
+
+jobs:
+  test_stock_parity:
+    name: Compare Stock vs Sunnypilot Model Replay
+    runs-on: macos-latest
+
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          submodules: true
+      - run: ./tools/op.sh setup
+      - run: scons -j$(nproc 2>/dev/null || sysctl -n hw.logicalcpu) openpilot/cereal msgq_repo openpilot/common
+      - name: Fetch Big Model ONNX
+        run: |
+          mkdir -p /tmp/onnx_models
+          if [ -n "${{ inputs.model_ref }}" ]; then
+            echo "Fetching ONNX from upstream openpilot ref ${{ inputs.model_ref }}..."
+            git clone --depth 1 https://github.com/commaai/openpilot.git /tmp/upstream_openpilot
+            cd /tmp/upstream_openpilot
+            git fetch --depth 1 origin ${{ inputs.model_ref }}
+            git checkout ${{ inputs.model_ref }}
+            git lfs pull -I "**/selfdrive/modeld/models/big_driving_supercombo.onnx"
+            find . -name "big_driving_supercombo.onnx" -exec cp {} /tmp/onnx_models/ \;
+          else
+            cp openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx /tmp/onnx_models/
+          fi
+
+      - name: Compile models
+        env:
+          DEV: "CPU"
+          JIT_BATCH_SIZE: "0"
+        run: |
+          BIG_ONNX="/tmp/onnx_models/big_driving_supercombo.onnx"
+          MODEL_SIZE=$(python3 -c "from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE as s; print(f'{s[0]}x{s[1]}')")
+          CAMERA_RESOLUTIONS=$(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}')")
+
+          python3 openpilot/selfdrive/modeld/compile_modeld.py \
+            --onnx "$BIG_ONNX" \
+            --model-size "$MODEL_SIZE" \
+            --camera-resolutions $CAMERA_RESOLUTIONS \
+            --output /tmp/stock_model.pkl \
+            --frame-skip 4 \
+            --benchmark-runs 1 &
+
+          python3 openpilot/sunnypilot/modeld_v2/compile_modeld.py \
+            --model-type supercombo \
+            --supercombo-onnx "$BIG_ONNX" \
+            --model-size "$MODEL_SIZE" \
+            --camera-resolutions $CAMERA_RESOLUTIONS \
+            --output /tmp/sunnypilot_model.pkl \
+            --frame-skip 4 \
+            --benchmark-runs 1 &
+          wait
+
+      - name: Run model replay
+        env:
+          DEV: "CPU"
+        run: |
+          python3 openpilot/sunnypilot/modeld_v2/model_replay.py \
+            --sunnypilot-model /tmp/sunnypilot_model.pkl \
+            --stock-model /tmp/stock_model.pkl \
+            --frames 20 \
+            --plot-dir /tmp/replay_plots
+
+      - name: Upload Replay Plots
+        uses: actions/upload-artifact@v4
+        if: always()
+        continue-on-error: true
+        with:
+          name: model_replay_plots_${{ github.event.number || github.sha }}
+          path: /tmp/replay_plots
+
+      - name: Checkout ci-artifacts
+        if: github.repository == 'sunnypilot/sunnypilot' && github.event_name == 'pull_request'
+        uses: actions/checkout@v4
+        with:
+          repository: sunnypilot/ci-artifacts
+          ssh-key: ${{ secrets.CI_ARTIFACTS_DEPLOY_KEY }}
+          path: ${{ github.workspace }}/ci-artifacts
+
+      - name: Push plots to ci-artifacts
+        if: github.repository == 'sunnypilot/sunnypilot' && github.event_name == 'pull_request'
+        working-directory: ${{ github.workspace }}/ci-artifacts
+        run: |
+          git config user.name "GitHub Actions Bot"
+          git config user.email "<>"
+          BRANCH="model_replay_pr_${{ github.event.number }}"
+          git fetch origin $BRANCH || true
+          git checkout $BRANCH 2>/dev/null || git checkout --orphan $BRANCH
+          rm -rf plots && mkdir -p plots
+          cp /tmp/replay_plots/*.png plots/
+          echo "${{ github.sha }}" > ref_commit
+          git add plots ref_commit
+          git commit -m "Model replay plots for PR #${{ github.event.number }}@${{ github.sha }}" || echo "No changes to commit"
+          git push origin $BRANCH --force
+
+      - name: Comment Model Replay Report on PR
+        if: github.repository == 'sunnypilot/sunnypilot' && github.event_name == 'pull_request'
+        uses: actions/github-script@v7
+        with:
+          script: |
+            const fs = require('fs');
+            const prNumber = context.payload.pull_request.number;
+            const branch = `model_replay_pr_${prNumber}`;
+            const baseUrl = `https://raw.githubusercontent.com/sunnypilot/ci-artifacts/refs/heads/${branch}/plots`;
+
+            const priorityPlots = ['desiredCurvature.png', 'desiredAcceleration.png', 'velocity.x.png', 'leadsV3.x.png', 'execution_timings.png'];
+            const allFiles = fs.readdirSync('/tmp/replay_plots').filter(f => f.endsWith('.png'));
+            const orderedFiles = [
+              ...priorityPlots.filter(f => allFiles.includes(f)),
+              ...allFiles.filter(f => !priorityPlots.includes(f)).sort()
+            ];
+
+            let table = '<table>';
+            for (let i = 0; i < orderedFiles.length; i += 2) {
+              table += '<tr>';
+              table += `<td><img src="${baseUrl}/${orderedFiles[i]}" alt="${orderedFiles[i]}"><br><b>${orderedFiles[i].replace('.png', '')}</b></td>`;
+              if (i + 1 < orderedFiles.length) {
+                table += `<td><img src="${baseUrl}/${orderedFiles[i+1]}" alt="${orderedFiles[i+1]}"><br><b>${orderedFiles[i+1].replace('.png', '')}</b></td>`;
+              } else {
+                table += '<td></td>';
+              }
+              table += '</tr>';
+            }
+            table += '</table>';
+
+            const body = `### Model Replay Parity Report for PR #${prNumber} (@${context.sha.substring(0, 7)})\n\n` +
+              `<details><summary>All Model Replay Plots</summary>\n\n${table}\n\n</details>`;
+
+            await github.rest.issues.createComment({
+              owner: context.repo.owner,
+              repo: context.repo.repo,
+              issue_number: prNumber,
+              body: body
+            });

openpilot/common/params_keys.h

@@ -139,7 +139,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"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}},
     {"ChestnutModelError", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}},
-    {"AuxPowerSave", {PERSISTENT | BACKUP, BOOL}},
     {"Version", {PERSISTENT, STRING}},
 
     // --- sunnypilot params --- //

openpilot/selfdrive/ui/ui_state.py

@@ -225,7 +225,6 @@ class UIState(UIStateSP):
                              ChestnutState.UNCOMPILED if detected else ChestnutState.DISCONNECTED)
       return
 
-    self.chestnut_present = self.chestnut_present or detected
     model_seen = self.sm.recv_frame["modelV2"] > self.started_frame
     if not self.chestnut_present:
       self.chestnut_state = ChestnutState.DISCONNECTED

openpilot/sunnypilot/modeld_v2/model_replay.py

@@ -0,0 +1,197 @@
+"""
+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.
+"""
+
+import argparse
+import os
+import sys
+import time
+import matplotlib.pyplot as plt
+import numpy as np
+from tinygrad.device import Device
+
+from openpilot.common.file_chunker import open_file_chunked
+from openpilot.selfdrive.modeld.compile_modeld import MODELD_INPUTS, make_input_queues
+from openpilot.selfdrive.modeld.helpers import load_oob
+from openpilot.selfdrive.test.process_replay.model_replay import SEGMENT, TEST_ROUTE
+from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
+from openpilot.tools.lib.framereader import FrameReader
+from openpilot.tools.lib.openpilotci import get_url
+from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, nv12_copy_size
+from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser
+
+
+def get_replay_video_source(route_or_path=None, segment_index=SEGMENT, camera_type="fcamera.hevc"):
+  if route_or_path and os.path.exists(route_or_path):
+    return route_or_path
+  selected_route = route_or_path or TEST_ROUTE
+  return get_url(selected_route, segment_index, camera_type)
+
+
+def initialize_replay_queues(model_dictionary, device="CPU"):
+  metadata = model_dictionary.get("metadata", {})
+  model_meta = metadata.get("model", metadata)
+  input_shapes = model_meta.get("input_shapes", {})
+  cam_resolutions = list(model_dictionary.get("run_model", {}).keys())
+  cam_width, cam_height = cam_resolutions[0] if cam_resolutions else (1928, 1208)
+  nv12_info = get_nv12_info(cam_width, cam_height)
+  frame_copy_size = nv12_copy_size(nv12_info[0], nv12_info[1], nv12_info[2])
+  frame_skip = model_meta.get("frame_skip") or derive_frame_skip({}, input_shapes)
+  queues, npy_views, frame_views = make_input_queues(input_shapes, frame_skip, device, frame_copy_size)
+  if "tfm" in npy_views:
+    npy_views["tfm"][:] = np.eye(3, dtype=np.float32)
+  if "big_tfm" in npy_views:
+    npy_views["big_tfm"][:] = np.eye(3, dtype=np.float32)
+  if "traffic_convention" in npy_views:
+    npy_views["traffic_convention"][:] = np.array([1.0, 0.0], dtype=np.float32)
+  return queues, npy_views, frame_views, model_meta
+
+
+def replay_model_on_frames(model_path, frame_reader, number_of_frames=20):
+  with open_file_chunked(model_path) as file_handle:
+    model_data = load_oob(file_handle)
+  run_model_dict = model_data.get("run_model", {})
+  runner = next(iter(run_model_dict.values()), None)
+  if runner is None:
+    raise ValueError("Failed to resolve runner from model dictionary")
+
+  queues, npy_views, frame_views, model_meta = initialize_replay_queues(model_data)
+  output_slices = model_meta.get("output_slices", {})
+  hidden_state_slice = output_slices.get("hidden_state")
+  parser = Parser(ignore_missing=True)
+  recorded_outputs = []
+  max_frames = min(number_of_frames, getattr(frame_reader, "frame_count", number_of_frames))
+
+  for frame_index in range(max_frames):
+    frame_raw = frame_reader.get(frame_index)
+    if frame_raw is not None:
+      for view in frame_views.values():
+        copy_length = min(view.size, frame_raw.size)
+        view.flat[:copy_length] = frame_raw.flat[:copy_length]
+
+    execution_arguments = {key: queues[key] for key in MODELD_INPUTS if key in queues}
+    execution_start = time.perf_counter()
+    step_output = runner(**execution_arguments)
+    Device.default.synchronize()
+    step_duration = time.perf_counter() - execution_start
+
+    output_array = (step_output[0].numpy() if hasattr(step_output[0], "numpy") else np.array(step_output[0]))
+    flat_output = output_array.flatten()
+
+    if hidden_state_slice and "prev_feat" in npy_views:
+      features_flat = flat_output[hidden_state_slice]
+      target_slice = min(features_flat.size, npy_views["prev_feat"].size)
+      npy_views["prev_feat"].flat[:target_slice] = features_flat[:target_slice]
+
+    sliced_outputs = {slice_name: flat_output[np.newaxis, slice_range] for slice_name, slice_range in output_slices.items()}
+    parser.parse_outputs(sliced_outputs)
+
+    recorded_outputs.append({
+      "frame_index": frame_index,
+      "raw_output": output_array,
+      "parsed_outputs": sliced_outputs,
+      "execution_time": step_duration,
+    })
+  return recorded_outputs
+
+
+def plot_comparison(series_a, series_b, title, output_directory, label_a="modeld_v2 model", label_b="stock"):
+  os.makedirs(output_directory, exist_ok=True)
+  figure, axis = plt.subplots()
+  axis.plot(series_b, label=label_b)
+  axis.plot(series_a, label=label_a, linestyle="--")
+  axis.set_title(title)
+  axis.legend(loc="best")
+  plot_path = os.path.join(output_directory, f"{title}.png")
+  figure.savefig(plot_path)
+  plt.close(figure)
+  return plot_path
+
+
+def compare_models_on_route(new_model_path, old_model_path, route_or_path=None, segment_index=SEGMENT,
+                            number_of_frames=20, tolerance=1e-4, label_a="modeld_v2 model", label_b="stock",
+                            plot_directory=None, enforce_timings=False):
+  video_url_or_path = get_replay_video_source(route_or_path, segment_index)
+  frame_reader = FrameReader(video_url_or_path, pix_fmt="nv12")
+  old_results = replay_model_on_frames(old_model_path, frame_reader, number_of_frames)
+  new_results = replay_model_on_frames(new_model_path, frame_reader, number_of_frames)
+
+  for step_index, (new_step, old_step) in enumerate(zip(new_results, old_results, strict=True)):
+    new_array = new_step["raw_output"]
+    old_array = old_step["raw_output"]
+    if not np.allclose(new_array, old_array, atol=tolerance, rtol=tolerance):
+      max_absolute_error = np.max(np.abs(new_array - old_array))
+      sys.stderr.write(
+        f"Replay mismatch at frame {step_index}: max absolute error {max_absolute_error:.6f} exceeds tolerance {tolerance}\n"
+      )
+      return False
+
+  if len(new_results) > 1 and len(old_results) > 1:
+    new_timings = [step["execution_time"] * 1000.0 for step in new_results[1:] if "execution_time" in step]
+    old_timings = [step["execution_time"] * 1000.0 for step in old_results[1:] if "execution_time" in step]
+    if new_timings and old_timings:
+      print("------------------------------------------------")
+      print("----------------- Model Timing -----------------")
+      print("------------------------------------------------")
+      print(f"{label_a}: avg {np.mean(new_timings):6.2f} ms | max {np.max(new_timings):6.2f} ms")
+      print(f"{label_b}: avg {np.mean(old_timings):6.2f} ms | max {np.max(old_timings):6.2f} ms")
+
+      if plot_directory:
+        first_step_outputs = new_results[0].get("parsed_outputs", {})
+        if "action" in first_step_outputs:
+          series_a_curv = [step["parsed_outputs"]["action"].flatten()[0] for step in new_results]
+          series_b_curv = [step["parsed_outputs"]["action"].flatten()[0] for step in old_results]
+          plot_comparison(series_a_curv, series_b_curv, "desiredCurvature", plot_directory, label_a, label_b)
+
+          series_a_accel = [step["parsed_outputs"]["action"].flatten()[1] for step in new_results]
+          series_b_accel = [step["parsed_outputs"]["action"].flatten()[1] for step in old_results]
+          plot_comparison(series_a_accel, series_b_accel, "desiredAcceleration", plot_directory, label_a, label_b)
+
+        if "plan" in first_step_outputs:
+          series_a_vel = [step["parsed_outputs"]["plan"].flatten()[0] for step in new_results]
+          series_b_vel = [step["parsed_outputs"]["plan"].flatten()[0] for step in old_results]
+          plot_comparison(series_a_vel, series_b_vel, "velocity.x", plot_directory, label_a, label_b)
+
+        if "lead" in first_step_outputs:
+          series_a_lead = [step["parsed_outputs"]["lead"].flatten()[0] for step in new_results]
+          series_b_lead = [step["parsed_outputs"]["lead"].flatten()[0] for step in old_results]
+          plot_comparison(series_a_lead, series_b_lead, "leadsV3.x", plot_directory, label_a, label_b)
+
+        plot_comparison(new_timings, old_timings, "execution_timings", plot_directory, label_a, label_b)
+
+        for slice_name in first_step_outputs:
+          series_a = [np.mean(step["parsed_outputs"][slice_name]) for step in new_results if slice_name in step["parsed_outputs"]]
+          series_b = [np.mean(step["parsed_outputs"][slice_name]) for step in old_results if slice_name in step["parsed_outputs"]]
+          if series_a and series_b:
+            plot_comparison(series_a, series_b, f"output_{slice_name}", plot_directory, label_a, label_b)
+  print(f"Replay comparison result on route ({label_a} vs {label_b}): True")
+  return True
+
+
+if __name__ == "__main__":
+  argument_parser = argparse.ArgumentParser(description="Model Replay on Real Driving Video")
+  argument_parser.add_argument("--sunnypilot-model", dest="model_a", default=None)
+  argument_parser.add_argument("--stock-model", dest="model_b", default=None)
+  argument_parser.add_argument("--route", default=TEST_ROUTE)
+  argument_parser.add_argument("--segment", type=int, default=SEGMENT)
+  argument_parser.add_argument("--frames", type=int, default=20)
+  argument_parser.add_argument("--plot-dir", default=None)
+  parsed_arguments = argument_parser.parse_args()
+
+  if not parsed_arguments.model_a:
+    argument_parser.error("Must provide either --sunnypilot-model, --new-model, or --model-path-a")
+
+  if parsed_arguments.model_b:
+    matches = compare_models_on_route(parsed_arguments.model_a, parsed_arguments.model_b, route_or_path=parsed_arguments.route,
+                                      segment_index=parsed_arguments.segment, number_of_frames=parsed_arguments.frames,
+                                      tolerance=1e-4, label_a="modeld_v2 model",
+                                      label_b="stock", plot_directory=parsed_arguments.plot_dir)
+    if not matches:
+      sys.exit(1)
+  else:
+    source_url = get_replay_video_source(parsed_arguments.route, parsed_arguments.segment)
+    reader = FrameReader(source_url, pix_fmt="nv12")
+    results = replay_model_on_frames(parsed_arguments.model_a, reader, parsed_arguments.frames)

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -1675,12 +1675,6 @@
               "widget": "toggle",
               "title": "Onroad Uploads"
             },
-            {
-              "key": "AuxPowerSave",
-              "widget": "toggle",
-              "title": "Disable Aux Port When Offroad",
-              "description": "Power off the aux USB-C port while offroad to save power. It powers back on automatically when you go onroad."
-            },
             {
               "key": "MaxTimeOffroad",
               "widget": "option",

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/device.yaml

@@ -30,10 +30,6 @@ sections:
   - key: OnroadUploads
     widget: toggle
     title: Onroad Uploads
-  - key: AuxPowerSave
-    widget: toggle
-    title: Disable Aux Port When Offroad
-    description: Power off the aux USB-C port while offroad to save power. It powers back on automatically when you go onroad.
   - key: MaxTimeOffroad
     widget: option
     title: Max Time Offroad

openpilot/system/hardware/hardwared.py

@@ -21,7 +21,6 @@ from openpilot.common.hardware import HARDWARE, COMMA_HARDWARE
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.git import get_short_branch
 from openpilot.common.hardware.usb import CHESTNUT_FW_VERSION, CHESTNUT_USB_PRODUCT, get_usb_state, get_usb_topology, is_chestnut_usb_id, set_usb_state
-from openpilot.system.hardware.chestnut.flash import VBUS_PATH
 from openpilot.common.linux import LinuxSystemStats
 from openpilot.system.loggerd.config import get_available_percent
 from openpilot.common.swaglog import cloudlog
@@ -51,10 +50,6 @@ class Chestnut:
     self.last_attempt = 0.
     self.flashed = False
     self.mismatch = False
-    self.vbus_on = None
-    self.params = Params()
-    self.powersave = False
-    self.last_offroad = None
 
   @property
   def failed(self) -> bool:
@@ -66,19 +61,9 @@ class Chestnut:
     cloudlog.event("chestnut flash done", returncode=ret.returncode, output=ret.stdout[-1000:], error=ret.returncode != 0)
     self.flashed = ret.returncode == 0
 
-  def set_vbus(self, on: bool) -> None:
-    if on == self.vbus_on:
-      return
-    subprocess.run(["sudo", "tee", VBUS_PATH], input=b"1" if on else b"0", stdout=subprocess.DEVNULL, check=False)
-    self.vbus_on = on
-
   def update(self, offroad: bool, usb_state: list[dict]) -> None:
     self.mismatch = any(is_chestnut_usb_id(d["vendorId"], d["productId"], include_bootloader=True) and
                         d["product"] != CHESTNUT_USB_PRODUCT for d in usb_state)
-    if offroad != self.last_offroad:
-      self.powersave = self.params.get_bool("AuxPowerSave")
-      self.last_offroad = offroad
-    self.set_vbus((not offroad or self.mismatch) or not self.powersave)
     if not self.mismatch:
       self.flashed = False
       return