Ford: experiment with earlier C1 release from model path shape

Bound the model heading request toward zero using terminal spatial
curvature at the existing preview point. Preserve C0, command limits,
slew, 20 Hz cadence, and the existing Sunnylink selection.

This changes requests before some turn peaks as well as during unwind.
Document that authority tradeoff and distinguish command-level evidence
from unverified physical PSCM release.

Validation: Ford controller/sender/safety tests, Sunnylink schema tests,
independent geometry and replay tests, 28,138 stress CAN round trips,
and 35,775 recorded cycles with 71,550 baseline/candidate CAN checks.

Assisted-by: OpenAI Codex

.gitattributes

@@ -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

.github/CODEOWNERS

@@ -1,11 +0,0 @@
-*       @sunnypilot/dev-internal
-/.github/ @devtekve @sunnyhaibin
-/release/ci/ @devtekve @sunnyhaibin
-/tinygrad_repo @devtekve @Discountchubbs
-/tinygrad/ @devtekve @Discountchubbs
-/selfdrive/controls/lib/longitudinal_planner.py @devtekve @Discountchubbs
-/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @devtekve @Discountchubbs
-/selfdrive/modeld/ @devtekve @Discountchubbs
-/sunnypilot/model* @devtekve @Discountchubbs
-/sunnypilot/sunnylink/ @devtekve
-/system/athena/ @devtekve

.github/workflows/build-all-tinygrad-models.yaml

@@ -8,13 +8,13 @@ on:
         required: true
         type: string
       target_hardware:
-        description: 'Hardware target to compile for (qcom or usbgpu)'
+        description: 'Hardware target to compile for (qcom or chestnut)'
         required: true
         type: choice
         default: 'qcom'
         options:
           - qcom
-          - usbgpu
+          - chestnut
       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 == 'usbgpu' && 'usbgpu_' || '' }}v"
+          PREFIX="driving_models_${{ inputs.target_hardware == 'chestnut' && 'chestnut_' || '' }}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"
@@ -78,6 +78,7 @@ jobs:
       - name: Get next recompiled dir number
         id: create-recompiled-dir
         env:
+          HF_TOKEN: ${{ secrets.HF_TOKEN }}
           HF_REPO: ${{ github.event.inputs.hf_repo }}
         run: |
           pip install huggingface_hub

.github/workflows/build-default-big-model.yaml

@@ -1,83 +0,0 @@
-name: Build default big model
-
-on:
-  workflow_dispatch:
-
-env:
-  HF_REPO: sunnypilot/sunnypilot_models_v1
-  HF_DEFAULTS_PATH: models/defaults/big
-
-jobs:
-  resolve_name:
-    runs-on: ubuntu-24.04
-    outputs:
-      model_name: ${{ steps.name.outputs.model_name }}
-      onnx_ref: ${{ steps.name.outputs.onnx_ref }}
-    steps:
-      - uses: actions/checkout@v4
-      - id: name
-        run: |
-          NAME=$(PYTHONPATH=${{ github.workspace }} python3 -c "from openpilot.sunnypilot.models.model_name import DEFAULT_BIG_MODEL; print(DEFAULT_BIG_MODEL)")
-          ONNX_REF=$(git log -1 --format='%H' -- openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx)
-          echo "model_name=${NAME}" >> $GITHUB_OUTPUT
-          echo "onnx_ref=$ONNX_REF" >> $GITHUB_OUTPUT
-
-  build_model:
-    needs: resolve_name
-    uses: ./.github/workflows/sunnypilot-build-model.yaml
-    with:
-      upstream_branch: ${{ needs.resolve_name.outputs.onnx_ref }}
-      custom_name: ${{ needs.resolve_name.outputs.model_name }}
-      target_hardware: usbgpu
-    secrets: inherit
-
-  upload_defaults:
-    needs: [ resolve_name, build_model ]
-    runs-on: ubuntu-24.04
-    permissions:
-      id-token: write
-      contents: write
-    steps:
-      - uses: actions/checkout@v4
-        with:
-          submodules: recursive
-      - run: git lfs pull -I "openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx"
-
-      - name: Install huggingface_hub
-        run: pip install --upgrade "huggingface_hub>=0.22.0"
-
-      - name: Download artifact name
-        uses: actions/download-artifact@v4
-        with:
-          name: artifact-name-${{ needs.resolve_name.outputs.model_name }}
-          path: artifact_name
-
-      - name: Read artifact name
-        id: artifact
-        run: |
-          ARTIFACT_NAME=$(cat artifact_name/artifact_name.txt)
-          echo "artifact_name=$ARTIFACT_NAME" >> $GITHUB_OUTPUT
-
-      - name: Download model artifact
-        uses: actions/download-artifact@v4
-        with:
-          name: ${{ steps.artifact.outputs.artifact_name }}
-          path: output
-
-      - name: Upload to HF and update default_models.json
-        env:
-          HF_OIDC_RESOURCE: datasets/${{ env.HF_REPO }}
-          ARTIFACT_NAME: ${{ steps.artifact.outputs.artifact_name }}
-        run: |
-          rm -f output/artifact_name.txt
-          export PYTHONPATH=$(pwd)
-          python3 release/ci/upload_default_model.py \
-            --hf-repo "${{ env.HF_REPO }}" \
-            --hf-defaults-path "${{ env.HF_DEFAULTS_PATH }}" \
-            --artifact-name "$ARTIFACT_NAME" \
-            --model-dir output \
-            --onnx-path "openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx" \
-            --onnx-ref "${{ needs.resolve_name.outputs.onnx_ref }}" \
-            --model-name "${{ needs.resolve_name.outputs.model_name }}" \
-            --tinygrad-ref "$(python3 openpilot/sunnypilot/models/tinygrad_ref.py)" \
-            --run-number "${{ github.run_number }}"

.github/workflows/build-default-models.yaml

@@ -0,0 +1,522 @@
+name: Build default models
+
+on:
+  workflow_dispatch:
+    inputs:
+      target:
+        description: 'Model target to build'
+        required: true
+        type: choice
+        options:
+          - small
+          - big
+          - dm
+  workflow_call:
+    inputs:
+      target:
+        description: 'Model target to build (small, big, or dm)'
+        required: true
+        type: string
+
+concurrency:
+  group: build-default-models-${{ inputs.target }}
+  cancel-in-progress: false
+
+env:
+  HF_REPO: sunnypilot/sunnypilot_models_v1
+
+jobs:
+  resolve:
+    runs-on: ubuntu-24.04
+    outputs:
+      model_name: ${{ steps.resolve.outputs.model_name }}
+      safe_model_name: ${{ steps.resolve.outputs.safe_model_name }}
+      onnx_ref: ${{ steps.resolve.outputs.onnx_ref }}
+      onnx_path: ${{ steps.resolve.outputs.onnx_path }}
+      hf_defaults_path: ${{ steps.resolve.outputs.hf_defaults_path }}
+      tinygrad_ref: ${{ steps.resolve.outputs.tinygrad_ref }}
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          submodules: recursive
+
+      - id: resolve
+        run: |
+          export PYTHONPATH=${{ github.workspace }}
+
+          if [ "${{ inputs.target }}" = "big" ]; then
+            NAME=$(python3 -c "from openpilot.sunnypilot.models.model_name import DEFAULT_BIG_MODEL; print(DEFAULT_BIG_MODEL)")
+            ONNX_PATH="openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx"
+            HF_DEFAULTS_PATH="models/defaults/big"
+          elif [ "${{ inputs.target }}" = "dm" ]; then
+            ONNX_PATH="openpilot/selfdrive/modeld/models/dmonitoring_model.onnx"
+            HF_DEFAULTS_PATH="models/defaults/dm"
+            NAME="dmonitoring_model ($(git log -1 --format=%cd --date=format:'%B %d, %Y' -- "$ONNX_PATH"))"
+          else
+            NAME=$(python3 -c "from openpilot.sunnypilot.models.model_name import DEFAULT_MODEL; print(DEFAULT_MODEL)")
+            ONNX_PATH="openpilot/selfdrive/modeld/models/driving_supercombo.onnx"
+            HF_DEFAULTS_PATH="models/defaults/small"
+          fi
+
+          ONNX_REF=$(git log -1 --format='%H' -- "$ONNX_PATH")
+          TINYGRAD_REF=$(python3 openpilot/sunnypilot/models/tinygrad_ref.py)
+          if [ -z "$TINYGRAD_REF" ]; then
+            echo "::error::Failed to resolve tinygrad ref"
+            exit 1
+          fi
+
+          SAFE_NAME="${NAME// /-}"
+          echo "model_name=${NAME}" >> $GITHUB_OUTPUT
+          echo "safe_model_name=${SAFE_NAME}" >> $GITHUB_OUTPUT
+          echo "onnx_ref=${ONNX_REF}" >> $GITHUB_OUTPUT
+          echo "onnx_path=${ONNX_PATH}" >> $GITHUB_OUTPUT
+          echo "hf_defaults_path=${HF_DEFAULTS_PATH}" >> $GITHUB_OUTPUT
+          echo "tinygrad_ref=${TINYGRAD_REF}" >> $GITHUB_OUTPUT
+
+  build_small_model:
+    needs: resolve
+    if: ${{ inputs.target == 'small' }}
+    runs-on: [self-hosted, tici]
+    env:
+      SMALL_ONNX: openpilot/selfdrive/modeld/models/driving_supercombo.onnx
+      SMALL_PKL: openpilot/selfdrive/modeld/models/driving_tinygrad.pkl
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          submodules: recursive
+
+      - name: Pull ONNX via LFS
+        run: git lfs pull -I "${{ env.SMALL_ONNX }}"
+
+      - name: Set environment variables
+        run: |
+          source /etc/profile
+          export UV_PROJECT_ENVIRONMENT=${HOME}/venv
+          export UV_PYTHON_PREFERENCE=managed
+          export UV_PYTHON_INSTALL_DIR=${HOME}/uv/python
+          export VIRTUAL_ENV=$UV_PROJECT_ENVIRONMENT
+          uv sync --frozen
+          printenv >> $GITHUB_ENV
+
+      - name: Disable powersave
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          PYTHONPATH=$PYTHONPATH:${{ github.workspace }}/ ${{ github.workspace }}/scripts/manage-powersave.py --disable
+
+      - name: Compile small model with stock compiler
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          export PYTHONPATH="${PYTHONPATH}:${{ github.workspace }}/tinygrad_repo:${{ github.workspace }}"
+
+          MODEL_SIZE=$(python3 -c "from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE as s; print(f'{s[0]}x{s[1]}')")
+          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}')")
+          FRAME_SKIP=$(python3 -c "from openpilot.selfdrive.modeld.constants import ModelConstants as MC; print(MC.MODEL_RUN_FREQ // MC.MODEL_CONTEXT_FREQ)")
+
+          TG_FLAGS="DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
+
+          env ${TG_FLAGS} python3 \
+            ${{ github.workspace }}/openpilot/selfdrive/modeld/compile_modeld.py \
+            --onnx ${{ github.workspace }}/${{ env.SMALL_ONNX }} \
+            --model-size $MODEL_SIZE \
+            --camera-resolutions $CAMERA_RES \
+            --frame-skip $FRAME_SKIP \
+            --output ${{ github.workspace }}/${{ env.SMALL_PKL }}
+
+      - name: Chunk small pkl
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          export PYTHONPATH=${{ github.workspace }}
+          python3 -c "
+          from openpilot.common.file_chunker import chunk_file, get_chunk_targets
+          import os
+          pkl = '${{ github.workspace }}/${{ env.SMALL_PKL }}'
+          size = os.path.getsize(pkl)
+          targets = get_chunk_targets(pkl, size)
+          chunk_file(pkl, targets)
+          print(f'Chunked into {len(targets)} files')
+          "
+
+      - name: Prepare output
+        env:
+          MODEL_NAME: ${{ needs.resolve.outputs.safe_model_name }}
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          export PYTHONPATH=${{ github.workspace }}
+          MODELS_DIR="${{ github.workspace }}/openpilot/selfdrive/modeld/models"
+          OUTPUT_DIR="${{ github.workspace }}/small_output"
+          PKL_BASE="driving_tinygrad.pkl"
+          mkdir -p "$OUTPUT_DIR"
+
+          cp "$MODELS_DIR/${PKL_BASE}".chunk* "$OUTPUT_DIR/"
+          cp "$MODELS_DIR/${PKL_BASE}.chunkmanifest" "$OUTPUT_DIR/"
+
+          python3 "${{ github.workspace }}/release/ci/model_generator.py" \
+            --model-dir "$MODELS_DIR" \
+            --output-dir "$OUTPUT_DIR" \
+            --custom-name "$MODEL_NAME" \
+            --upstream-branch "${{ needs.resolve.outputs.onnx_ref }}"
+
+          echo "model-${MODEL_NAME}-${{ github.run_number }}" > "$OUTPUT_DIR/artifact_name.txt"
+
+      - name: Upload small model artifact
+        uses: actions/upload-artifact@v4
+        with:
+          name: model-${{ needs.resolve.outputs.safe_model_name }}-${{ github.run_number }}
+          path: ${{ github.workspace }}/small_output/
+
+      - name: Upload artifact name file
+        uses: actions/upload-artifact@v4
+        with:
+          name: artifact-name-${{ needs.resolve.outputs.safe_model_name }}
+          path: ${{ github.workspace }}/small_output/artifact_name.txt
+
+      - name: Re-enable powersave
+        if: always()
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          PYTHONPATH=$PYTHONPATH:${{ github.workspace }}/ ${{ github.workspace }}/scripts/manage-powersave.py --enable
+
+  build_big_model:
+    needs: resolve
+    if: ${{ inputs.target == 'big' }}
+    runs-on: [self-hosted, chestnut]
+    env:
+      BIG_ONNX: openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx
+      BIG_PKL: openpilot/selfdrive/modeld/models/big_driving_tinygrad.pkl
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          submodules: recursive
+
+      - name: Pull big ONNX via LFS
+        run: git lfs pull -I "${{ env.BIG_ONNX }}"
+
+      - name: Set environment variables
+        run: |
+          source /etc/profile
+          export UV_PROJECT_ENVIRONMENT=${HOME}/venv
+          export UV_PYTHON_PREFERENCE=managed
+          export UV_PYTHON_INSTALL_DIR=${HOME}/uv/python
+          export VIRTUAL_ENV=$UV_PROJECT_ENVIRONMENT
+          uv sync --frozen
+          printenv >> $GITHUB_ENV
+
+      - name: Disable powersave
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          PYTHONPATH=$PYTHONPATH:${{ github.workspace }}/ ${{ github.workspace }}/scripts/manage-powersave.py --disable
+
+      - name: Wait for chestnut PCIe link
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          export PYTHONPATH="${PYTHONPATH}:${{ github.workspace }}/tinygrad_repo:${{ github.workspace }}"
+          python3 -c "
+          import time
+          from openpilot.system.hardware.chestnut.flash import link_up
+          for i in range(10):
+              if link_up():
+                  print(f'PCIe link up after {i+1} attempt(s)')
+                  break
+              time.sleep(1)
+          else:
+              raise RuntimeError('Chestnut PCIe link not ready after 10 attempts')
+          "
+
+      - name: Compile big model with stock compiler
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          export PYTHONPATH="${PYTHONPATH}:${{ github.workspace }}/tinygrad_repo:${{ github.workspace }}"
+
+          MODEL_SIZE=$(python3 -c "from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE as s; print(f'{s[0]}x{s[1]}')")
+          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}')")
+          FRAME_SKIP=$(python3 -c "from openpilot.selfdrive.modeld.constants import ModelConstants as MC; print(MC.MODEL_RUN_FREQ // MC.MODEL_CONTEXT_FREQ)")
+
+          TG_FLAGS="DEBUG=2 DEV=USB+AMD:LLVM WARP_DEV=QCOM FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2"
+
+          env ${TG_FLAGS} python3 \
+            ${{ github.workspace }}/openpilot/selfdrive/modeld/compile_modeld.py \
+            --onnx ${{ github.workspace }}/${{ env.BIG_ONNX }} \
+            --model-size $MODEL_SIZE \
+            --camera-resolutions $CAMERA_RES \
+            --frame-skip $FRAME_SKIP \
+            --output ${{ github.workspace }}/${{ env.BIG_PKL }}
+
+      - name: Chunk big pkl
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          export PYTHONPATH=${{ github.workspace }}
+          python3 -c "
+          from openpilot.common.file_chunker import chunk_file, get_chunk_targets
+          import os
+          pkl = '${{ github.workspace }}/${{ env.BIG_PKL }}'
+          size = os.path.getsize(pkl)
+          targets = get_chunk_targets(pkl, size)
+          chunk_file(pkl, targets)
+          print(f'Chunked into {len(targets)} files')
+          "
+
+      - name: Prepare output
+        env:
+          MODEL_NAME: ${{ needs.resolve.outputs.safe_model_name }}
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          export PYTHONPATH=${{ github.workspace }}
+          MODELS_DIR="${{ github.workspace }}/openpilot/selfdrive/modeld/models"
+          OUTPUT_DIR="${{ github.workspace }}/big_output"
+          PKL_BASE="big_driving_tinygrad.pkl"
+          mkdir -p "$OUTPUT_DIR"
+
+          cp "$MODELS_DIR/${PKL_BASE}".chunk* "$OUTPUT_DIR/"
+          cp "$MODELS_DIR/${PKL_BASE}.chunkmanifest" "$OUTPUT_DIR/"
+
+          python3 "${{ github.workspace }}/release/ci/model_generator.py" \
+            --model-dir "$MODELS_DIR" \
+            --output-dir "$OUTPUT_DIR" \
+            --custom-name "$MODEL_NAME" \
+            --upstream-branch "${{ needs.resolve.outputs.onnx_ref }}"
+
+          echo "model-${MODEL_NAME}-${{ github.run_number }}" > "$OUTPUT_DIR/artifact_name.txt"
+
+      - name: Upload big model artifact
+        uses: actions/upload-artifact@v4
+        with:
+          name: model-${{ needs.resolve.outputs.safe_model_name }}-${{ github.run_number }}
+          path: ${{ github.workspace }}/big_output/
+
+      - name: Upload artifact name file
+        uses: actions/upload-artifact@v4
+        with:
+          name: artifact-name-${{ needs.resolve.outputs.safe_model_name }}
+          path: ${{ github.workspace }}/big_output/artifact_name.txt
+
+      - name: Re-enable powersave
+        if: always()
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          PYTHONPATH=$PYTHONPATH:${{ github.workspace }}/ ${{ github.workspace }}/scripts/manage-powersave.py --enable
+
+  upload_defaults:
+    needs: [ resolve, build_small_model, build_big_model, build_dm_model ]
+    if: |
+      ${{
+        !cancelled() &&
+        (inputs.target == 'big' && needs.build_big_model.result == 'success' ||
+         inputs.target == 'small' && needs.build_small_model.result == 'success' ||
+         inputs.target == 'dm' && needs.build_dm_model.result == 'success')
+      }}
+    runs-on: ubuntu-24.04
+    permissions:
+      id-token: write
+      contents: write
+    steps:
+      - uses: actions/checkout@v4
+
+      - name: Pull ONNX via LFS
+        run: git lfs pull -I "${{ needs.resolve.outputs.onnx_path }}"
+
+      - name: Install huggingface_hub
+        run: pip install --upgrade "huggingface_hub>=0.22.0"
+
+      - name: Download artifact name
+        if: ${{ inputs.target == 'small' || inputs.target == 'big' }}
+        uses: actions/download-artifact@v4
+        with:
+          name: artifact-name-${{ needs.resolve.outputs.safe_model_name }}
+          path: artifact_name
+
+      - name: Read artifact name
+        if: ${{ inputs.target == 'small' || inputs.target == 'big' }}
+        id: artifact
+        run: |
+          ARTIFACT_NAME=$(cat artifact_name/artifact_name.txt)
+          echo "artifact_name=$ARTIFACT_NAME" >> $GITHUB_OUTPUT
+
+      - name: Download model artifact
+        if: ${{ inputs.target == 'small' || inputs.target == 'big' }}
+        uses: actions/download-artifact@v4
+        with:
+          name: ${{ steps.artifact.outputs.artifact_name }}
+          path: output
+
+      - name: Upload model to HF
+        if: ${{ inputs.target == 'small' || inputs.target == 'big' }}
+        env:
+          HF_TOKEN: ${{ secrets.HF_TOKEN }}
+          ARTIFACT_NAME: ${{ steps.artifact.outputs.artifact_name }}
+        run: |
+          rm -f output/artifact_name.txt
+          export PYTHONPATH=$(pwd)
+          python3 release/ci/upload_default_model.py \
+            --hf-repo "${{ env.HF_REPO }}" \
+            --hf-defaults-path "${{ needs.resolve.outputs.hf_defaults_path }}" \
+            --artifact-name "$ARTIFACT_NAME" \
+            --model-dir output \
+            --onnx-path "${{ needs.resolve.outputs.onnx_path }}" \
+            --onnx-ref "${{ needs.resolve.outputs.onnx_ref }}" \
+            --model-name "${{ needs.resolve.outputs.model_name }}" \
+            --tinygrad-ref "${{ needs.resolve.outputs.tinygrad_ref }}" \
+            --run-number "${{ github.run_number }}"
+
+      - name: Download DM artifact
+        if: ${{ inputs.target == 'dm' }}
+        uses: actions/download-artifact@v4
+        with:
+          name: dm-model-${{ github.run_number }}
+          path: dm_output
+
+      - name: Generate DM metadata and upload to HF
+        if: ${{ inputs.target == 'dm' }}
+        env:
+          HF_TOKEN: ${{ secrets.HF_TOKEN }}
+        run: |
+          export PYTHONPATH=$(pwd)
+          python3 -c "
+          import json, hashlib
+          from pathlib import Path
+          from datetime import datetime, UTC
+
+          dm_dir = Path('dm_output')
+          manifest = list(dm_dir.glob('*.chunkmanifest'))
+          assert manifest, 'No chunkmanifest found'
+          pkl_name = manifest[0].name.removesuffix('.chunkmanifest')
+          num_chunks = int(manifest[0].read_text().strip())
+
+          chunks = []
+          for i in range(num_chunks):
+              chunk = dm_dir / f'{pkl_name}.chunk{i+1:02d}of{num_chunks:02d}'
+              chunks.append({
+                  'file_name': chunk.name,
+                  'sha256': hashlib.sha256(chunk.read_bytes()).hexdigest()
+              })
+
+          digest = hashlib.sha256()
+          for c in chunks:
+              with open(dm_dir / c['file_name'], 'rb') as f:
+                  while block := f.read(1024*1024):
+                      digest.update(block)
+
+          metadata = {
+              'bundles': [{
+                  'short_name': 'DMMODEL',
+                  'display_name': '${{ needs.resolve.outputs.model_name }}',
+                  'ref': '${{ needs.resolve.outputs.onnx_ref }}',
+                  'runner': 'tinygrad',
+                  'build_time': datetime.now(UTC).strftime('%Y-%m-%dT%H:%M:%SZ'),
+                  'models': [{
+                      'type': 'chunked',
+                      'artifact': {
+                          'file_name': pkl_name,
+                          'download_uri': {'url': '', 'sha256': digest.hexdigest()},
+                          'chunks': chunks
+                      }
+                  }]
+              }]
+          }
+          with open(dm_dir / 'metadata.json', 'w') as f:
+              json.dump(metadata, f, indent=2)
+          print('Generated DM metadata.json')
+          "
+
+          python3 release/ci/upload_default_model.py \
+            --hf-repo "${{ env.HF_REPO }}" \
+            --hf-defaults-path "${{ needs.resolve.outputs.hf_defaults_path }}" \
+            --artifact-name "dm-model-${{ github.run_number }}" \
+            --model-dir dm_output \
+            --onnx-path "${{ needs.resolve.outputs.onnx_path }}" \
+            --onnx-ref "${{ needs.resolve.outputs.onnx_ref }}" \
+            --model-name "${{ needs.resolve.outputs.model_name }}" \
+            --tinygrad-ref "${{ needs.resolve.outputs.tinygrad_ref }}" \
+            --run-number "${{ github.run_number }}"
+
+  build_dm_model:
+    needs: resolve
+    if: ${{ inputs.target == 'dm' }}
+    runs-on: [self-hosted, tici]
+    env:
+      DM_ONNX: openpilot/selfdrive/modeld/models/dmonitoring_model.onnx
+      DM_PKL: openpilot/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          submodules: recursive
+
+      - name: Pull DM ONNX via LFS
+        run: git lfs pull -I "${{ env.DM_ONNX }}"
+
+      - name: Set environment variables
+        run: |
+          source /etc/profile
+          export UV_PROJECT_ENVIRONMENT=${HOME}/venv
+          export UV_PYTHON_PREFERENCE=managed
+          export UV_PYTHON_INSTALL_DIR=${HOME}/uv/python
+          export VIRTUAL_ENV=$UV_PROJECT_ENVIRONMENT
+          uv sync --frozen
+          printenv >> $GITHUB_ENV
+
+      - name: Disable powersave
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          PYTHONPATH=$PYTHONPATH:${{ github.workspace }}/ ${{ github.workspace }}/scripts/manage-powersave.py --disable
+
+      - name: Compile DM model
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          export PYTHONPATH="${PYTHONPATH}:${{ github.workspace }}/tinygrad_repo:${{ github.workspace }}"
+
+          TG_FLAGS="DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
+
+          taskset -c 7 env ${TG_FLAGS} python3 \
+            ${{ github.workspace }}/tinygrad_repo/examples/openpilot/compile3.py \
+            ${{ github.workspace }}/${{ env.DM_ONNX }} \
+            ${{ github.workspace }}/${{ env.DM_PKL }}
+
+      - name: Chunk DM pkl
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          export PYTHONPATH=${{ github.workspace }}
+          python3 -c "
+          from openpilot.common.file_chunker import chunk_file, get_chunk_targets
+          import os
+          pkl = '${{ github.workspace }}/${{ env.DM_PKL }}'
+          size = os.path.getsize(pkl)
+          targets = get_chunk_targets(pkl, size)
+          chunk_file(pkl, targets)
+          print(f'Chunked {pkl} into {len(targets)} chunks')
+          "
+
+      - name: Compile DM warp
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          export PYTHONPATH="${PYTHONPATH}:${{ github.workspace }}/tinygrad_repo:${{ github.workspace }}"
+
+          TG_FLAGS="DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
+          MODEL_DIR="${{ github.workspace }}/openpilot/selfdrive/modeld"
+          DM_SIZE=$(python3 -c "from openpilot.common.transformations.model import DM_INPUT_SIZE as s; print(f'{s[0]}x{s[1]}')")
+
+          for res in $(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}')"); do
+            WARP_PKL="${MODEL_DIR}/models/dm_warp_${res}_tinygrad.pkl"
+            taskset -c 7 env ${TG_FLAGS} python3 ${MODEL_DIR}/compile_dm_warp.py \
+              --camera-resolution ${res} \
+              --warp-to ${DM_SIZE} \
+              --output ${WARP_PKL}
+          done
+
+      - name: Prepare DM output
+        run: |
+          mkdir -p dm_output
+          cp ${{ github.workspace }}/${{ env.DM_PKL }}.chunk* dm_output/
+          cp ${{ github.workspace }}/${{ env.DM_PKL }}.chunkmanifest dm_output/
+          cp ${{ github.workspace }}/openpilot/selfdrive/modeld/models/dm_warp_* dm_output/
+
+      - name: Upload DM artifact
+        uses: actions/upload-artifact@v4
+        with:
+          name: dm-model-${{ github.run_number }}
+          path: dm_output/
+
+      - name: Re-enable powersave
+        if: always()
+        run: |
+          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
+          PYTHONPATH=$PYTHONPATH:${{ github.workspace }}/ ${{ github.workspace }}/scripts/manage-powersave.py --enable
+

.github/workflows/build-single-tinygrad-model.yaml

@@ -30,7 +30,7 @@ on:
         type: boolean
         default: true
       target_hardware:
-        description: 'Hardware target to compile for (qcom or usbgpu)'
+        description: 'Hardware target to compile for (qcom or chestnut)'
         required: false
         type: string
         default: 'qcom'
@@ -101,7 +101,7 @@ on:
         default: 'qcom'
         options:
           - qcom
-          - usbgpu
+          - chestnut
       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 == 'usbgpu' && 'usbgpu_v' || 'v' }}${{ inputs.json_version }}.json
+  JSON_FILE: docs/docs/driving_models_${{ inputs.target_hardware == 'chestnut' && 'chestnut_v' || 'v' }}${{ inputs.json_version }}.json
 
 jobs:
   build_model:
@@ -146,7 +146,7 @@ jobs:
 
       - name: Validate hf_repo and JSON version
         env:
-          HF_OIDC_RESOURCE: datasets/${{ inputs.hf_repo }}
+          HF_TOKEN: ${{ secrets.HF_TOKEN }}
         run: |
           if [ ! -f "$JSON_FILE" ]; then
             echo "JSON file $JSON_FILE does not exist!"
@@ -155,13 +155,8 @@ jobs:
           python3 -c "
           import sys
           from huggingface_hub import HfApi
-          try:
-              api = HfApi()
-              api.repo_info(repo_id=sys.argv[1], repo_type='dataset')
-              print(f'Success: Repo {sys.argv[1]} exists.')
-          except Exception as e:
-              print('HF validation failed:', e)
-              sys.exit(1)
+          HfApi().repo_info(repo_id=sys.argv[1], repo_type='dataset')
+          print(f'Success: Repo {sys.argv[1]} exists.')
           " "${{ inputs.hf_repo }}"
 
       - name: Download artifact name file
@@ -192,7 +187,7 @@ jobs:
 
       - name: Upload to Hugging Face
         env:
-          HF_OIDC_RESOURCE: datasets/${{ inputs.hf_repo }}
+          HF_TOKEN: ${{ secrets.HF_TOKEN }}
           ARTIFACT_NAME: ${{ steps.read-artifact-name.outputs.artifact_name }}
         run: |
           hf upload ${{ inputs.hf_repo }} \

.github/workflows/download-hf-model-chunks/action.yml

@@ -0,0 +1,73 @@
+name: Download HF model chunks
+description: Resolve and download model chunks from HuggingFace in parallel
+
+inputs:
+  hf_repo:
+    description: HuggingFace dataset repo
+    required: true
+  models:
+    description: 'JSON array of {hf_path, onnx_hash, canonical} objects'
+    required: true
+  dest_dir:
+    description: Destination directory for downloaded chunks
+    required: true
+
+runs:
+  using: composite
+  steps:
+    - name: Download model chunks
+      shell: bash
+      env:
+        HF_REPO: ${{ inputs.hf_repo }}
+        MODELS_JSON: ${{ inputs.models }}
+        DEST_DIR: ${{ inputs.dest_dir }}
+      run: |
+        set -eo pipefail
+        DOWNLOAD_LIST=$(mktemp)
+
+        resolve_chunks() {
+          local HF_PATH="$1" ONNX_HASH="$2" CANONICAL="$3" DEST_DIR="$4"
+          local JSON_URL="https://huggingface.co/datasets/${HF_REPO}/resolve/main/${HF_PATH}/default_models.json"
+          local DEFAULTS BUNDLE ARTIFACT BASE_URL NUM_CHUNKS
+          DEFAULTS=$(curl -fsSL "$JSON_URL")
+          BUNDLE=$(echo "$DEFAULTS" | jq --arg hash "$ONNX_HASH" '.bundles[] | select(.onnx_sha256 == $hash)')
+          ARTIFACT=$(echo "$BUNDLE" | jq -r '.models[0].artifact')
+          BASE_URL=$(echo "$ARTIFACT" | jq -r '.download_uri.url' | sed 's|/[^/]*$||')
+          NUM_CHUNKS=$(echo "$ARTIFACT" | jq -r '.chunks | length')
+
+          mkdir -p "$DEST_DIR"
+          while IFS= read -r CHUNK_NAME; do
+            CHUNK_IDX=$(echo "$CHUNK_NAME" | grep -oP 'chunk\K[0-9]+of[0-9]+' || true)
+            if [ -z "$CHUNK_IDX" ]; then
+              echo "::error::Failed to parse chunk index from: $CHUNK_NAME"
+              return 1
+            fi
+            ENCODED_URL=$(python3 -c "import urllib.parse; print(urllib.parse.quote('${BASE_URL}/${CHUNK_NAME}', safe=':/'))")
+            printf '%s\t%s\n' "$ENCODED_URL" "${DEST_DIR}/${CANONICAL}.chunk${CHUNK_IDX}" >> "$DOWNLOAD_LIST"
+          done < <(echo "$ARTIFACT" | jq -r '.chunks[].file_name')
+          echo "$NUM_CHUNKS" > "${DEST_DIR}/${CANONICAL}.chunkmanifest"
+
+          if [ "$CANONICAL" = "dmonitoring_model_tinygrad.pkl" ]; then
+            for warp in dm_warp_1928x1208_tinygrad.pkl dm_warp_1344x760_tinygrad.pkl; do
+              ENCODED_URL=$(python3 -c "import urllib.parse; print(urllib.parse.quote('${BASE_URL}/${warp}', safe=':/'))")
+              printf '%s\t%s\n' "$ENCODED_URL" "${DEST_DIR}/${warp}" >> "$DOWNLOAD_LIST"
+            done
+          fi
+        }
+
+        echo "$MODELS_JSON" | jq -c '.[]' | while IFS= read -r model; do
+          HF_PATH=$(echo "$model" | jq -r '.hf_path')
+          ONNX_HASH=$(echo "$model" | jq -r '.onnx_hash')
+          CANONICAL=$(echo "$model" | jq -r '.canonical')
+          resolve_chunks "$HF_PATH" "$ONNX_HASH" "$CANONICAL" "$DEST_DIR"
+        done
+
+        TOTAL=$(wc -l < "$DOWNLOAD_LIST")
+        echo "Downloading $TOTAL chunks with 8 parallel connections..."
+        xargs -P8 -d'\n' -I{} bash -c '
+          URL="${1%%	*}"
+          DEST="${1#*	}"
+          echo "Downloading $(basename "$DEST")"
+          curl -fsSL --retry 3 --retry-delay 5 -o "$DEST" "$URL"
+        ' _ {} < "$DOWNLOAD_LIST"
+        rm -f "$DOWNLOAD_LIST"

.github/workflows/sunnypilot-build-model.yaml

@@ -31,7 +31,7 @@ on:
         type: string
         default: ''
       target_hardware:
-        description: 'Hardware target to compile for (qcom or usbgpu)'
+        description: 'Hardware target to compile for (qcom or chestnut)'
         required: false
         type: string
         default: 'qcom'
@@ -57,7 +57,7 @@ on:
         type: choice
         options:
           - qcom
-          - usbgpu
+          - chestnut
         default: 'qcom'
 
 
@@ -102,7 +102,7 @@ jobs:
           cat $GITHUB_OUTPUT
       - run: |
           cd ${{ github.workspace }}/openpilot/openpilot
-          if [ "${{ inputs.target_hardware }}" != "usbgpu" ]; then
+          if [ "${{ inputs.target_hardware }}" != "chestnut" ]; 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, usbgpu]
+    runs-on: [self-hosted, "${{ inputs.target_hardware == 'chestnut' && 'chestnut' || 'tici' }}"]
     needs: get_model
     env:
       MODEL_NAME: ${{ inputs.custom_name || inputs.upstream_branch }} (${{ needs.get_model.outputs.model_date }})
@@ -185,10 +185,10 @@ 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 }}" == "usbgpu" ]; then
-            echo "USBGPU build"
-            export USBGPU=1
-            TG_FLAGS="DEBUG=2 DEV=USB+AMD:LLVM WARP_DEV=QCOM FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2"
+          if [ "${{ inputs.target_hardware }}" == "chestnut" ]; then
+            echo "CHESTNUT build"
+            export CHESTNUT=1
+            TG_FLAGS="DEBUG=1 DEV=USB+AMD:LLVM FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2 TC_OCCUPANCY_OPT=1"
             OUTPUT_PKL="${{ env.MODELS_DIR }}/big_driving_tinygrad.pkl"
           else
             echo "QCOM build"

.github/workflows/sunnypilot-build-prebuilt.yaml

@@ -39,6 +39,8 @@ jobs:
       include_big_model: ${{ steps.strategy.outputs.include_big_model }}
     steps:
       - uses: actions/checkout@v4
+        with:
+          fetch-depth: 1
       - name: Extract deploy strategy
         id: strategy
         run: |
@@ -96,6 +98,8 @@ jobs:
       }}
     steps:
       - uses: actions/checkout@v4
+        with:
+          fetch-depth: 1
       - name: Wait for Tests
         uses: ./.github/workflows/wait-for-action # Path to where you place the action
         with:
@@ -119,6 +123,7 @@ jobs:
     steps:
       - uses: actions/checkout@v4
         with:
+          fetch-depth: 1
           submodules: recursive
           ref: ${{ env.SOURCE_BRANCH }}
           repository: ${{ github.event.pull_request.head.repo.fork && github.event.pull_request.head.repo.full_name || github.repository }}
@@ -165,7 +170,7 @@ jobs:
           scons -j1 cache_dir="$SCONS_CACHE" --minimal \
             openpilot/selfdrive/locationd openpilot/sunnypilot/selfdrive/locationd
           echo "Building rest of sunnypilot"
-          /usr/bin/time -v scons -j$(nproc) cache_dir="$SCONS_CACHE" --minimal
+          SKIP_TINYGRAD_COMPILE=1 /usr/bin/time -v scons -j$(nproc) cache_dir="$SCONS_CACHE" --minimal
           touch ${BUILD_DIR}/prebuilt
           if [[ "${{ runner.debug }}" == "1" ]]; then
             ls -la ${BUILD_DIR}
@@ -211,91 +216,245 @@ jobs:
     needs: [ prepare_strategy ]
     runs-on: ubuntu-24.04
     if: ${{ needs.prepare_strategy.outputs.include_big_model == 'true' }}
+    concurrency:
+      group: prepare-chestnut
+      cancel-in-progress: false
+    outputs:
+      onnx_sha256: ${{ steps.resolve.outputs.onnx_sha256 }}
     env:
+      GH_REPO: ${{ github.repository }}
       HF_REPO: sunnypilot/sunnypilot_models_v1
       HF_DEFAULTS_PATH: models/defaults/big
     steps:
-      - uses: actions/checkout@v4
-        with:
-          ref: ${{ github.head_ref || github.ref_name }}
-
-      - run: git lfs pull -I "openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx"
-
-      - name: Check HF defaults and build if needed
+      - name: Resolve ONNX hash and tinygrad ref via API
         id: resolve
         run: |
-          ACTUAL_ONNX_HASH=$(sha256sum "openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx" | cut -d' ' -f1)
-          echo "Repo ONNX hash: $ACTUAL_ONNX_HASH"
+          REF="${{ github.head_ref || github.ref_name }}"
+
+          BLOB_SHA=$(gh api "repos/${GH_REPO}/contents/openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx?ref=${REF}" --jq '.sha')
+          ONNX_HASH=$(gh api "repos/${GH_REPO}/git/blobs/${BLOB_SHA}" --jq '.content' | base64 -d | grep '^oid sha256:' | cut -d: -f2)
+          echo "ONNX hash: $ONNX_HASH"
+          [ -n "$ONNX_HASH" ] || { echo "::error::Failed to extract ONNX hash"; exit 1; }
+          echo "onnx_sha256=$ONNX_HASH" >> $GITHUB_OUTPUT
+
+          TINYGRAD_REF=$(gh api "repos/${GH_REPO}/contents/tinygrad_repo?ref=${REF}" --jq '.sha')
+          echo "tinygrad ref: $TINYGRAD_REF"
 
           JSON_URL="https://huggingface.co/datasets/${HF_REPO}/resolve/main/${HF_DEFAULTS_PATH}/default_models.json"
 
-          check_hash() {
-            DEFAULTS=$(curl -fsSL "$JSON_URL" 2>/dev/null) || return 1
-            BUNDLE=$(echo "$DEFAULTS" | jq --arg hash "$ACTUAL_ONNX_HASH" '.bundles[] | select(.onnx_sha256 == $hash)' 2>/dev/null)
+          check_defaults() {
+            DEFAULTS=$(curl -fsSL "${JSON_URL}?t=$(date +%s)" 2>/dev/null) || return 1
+            TINYGRAD_MATCH=$(echo "$DEFAULTS" | jq -r --arg ref "$TINYGRAD_REF" '.tinygrad_ref == $ref' 2>/dev/null)
+            [ "$TINYGRAD_MATCH" = "true" ] || return 1
+            BUNDLE=$(echo "$DEFAULTS" | jq --arg hash "$ONNX_HASH" '.bundles[] | select(.onnx_sha256 == $hash)' 2>/dev/null)
             [ -n "$BUNDLE" ] && [ "$BUNDLE" != "null" ]
           }
 
-          if check_hash; then
-            echo "HF defaults match repo ONNX"
-          else
-            echo "No matching model on HF — triggering build"
-            gh workflow run build-default-big-model.yaml --ref "${{ github.head_ref || github.ref_name }}"
-
-            echo "Waiting for build to start..."
-            sleep 120
-
-            RUN_ID=$(gh run list --workflow=build-default-big-model.yaml --branch="${{ github.head_ref || github.ref_name }}" --limit=1 --json databaseId --jq '.[0].databaseId')
-            if [ -z "$RUN_ID" ] || [ "$RUN_ID" = "null" ]; then
-              echo "::error::Failed to find build-default-big-model run"
-              exit 1
-            fi
-
-            echo "Waiting for run $RUN_ID..."
-            gh run watch "$RUN_ID"
-
-            CONCLUSION=$(gh run view "$RUN_ID" --json conclusion --jq '.conclusion')
-            if [ "$CONCLUSION" != "success" ]; then
-              echo "::error::build-default-big-model failed: $CONCLUSION"
-              exit 1
-            fi
-
-            if ! check_hash; then
-              echo "::error::HF defaults still don't match after build"
-              exit 1
-            fi
+          if check_defaults; then
+            echo "HF defaults match repo ONNX hash and tinygrad ref"
+            exit 0
           fi
+
+          echo "No matching model on HF — dispatching build"
+          gh workflow run build-default-models.yaml --ref "$REF" -f target=big
+          sleep 10
+
+          BUILD_RUN_ID=$(gh run list --workflow build-default-models.yaml --branch "$REF" --limit 1 --json databaseId --jq '.[0].databaseId')
+          echo "Dispatched build run: $BUILD_RUN_ID"
+
+          echo "Waiting for build run to complete..."
+          for i in $(seq 1 90); do
+            sleep 30
+            STATUS=$(gh api "repos/${GH_REPO}/actions/runs/${BUILD_RUN_ID}" --jq '.status')
+            CONCLUSION=$(gh api "repos/${GH_REPO}/actions/runs/${BUILD_RUN_ID}" --jq '.conclusion')
+            echo "Poll $i/90: status=$STATUS conclusion=$CONCLUSION"
+            if [ "$STATUS" = "completed" ]; then
+              if [ "$CONCLUSION" = "success" ]; then
+                echo "Build run succeeded, verifying HF..."
+                sleep 10
+                if check_defaults; then
+                  echo "Big model verified on HF"
+                  exit 0
+                fi
+                echo "::error::Build succeeded but model not found on HF"
+                exit 1
+              else
+                echo "::error::Build run failed with conclusion=$CONCLUSION"
+                exit 1
+              fi
+            fi
+          done
+
+          echo "::error::Build run did not complete within 45 minutes"
+          exit 1
         env:
           GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
 
-      - name: Download big model chunks
+      - name: Cancel run on failure
+        if: failure()
+        run: gh run cancel ${{ github.run_id }}
+        env:
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+
+  prepare_small_model:
+    needs: [ prepare_strategy ]
+    runs-on: ubuntu-24.04
+    concurrency:
+      group: prepare-small-model
+      cancel-in-progress: false
+    outputs:
+      driving_onnx_sha256: ${{ steps.resolve.outputs.driving_onnx_sha256 }}
+    env:
+      GH_REPO: ${{ github.repository }}
+      HF_REPO: sunnypilot/sunnypilot_models_v1
+      HF_DEFAULTS_PATH: models/defaults/small
+    steps:
+      - name: Resolve ONNX hash and tinygrad ref via API
+        id: resolve
         run: |
-          ACTUAL_ONNX_HASH=$(sha256sum "openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx" | cut -d' ' -f1)
+          REF="${{ github.head_ref || github.ref_name }}"
+
+          BLOB_SHA=$(gh api "repos/${GH_REPO}/contents/openpilot/selfdrive/modeld/models/driving_supercombo.onnx?ref=${REF}" --jq '.sha')
+          DRIVING_HASH=$(gh api "repos/${GH_REPO}/git/blobs/${BLOB_SHA}" --jq '.content' | base64 -d | grep '^oid sha256:' | cut -d: -f2)
+          echo "Driving ONNX hash: $DRIVING_HASH"
+          [ -n "$DRIVING_HASH" ] || { echo "::error::Failed to extract driving ONNX hash"; exit 1; }
+          echo "driving_onnx_sha256=$DRIVING_HASH" >> $GITHUB_OUTPUT
+
+          TINYGRAD_REF=$(gh api "repos/${GH_REPO}/contents/tinygrad_repo?ref=${REF}" --jq '.sha')
+          echo "tinygrad ref: $TINYGRAD_REF"
+
           JSON_URL="https://huggingface.co/datasets/${HF_REPO}/resolve/main/${HF_DEFAULTS_PATH}/default_models.json"
-          DEFAULTS=$(curl -fsSL "$JSON_URL")
-          BUNDLE=$(echo "$DEFAULTS" | jq --arg hash "$ACTUAL_ONNX_HASH" '.bundles[] | select(.onnx_sha256 == $hash)')
 
-          mkdir -p big_model_chunks
-          ARTIFACT=$(echo "$BUNDLE" | jq -r '.models[0].artifact')
-          BASE_URL=$(echo "$ARTIFACT" | jq -r '.download_uri.url' | sed 's|/[^/]*$||')
-          NUM_CHUNKS=$(echo "$ARTIFACT" | jq -r '.chunks | length')
+          check_defaults() {
+            DEFAULTS=$(curl -fsSL "${JSON_URL}?t=$(date +%s)" 2>/dev/null) || return 1
+            TINYGRAD_MATCH=$(echo "$DEFAULTS" | jq -r --arg ref "$TINYGRAD_REF" '.tinygrad_ref == $ref' 2>/dev/null)
+            [ "$TINYGRAD_MATCH" = "true" ] || return 1
+            DRIVING=$(echo "$DEFAULTS" | jq --arg hash "$DRIVING_HASH" '.bundles[] | select(.onnx_sha256 == $hash)' 2>/dev/null)
+            [ -n "$DRIVING" ] && [ "$DRIVING" != "null" ] || return 1
+          }
 
-          CANONICAL="big_driving_tinygrad.pkl"
-          echo "$ARTIFACT" | jq -r '.chunks[].file_name' | while read CHUNK_NAME; do
-            CHUNK_IDX=$(echo "$CHUNK_NAME" | grep -oP 'chunk\K[0-9]+of[0-9]+')
-            CANONICAL_CHUNK="${CANONICAL}.chunk${CHUNK_IDX}"
-            ENCODED_URL=$(python3 -c "import urllib.parse; print(urllib.parse.quote('${BASE_URL}/${CHUNK_NAME}', safe=':/'))")
-            echo "Downloading $CHUNK_NAME -> $CANONICAL_CHUNK"
-            curl -fsSL -o "big_model_chunks/${CANONICAL_CHUNK}" "$ENCODED_URL"
+          if check_defaults; then
+            echo "HF defaults match repo ONNX hash and tinygrad ref"
+            exit 0
+          fi
+
+          echo "No matching model on HF — dispatching build"
+          gh workflow run build-default-models.yaml --ref "$REF" -f target=small
+          sleep 10
+
+          BUILD_RUN_ID=$(gh run list --workflow build-default-models.yaml --branch "$REF" --limit 1 --json databaseId --jq '.[0].databaseId')
+          echo "Dispatched build run: $BUILD_RUN_ID"
+
+          echo "Waiting for build run to complete..."
+          for i in $(seq 1 60); do
+            sleep 30
+            STATUS=$(gh api "repos/${GH_REPO}/actions/runs/${BUILD_RUN_ID}" --jq '.status')
+            CONCLUSION=$(gh api "repos/${GH_REPO}/actions/runs/${BUILD_RUN_ID}" --jq '.conclusion')
+            echo "Poll $i/60: status=$STATUS conclusion=$CONCLUSION"
+            if [ "$STATUS" = "completed" ]; then
+              if [ "$CONCLUSION" = "success" ]; then
+                echo "Build run succeeded, verifying HF..."
+                sleep 10
+                if check_defaults; then
+                  echo "Small model verified on HF"
+                  exit 0
+                fi
+                echo "::error::Build succeeded but model not found on HF"
+                exit 1
+              else
+                echo "::error::Build run failed with conclusion=$CONCLUSION"
+                exit 1
+              fi
+            fi
           done
 
-          echo "$NUM_CHUNKS" > "big_model_chunks/${CANONICAL}.chunkmanifest"
+          echo "::error::Small model build did not complete within 30 minutes"
+          exit 1
+        env:
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
 
-      - name: Upload big model chunks
-        uses: actions/upload-artifact@v4
-        with:
-          name: big-model-chunks
-          path: big_model_chunks/
-          compression-level: 0
+      - name: Cancel run on failure
+        if: failure()
+        run: gh run cancel ${{ github.run_id }}
+        env:
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+
+  prepare_dm_model:
+    needs: [ prepare_strategy ]
+    runs-on: ubuntu-24.04
+    concurrency:
+      group: prepare-dm-model
+      cancel-in-progress: false
+    outputs:
+      dm_onnx_sha256: ${{ steps.resolve.outputs.dm_onnx_sha256 }}
+    env:
+      GH_REPO: ${{ github.repository }}
+      HF_REPO: sunnypilot/sunnypilot_models_v1
+      HF_DEFAULTS_PATH: models/defaults/dm
+    steps:
+      - name: Resolve ONNX hash and tinygrad ref via API
+        id: resolve
+        run: |
+          REF="${{ github.head_ref || github.ref_name }}"
+
+          BLOB_SHA=$(gh api "repos/${GH_REPO}/contents/openpilot/selfdrive/modeld/models/dmonitoring_model.onnx?ref=${REF}" --jq '.sha')
+          DM_HASH=$(gh api "repos/${GH_REPO}/git/blobs/${BLOB_SHA}" --jq '.content' | base64 -d | grep '^oid sha256:' | cut -d: -f2)
+          echo "DM ONNX hash: $DM_HASH"
+          [ -n "$DM_HASH" ] || { echo "::error::Failed to extract DM ONNX hash"; exit 1; }
+          echo "dm_onnx_sha256=$DM_HASH" >> $GITHUB_OUTPUT
+
+          TINYGRAD_REF=$(gh api "repos/${GH_REPO}/contents/tinygrad_repo?ref=${REF}" --jq '.sha')
+          echo "tinygrad ref: $TINYGRAD_REF"
+
+          JSON_URL="https://huggingface.co/datasets/${HF_REPO}/resolve/main/${HF_DEFAULTS_PATH}/default_models.json"
+
+          check_defaults() {
+            DEFAULTS=$(curl -fsSL "${JSON_URL}?t=$(date +%s)" 2>/dev/null) || return 1
+            TINYGRAD_MATCH=$(echo "$DEFAULTS" | jq -r --arg ref "$TINYGRAD_REF" '.tinygrad_ref == $ref' 2>/dev/null)
+            [ "$TINYGRAD_MATCH" = "true" ] || return 1
+            DM=$(echo "$DEFAULTS" | jq --arg hash "$DM_HASH" '.bundles[] | select(.onnx_sha256 == $hash)' 2>/dev/null)
+            [ -n "$DM" ] && [ "$DM" != "null" ] || return 1
+          }
+
+          if check_defaults; then
+            echo "HF defaults match DM ONNX hash and tinygrad ref"
+            exit 0
+          fi
+
+          echo "No matching DM model on HF — dispatching build"
+          gh workflow run build-default-models.yaml --ref "$REF" -f target=dm
+          sleep 10
+
+          BUILD_RUN_ID=$(gh run list --workflow build-default-models.yaml --branch "$REF" --limit 1 --json databaseId --jq '.[0].databaseId')
+          echo "Dispatched build run: $BUILD_RUN_ID"
+
+          echo "Waiting for build run to complete..."
+          for i in $(seq 1 60); do
+            sleep 30
+            STATUS=$(gh api "repos/${GH_REPO}/actions/runs/${BUILD_RUN_ID}" --jq '.status')
+            CONCLUSION=$(gh api "repos/${GH_REPO}/actions/runs/${BUILD_RUN_ID}" --jq '.conclusion')
+            echo "Poll $i/60: status=$STATUS conclusion=$CONCLUSION"
+            if [ "$STATUS" = "completed" ]; then
+              if [ "$CONCLUSION" = "success" ]; then
+                echo "Build run succeeded, verifying HF..."
+                sleep 10
+                if check_defaults; then
+                  echo "DM model verified on HF"
+                  exit 0
+                fi
+                echo "::error::Build succeeded but DM model not found on HF"
+                exit 1
+              else
+                echo "::error::Build run failed with conclusion=$CONCLUSION"
+                exit 1
+              fi
+            fi
+          done
+
+          echo "::error::DM model build did not complete within 30 minutes"
+          exit 1
+        env:
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
 
       - name: Cancel run on failure
         if: failure()
@@ -305,23 +464,24 @@ jobs:
 
   publish:
     concurrency:
-      # We do a bit of a hack here to avoid canceling the publishing job if a new commit comes in while we're publishing by adding the sha to the group name.
-      # This means that if multiple commits come in while we're publishing, they will be queued up and publish one after the other.
-      # Otherwise, if a job is waiting to be published due to environment wait time, it would be canceled by a new commit and restart the wait time.
       group: ${{ needs.prepare_strategy.outputs.publish_concurrency_group }}
       cancel-in-progress: ${{ needs.prepare_strategy.outputs.cancel_publish_in_progress == 'true' }}
     if: ${{
       always() && !cancelled() &&
       needs.build.result == 'success' &&
       needs.prepare_strategy.result == 'success' &&
+      needs.prepare_small_model.result == 'success' &&
+      needs.prepare_dm_model.result == 'success' &&
       (!contains(github.event_name, 'pull_request') || (github.event.action == 'labeled' && github.event.label.name == 'prebuilt')) &&
       (needs.prepare_strategy.outputs.include_big_model != 'true' || needs.prepare_chestnut.result == 'success')
       }}
-    needs: [ build, prepare_strategy, prepare_chestnut ]
+    needs: [ build, prepare_strategy, prepare_chestnut, prepare_small_model, prepare_dm_model ]
     runs-on: ubuntu-24.04
     environment: ${{ needs.prepare_strategy.outputs.environment }}
     steps:
       - uses: actions/checkout@v4
+        with:
+          fetch-depth: 1
 
       - name: Download prebuilt artifact
         uses: actions/download-artifact@v4
@@ -333,23 +493,16 @@ jobs:
           mkdir -p ${{ env.OUTPUT_DIR }}
           tar xzf prebuilt.tar.gz -C ${{ env.OUTPUT_DIR }}
 
-      - name: Prepare chestnut output
-        if: ${{ needs.prepare_chestnut.result == 'success' }}
-        run: |
-          mkdir -p "${{ github.workspace }}/chestnut_output"
-          tar xzf prebuilt.tar.gz -C "${{ github.workspace }}/chestnut_output"
-
-      - name: Download big model chunks
-        if: ${{ needs.prepare_chestnut.result == 'success' }}
-        uses: actions/download-artifact@v4
+      - name: Download model chunks from HF
+        uses: ./.github/workflows/download-hf-model-chunks
         with:
-          name: big-model-chunks
-          path: big_model_chunks
-
-      - name: Inject big model into chestnut
-        if: ${{ needs.prepare_chestnut.result == 'success' }}
-        run: |
-          cp big_model_chunks/* "${{ github.workspace }}/chestnut_output/openpilot/selfdrive/modeld/models/"
+          hf_repo: sunnypilot/sunnypilot_models_v1
+          dest_dir: ${{ env.OUTPUT_DIR }}/openpilot/selfdrive/modeld/models
+          models: |
+            [
+              {"hf_path": "models/defaults/small", "onnx_hash": "${{ needs.prepare_small_model.outputs.driving_onnx_sha256 }}", "canonical": "driving_tinygrad.pkl"},
+              {"hf_path": "models/defaults/dm", "onnx_hash": "${{ needs.prepare_dm_model.outputs.dm_onnx_sha256 }}", "canonical": "dmonitoring_model_tinygrad.pkl"}
+            ]
 
       - name: Configure Git
         run: |
@@ -371,22 +524,6 @@ jobs:
             "https://x-access-token:${{github.token}}@github.com/sunnypilot/sunnypilot.git" \
             "${{ needs.prepare_strategy.outputs.extra_version_identifier }}"
 
-      - name: Publish chestnut branch
-        if: ${{ needs.prepare_chestnut.result == 'success' }}
-        env:
-          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
-        run: |
-          CHESTNUT_BRANCH="${{ needs.prepare_strategy.outputs.new_branch }}-chestnut"
-          CHESTNUT_DIR="${{ github.workspace }}/chestnut_output"
-
-          ${{ env.CI_DIR }}/publish.sh \
-            "${{ github.workspace }}" \
-            "$CHESTNUT_DIR" \
-            "$CHESTNUT_BRANCH" \
-            "${{ needs.prepare_strategy.outputs.version }}" \
-            "https://x-access-token:${{github.token}}@github.com/sunnypilot/sunnypilot.git" \
-            "${{ needs.prepare_strategy.outputs.extra_version_identifier }}"
-
       - name: Tag ${{ needs.prepare_strategy.outputs.environment }}
         if: ${{ needs.prepare_strategy.outputs.is_stable_branch == 'true' && (github.event_name != 'push' || !startsWith(github.ref, 'refs/tags/')) }}
         run: |
@@ -394,12 +531,77 @@ jobs:
             git tag -f -a ${TAG} -m "${{ needs.prepare_strategy.outputs.environment }} @ ${{ needs.prepare_strategy.outputs.version }} of build ${{ needs.prepare_strategy.outputs.build }}."
             git push -f origin ${TAG}
 
+  publish_chestnut:
+    concurrency:
+      group: ${{ needs.prepare_strategy.outputs.publish_concurrency_group }}-chestnut
+      cancel-in-progress: ${{ needs.prepare_strategy.outputs.cancel_publish_in_progress == 'true' }}
+    if: ${{
+      always() && !cancelled() &&
+      needs.build.result == 'success' &&
+      needs.prepare_strategy.result == 'success' &&
+      needs.prepare_small_model.result == 'success' &&
+      needs.prepare_dm_model.result == 'success' &&
+      needs.prepare_chestnut.result == 'success' &&
+      (!contains(github.event_name, 'pull_request') || (github.event.action == 'labeled' && github.event.label.name == 'prebuilt'))
+      }}
+    needs: [ build, prepare_strategy, prepare_chestnut, prepare_small_model, prepare_dm_model ]
+    runs-on: ubuntu-24.04
+    environment: ${{ needs.prepare_strategy.outputs.environment }}
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 1
+
+      - name: Download prebuilt artifact
+        uses: actions/download-artifact@v4
+        with:
+          name: prebuilt
+
+      - name: Untar prebuilt
+        run: |
+          mkdir -p ${{ env.OUTPUT_DIR }}
+          tar xzf prebuilt.tar.gz -C ${{ env.OUTPUT_DIR }}
+
+      - name: Download model chunks from HF
+        uses: ./.github/workflows/download-hf-model-chunks
+        with:
+          hf_repo: sunnypilot/sunnypilot_models_v1
+          dest_dir: ${{ env.OUTPUT_DIR }}/openpilot/selfdrive/modeld/models
+          models: |
+            [
+              {"hf_path": "models/defaults/small", "onnx_hash": "${{ needs.prepare_small_model.outputs.driving_onnx_sha256 }}", "canonical": "driving_tinygrad.pkl"},
+              {"hf_path": "models/defaults/dm", "onnx_hash": "${{ needs.prepare_dm_model.outputs.dm_onnx_sha256 }}", "canonical": "dmonitoring_model_tinygrad.pkl"},
+              {"hf_path": "models/defaults/big", "onnx_hash": "${{ needs.prepare_chestnut.outputs.onnx_sha256 }}", "canonical": "big_driving_tinygrad.pkl"}
+            ]
+
+      - name: Configure Git
+        run: |
+          git config --global user.email "github-actions[bot]@users.noreply.github.com"
+          git config --global user.name "github-actions[bot]"
+
+      - name: Publish chestnut branch
+        env:
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+        run: |
+          CHESTNUT_BRANCH="${{ needs.prepare_strategy.outputs.new_branch }}-chestnut"
+
+          ${{ env.CI_DIR }}/publish.sh \
+            "${{ github.workspace }}" \
+            "${{ env.OUTPUT_DIR }}" \
+            "$CHESTNUT_BRANCH" \
+            "${{ needs.prepare_strategy.outputs.version }}" \
+            "https://x-access-token:${{github.token}}@github.com/sunnypilot/sunnypilot.git" \
+            "${{ needs.prepare_strategy.outputs.extra_version_identifier }}"
+
   notify:
     needs:
     - prepare_strategy
     - build
     - publish
+    - publish_chestnut
     - prepare_chestnut
+    - prepare_small_model
+    - prepare_dm_model
     runs-on: ubuntu-24.04
     if: ${{ (always() && !cancelled() && !failure())
       && needs.publish.result == 'success'
@@ -407,6 +609,8 @@ jobs:
       && (fromJSON(vars.DEV_FEEDBACK_NOTIFICATION_BRANCHES_V2)[github.head_ref || github.ref_name] != null) }}
     steps:
       - uses: actions/checkout@v4
+        with:
+          fetch-depth: 1
 
       - name: Prepare notification message
         id: message

docs/ford_model_action_cadence.md

@@ -0,0 +1,60 @@
+# Selected-action 20 Hz cadence experiment
+
+The selected-action controller now sends LateralMotionControl2 every fifth
+100 Hz update (20 Hz, nominally 50 ms). Each send uses the latest published
+C0/C1. The v6 controller module is byte-identical to `c70a9ee84`: its geometry,
+150 ms forecast, caps, two states, and 4 m/s / 0.5 rad/s slew are unchanged.
+C2 and C3 remain zero. This tests transport cadence, not a new strength gain.
+
+The existing default-off `FordModelActionController` Sunnylink setting is
+snapshotted by `card` into `CarParamsSP.flags` on the supported CAN FD Lightning.
+Both controller selection and send cadence use that snapshot. An onroad setting
+write cannot switch either one. Complete an offroad-to-onroad cycle after updating.
+The diagnostic hypothesis remains `model-action-measured-pose-v6`; the build
+commit, CarParamsSP flag, and measured send cadence distinguish this experiment.
+
+With the toggle off, existing CAN FD controllers retain 100 Hz transmission.
+Legacy CAN stays at 20 Hz. Panda safety is byte-identical to `c21a9013`.
+The sender retains Panda's existing per-message C2 slew bound even on the
+20 Hz path; this controller does not use C2. No safety limit is relaxed.
+Counters advance once per transmitted request, including wrap from 15 to 0.
+Invalid paths zero the next scheduled request; disengagement sends mode 0 on
+that request, without a new ramp-out sequence. Relative to a 100 Hz sender,
+a change can wait up to four more control ticks (nominally 40 ms).
+
+## Evidence and limits
+
+On route `84865544361f55cb_000000a5--d0f935d323`, the camera's observed inactive
+LMC2 stream ran at 19.993 Hz (7,952 messages, median 50.051 ms). Our sender ran
+at 99.321 Hz (38,496 messages, median 9.938 ms). The camera data does not establish
+the factory's active-mode cadence. All supplied v1–v6 drives already used
+100 Hz; cadence has not been established as the cause of weak tracking.
+
+The [transport replay record](ford_model_action_cadence_validation.json) covers
+38,496 recorded send cycles at each of five possible scheduling phases:
+192,480 sender updates and 38,496 transmitted requests in total. Every emitted
+C0/C1 exactly matched the corresponding recorded request; C2/C3 stayed zero;
+mode, counter and checksum checks passed. Every request passed the unchanged,
+compiled Panda TX hook with controls eligibility set from the recorded mode.
+This tests TX bounds, not a full Panda RX watchdog, vehicle response, or device boot.
+
+Targeted tests additionally cover exact send intervals, latest-sample delivery,
+counter wrap, every disengagement/invalid-input phase, unchanged fallback cadence,
+100 Hz core slew, and a shared selection snapshot surviving serialization and
+subsequent stored-toggle changes. The broader offline run passed 689 tests and
+9,146 subtests; 178 inherited safety cases were skipped as inapplicable.
+Ruff and typechecking of the touched production modules passed.
+
+No physical tracking improvement is claimed. The next drive must establish
+whether the lower cadence helps ordinary bends and turn exits, while checking
+for added turn-in delay. Offline replay cannot predict that closed-loop response.
+
+## Reproduce
+
+Initialize the pinned submodule and use the project's built Python/native environment:
+
+```sh
+export PYTHONDONTWRITEBYTECODE=1
+export PYTHONPATH=.:opendbc_repo
+python -m pytest -q openpilot/selfdrive/controls/tests/test_ford*.py tools/ford_pscm_lab opendbc_repo/opendbc/car/ford/tests/test_ford.py openpilot/sunnypilot/sunnylink/tests openpilot/sunnypilot/mads/tests openpilot/selfdrive/car/tests/test_ford_pscm_status.py openpilot/common/tests/test_params.py opendbc_repo/opendbc/safety/tests/test_ford.py
+```

docs/ford_model_action_cadence_validation.json

@@ -0,0 +1,45 @@
+{
+  "scope": "Frozen a5 publications through actual 20Hz CarController and unchanged compiled Panda TX hook, all five scheduling phases. No physical response simulation.",
+  "route": "84865544361f55cb_000000a5--d0f935d323",
+  "baseline_root": "c70a9ee84bbf6db9a687d63ef674ed4db9c13e3a",
+  "baseline_opendbc": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+  "input_sha256": "815d1e248ff5c3e5e5cfc11dfbd0690d0d13ddafe1436890e239975a72dae9b8",
+  "phases": [
+    {
+      "phase": 0,
+      "input_cycles": 38496,
+      "sent_and_accepted": 7700
+    },
+    {
+      "phase": 1,
+      "input_cycles": 38496,
+      "sent_and_accepted": 7699
+    },
+    {
+      "phase": 2,
+      "input_cycles": 38496,
+      "sent_and_accepted": 7699
+    },
+    {
+      "phase": 3,
+      "input_cycles": 38496,
+      "sent_and_accepted": 7699
+    },
+    {
+      "phase": 4,
+      "input_cycles": 38496,
+      "sent_and_accepted": 7699
+    }
+  ],
+  "core_byte_identical": true,
+  "ford_safety_byte_identical": true,
+  "source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "326539389b04034932db41ca2c67794779403b2c0e97f408a81a9f5734114899",
+    "openpilot/selfdrive/controls/controlsd.py": "002d57a0b5b6e4e3a04789bee20b9b175a71d6893dd06074cebca2738f3986da",
+    "openpilot/sunnypilot/mads/helpers.py": "24970993d37242fe8a0457bac118e2c265c4c32427dc930c0001de12e5f204dd",
+    "opendbc_repo/opendbc/car/ford/carcontroller.py": "591b0d8455d256f7504cbd2a6a11fe1a54ba6ad3e12ac16a6952dbd92274f303",
+    "opendbc_repo/opendbc/car/ford/values.py": "edadaacc13581642917d1fba473ef3efc90d7145dbe801915f860747a4294046",
+    "opendbc_repo/opendbc/safety/modes/ford.h": "1d9d996292d6697ab4f02d55fae348d6aca1df94a07f7bdae48b68971b91afe7",
+    ".cache/ford_cadence/replay.py": "7227f0b4f130be0e5b28f517416ffbf281f0b33085b8391f66814aa531c3e514"
+  }
+}

docs/ford_model_action_candidate.md

@@ -0,0 +1,153 @@
+# Offline Ford selected-action candidate
+
+This document and `ford_model_action_validation.json` record the offline
+stage committed as `7ca3c6e3b`. The candidate is now available behind a
+separate default-off Sunnylink toggle; see
+[drive-test setup and validation](ford_model_action_drive_test.md).
+The counts, source hashes and selector status below describe that earlier
+stage, not the subsequent wiring change.
+
+The decision is `C0 = current model y(7 m)`,
+`C1 = max(7 m, speed × 1 s) × selected upstream-limited desiredCurvature`,
+with C2=C3=0. The 7 m station and one-second scale are engineering choices,
+not identified PSCM gains. `calibration_approved=false`.
+
+`openpilot/selfdrive/controls/lib/ford_model_action.py` contains the core
+and a separate adapter compatible with the existing controlsd call.
+At that stage, the production selector, v8 implementation, settings, opendbc
+submodule and Panda safety remained unchanged. Tests injected the adapter
+offline; there was no production setting. No hardware or CAN transmission
+occurs in the lab tools.
+
+## Construction and integration
+
+Only the unquantized C0 and C1 slew positions persist in the core.
+Each field is clipped independently (±5.11 m / ±0.5 rad), slewed independently
+(4 m/s / 0.5 rad/s), then packed using the existing Float32/sign-negation
+rounding contract (0.01 m / 0.0005 rad). Heading overflow is not transferred
+to C0. No yaw integral, blend, additional curvature contribution, reference
+filter, turn modes, or 10 m C1 cap is introduced.
+
+The selected standalone implementation from worktree 3548 is the provenance
+for this law. Its two-state packer has been moved into the library core so
+the controller does not depend on experimental lab code. Invalid numeric
+types, overflowing arc geometry and malformed paths reset the core instead
+of throwing or retaining a command.
+
+Arc stations use cumulative model x/y distance, not forward x. As in the
+reviewed standalone core, a path ending before 7 m holds its available
+endpoint instead of extrapolating. This matters: route95 contains 44 active
+cycles with 5.45–6.94 m of path at 2.78–3.46 m/s. A tested strict 7 m
+coverage gate would have introduced disengagements and was removed. There
+is no speed-dependent C0 horizon beyond this existing endpoint behavior.
+
+The adapter retains the existing input age allowance (−5 to +150 ms),
+speed domain (0.3–55 m/s), yaw sanity bound (±3 rad/s), selected curvature
+sanity bound (±1/m), and control interval (2–100 ms). It rejects backward
+model/measurement timestamps and invalid services. Repeated timestamps may
+continue slew, but geometry is validated again on each tick. Disengagement,
+invalid inputs and timing faults clear all command and adapter timing state.
+The first valid tick after reset uses 10 ms, as v8 does.
+
+controlsd still owns reference selection, upstream curvature limiting,
+service health and engagement. Tests execute its actual source-selection
+and limiter code, its Ford call, Float32 publication in ControlsExt, conversion
+to CarControlSP, and the pinned Ford CarController's in-memory CAN builder.
+Both model-action and maneuver-planner selection are covered, including
+disabling latActive after invalid output. Only the test chooses the adapter.
+
+Yaw is not an input to the control law. The adapter checks it solely for the
+inherited invalid-input policy. Driver override and optional PSCM status
+do not modify the candidate base; existing engagement and downstream driver
+arbitration remain responsible for authorization, as with v8's base request.
+
+## Offline evidence
+
+The checked-in `ford_model_action_validation.json` records the completed
+checks and source hashes. Full arrays and detailed reports are generated
+locally under `.cache/ford_model_action/`; original route files are read-only.
+
+Completed validation: **264 Ford tests and 150 subtests pass**, including
+120 new core/adapter/replay-validator cases. The candidate module has 100%
+statement and branch coverage (78 statements, 24 branches). Ruff and Ty pass.
+The 200,000-cycle numerical stress test also checks 200,000 mirrored core
+updates and 18,138 field-boundary cases. Across route and stress runs,
+485,238 Float32/CAN round trips pass. Eight deliberately injected faults
+(heading gain/cap, erased C0, wrong C0 slew, retained invalid state, stale
+model acceptance, model clock rollback and reversed C0 sign) are all caught
+by the tests. Mutation runs replace code only inside isolated Python
+processes; production source files are never modified by those probes.
+
+Independent Standards and Spec reviews reported zero findings. The full
+suite's Params setting test uses an existing local native library from
+worktree 3548 after checking relevant source files are byte-identical;
+its hash and provenance are in the manifest. That library is an ignored
+test dependency, not part of this change. This is the full relevant Ford
+suite, not the hardware-dependent test suite for every openpilot subsystem.
+
+The replay has two separate passes:
+
+* Core compatibility uses the archived eligibility mask and requires exact
+  equality with the independently implemented `action_heading` commands.
+* Adapter reconstruction derives eligibility from recorded service streams
+  independently of the archived output mask. It retains original timestamps,
+  gaps and consumed model frames. Controls publication time proxies the
+  unlogged computation clock, and complete SubMaster health is unavailable.
+
+All 54,738 route95 and 78,812 route90 core cycles match exactly, including
+37,614 and 73,055 active cycles. The adapter preserves those active counts.
+Its 59 / 19 changed commands arise solely from the fresh 10 ms engagement
+tick instead of the archived harness's preceding publication interval;
+the replay checks that attribution on every cycle. Maximum differences are
+0.01 m / 0.001 rad (95) and 0.02 m / 0.002 rad (90).
+
+Every core and adapter replay output is round-tripped through Float32 and
+the real CAN packer/parser, including zero C2/C3, signs, mode and counter.
+Continuous field slew and quantization allowance are checked separately
+from immediate invalid-command resets. The original driver-clean cohorts,
+speed strata and command RMS are reproduced without redoing the encoder search.
+
+The numerical stress harness uses analytic rotated paths, scalar slew
+arithmetic, mirrored requests, irregular intervals and invalid-input resets.
+It also sweeps every representable host field value and the Float32 values
+immediately below, at and above every half-quantum boundary. Direct CAN
+packing of the continuous state must agree with the host's quantized output.
+The unit tests cover releases, reversals, clipping, service freshness,
+clock resets, malformed inputs, endpoint fallback and actual integration.
+
+## Limits of the result
+
+On turns at ≥15 m/s, candidate C0 RMS is 79%/81% below v8 on routes95/90,
+while C1 is 33%/41% higher. Those are command changes, not evidence of
+equivalent steering authority. The PSCM's independent C0/C1 response remains
+unknown. Replay cannot establish physical model following, strong turns,
+centering, overshoot, oscillation or closed-loop stability.
+
+The release probe is intentionally explicit: a model bend can increase
+while selected curvature decreases. At 20 m/s, one synthetic probe changes
+C0/C1 from 0.24 m / 0.10 rad to 0.49 m / 0.08 rad. Zero selected curvature
+sets the C1 target to zero but does not erase a nonzero current model C0.
+Removing a yaw-integral tail does not prove that physical overshoot is solved.
+No additional release policy or unsupported plant model is added to hide
+that uncertainty.
+
+## Reproduce
+
+From this worktree, use the logged construction dependency explicitly:
+
+```sh
+export PYTHONDONTWRITEBYTECODE=1
+export PYTHONPATH=.:/Users/ibpersonal/.codex/worktrees/b926/sunnypilot/opendbc_repo
+PY=/Users/ibpersonal/dev/sunnypilot/.venv/bin/python
+EVIDENCE=/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904
+$PY -m pytest -q -p no:cacheprovider openpilot/selfdrive/controls/tests/test_ford_*.py tools/ford_pscm_lab openpilot/selfdrive/car/tests/test_ford_pscm_status.py
+$PY -m tools.ford_pscm_lab.model_action_replay "$EVIDENCE/route95" --output .cache/ford_model_action/route95
+$PY -m tools.ford_pscm_lab.model_action_replay "$EVIDENCE/route90" --output .cache/ford_model_action/route90
+$PY -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --output .cache/ford_model_action/stress.json
+```
+
+The route replay refuses an opendbc revision other than
+`72a775d35e54c21ff5c5798acef22016eedcc0a7`. Stress defaults to this pin and
+also accepts an explicitly required commit with `--opendbc-revision` for
+deployment checks. A mismatch still fails. This historical pin reproduces
+logged construction; it does not change the merge's submodule pointer.

docs/ford_model_action_damping.md

@@ -0,0 +1,88 @@
+# Experimental Ford offset damping, v2
+
+This document and its validation counts describe the archived v2 source. The
+[current v4 experiment](ford_model_action_full_prediction.md) uses full path prediction.
+
+Segment 10 of the supplied route9b recording shows measured turning persisting
+as requested right curvature falls. At about 643.0 s, before strong driver
+intervention, device-gyro curvature is approximately 0.01786/m against a
+0.01172/m request. Around 643.9 s, heading demand has reversed slightly but
+C0 still requests approximately +0.12 m into the turn. Strong column input
+starts around 643.852 s; later motion cannot establish autonomous recovery.
+Earlier light driver input also exists.
+
+The outgoing CAN commands match preceding publications. All 70,937 decoded
+frames have zero C2/C3 and valid checksums. Focused exit diagnostics have fresh
+model/carState inputs and targets within normal quantization of the outputs.
+This supports trying less residual C0; it does not identify PSCM dynamics or
+prove C0 alone caused the physical oversteer.
+
+## Change
+
+C0 starts from the clipped current model offset at 7 m. When C0 and measured
+host yaw point in the same direction, compute:
+
+```
+requested_yaw = max(0, sign(C0) * speed * desiredCurvature)
+excess = max(0, sign(C0) * yaw - requested_yaw - 0.02 rad/s)
+reduction = 7 m * 0.2 s * excess
+target = sign(C0) * max(0, abs(C0) - reduction)
+```
+
+Opposing centering demand is unchanged. The correction cannot increase the
+target's magnitude or reverse its sign. Opposite-direction planned curvature
+cannot amplify a small yaw bias into a correction. Existing 4 m/s C0 slew still
+applies; this target bound is not a claim that every stateful output is smaller than a
+separate v1 controller after arbitrary direction reversals. C1 construction,
+clipping and slew are unchanged; C2=C3=0. Only C0/C1 slew states persist.
+There is no integral, model-history filter, turn state machine or fitted plant.
+
+Host yaw is `-carState.yawRate`, as in the existing Ford call path. The
+0.02 rad/s deadband exceeds the approximately 0.008 rad/s offset measured
+against the device gyro on quiet straights. The 0.2 s scale is an initial
+engineering choice, not an identified delay or gain. Both remain physically
+unvalidated. Large biased or noisy yaw within the existing sanity gate can
+still attenuate useful centering; fixed-input replay cannot establish stability.
+
+## Offline evidence
+
+The complete 12-rlog route is replayed at original controls publication times,
+with exact consumed model geometry, causal carState, and carControl matched
+within 5 ms. These times proxy computation; full SubMaster health is unavailable.
+V1 reconstruction is within one field quantum of all 64,701 paired active
+publications. V2 has identical eligibility and exactly identical C1.
+
+On 381.78 seconds of driver-clean low requests above 8 m/s, only 3 of 37,937 cycles
+change C0, each by one 0.01 m quantum. Across the 642.7–643.852 s exit window,
+C0 changes on all 115 cycles, averaging 0.080 m reduction. Entry/peak C0
+maximum stays 2.80 m; some entry-window samples decrease by up to 0.05 m.
+These are command comparisons on recorded inputs, not predicted tracking.
+Low request means requested lateral acceleration below 0.15 m/s²; it is a
+proxy for straight driving and does not establish a physically straight path.
+
+The original routes90/95 also run through v2. Their zero-yaw baseline pass
+checks archived v1 compatibility; the measured-yaw adapter pass checks current
+construction, eligibility, field limits and packing. It is not an exact match
+to v1 or v8. Randomized testing checks the damping against an independent
+piecewise oracle, mirror symmetry, resets and slew, with real Float32/CAN
+round trips. See `ford_model_action_damping_validation.json` for counts and hashes.
+
+Final validation passes 325 tests and 26 subtests, with 100% controller
+statement/branch coverage, 204,946 original route cycles and 628,030 CAN round
+trips. The module is 166 total lines, including 107 code lines excluding
+comments, blanks and docstrings. Standards and Spec reviews have no remaining findings.
+
+## Reproduce
+
+Use the dependency setup and suite command in the [drive-test guide](ford_model_action_drive_test.md).
+The new route replay requires the deployment opendbc pin recorded there:
+
+```sh
+python -m tools.ford_pscm_lab.damping_replay /path/to/complete/rlogs --baseline v1 --candidate v2 --window segment10_entry_peak 637 640 --window segment10_exit_before_strong_input 642.7 643.852 --output /path/to/separate/results
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output /path/to/stress.json
+```
+
+At the v2 revision, the same default-off Sunnylink toggle selects v2; no additional setting is
+introduced. Updating an installation with the toggle already enabled selects
+v2 at the next controlsd startup. `calibration_approved=false` remains explicit.
+No physical fix, hardware build or device boot is established by these checks.

docs/ford_model_action_damping_validation.json

@@ -0,0 +1,164 @@
+{
+  "date": "2026-09-07",
+  "baseline_commit": "5fc16abc7662020706e29f57d31a6d5e2bc1293a",
+  "deployment_target": {
+    "repository": "sunnypilot/sunnypilot",
+    "branch": "hiimisaac-dev"
+  },
+  "hypothesis": "model-action-c0-c1-yaw-damping-v2",
+  "scope": "Experimental bounded offset damping; fixed-input offline evidence only.",
+  "calibration_approved": false,
+  "hardware_build_and_device_boot": "not performed",
+  "controller_size": {
+    "total_lines": 166,
+    "code_lines_excluding_blanks_comments_docstrings": 107,
+    "core_persistent_values": 2,
+    "adapter_timestamps": 3
+  },
+  "checks": {
+    "combined_ford_params_sunnylink_suite": "325 passed, 26 subtests passed; no skips",
+    "suite_log_sha256": "69cb8ab40e93e00a9ff7b6ea1933e4c3554336746860efb53dad3b29f94fc9d3",
+    "coverage": {
+      "statements": 98,
+      "branches": 28,
+      "percent": 100.0
+    },
+    "ruff": "pass",
+    "ty_controller_and_lab": "pass",
+    "settings_compiler_check": "pass",
+    "standards_review_remaining_findings": 0,
+    "spec_review_remaining_findings": 0,
+    "review_resolutions": [
+      "Prevent opposite-direction planned curvature amplifying small yaw bias; eight new cases failed before the fix and passed after it.",
+      "Relabel requested-acceleration cohort as low request; it does not establish physically straight driving."
+    ],
+    "mutation_probe": "Disabling damping fails all four mirrored recorded-exit cases."
+  },
+  "stress": {
+    "random_cycles": 200000,
+    "mirrored_core_updates": 200000,
+    "field_boundary_cases": 18138,
+    "float32_can_round_trips": 218138,
+    "bounded_excess_yaw_damping_checked": true,
+    "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9"
+  },
+  "routes": {
+    "route90": {
+      "cycles": 78812,
+      "core_exact_archived_match": true,
+      "adapter_active_cycles": 73055,
+      "adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 2280,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.1900000000000004,
+        0.0020000000000000018
+      ],
+      "float32_can_round_trips": 157624,
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7"
+    },
+    "route95": {
+      "cycles": 54738,
+      "core_exact_archived_match": true,
+      "adapter_active_cycles": 37614,
+      "adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 2051,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.22999999999999998,
+        0.0010000000000000009
+      ],
+      "float32_can_round_trips": 109476,
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7"
+    },
+    "route9b": {
+      "cycles": 71396,
+      "eligible_cycles": 64701,
+      "same_validity": true,
+      "c1_exactly_unchanged": true,
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 142792,
+      "v1_reconstruction_vs_recorded": {
+        "paired_cycles": 64701,
+        "within_one_quantum_cycles": 64701,
+        "maximum_absolute_error_c0_c1": [
+          0.010000114440917862,
+          0.0005000143051147043
+        ]
+      },
+      "timing": "Publication-time proxy, causal carState, exact consumed model; full SubMaster health unavailable.",
+      "baseline": "Current adapter with zero yaw retains v1 targets and actual-yaw sanity gate.",
+      "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+      "cohorts": {
+        "driver_clean_low_request_above_8mps": {
+          "cycles": 37937,
+          "seconds": 381.77672216900055,
+          "changed_c0_cycles": 3,
+          "mean_absolute_c0_change_m": 8.419774473786704e-07,
+          "max_absolute_c0_change_m": 0.009999999999999787,
+          "v1_peak_absolute_c0_m": 0.08999999999999986,
+          "v2_peak_absolute_c0_m": 0.08999999999999986
+        },
+        "segment10_entry_peak": {
+          "cycles": 298,
+          "seconds": 2.9932484459999387,
+          "changed_c0_cycles": 100,
+          "mean_absolute_c0_change_m": 0.007564164795694466,
+          "max_absolute_c0_change_m": 0.04999999999999982,
+          "v1_peak_absolute_c0_m": 2.8000000000000003,
+          "v2_peak_absolute_c0_m": 2.8000000000000003
+        },
+        "segment10_exit_before_strong_input": {
+          "cycles": 115,
+          "seconds": 1.156770048999988,
+          "changed_c0_cycles": 115,
+          "mean_absolute_c0_change_m": 0.0799313220721162,
+          "max_absolute_c0_change_m": 0.1200000000000001,
+          "v1_peak_absolute_c0_m": 0.75,
+          "v2_peak_absolute_c0_m": 0.7000000000000002
+        }
+      },
+      "exit_c0_strictly_lower_on_all_115_cycles": true,
+      "rlog_sha256_by_segment": {
+        "0": "22746f7119109b73ed7f2c26ce8c99f87136e9124fb7fc14c9554409a28a7c3f",
+        "1": "4c2e1d7083c31a2b37d0f8dd3be4d330898511b7e02c26f7d40ca9bc2779397d",
+        "2": "62f3e049e220cd3681fadf386f2969537bd571998ae2f6ba2d08479428b5a28f",
+        "3": "83bf0131b2d36b2ba7e5ba050bbc13c0a3350feb5c9b89dc9c87d3a37abebfb3",
+        "4": "430985a80dd6e10f7abeb89457a17022e6bb6978617f415c905f584b1647603e",
+        "5": "8c0c5ae6323ec33b3e14f84ca834f70cb56f6b29f471a350f1e3efc06b6ba553",
+        "6": "db53dfa8156b9d66792c3eff0b2ce5d31b71ad41cc580dec85f528845593c184",
+        "7": "91b0b3be10cb7d7d7f7dd2024d8f9ee99d1e9fd2204203a3a9a2f2f1c6e3fa03",
+        "8": "687dbbfc49837efbfe8fa6bc091e40f7fad2908832234d7884f4616d1bc9ccff",
+        "9": "a88ec4d25b04cdbf5844686fc77f6b28dca920c9b164e37ebf69844a3ae398fc",
+        "10": "fe6b29580a6c94e1c236d13e18db4cd9f31cc1b25d52e1e6e19a5021125c9932",
+        "11": "150d31b1944d7a1b8c562f3aee20b66cefa6c4e8d02660ec889907d835142f45"
+      }
+    }
+  },
+  "total_original_route_cycles": 204946,
+  "total_float32_can_round_trips": 628030,
+  "source_sha256": {
+    "docs/ford_model_action_damping.md": "1aca8ca8e78d953beeda5b0c9803161a1d7c58556b1966040c71f809c3960bf8",
+    "docs/ford_model_action_drive_test.md": "3ce8bf6cb511a4461fa7abf194f47020af7ef5a6c90fcae7ed09571d1f750846",
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "59d66297a017557f3d4f28b115be3f6220b800566c11935e2284b3814783fb7e",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "318742bd707ae526d0f5181bf7f081660c2c55de4bdf28518ccdf50d60e88080",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "de6f8524347f7c4a339941bc8565ccaa131cb93aa0418c75006ce08ab7edeb99",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_damping.py": "76466997ab4fef435f44339a6cb2d06303d5f0ab8717d487f27655297bd429d1",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "dbb78c98f57eef532f0dff0cb0b38396442882876e6d115f0d3c36159f64baf5",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "b92b55e23467e74227988fb39fff13a4ebba3c960fb90e34a6b341411699fc95",
+    "tools/ford_pscm_lab/damping_replay.py": "2f52fef12116ce87c0a1f465cf13d6af5e9fd76ce05d4318d72e34b70bc6a11f",
+    "tools/ford_pscm_lab/model_action_replay.py": "05a658dfcaf81693bf0d92184c0edff0172802f351a61a9e866a7967e74ae46d",
+    "tools/ford_pscm_lab/stress_model_action.py": "3e308733f4af0101ad0c414fbd724f6a99f56269ed8e791f0e15526d8bfd8f17"
+  },
+  "artifact_sha256": {
+    "route9b/report.json": "1b9317850c1724f433269c6a58349d0a0ee4eb6c9a03d6ec5858fa17713796c5",
+    "route9b/commands.npz": "d9f553a5384c84416a27c52b5dda0e751dda25edc5d6620fc311977f34a7a946",
+    "route90/report.json": "e37f71dc032e375b1c9b0beb4d6e0c915257bf72e59b0785ac0ca49c2472d2c2",
+    "route95/report.json": "d42d5a080fef8a1f0b3c7ae2cabcad20c88ce01d91be07ab33770ff5948c06b6",
+    "stress.json": "06e69a23340e3f5ed174e8e0b2e5791b320686dce7df963233d50a9982dca17b",
+    "coverage.json": "8f7915b9bd884abedfdbc2e0c18ef4474737e27225a714414384242535cc396f",
+    "mutation.txt": "67a76549fd7bb71e7092a155d4d0c3459be7b04dc2ca36eef9b54fbb574ad83d",
+    "bias_regression_red.txt": "8903c5a967e8c376050db85f7cf5f73abf71ee972f6025b25eb874479f94c62c",
+    "segment10_damping.png": "7d60cf9aabdcca9000fcf49bd14bd6130418aa6ab1b57af2678b171498ee9505"
+  }
+}

docs/ford_model_action_drive_test.md

@@ -0,0 +1,82 @@
+# Ford model-point drive-test branch
+
+The default-off **Selected-Action Path Tracking (Experimental)** Sunnylink toggle
+now selects the C1 early-release candidate on the **Ford CAN FD F-150 Lightning**.
+The stored key remains `FordModelActionController`; an already enabled setting
+selects this revision after updating and completing an offroad-to-onroad cycle.
+
+The controller reads **model lateral position and model heading at the same
+point**. Start with the model's predicted distance at one second, enforce the
+existing seven-metre minimum, and hold the available endpoint when necessary.
+C0 is that point's lateral position in metres. C1 starts from its unwrapped
+heading and is bounded toward zero using the model's terminal spatial curvature
+at the same point. This asks for earlier release when the path straightens
+ahead. C2 and C3 stay zero. See [the exact rule and its authority tradeoff](ford_model_release.md).
+
+This point choice is an engineering guess, not an identified Ford reference or
+PSCM calibration. `calibration_approved=false`: offline tests do not establish
+physical tracking, turn-exit behavior, or stability across different PSCMs.
+The [v7 model-point decision and validation](ford_model_points.md) is historical.
+
+## Select and restore
+
+1. Install branch `hiimisaac-dev` from `sunnypilot/sunnypilot` and allow the build
+   to finish.
+2. While offroad, open Sunnylink device settings → Vehicle → Ford. Keep or enable
+   **Selected-Action Path Tracking (Experimental)**.
+3. Complete a real offroad-to-onroad cycle. `card` snapshots the toggle into
+   `CarParamsSP`; the sender and `controlsd` share that selection. Changing a
+   stored toggle or disengaging alone cannot swap an active controller.
+
+The startup event `Ford path controller selected` reports
+`FordModelActionController`. Periodic `Ford C2-free path tracking` events report
+`hypothesis=model-pose-terminal-c1-v1`, `pose_source=model`, `preview_time_s=1.0`
+and `minimum_station_m=7.0`, plus input ages, slew state and the command tuple.
+Active diagnostics also report `c1_release=terminal_spatial_curvature`.
+Selected desired curvature is still logged, but does not construct C0/C1.
+
+Turning the toggle off and completing another offroad-to-onroad cycle restores
+**PSCM Coefficient Observer** if selected, otherwise the original Ford path
+controller. Other vehicles keep their previous selection. The retired v8 toggle
+cannot select this candidate.
+
+## Wiring and limits
+
+Both model fields must have matching, finite, strictly increasing time arrays
+starting at zero. Malformed geometry, stale required services, invalid timing,
+or disengagement resets both actuator states. Freshness still requires model,
+car-state and reference publications no older than 150 ms, with at most 5 ms
+future skew. The valid control timestep remains 2–100 ms.
+
+Only the two unquantized C0/C1 slew positions persist in the core. Field caps are
+±5.11 m / ±0.5 rad and slew rates are 4 m/s / 0.5 rad/s. Calculation stays at
+100 Hz; the existing [cadence experiment](ford_model_action_cadence.md) sends this
+candidate at 20 Hz. Float32 publication, host-to-wire negation, packing and
+Panda safety are unchanged. The opendbc pin remains
+`87ca78e6e641eefb2d654f260a6ab08df3058bd5`.
+
+Normal operation uses the model point with the one-sided C1 release bound. The upstream scalar curvature
+and its clipping still exist for logging/other controllers, but no longer bound
+this candidate's heading target. Its C0/C1 field caps and slew still apply;
+passing Panda TX checks does not establish an actual vehicle acceleration bound.
+Lateral maneuver test mode supplies only a scalar curvature, not a model pose.
+It explicitly invalidates/disengages this candidate as `unsupported_reference`.
+Optional measured motion is no longer a command input.
+
+## Reproduce offline checks
+
+Initialize the pinned submodule and build the project's native Python dependencies:
+
+```sh
+git submodule update --init opendbc_repo
+export PYTHONDONTWRITEBYTECODE=1
+export PYTHONPATH=.:opendbc_repo
+python -m pytest -q openpilot/selfdrive/controls/tests/test_ford*.py tools/ford_pscm_lab opendbc_repo/opendbc/car/ford/tests/test_ford.py openpilot/sunnypilot/sunnylink/tests openpilot/sunnypilot/mads/tests openpilot/selfdrive/car/tests/test_ford_pscm_status.py openpilot/common/tests/test_params.py opendbc_repo/opendbc/safety/tests/test_ford.py
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260908 --opendbc-revision 87ca78e6e641eefb2d654f260a6ab08df3058bd5 --output .cache/ford_model_points/stress.json
+```
+
+Historical v1–v7 validation files retain their original source hashes and apply
+to those revisions. In particular, `ford_model_action_measured_pose_validation.json`
+describes v6, not the current model-point mapping. The hardware build and device
+boot are not performed by these offline checks. Pushing a branch does not update
+a device or change its stored settings.

docs/ford_model_action_drive_test_validation.json

@@ -0,0 +1,145 @@
+{
+  "date": "2026-09-07",
+  "baseline_commit": "7ca3c6e3b3e659c6f446039501c5826bbd14092e",
+  "branch": "codex/ford-model-action-drive-test",
+  "scope": "Default-off Sunnylink selection and v8 retirement; offline validation only. No device installation or physical performance validation.",
+  "calibration_approved": false,
+  "production_selector_changed": true,
+  "toggle": "FordModelActionController",
+  "default_enabled": false,
+  "v8_removed": true,
+  "panda_safety_changed": false,
+  "opendbc_submodule_changed": false,
+  "deployment_opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+  "controller_size": {
+    "total_lines": 145,
+    "code_lines_excluding_blanks_comments_docstrings": 95,
+    "core_persistent_values": 2,
+    "adapter_timestamps": 3,
+    "removed_v8_module_lines": 469
+  },
+  "tests": {
+    "combined_ford_params_sunnylink_suite": "284 passed, 26 subtests passed in 2.63s",
+    "suite_log_sha256": "2e223a507f0630481cf6f83b9f8893d226f3f4273a79a09fc35905aa875b1d2c",
+    "coverage": {
+      "covered_lines": 87,
+      "num_statements": 87,
+      "percent_covered": 100.0,
+      "percent_covered_display": "100",
+      "missing_lines": 0,
+      "excluded_lines": 0,
+      "percent_statements_covered": 100.0,
+      "percent_statements_covered_display": "100",
+      "num_branches": 26,
+      "num_partial_branches": 0,
+      "covered_branches": 26,
+      "missing_branches": 0,
+      "percent_branches_covered": 100.0,
+      "percent_branches_covered_display": "100"
+    },
+    "ruff": "pass",
+    "ty_controller_and_lab": "pass",
+    "settings_compiler_check": "pass",
+    "standards_review_remaining_findings": 0,
+    "spec_review_remaining_findings": 0,
+    "resolved_review_finding": "Updated YAML authoring source and regenerated settings JSON before final compiler/schema suite."
+  },
+  "routes": {
+    "route95": {
+      "cycles": 54738,
+      "core_active_cycles": 37614,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 37614,
+      "adapter_exact_match_with_fresh_engagement_dt": true,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 59,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.010000000000000675,
+        0.0010000000000000009
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 109476,
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+      "report_sha256": "72fab710dc81c8c7d9b75d371b97fec32402d3b01fa05517dfa814d8daff3134"
+    },
+    "route90": {
+      "cycles": 78812,
+      "core_active_cycles": 73055,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 73055,
+      "adapter_exact_match_with_fresh_engagement_dt": true,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 19,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.020000000000000462,
+        0.0020000000000000018
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 157624,
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+      "report_sha256": "cc2224bae597a341697a7681560a077cb209d77e4d060c0850997691c57d32fb"
+    }
+  },
+  "stress": {
+    "seed": 20260907,
+    "random_cycles": 200000,
+    "mirrored_core_updates": 200000,
+    "invalid_or_inactive_resets": 3537,
+    "field_boundary_cases": 18138,
+    "float32_can_round_trips": 218138,
+    "analytic_targets_scalar_slew_and_mirror_checks_pass": true,
+    "direct_raw_float32_packing_matches_host_output": true,
+    "max_continuous_step_c0_c1": [
+      0.40000000000000147,
+      0.05000000000000002
+    ],
+    "calibration_approved": false,
+    "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.",
+    "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9"
+  },
+  "total_float32_can_round_trips": 485238,
+  "native_params": {
+    "source": "Rebuilt locally from this branch with clang++ and generated Capnp headers; ignored test dependency, not committed binary.",
+    "library_sha256": "270bf43241cf7c02cc432cf78ec9411a62d7653ca445695efe785ae82241aa09",
+    "sources_sha256": {
+      "openpilot/common/params_c.cc": "57e3bcc7eba939bc91aadafb4ed1248b8123a8fe5c48fd8530298d966ea4db63",
+      "openpilot/common/params.cc": "a5adacb1d47cb3bf6e0d87d44ce158b41982d7eaf2e8114e32c48d3a6631304c",
+      "openpilot/common/params.h": "ed03d137e126ecd6f1608016020af18c0339fb987e27d0a2aa6830bba396970c",
+      "openpilot/common/params_keys.h": "39d36465f66405843b926ba18473fb6aee81c0f1c7bea87246aa08ffe3f67c58",
+      "openpilot/common/util.cc": "4479ecf72465e8f453d8af78447f7715f02d9397c58a49048f2bbc87a96d6b8a",
+      "openpilot/common/swaglog.cc": "9c2f88a2f1c3c4253b73defb264cc367a13ade23e02928e1d469b5c5833df176"
+    }
+  },
+  "test_dependency_notes": {
+    "python": "/Users/ibpersonal/dev/sunnypilot/.venv/bin/python",
+    "pyyaml": "6.0.3 from local uv cache",
+    "jsonschema": "Local cached package appended after venv to run schema validator without skips",
+    "hardware_build_and_device_boot": "not performed"
+  },
+  "source_sha256": {
+    "docs/ford_model_action_candidate.md": "c968132348d20891a9396f6e69db1315d570505796e19e09ffbb2da748e6e687",
+    "docs/ford_virtual_angle_experiment.md": "da6322f3c3d2d81463e44c50cc6cad1a962f97008ff9425c314da333ebe47a87",
+    "openpilot/common/params_c.cc": "57e3bcc7eba939bc91aadafb4ed1248b8123a8fe5c48fd8530298d966ea4db63",
+    "openpilot/common/params_keys.h": "39d36465f66405843b926ba18473fb6aee81c0f1c7bea87246aa08ffe3f67c58",
+    "openpilot/common/tests/test_params.py": "557a1f616af5fd9f5e623fbee0fd6f44cb059a30c290c68ce2b57e9bcceed081",
+    "openpilot/selfdrive/controls/controlsd.py": "102b383e5beff43b8dd7c219178bef62e8a4b54443ebe682694606862bcd4e7f",
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "8f3bc5d68e0051776f614a2ccffae84a88f7898dc95bdc12c23dcfe10dfe676a",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "1c9448d88d8021e5d34a5dccd14a17c6c1bc64b5531342bfc6d15574d9d3e710",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "2c5b14f814e84e59d749f61a43ef1dcfe06253f6e185443fa123c37525ac8466",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "9974df3ac4cc58ae78d47848cd18ef4aca1bcbb00edb257f28b4220d92890528",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "7a3fa18e562d5a03a5b85c72ee3f3ebeb836c497285dcd9aaad24f8fb4dd6942",
+    "openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py": "3db566612381fd87d3655a3ccff470be7da998c4ea7f6365f53e58cdb9c0ffb7",
+    "tools/ford_pscm_lab/model_action_replay.py": "827a6dc488d554bdf6e87438c6a2a985b3195bf6d01ab09002bfd6049d22a868",
+    "tools/ford_pscm_lab/stress_model_action.py": "2d5c72cc4b8ae214f2f5a19a050fe138f5c2ab0294d92d24e2481af5f8185613",
+    "tools/ford_pscm_lab/test_model_action_replay.py": "ebf6bcd9260745100311521f8e11e85b7aebdd5561ab0876bfc2e802429d6896",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py": "f826c6328f0abac2a61f1a0a6f8d119fdbab858e363cb466d84ba9a7783059cd",
+    "docs/ford_model_action_drive_test.md": "d825b177cd099efd797fe89b7041695d6d41e4b9e8bba6bcaeb32aece164262b"
+  },
+  "deployment_target": {
+    "repository": "sunnypilot/sunnypilot",
+    "branch": "hiimisaac-dev",
+    "validated_code_commit": "ea1ed70c718d32539ef6b9a89b89c0e297c92e06"
+  }
+}

docs/ford_model_action_full_prediction.md

@@ -0,0 +1,86 @@
+# Experimental Ford full path prediction, v4
+
+This document archives v4. The [current v5 controller](ford_model_action_no_yaw_damping.md)
+retains this prediction and removes yaw damping.
+
+V4 removes the extra 15 cm / 25% limit on the geometric prediction introduced
+in [v3](ford_model_action_prediction.md). Those numbers were hand-chosen tuning
+bounds, not identified Ford response limits. The current user request is to
+remove that restriction; the existing default-off Sunnylink toggle remains.
+
+The controller now uses the full predicted offset from the same model path,
+assuming 150 ms of motion along the selected, upstream-limited curvature. The
+150 ms horizon remains an engineering assumption. Available model geometry
+still limits the prediction distance, with endpoint hold for short paths and
+fallback to the valid base offset if prediction arithmetic is nonfinite.
+
+The existing total command limits (C0 ±5.11 m, C1 ±0.5 rad), independent slew
+rates (4 m/s, 0.5 rad/s), quantization, yaw damping, input/service gates and
+zero C2/C3 remain unchanged. Only two control states persist. No integrator,
+model history, extra toggle or PSCM feedback loop is added.
+
+Removing the adjustment cap also permits the predicted C0 to oppose the
+original offset or become nonzero from a zero original offset. For example,
+a straight path with a nonzero selected turn request can have an opposing
+future-frame offset. Tests cover that behavior, mirrored turn releases,
+return to zero, and unchanged slew; sign preservation of the original C0 is
+no longer claimed. The yaw damper still cannot reverse its input target.
+
+## Evidence and interpretation
+
+The PSCM reports a generic `LimitReached` state. It does not tell us whether
+an incoming target is geometrically correct or well timed. Its internal
+limits cannot establish the tracking performance of this predictor. Removal
+is an experiment supported by command comparisons, not by an assumption that
+the PSCM will correct an excessive or mistimed request.
+
+Compared with capped v3 on identical recorded inputs:
+
+| Interval | Effect of removing the extra cap |
+| --- | --- |
+| Latest tight-left entry, 173–175.4 s | Mean C0 magnitude +0.032 m, maximum change 0.07 m |
+| Earlier right entry, 637–640 s | Mean magnitude +0.028 m, maximum change 0.10 m |
+| Earlier right exit, 642.7–643.852 s | All 115 commands unchanged |
+| Driver-clean low requests above 8 m/s, routes9b/9e | Mean absolute change 0.0018 / 0.0014 m; maximum 0.02 m |
+| All eligible samples on either recent route | Maximum absolute command change 0.13 m |
+
+C1 and command eligibility are exactly identical to v3 on both recent routes.
+Some command signs change near zero: this is an intended consequence of using
+the full transform, not proof those corrections improve driving. Entry windows
+include driver input, reported in the validation record. The magnitude changes
+above describe controller C0, not measured lateral vehicle displacement.
+
+The earlier v1 archive is also reproduced exactly on routes90/95, separately
+from the current controller pass. All replay uses original timestamps;
+publication times proxy computation, exact consumed model frames and causal
+carState are retained, and complete SubMaster health is unavailable. The
+recorded model and vehicle motion remain fixed. There is no measured physical
+improvement, stability result or new desired-versus-actual steering trajectory.
+
+Final validation passes 374 tests and 26 subtests with 100% controller statement
+and branch coverage, 299,604 original route cycles and 817,346 Float32/CAN
+round trips. The controller is 190 total lines / 122 code lines excluding
+blanks, comments and docstrings; two control states persist.
+
+See `ford_model_action_full_prediction_validation.json` for final test counts,
+coverage, dependency pins, source hashes and route/packing results. The initial
+uncapped variant was evaluated in a separate lab file before editing production;
+final route checks execute the production v4 source.
+
+## Reproduce and select
+
+Use the dependencies and combined suite command in the
+[drive-test guide](ford_model_action_drive_test.md). For the recent routes:
+
+```sh
+python -m tools.ford_pscm_lab.damping_replay /path/to/route9e/rlogs --baseline v3 --candidate current --window left_entry 173 175.4 --window left_peak 175.4 178.3 --window left_exit 178.3 180.5 --window reversal 728 734 --output /path/to/separate/route9e-results
+python -m tools.ford_pscm_lab.damping_replay /path/to/route9b/rlogs --baseline v3 --candidate current --window right_entry 637 640 --window right_exit 642.7 643.852 --output /path/to/separate/route9b-results
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output /path/to/stress.json
+```
+
+The same **Selected-Action Path Tracking (Experimental)** toggle selects v4
+on the CAN FD F-150 Lightning. An installation with the toggle already enabled
+selects v4 after updating and restarting controlsd. Diagnostics identify
+`model-action-c0-c1-prediction-v4`; `calibration_approved=false` remains explicit.
+Deployment branch: `sunnypilot/sunnypilot`, `hiimisaac-dev`. This work does not
+install software on the device or change its settings.

docs/ford_model_action_full_prediction_validation.json

@@ -0,0 +1,799 @@
+{
+  "date": "2026-09-07",
+  "baseline_commit": "01f8d51c82b3e863f1012d383b5994813ef01b81",
+  "hypothesis": "model-action-c0-c1-prediction-v4",
+  "scope": "Removal of only the extra prediction adjustment cap; no physical tracking or stability claim.",
+  "deployment_target": {
+    "repository": "sunnypilot/sunnypilot",
+    "branch": "hiimisaac-dev"
+  },
+  "calibration_approved": false,
+  "hardware_build_and_device_boot": "not performed",
+  "controller_size": {
+    "total_lines": 190,
+    "code_lines_excluding_blanks_comments_docstrings": 122,
+    "core_persistent_values": 2,
+    "adapter_timestamps": 3
+  },
+  "checks": {
+    "combined_ford_params_sunnylink_suite": "374 passed, 26 subtests passed; no skips",
+    "coverage": {
+      "covered_lines": 113,
+      "num_statements": 113,
+      "percent_covered": 100.0,
+      "percent_covered_display": "100",
+      "missing_lines": 0,
+      "excluded_lines": 0,
+      "percent_statements_covered": 100.0,
+      "percent_statements_covered_display": "100",
+      "num_branches": 32,
+      "num_partial_branches": 0,
+      "covered_branches": 32,
+      "missing_branches": 0,
+      "percent_branches_covered": 100.0,
+      "percent_branches_covered_display": "100"
+    },
+    "ruff": "pass",
+    "ty_controller_and_lab": "pass",
+    "settings_compiler_check": "pass",
+    "cap_removal_red_probe": "11 tests fail with the v3 cap present; all 40 prediction tests pass after removal.",
+    "standards_review_remaining_findings": 0,
+    "spec_review_remaining_findings": 0,
+    "independent_review_verification": "Each reviewer passed 172 focused tests and verified source/artifact hashes and route/packing totals."
+  },
+  "stress": {
+    "seed": 20260907,
+    "random_cycles": 200000,
+    "mirrored_core_updates": 200000,
+    "invalid_or_inactive_resets": 3537,
+    "field_boundary_cases": 18138,
+    "float32_can_round_trips": 218138,
+    "analytic_targets_scalar_slew_and_mirror_checks_pass": true,
+    "bounded_excess_yaw_damping_checked": true,
+    "full_geometric_prediction_checked": true,
+    "direct_raw_float32_packing_matches_host_output": true,
+    "max_continuous_step_c0_c1": [
+      0.400000000000329,
+      0.05000000000000002
+    ],
+    "calibration_approved": false,
+    "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.",
+    "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+    "source_sha256": {
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/stress_model_action.py": "66adef6cba120a1a3d8e9730ea987ef49f8fe283023362dbb99608dee0e3229f",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "7cddac7ce9e88cc32bc7afbec7f9df79b66bb4dfa2fa5f9b36654891fae23a19"
+    }
+  },
+  "routes": {
+    "route90": {
+      "scope": "Command construction and adapter reconstruction only; no counterfactual closed-loop score.",
+      "calibration_approved": false,
+      "executes_live_selector": false,
+      "cycles": 78812,
+      "core_active_cycles": 73055,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 73055,
+      "adapter_status_counts": {
+        "inactive": 5757,
+        "active": 73055
+      },
+      "adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
+      "core_active_path_shorter_than_7m_cycles": 0,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 42170,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.33999999999999986,
+        0.0020000000000000018
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 157624,
+      "timing": "Original controls publication timestamps proxy computation time; repeated frames and gaps retained. No identified delay.",
+      "eligibility": "Adapter checks recorded services independently; full SubMaster health is unavailable. Core uses archived validity.",
+      "reference": "Recorded controlsState.desiredCurvature, already selected/limited. These two routes have no maneuver publications.",
+      "host_yaw": "Extract cs.yaw equals -carState.yawRate; current adapter uses it for bounded damping.",
+      "archived_core_revision": "5fc16abc7662020706e29f57d31a6d5e2bc1293a",
+      "archived_core_source_sha256": "8f3bc5d68e0051776f614a2ccffae84a88f7898dc95bdc12c23dcfe10dfe676a",
+      "cohorts": {
+        "small_request": {
+          "seconds": 475.24161910000026,
+          "core_c0_c1_rms": [
+            0.06125184365338734,
+            0.0057692154738982855
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.057970374224682646,
+            0.007060795713496332
+          ],
+          "adapter_eligible_seconds": 475.24161910000026,
+          "adapter_c0_c1_rms": [
+            0.0666350702799088,
+            0.0057692154738982855
+          ]
+        },
+        "turn": {
+          "seconds": 47.85871389900012,
+          "core_c0_c1_rms": [
+            0.18725251016049418,
+            0.046449510236699354
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.4773534845264775,
+            0.04315935101392594
+          ],
+          "adapter_eligible_seconds": 47.85871389900012,
+          "adapter_c0_c1_rms": [
+            0.21255148222786066,
+            0.046449510236699354
+          ]
+        },
+        "small_speed_2_8": {
+          "seconds": 38.696717235999785,
+          "core_c0_c1_rms": [
+            0.209264275431127,
+            0.014019080484620104
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.1593558816527209,
+            0.01823849401244768
+          ],
+          "adapter_eligible_seconds": 38.696717235999785,
+          "adapter_c0_c1_rms": [
+            0.22422047076469706,
+            0.014019080484620104
+          ]
+        },
+        "turn_speed_2_8": {
+          "seconds": 0.30159887100000304,
+          "core_c0_c1_rms": [
+            2.090789090165622,
+            0.21878942570759013
+          ],
+          "recorded_v8_c0_c1_rms": [
+            2.155728831937131,
+            0.38375302254248855
+          ],
+          "adapter_eligible_seconds": 0.30159887100000304,
+          "adapter_c0_c1_rms": [
+            2.2977717861671105,
+            0.21878942570759013
+          ]
+        },
+        "small_speed_8_15": {
+          "seconds": 79.2599167590001,
+          "core_c0_c1_rms": [
+            0.02250905681378238,
+            0.005843904113223639
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03477379324171293,
+            0.006545440153133947
+          ],
+          "adapter_eligible_seconds": 79.2599167590001,
+          "adapter_c0_c1_rms": [
+            0.027461818062006878,
+            0.005843904113223639
+          ]
+        },
+        "turn_speed_8_15": {
+          "seconds": 0.022264622000001566,
+          "core_c0_c1_rms": [
+            0.20999999999999996,
+            0.04349999999999998
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.25,
+            0.04050000011920929
+          ],
+          "adapter_eligible_seconds": 0.022264622000001566,
+          "adapter_c0_c1_rms": [
+            0.20999999999999996,
+            0.04349999999999998
+          ]
+        },
+        "small_speed_15_55": {
+          "seconds": 357.2849851050004,
+          "core_c0_c1_rms": [
+            0.011622097035118838,
+            0.003925571829874466
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03809717934501699,
+            0.0045587590980714285
+          ],
+          "adapter_eligible_seconds": 357.2849851050004,
+          "adapter_c0_c1_rms": [
+            0.017138026451004092,
+            0.003925571829874466
+          ]
+        },
+        "turn_speed_15_55": {
+          "seconds": 47.53485040600012,
+          "core_c0_c1_rms": [
+            0.08686835454709245,
+            0.043216347944464766
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.4471065308273131,
+            0.030663943784303503
+          ],
+          "adapter_eligible_seconds": 47.53485040600012,
+          "adapter_c0_c1_rms": [
+            0.10939067791228956,
+            0.043216347944464766
+          ]
+        },
+        "pose_quiet": {
+          "seconds": 427.99642020700026,
+          "core_c0_c1_rms": [
+            0.01261133542366393,
+            0.00422389167351435
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03724956081568198,
+            0.0046880581807907966
+          ],
+          "adapter_eligible_seconds": 427.99642020700026,
+          "adapter_c0_c1_rms": [
+            0.017590935519277658,
+            0.00422389167351435
+          ]
+        }
+      },
+      "workspace_head": "01f8d51c82b3e863f1012d383b5994813ef01b81",
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+      "opendbc_import_path": "/Users/ibpersonal/.codex/worktrees/b926/sunnypilot/opendbc_repo",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "7cddac7ce9e88cc32bc7afbec7f9df79b66bb4dfa2fa5f9b36654891fae23a19",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_path.py": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/route.npz": "51e8c26eedde253e171af47d704c1967ba45ae6825d883393bec1fb9e00251c1",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/metadata.json": "742afec55ed629155b22d387f376f878f7b2765221fe94f515a829052bdd916f",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/encoder_comparison.npz": "7a625d3ed5cbd8013d1028aa3bc421740551dcae5c3d60981208bd047af9794c",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/encoder_comparison.json": "64e1cc4be84394ac7ec408d383b99b48ada9b3fd129cdef2fd846e5fac620d66",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/pose_candidate/pose_replay.npz": "4457ccc0868354749da5b72c1dea0faf750f783dfdc87038101288fdcba1e707"
+      }
+    },
+    "route95": {
+      "scope": "Command construction and adapter reconstruction only; no counterfactual closed-loop score.",
+      "calibration_approved": false,
+      "executes_live_selector": false,
+      "cycles": 54738,
+      "core_active_cycles": 37614,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 37614,
+      "adapter_status_counts": {
+        "inactive": 17124,
+        "active": 37614
+      },
+      "adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
+      "core_active_path_shorter_than_7m_cycles": 44,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 25912,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.2999999999999998,
+        0.0010000000000000009
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 109476,
+      "timing": "Original controls publication timestamps proxy computation time; repeated frames and gaps retained. No identified delay.",
+      "eligibility": "Adapter checks recorded services independently; full SubMaster health is unavailable. Core uses archived validity.",
+      "reference": "Recorded controlsState.desiredCurvature, already selected/limited. These two routes have no maneuver publications.",
+      "host_yaw": "Extract cs.yaw equals -carState.yawRate; current adapter uses it for bounded damping.",
+      "archived_core_revision": "5fc16abc7662020706e29f57d31a6d5e2bc1293a",
+      "archived_core_source_sha256": "8f3bc5d68e0051776f614a2ccffae84a88f7898dc95bdc12c23dcfe10dfe676a",
+      "cohorts": {
+        "small_request": {
+          "seconds": 160.93427628100014,
+          "core_c0_c1_rms": [
+            0.027283284164810895,
+            0.006054573235641941
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.04189890174428264,
+            0.0075342351227490395
+          ],
+          "adapter_eligible_seconds": 160.93427628100014,
+          "adapter_c0_c1_rms": [
+            0.03232621215993653,
+            0.006054573235641941
+          ]
+        },
+        "turn": {
+          "seconds": 58.39837079799993,
+          "core_c0_c1_rms": [
+            0.5425346915921071,
+            0.09102985199238507
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.63700479987207,
+            0.09691236120507696
+          ],
+          "adapter_eligible_seconds": 58.39837079799993,
+          "adapter_c0_c1_rms": [
+            0.5674989495695087,
+            0.09102985199238507
+          ]
+        },
+        "small_speed_2_8": {
+          "seconds": 8.083969085000064,
+          "core_c0_c1_rms": [
+            0.09941192804175551,
+            0.016572084086884566
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.07597383189748476,
+            0.016403469943482765
+          ],
+          "adapter_eligible_seconds": 8.083969085000064,
+          "adapter_c0_c1_rms": [
+            0.11170387210996009,
+            0.016572084086884566
+          ]
+        },
+        "turn_speed_2_8": {
+          "seconds": 2.9056519089999995,
+          "core_c0_c1_rms": [
+            2.286598281178436,
+            0.3420570411209625
+          ],
+          "recorded_v8_c0_c1_rms": [
+            2.4371443775668484,
+            0.37360610108051345
+          ],
+          "adapter_eligible_seconds": 2.9056519089999995,
+          "adapter_c0_c1_rms": [
+            2.3669481787447944,
+            0.3420570411209625
+          ]
+        },
+        "small_speed_8_15": {
+          "seconds": 32.021085868,
+          "core_c0_c1_rms": [
+            0.028387860150467858,
+            0.0067602044219821
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.036941169841952046,
+            0.007349671742584858
+          ],
+          "adapter_eligible_seconds": 32.021085868,
+          "adapter_c0_c1_rms": [
+            0.033317964465740625,
+            0.0067602044219821
+          ]
+        },
+        "turn_speed_8_15": {
+          "seconds": 22.160645306000006,
+          "core_c0_c1_rms": [
+            0.28668316323581683,
+            0.0671915746138579
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.3391022553367454,
+            0.07297678510653233
+          ],
+          "adapter_eligible_seconds": 22.160645306000006,
+          "adapter_c0_c1_rms": [
+            0.32040564199313326,
+            0.0671915746138579
+          ]
+        },
+        "small_speed_15_55": {
+          "seconds": 120.82922132800007,
+          "core_c0_c1_rms": [
+            0.010802222799973712,
+            0.004282512931613862
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03987958989535526,
+            0.0065793876512313535
+          ],
+          "adapter_eligible_seconds": 120.82922132800007,
+          "adapter_c0_c1_rms": [
+            0.016212091011218406,
+            0.004282512931613862
+          ]
+        },
+        "turn_speed_15_55": {
+          "seconds": 33.332073582999925,
+          "core_c0_c1_rms": [
+            0.07257996778378971,
+            0.03628926246224237
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.3416081588451539,
+            0.027322786376822172
+          ],
+          "adapter_eligible_seconds": 33.332073582999925,
+          "adapter_c0_c1_rms": [
+            0.08724830487921002,
+            0.03628926246224237
+          ]
+        },
+        "pose_quiet": {
+          "seconds": 145.24136911700015,
+          "core_c0_c1_rms": [
+            0.012566204405760934,
+            0.004614194153565963
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03855743841849719,
+            0.006621274926345545
+          ],
+          "adapter_eligible_seconds": 145.24136911700015,
+          "adapter_c0_c1_rms": [
+            0.01710948709543248,
+            0.004614194153565963
+          ]
+        }
+      },
+      "workspace_head": "01f8d51c82b3e863f1012d383b5994813ef01b81",
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+      "opendbc_import_path": "/Users/ibpersonal/.codex/worktrees/b926/sunnypilot/opendbc_repo",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "7cddac7ce9e88cc32bc7afbec7f9df79b66bb4dfa2fa5f9b36654891fae23a19",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_path.py": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/route.npz": "6e5438867ea618e53ca395b60ff4b9b146256f1bf6f07dc8e92d663286074154",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/metadata.json": "f7c95a102e0e084bbcc9938399219080c1d3b26274b4ea3c3a9514ff7dc8ea8d",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/encoder_comparison.npz": "23bd05c4b6400299844acaba1d97051c96c23c682bf17b46e89e7f2cca5fce38",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/encoder_comparison.json": "1ab24a796d14817b8672ea6dfd39711d65cb06744eaaef7dcc02a840c392c4d3",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/pose_candidate/pose_replay.npz": "9cfbb3c6f0b1fdb7e3d38e6b64b8c94cffbcc9fabe41f6344d84a9b657b6af9b"
+      }
+    },
+    "route9b": {
+      "scope": "Fixed-input command replay only; no physical improvement or stability claim.",
+      "calibration_approved": false,
+      "cycles": 82386,
+      "eligible_cycles": 70703,
+      "status_counts": {
+        "inactive": 11683,
+        "active": 70703
+      },
+      "c1_exactly_unchanged": true,
+      "same_validity": true,
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 164772,
+      "cohorts": {
+        "eligible": {
+          "cycles": 70703,
+          "seconds": 711.571703289,
+          "changed_c0_cycles": 10475,
+          "mean_absolute_c0_change_m": 0.00219777369954635,
+          "max_absolute_c0_change_m": 0.13000000000000078,
+          "increased_absolute_c0_cycles": 9822,
+          "decreased_absolute_c0_cycles": 633,
+          "driver_input_percent": 3.0297623863274845,
+          "baseline_peak_absolute_c0_m": 3.96,
+          "candidate_peak_absolute_c0_m": 3.99
+        },
+        "driver_clean": {
+          "cycles": 63267,
+          "seconds": 636.7469998759999,
+          "changed_c0_cycles": 8901,
+          "mean_absolute_c0_change_m": 0.0014713899370902674,
+          "max_absolute_c0_change_m": 0.05999999999999961,
+          "increased_absolute_c0_cycles": 8314,
+          "decreased_absolute_c0_cycles": 568,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.24,
+          "candidate_peak_absolute_c0_m": 2.24
+        },
+        "driver_clean_low_request_above_8mps": {
+          "cycles": 40656,
+          "seconds": 409.13067679800054,
+          "changed_c0_cycles": 7132,
+          "mean_absolute_c0_change_m": 0.0017587822497730527,
+          "max_absolute_c0_change_m": 0.020000000000000462,
+          "increased_absolute_c0_cycles": 6888,
+          "decreased_absolute_c0_cycles": 244,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.11000000000000032,
+          "candidate_peak_absolute_c0_m": 0.11000000000000032
+        },
+        "turn": {
+          "cycles": 5459,
+          "seconds": 54.939139939000256,
+          "changed_c0_cycles": 119,
+          "mean_absolute_c0_change_m": 0.00021719119817404867,
+          "max_absolute_c0_change_m": 0.010000000000000675,
+          "increased_absolute_c0_cycles": 119,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.24,
+          "candidate_peak_absolute_c0_m": 2.24
+        },
+        "right_entry": {
+          "cycles": 298,
+          "seconds": 2.9932484459999387,
+          "changed_c0_cycles": 216,
+          "mean_absolute_c0_change_m": 0.028050765542768245,
+          "max_absolute_c0_change_m": 0.10000000000000009,
+          "increased_absolute_c0_cycles": 216,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 20.395198628296086,
+          "baseline_peak_absolute_c0_m": 2.95,
+          "candidate_peak_absolute_c0_m": 3.0300000000000002
+        },
+        "right_exit": {
+          "cycles": 115,
+          "seconds": 1.156770048999988,
+          "changed_c0_cycles": 0,
+          "mean_absolute_c0_change_m": 0.0,
+          "max_absolute_c0_change_m": 0.0,
+          "increased_absolute_c0_cycles": 0,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.7199999999999998,
+          "candidate_peak_absolute_c0_m": 0.7199999999999998
+        }
+      },
+      "baseline_commands_vs_recorded_publications": {
+        "paired_cycles": 70703,
+        "within_one_quantum_cycles": 65890,
+        "maximum_absolute_error_c0_c1": [
+          0.25000004768371564,
+          0.0005000143051147043
+        ]
+      },
+      "timing": "Publication-time proxy, causal carState, exact consumed model; full SubMaster health unavailable.",
+      "baseline_version": "v3",
+      "baseline_revision": "01f8d51c82b3e863f1012d383b5994813ef01b81",
+      "candidate_version": "current",
+      "candidate_revision": "working_tree",
+      "focus_windows": [
+        [
+          "right_entry",
+          "637",
+          "640"
+        ],
+        [
+          "right_exit",
+          "642.7",
+          "643.852"
+        ]
+      ],
+      "baseline_source_sha256": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+      "candidate_source_sha256": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+      "source_rlog_sha256": {
+        "84865544361f55cb_0000009b--e4616dddaa--0--rlog.zst": "22746f7119109b73ed7f2c26ce8c99f87136e9124fb7fc14c9554409a28a7c3f",
+        "84865544361f55cb_0000009b--e4616dddaa--1--rlog.zst": "4c2e1d7083c31a2b37d0f8dd3be4d330898511b7e02c26f7d40ca9bc2779397d",
+        "84865544361f55cb_0000009b--e4616dddaa--2--rlog.zst": "62f3e049e220cd3681fadf386f2969537bd571998ae2f6ba2d08479428b5a28f",
+        "84865544361f55cb_0000009b--e4616dddaa--3--rlog.zst": "83bf0131b2d36b2ba7e5ba050bbc13c0a3350feb5c9b89dc9c87d3a37abebfb3",
+        "84865544361f55cb_0000009b--e4616dddaa--4--rlog.zst": "430985a80dd6e10f7abeb89457a17022e6bb6978617f415c905f584b1647603e",
+        "84865544361f55cb_0000009b--e4616dddaa--5--rlog.zst": "8c0c5ae6323ec33b3e14f84ca834f70cb56f6b29f471a350f1e3efc06b6ba553",
+        "84865544361f55cb_0000009b--e4616dddaa--6--rlog.zst": "db53dfa8156b9d66792c3eff0b2ce5d31b71ad41cc580dec85f528845593c184",
+        "84865544361f55cb_0000009b--e4616dddaa--7--rlog.zst": "91b0b3be10cb7d7d7f7dd2024d8f9ee99d1e9fd2204203a3a9a2f2f1c6e3fa03",
+        "84865544361f55cb_0000009b--e4616dddaa--8--rlog.zst": "687dbbfc49837efbfe8fa6bc091e40f7fad2908832234d7884f4616d1bc9ccff",
+        "84865544361f55cb_0000009b--e4616dddaa--9--rlog.zst": "a88ec4d25b04cdbf5844686fc77f6b28dca920c9b164e37ebf69844a3ae398fc",
+        "84865544361f55cb_0000009b--e4616dddaa--10--rlog.zst": "fe6b29580a6c94e1c236d13e18db4cd9f31cc1b25d52e1e6e19a5021125c9932",
+        "84865544361f55cb_0000009b--e4616dddaa--11--rlog.zst": "150d31b1944d7a1b8c562f3aee20b66cefa6c4e8d02660ec889907d835142f45",
+        "84865544361f55cb_0000009b--e4616dddaa--12--rlog.zst": "e105e5f703a70f36f1989c66fb46c35a65ff60a265b4332e10fa2e6875a2ced4",
+        "84865544361f55cb_0000009b--e4616dddaa--13--rlog.zst": "69304cf0c81401374f047c5022ca47d257ff9e59e30ec171325a2ee1e7ed13d4"
+      },
+      "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/damping_replay.py": "8c4f1f987b65e2be146c473a6e529168f08b1f7ce3c9514e22a9874450fa430e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21"
+      }
+    },
+    "route9e": {
+      "scope": "Fixed-input command replay only; no physical improvement or stability claim.",
+      "calibration_approved": false,
+      "cycles": 83668,
+      "eligible_cycles": 74669,
+      "status_counts": {
+        "inactive": 8999,
+        "active": 74669
+      },
+      "c1_exactly_unchanged": true,
+      "same_validity": true,
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 167336,
+      "cohorts": {
+        "eligible": {
+          "cycles": 74669,
+          "seconds": 751.5479497329998,
+          "changed_c0_cycles": 8108,
+          "mean_absolute_c0_change_m": 0.00147612892002983,
+          "max_absolute_c0_change_m": 0.13000000000000034,
+          "increased_absolute_c0_cycles": 7665,
+          "decreased_absolute_c0_cycles": 423,
+          "driver_input_percent": 2.7324748055386094,
+          "baseline_peak_absolute_c0_m": 4.38,
+          "candidate_peak_absolute_c0_m": 4.38
+        },
+        "driver_clean": {
+          "cycles": 66321,
+          "seconds": 667.5291971840106,
+          "changed_c0_cycles": 7257,
+          "mean_absolute_c0_change_m": 0.0012096637080993352,
+          "max_absolute_c0_change_m": 0.08000000000000007,
+          "increased_absolute_c0_cycles": 6883,
+          "decreased_absolute_c0_cycles": 374,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.4699999999999998,
+          "candidate_peak_absolute_c0_m": 2.4699999999999998
+        },
+        "driver_clean_low_request_above_8mps": {
+          "cycles": 39635,
+          "seconds": 398.9613925449994,
+          "changed_c0_cycles": 5427,
+          "mean_absolute_c0_change_m": 0.0013748276784907216,
+          "max_absolute_c0_change_m": 0.020000000000000462,
+          "increased_absolute_c0_cycles": 5341,
+          "decreased_absolute_c0_cycles": 86,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.07000000000000028,
+          "candidate_peak_absolute_c0_m": 0.07000000000000028
+        },
+        "turn": {
+          "cycles": 6219,
+          "seconds": 62.59491144700405,
+          "changed_c0_cycles": 244,
+          "mean_absolute_c0_change_m": 0.000391833417652352,
+          "max_absolute_c0_change_m": 0.010000000000000675,
+          "increased_absolute_c0_cycles": 244,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.4699999999999998,
+          "candidate_peak_absolute_c0_m": 2.4699999999999998
+        },
+        "left_entry": {
+          "cycles": 238,
+          "seconds": 2.4048367620016506,
+          "changed_c0_cycles": 204,
+          "mean_absolute_c0_change_m": 0.03167584138917884,
+          "max_absolute_c0_change_m": 0.07000000000000028,
+          "increased_absolute_c0_cycles": 204,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 2.403816920672437,
+          "baseline_peak_absolute_c0_m": 2.6100000000000003,
+          "candidate_peak_absolute_c0_m": 2.6399999999999997
+        },
+        "left_peak": {
+          "cycles": 288,
+          "seconds": 2.891819394000777,
+          "changed_c0_cycles": 6,
+          "mean_absolute_c0_change_m": 0.00022373297976799967,
+          "max_absolute_c0_change_m": 0.019999999999999574,
+          "increased_absolute_c0_cycles": 6,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 6.41034524444729,
+          "baseline_peak_absolute_c0_m": 2.9400000000000004,
+          "candidate_peak_absolute_c0_m": 2.95
+        },
+        "left_exit": {
+          "cycles": 219,
+          "seconds": 2.203320298998733,
+          "changed_c0_cycles": 11,
+          "mean_absolute_c0_change_m": 0.0008062973780191794,
+          "max_absolute_c0_change_m": 0.019999999999999574,
+          "increased_absolute_c0_cycles": 11,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 9.936619705158035,
+          "baseline_peak_absolute_c0_m": 2.51,
+          "candidate_peak_absolute_c0_m": 2.51
+        },
+        "reversal": {
+          "cycles": 554,
+          "seconds": 5.570295188001182,
+          "changed_c0_cycles": 54,
+          "mean_absolute_c0_change_m": 0.002577813237060219,
+          "max_absolute_c0_change_m": 0.04999999999999982,
+          "increased_absolute_c0_cycles": 29,
+          "decreased_absolute_c0_cycles": 25,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.21999999999999975,
+          "candidate_peak_absolute_c0_m": 0.21999999999999975
+        }
+      },
+      "baseline_commands_vs_recorded_publications": {
+        "paired_cycles": 74669,
+        "within_one_quantum_cycles": 69690,
+        "maximum_absolute_error_c0_c1": [
+          0.369999988079071,
+          0.0010000016689301061
+        ]
+      },
+      "timing": "Publication-time proxy, causal carState, exact consumed model; full SubMaster health unavailable.",
+      "baseline_version": "v3",
+      "baseline_revision": "01f8d51c82b3e863f1012d383b5994813ef01b81",
+      "candidate_version": "current",
+      "candidate_revision": "working_tree",
+      "focus_windows": [
+        [
+          "left_entry",
+          "173",
+          "175.4"
+        ],
+        [
+          "left_peak",
+          "175.4",
+          "178.3"
+        ],
+        [
+          "left_exit",
+          "178.3",
+          "180.5"
+        ],
+        [
+          "reversal",
+          "728",
+          "734"
+        ]
+      ],
+      "baseline_source_sha256": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+      "candidate_source_sha256": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+      "source_rlog_sha256": {
+        "84865544361f55cb_0000009e--592f7dc149--0--rlog.zst": "de63532ae6aedf5dc7fd3ac8e47a2d96a8f065614f4aca79ae2f120ea00390c7",
+        "84865544361f55cb_0000009e--592f7dc149--1--rlog.zst": "67b58197158b3e8f0581643f6657d2a85c47b0bb75dd3e00306d9ffae790008b",
+        "84865544361f55cb_0000009e--592f7dc149--2--rlog.zst": "a4840f338f51b5f1864c79ce3a4f2b11dc13d58d52b8459961c9da57237f2cc2",
+        "84865544361f55cb_0000009e--592f7dc149--3--rlog.zst": "ff3ea8d948006ab19c4dbfeeff59a509de3f193da47b91c88faa27ecd0b3f1fc",
+        "84865544361f55cb_0000009e--592f7dc149--4--rlog.zst": "5ff1996c2336299128a11e657c32bc21921716b7e3bd401de69a2d49484aa223",
+        "84865544361f55cb_0000009e--592f7dc149--5--rlog.zst": "238763457e89896933afaf9a5df325469ee8c02ddc53550df252531bc94fc540",
+        "84865544361f55cb_0000009e--592f7dc149--6--rlog.zst": "2ee86de80cfd762be10cd2dfb2895ddbee6b813706e9c7261460203e09b9bc4d",
+        "84865544361f55cb_0000009e--592f7dc149--7--rlog.zst": "017801f080861c63d799f87aebe30ab57cdf82f078ac882e6187d3870c403538",
+        "84865544361f55cb_0000009e--592f7dc149--8--rlog.zst": "3729016bd1f00bb1077613b63fe25b21ba7112822b994e0e0aa2b4cd93bdb940",
+        "84865544361f55cb_0000009e--592f7dc149--9--rlog.zst": "c40b3c1f6eb9252b85f176fee32cae16c23eaad3db830f6bbf37a730034a5ccf",
+        "84865544361f55cb_0000009e--592f7dc149--10--rlog.zst": "e0ce8f231798073e5bbc34551fbd9493f169a06a068adc1957ef8cd914b71fbb",
+        "84865544361f55cb_0000009e--592f7dc149--11--rlog.zst": "90863353f7c862a05618b4d0761dda3fcedd3cf6b4234c21c6dcc2eee3ec20b2",
+        "84865544361f55cb_0000009e--592f7dc149--12--rlog.zst": "e064f8abde9615b2daf00f469ef36b438cd4b3a1637069467c84de91bee3bdf1",
+        "84865544361f55cb_0000009e--592f7dc149--13--rlog.zst": "790fc438ce2675e0690fdce3b07bf54fdf6d06b17bbe7f5ff447adfa71189754",
+        "84865544361f55cb_0000009e--592f7dc149--14--rlog.zst": "b98f5a5be660013b1fe11250fdc24ee4fe1d9dd1301b787ddfdff1f2bcd3e27f"
+      },
+      "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/damping_replay.py": "8c4f1f987b65e2be146c473a6e529168f08b1f7ce3c9514e22a9874450fa430e",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21"
+      }
+    }
+  },
+  "ablation_comparison": {
+    "route9b": {
+      "lab_variant_commands_exactly_match_production": true,
+      "opposed_nonzero_c0_commands": 24,
+      "max_absolute_difference_on_opposed_commands_m": 0.05999999999999961
+    },
+    "route9e": {
+      "lab_variant_commands_exactly_match_production": true,
+      "opposed_nonzero_c0_commands": 30,
+      "max_absolute_difference_on_opposed_commands_m": 0.02999999999999936
+    }
+  },
+  "total_original_route_cycles": 299604,
+  "total_float32_can_round_trips": 817346,
+  "source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "30cfffb86fcd3830320e9b2ec1f65cdfd219684a55d0bc0bb13f77966cc18b53",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "8ab5dd52958f13a2426e07f8713b7d8d56c52b8a05d173ffba87ffccc0087796",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_damping.py": "a7062d2c3c46b3abcd4c3facd97970644ce609fb55285daa0359681950948250",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_prediction.py": "eb50e1d6acdb5e7332fadc3dcfcfa1b3809545e607c47c54b0ba3de80b2df208",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py": "f826c6328f0abac2a61f1a0a6f8d119fdbab858e363cb466d84ba9a7783059cd",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "175a1969ac83b14b88798bbde37371f740cd05cf3f8f6e5d21bbf9afe857bb57",
+    "tools/ford_pscm_lab/__init__.py": "db4b8b7d2e317ed34ca0ec220bf9d53e7b80a23e766f4e1cb2c8224dada45f2e",
+    "tools/ford_pscm_lab/damping_replay.py": "8c4f1f987b65e2be146c473a6e529168f08b1f7ce3c9514e22a9874450fa430e",
+    "tools/ford_pscm_lab/model_action_replay.py": "7cddac7ce9e88cc32bc7afbec7f9df79b66bb4dfa2fa5f9b36654891fae23a19",
+    "tools/ford_pscm_lab/stress_model_action.py": "66adef6cba120a1a3d8e9730ea987ef49f8fe283023362dbb99608dee0e3229f",
+    "tools/ford_pscm_lab/test_model_action_replay.py": "1f7d90c06ec09b405571565632653dc7d984b106b955877916d4a0836c11e3b5",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "41ac31726b941e470f5be34e2523dc1fc2b25fb6a12705ab267a57ed4f3c27c8",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "c44c75e02042a0485b5bd7896e0b794761d768bbe513b6dc9c2c986ddd22b44c"
+  },
+  "artifact_sha256": {
+    ".cache/ford_prediction_uncapped/uncapped_red.txt": "09160dbc36320be886561cfdbdd21eef0c8bc420a11bae9bb1a8dd43f7b93a3c",
+    ".cache/ford_prediction_uncapped/suite_run.txt": "99b3bd2af256c9a4a083cb015a51a458a26ac02a8cad73c28c47470f9a5eb910",
+    ".cache/ford_prediction_uncapped/coverage.json": "50a282c90fa86147ed6925fdf37918471f8e5204a4c1e1ef9adb39335e220f42",
+    ".cache/ford_prediction_uncapped/stress.json": "e4226f403a3a914b9f3a7868b14a2b763215ec1f77e7d45ac06604859acc9fb6",
+    ".cache/ford_prediction_uncapped/route90/report.json": "c5cee3c56f9f7f292c8069be22dd1ffb1649055fccaf829b28d96bdec6558541",
+    ".cache/ford_prediction_uncapped/route95/report.json": "6fcc8a3bd7abb5b513888cb64426670e7e05f8b2100b1876be196a800cec2c90",
+    ".cache/ford_prediction_uncapped/final9b/report.json": "79faca3ff89e2015dad4823d4d988ea26ead55c7e51e4a594258102d32701b07",
+    ".cache/ford_prediction_uncapped/final9e/report.json": "9b003ce97225e65e03e351c5e133f3fed08dc79e026f73308098d51a98dd67ae",
+    ".cache/ford_prediction_uncapped/route90/commands.npz": "8787aabbe7007bcd7c68976c272085761aec6a4f360fa6cc8c8553bd1421c92b",
+    ".cache/ford_prediction_uncapped/route95/commands.npz": "349b716bd910cd030716b9ef519ee24f620fae5280776ffa871bc43318939400",
+    ".cache/ford_prediction_uncapped/final9b/commands.npz": "ff4a14fc74b144800f459c250be961ebb05a46a20e6ad1fc0394a659c9854394",
+    ".cache/ford_prediction_uncapped/final9e/commands.npz": "59f6046acb10b00ef00e7b9feb5b8547139a67a2f8767d7cbfe3603a59e6c77d"
+  }
+}

docs/ford_model_action_measured_pose_validation.json

@@ -0,0 +1,94 @@
+{
+  "version": "model-action-measured-pose-v6",
+  "base_commit": "72e9d94f62487d07e9b7c5cb17d5988d5ddfbedb",
+  "opendbc_revision": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+  "scope": "Offline runtime command replay on frozen recorded calibrated yaw, geometry and vehicle inputs; separate controlsd integration tests exercise health/freshness, calibration updates, publication and CAN. No counterfactual vehicle simulation or road-validation claim.",
+  "calibration_approved": false,
+  "source_sha256": {
+    "openpilot/selfdrive/controls/controlsd.py": "c12e8d9be37e21f27a2132974b0efe98f3e681fd07e12c263e897ad67a44871b",
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "326539389b04034932db41ca2c67794779403b2c0e97f408a81a9f5734114899",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "2a55779b1a32f4dc3317b568365efbadc1f6aab06a0b3398514be891025d5e2d",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "0e131ca17380a5a4fe37754659f77aeda550f5e3c113edb27b576673e57bf6b4",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_measured_pose.py": "4d81efa0f09350559ff4290fd2ec11e744be221623fd414de85adfe2fecb4289",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py": "4e87ff347046d8de7248b9272a0393dd5d12e10df5430f00540b675331d7fe58",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_yaw.py": "063b066aeddf0410dc208fa0017a58cda78effb1ca33c2af8b53422dbdad06f3",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "6f8900aa642d7f3883b003fd9244f29997343808fc22119c5ca953c3e1946fde",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "c8f7cc4c44e39027fecb52efc61dd5f3902e030193b10ac2f2c36c51cc6d4010"
+  },
+  "validation": {
+    "pytest": {
+      "passed": 427,
+      "subtests_passed": 26,
+      "skipped": 0,
+      "command": "python -m pytest -q openpilot/selfdrive/controls/tests/test_ford*.py tools/ford_pscm_lab opendbc_repo/opendbc/car/ford/tests/test_ford.py openpilot/sunnypilot/sunnylink/tests",
+      "output_sha256": "ed2ecdf280dadc52a834b2dfaa777e64505f7763d1a271849dadc729a86abb5d"
+    },
+    "type_check": "ty check openpilot/selfdrive/controls/lib/ford_model_action.py openpilot/selfdrive/controls/controlsd.py: pass",
+    "ruff_changed_python": "pass",
+    "settings_compiler_check": "pass",
+    "diff_check": "pass",
+    "regression_red_green": "49 measured-pose tests and 4 state-freshness/calibration tests failed before their implementation and passed afterward"
+  },
+  "replay": {
+    "cycles": 340757,
+    "wire_round_trips": 681514,
+    "command_matches_reviewed_candidate": true,
+    "baseline_matches_archived_v5": true,
+    "c1_eligibility_unchanged": true,
+    "checks": [
+      "finite values",
+      "caps",
+      "independent slew",
+      "inactive reset",
+      "Float32/CAN packing",
+      "zero C2 and C3",
+      "publication and filter-state freshness"
+    ],
+    "routes": {
+      "a2": {
+        "cycles": 71111,
+        "wire_round_trips": 142222,
+        "runtime_matches_reviewed_lab_candidate": true,
+        "baseline_matches_archive": true,
+        "identical_c1_eligibility": true
+      },
+      "a0": {
+        "cycles": 114582,
+        "wire_round_trips": 229164,
+        "runtime_matches_reviewed_lab_candidate": true,
+        "baseline_matches_archive": true,
+        "identical_c1_eligibility": true
+      },
+      "9b": {
+        "cycles": 71396,
+        "wire_round_trips": 142792,
+        "runtime_matches_reviewed_lab_candidate": true,
+        "baseline_matches_archive": true,
+        "identical_c1_eligibility": true
+      },
+      "9e": {
+        "cycles": 83668,
+        "wire_round_trips": 167336,
+        "runtime_matches_reviewed_lab_candidate": true,
+        "baseline_matches_archive": true,
+        "identical_c1_eligibility": true
+      }
+    },
+    "report_sha256": {
+      "a2_pose_report.json": "35d709991507ac7c10fd3148fd61dcf33a9606f97cd134d2bd94cee999f9cbe1",
+      "a0_pose_report.json": "1e34fbcdb14d04b23ce755ddd2e66fa6ab12377ab925693474e9f719b6339530",
+      "9b_pose_report.json": "e601662de44a354247f3943c233ca14ce5c449dbb4289ab9564ced21d524f02b",
+      "9e_pose_report.json": "fb0ee49e7aab65d45b61251aa83ad2247cd90e91f6c077f86d2f8474af7013e4"
+    },
+    "reproduce": "PYTHONDONTWRITEBYTECODE=1 PYTHONPATH=.:opendbc_repo python .cache/ford_v6/replay.py a2 a0 9b 9e",
+    "reproduction_requirements": "Original route extracts in the local .cache directories and the matching scripts/reports in the ford-v6 artifact. Replay scripts and private recordings are not shipped on the device."
+  },
+  "limitations": [
+    "All route extracts came from one truck; cross-PSCM behavior is unverified.",
+    "C0 command changes are not percentages of physical steering improvement.",
+    "The older overshoot example has nearby driver influence.",
+    "The same C1 cap remains active during tight-turn peak misses.",
+    "Quiet-path per-cycle C0-change RMS increases about 28%; physical centering is unverified.",
+    "Hardware build, device boot and physical driving were not performed."
+  ]
+}

docs/ford_model_action_no_yaw_damping.md

@@ -0,0 +1,67 @@
+# Experimental Ford selected-action controller, v5
+
+V5 removes the excess-yaw C0 attenuation at the user's request. The damping
+function, 0.02 rad/s deadband and 0.2 s reduction scale are deleted. Valid measured
+yaw no longer changes either command target. Existing yaw input-health checks
+and logging remain. No replacement gain, increment gate or controller state is added.
+
+The full 150 ms geometric prediction introduced in v4 remains, along with the
+7 m offset station and one-second heading scale. This is not a return to v1:
+v1 did not predict the offset. C0/C1 bounds, slew, quantization, service gates,
+engagement, downstream driver arbitration and zero C2/C3 are unchanged.
+
+The latest supplied route a0 ran v2, not v4. Prior same-input comparisons found
+identical v1/v2 commands during its driver-clean minor-bend warning intervals
+and the preceding five seconds. That evidence does not identify damping as the
+cause of those misses. Removing damping can restore C0 demand where the damper
+was active, including turn exits; it is not evidence of improved tracking or
+reduced oversteer.
+
+## Offline validation
+
+The regression suite checks yaw-independent commands through mirrored turn
+entry, release and reversal, including valid yaw extremes and small yaw offsets.
+Six cases fail with v4 damping present and pass after removal. Invalid yaw still
+resets the controller. Actual controlsd selection, upstream limiting, Float32
+publication and downstream CAN tests cover both model and maneuver references.
+
+Full-rlog comparisons run pinned v4 against production v5 on routes9b, 9e and a0.
+They preserve original clocks, exact consumed model frames and causal carState;
+publication times proxy computation time, and complete SubMaster health is
+unavailable. The numerical stress run checks independent geometric targets,
+scalar slew, mirrored turns and Float32/CAN packing. Recorded vehicle motion
+stays fixed: none of these checks establishes counterfactual steering response,
+closed-loop stability or a physical tracking improvement.
+
+Results and source hashes are recorded in
+`ford_model_action_no_yaw_damping_validation.json`. Earlier validation documents
+remain archives of their specified controller versions.
+
+Validation passes 356 tests and 26 subtests with 100% controller statement and
+branch coverage, 280,636 recorded route cycles and 779,410 Float32/CAN round trips,
+including 200,000 random stress cycles. C1 and input eligibility match v4 exactly
+on all three routes. Commands during all 1,578 driver-clean ordinary-bend warning
+cycles on route a0 also remain identical to v4. At the earlier right-turn exit,
+removing damping increases C0 magnitude by a mean 0.079 m, maximum 0.13 m; these
+are command offsets, not measured vehicle displacement.
+
+The module is 171 total lines, or 111 code lines excluding blanks, comments and
+docstrings, with two control states. No hardware build or device boot was performed.
+
+## Reproduce and select
+
+Use the dependency setup and combined suite in the
+[drive-test guide](ford_model_action_drive_test.md). Replay and stress commands:
+
+```sh
+python -m tools.ford_pscm_lab.damping_replay /path/to/route9b/rlogs --baseline v4 --candidate current --window right_entry 637 640 --window right_exit 642.7 643.852 --output /path/to/separate/route9b-results
+python -m tools.ford_pscm_lab.damping_replay /path/to/route9e/rlogs --baseline v4 --candidate current --window left_entry 173 175.4 --window left_peak 175.4 178.3 --window left_exit 178.3 180.5 --output /path/to/separate/route9e-results
+python -m tools.ford_pscm_lab.damping_replay /path/to/routea0/rlogs --baseline v4 --candidate current --window bends_5min 298 338 --window bend_7min 449 458 --window bend_9min 579 588 --output /path/to/separate/routea0-results
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260908 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output /path/to/stress.json
+```
+
+The same default-off **Selected-Action Path Tracking (Experimental)** Sunnylink
+toggle selects v5 on the CAN FD F-150 Lightning. Deployment remains
+`sunnypilot/sunnypilot`, branch `hiimisaac-dev`. After updating, restart controlsd
+through a real offroad-to-onroad cycle. Diagnostics identify
+`model-action-c0-c1-prediction-v5`; `calibration_approved=false` remains explicit.

docs/ford_model_action_no_yaw_damping_validation.json

@@ -0,0 +1,304 @@
+{
+  "date": "2026-09-08",
+  "baseline_commit": "7e63449749d112f096c56cb848dd289054e5f85b",
+  "hypothesis": "model-action-c0-c1-prediction-v5",
+  "scope": "Remove yaw damping only; retain full path prediction and existing input-health gates. Fixed-input command checks, not physical tracking or stability evidence.",
+  "deployment_target": {
+    "repository": "sunnypilot/sunnypilot",
+    "branch": "hiimisaac-dev"
+  },
+  "calibration_approved": false,
+  "hardware_build_and_device_boot": "not performed",
+  "opendbc_revision": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+  "controller_size": {
+    "total_lines": 171,
+    "code_lines_excluding_blanks_comments_docstrings": 111,
+    "core_persistent_values": 2,
+    "adapter_timestamps": 3
+  },
+  "checks": {
+    "combined_ford_params_sunnylink_suite": "356 passed, 26 subtests passed; no skips",
+    "coverage": {
+      "covered_lines": 102,
+      "num_statements": 102,
+      "percent_covered": 100.0,
+      "percent_covered_display": "100",
+      "missing_lines": 0,
+      "excluded_lines": 0,
+      "percent_statements_covered": 100.0,
+      "percent_statements_covered_display": "100",
+      "num_branches": 30,
+      "num_partial_branches": 0,
+      "covered_branches": 30,
+      "missing_branches": 0,
+      "percent_branches_covered": 100.0,
+      "percent_branches_covered_display": "100"
+    },
+    "ruff": "pass",
+    "ty_controller_and_lab": "pass",
+    "settings_compiler_check": "pass",
+    "removal_regression_probe": "6 cases fail with v4 damping present; all 21 yaw tests pass after removal.",
+    "reviews": {
+      "standards": {
+        "remaining_findings": 0
+      },
+      "spec": {
+        "remaining_findings": 0,
+        "independent_focused_tests_passed": 107
+      },
+      "corrected_findings": [
+        "Removed stale damping claim from Sunnylink YAML and regenerated JSON.",
+        "Corrected replay yaw-use metadata to input-health checks and diagnostics."
+      ]
+    }
+  },
+  "total_original_route_cycles": 280636,
+  "total_float32_can_round_trips": 779410,
+  "stress": {
+    "seed": 20260908,
+    "random_cycles": 200000,
+    "mirrored_core_updates": 200000,
+    "invalid_or_inactive_resets": 3537,
+    "field_boundary_cases": 18138,
+    "float32_can_round_trips": 218138,
+    "analytic_targets_scalar_slew_and_mirror_checks_pass": true,
+    "valid_yaw_does_not_affect_targets_checked": true,
+    "full_geometric_prediction_checked": true,
+    "direct_raw_float32_packing_matches_host_output": true,
+    "max_continuous_step_c0_c1": [
+      0.4000000000003041,
+      0.05000000000000002
+    ],
+    "calibration_approved": false,
+    "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.",
+    "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9"
+  },
+  "routes": {
+    "route9b": {
+      "cycles": 82386,
+      "eligible_cycles": 70703,
+      "same_validity": true,
+      "c1_exactly_unchanged": true,
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 164772,
+      "baseline_revision": "7e63449749d112f096c56cb848dd289054e5f85b",
+      "baseline_source_sha256": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+      "candidate_source_sha256": "213a4dfa586092c28d8e6e2c27a7dd3622c5862615db9afd5a4e2d38144b1af8",
+      "source_rlog_sha256": {
+        "84865544361f55cb_0000009b--e4616dddaa--0--rlog.zst": "22746f7119109b73ed7f2c26ce8c99f87136e9124fb7fc14c9554409a28a7c3f",
+        "84865544361f55cb_0000009b--e4616dddaa--1--rlog.zst": "4c2e1d7083c31a2b37d0f8dd3be4d330898511b7e02c26f7d40ca9bc2779397d",
+        "84865544361f55cb_0000009b--e4616dddaa--2--rlog.zst": "62f3e049e220cd3681fadf386f2969537bd571998ae2f6ba2d08479428b5a28f",
+        "84865544361f55cb_0000009b--e4616dddaa--3--rlog.zst": "83bf0131b2d36b2ba7e5ba050bbc13c0a3350feb5c9b89dc9c87d3a37abebfb3",
+        "84865544361f55cb_0000009b--e4616dddaa--4--rlog.zst": "430985a80dd6e10f7abeb89457a17022e6bb6978617f415c905f584b1647603e",
+        "84865544361f55cb_0000009b--e4616dddaa--5--rlog.zst": "8c0c5ae6323ec33b3e14f84ca834f70cb56f6b29f471a350f1e3efc06b6ba553",
+        "84865544361f55cb_0000009b--e4616dddaa--6--rlog.zst": "db53dfa8156b9d66792c3eff0b2ce5d31b71ad41cc580dec85f528845593c184",
+        "84865544361f55cb_0000009b--e4616dddaa--7--rlog.zst": "91b0b3be10cb7d7d7f7dd2024d8f9ee99d1e9fd2204203a3a9a2f2f1c6e3fa03",
+        "84865544361f55cb_0000009b--e4616dddaa--8--rlog.zst": "687dbbfc49837efbfe8fa6bc091e40f7fad2908832234d7884f4616d1bc9ccff",
+        "84865544361f55cb_0000009b--e4616dddaa--9--rlog.zst": "a88ec4d25b04cdbf5844686fc77f6b28dca920c9b164e37ebf69844a3ae398fc",
+        "84865544361f55cb_0000009b--e4616dddaa--10--rlog.zst": "fe6b29580a6c94e1c236d13e18db4cd9f31cc1b25d52e1e6e19a5021125c9932",
+        "84865544361f55cb_0000009b--e4616dddaa--11--rlog.zst": "150d31b1944d7a1b8c562f3aee20b66cefa6c4e8d02660ec889907d835142f45",
+        "84865544361f55cb_0000009b--e4616dddaa--12--rlog.zst": "e105e5f703a70f36f1989c66fb46c35a65ff60a265b4332e10fa2e6875a2ced4",
+        "84865544361f55cb_0000009b--e4616dddaa--13--rlog.zst": "69304cf0c81401374f047c5022ca47d257ff9e59e30ec171325a2ee1e7ed13d4"
+      },
+      "focus_cohorts": {
+        "right_entry": {
+          "cycles": 298,
+          "seconds": 2.9932484459999387,
+          "changed_c0_cycles": 100,
+          "mean_absolute_c0_change_m": 0.007683282515599457,
+          "max_absolute_c0_change_m": 0.04999999999999982,
+          "increased_absolute_c0_cycles": 100,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 20.395198628296086,
+          "baseline_peak_absolute_c0_m": 3.0300000000000002,
+          "candidate_peak_absolute_c0_m": 3.0300000000000002
+        },
+        "right_exit": {
+          "cycles": 115,
+          "seconds": 1.156770048999988,
+          "changed_c0_cycles": 115,
+          "mean_absolute_c0_change_m": 0.07934493768172877,
+          "max_absolute_c0_change_m": 0.13000000000000078,
+          "increased_absolute_c0_cycles": 115,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.7199999999999998,
+          "candidate_peak_absolute_c0_m": 0.7800000000000002
+        }
+      }
+    },
+    "route9e": {
+      "cycles": 83668,
+      "eligible_cycles": 74669,
+      "same_validity": true,
+      "c1_exactly_unchanged": true,
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 167336,
+      "baseline_revision": "7e63449749d112f096c56cb848dd289054e5f85b",
+      "baseline_source_sha256": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+      "candidate_source_sha256": "213a4dfa586092c28d8e6e2c27a7dd3622c5862615db9afd5a4e2d38144b1af8",
+      "source_rlog_sha256": {
+        "84865544361f55cb_0000009e--592f7dc149--0--rlog.zst": "de63532ae6aedf5dc7fd3ac8e47a2d96a8f065614f4aca79ae2f120ea00390c7",
+        "84865544361f55cb_0000009e--592f7dc149--1--rlog.zst": "67b58197158b3e8f0581643f6657d2a85c47b0bb75dd3e00306d9ffae790008b",
+        "84865544361f55cb_0000009e--592f7dc149--2--rlog.zst": "a4840f338f51b5f1864c79ce3a4f2b11dc13d58d52b8459961c9da57237f2cc2",
+        "84865544361f55cb_0000009e--592f7dc149--3--rlog.zst": "ff3ea8d948006ab19c4dbfeeff59a509de3f193da47b91c88faa27ecd0b3f1fc",
+        "84865544361f55cb_0000009e--592f7dc149--4--rlog.zst": "5ff1996c2336299128a11e657c32bc21921716b7e3bd401de69a2d49484aa223",
+        "84865544361f55cb_0000009e--592f7dc149--5--rlog.zst": "238763457e89896933afaf9a5df325469ee8c02ddc53550df252531bc94fc540",
+        "84865544361f55cb_0000009e--592f7dc149--6--rlog.zst": "2ee86de80cfd762be10cd2dfb2895ddbee6b813706e9c7261460203e09b9bc4d",
+        "84865544361f55cb_0000009e--592f7dc149--7--rlog.zst": "017801f080861c63d799f87aebe30ab57cdf82f078ac882e6187d3870c403538",
+        "84865544361f55cb_0000009e--592f7dc149--8--rlog.zst": "3729016bd1f00bb1077613b63fe25b21ba7112822b994e0e0aa2b4cd93bdb940",
+        "84865544361f55cb_0000009e--592f7dc149--9--rlog.zst": "c40b3c1f6eb9252b85f176fee32cae16c23eaad3db830f6bbf37a730034a5ccf",
+        "84865544361f55cb_0000009e--592f7dc149--10--rlog.zst": "e0ce8f231798073e5bbc34551fbd9493f169a06a068adc1957ef8cd914b71fbb",
+        "84865544361f55cb_0000009e--592f7dc149--11--rlog.zst": "90863353f7c862a05618b4d0761dda3fcedd3cf6b4234c21c6dcc2eee3ec20b2",
+        "84865544361f55cb_0000009e--592f7dc149--12--rlog.zst": "e064f8abde9615b2daf00f469ef36b438cd4b3a1637069467c84de91bee3bdf1",
+        "84865544361f55cb_0000009e--592f7dc149--13--rlog.zst": "790fc438ce2675e0690fdce3b07bf54fdf6d06b17bbe7f5ff447adfa71189754",
+        "84865544361f55cb_0000009e--592f7dc149--14--rlog.zst": "b98f5a5be660013b1fe11250fdc24ee4fe1d9dd1301b787ddfdff1f2bcd3e27f"
+      },
+      "focus_cohorts": {
+        "left_entry": {
+          "cycles": 238,
+          "seconds": 2.4048367620016506,
+          "changed_c0_cycles": 0,
+          "mean_absolute_c0_change_m": 0.0,
+          "max_absolute_c0_change_m": 0.0,
+          "increased_absolute_c0_cycles": 0,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 2.403816920672437,
+          "baseline_peak_absolute_c0_m": 2.6399999999999997,
+          "candidate_peak_absolute_c0_m": 2.6399999999999997
+        },
+        "left_peak": {
+          "cycles": 288,
+          "seconds": 2.891819394000777,
+          "changed_c0_cycles": 0,
+          "mean_absolute_c0_change_m": 0.0,
+          "max_absolute_c0_change_m": 0.0,
+          "increased_absolute_c0_cycles": 0,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 6.41034524444729,
+          "baseline_peak_absolute_c0_m": 2.95,
+          "candidate_peak_absolute_c0_m": 2.95
+        },
+        "left_exit": {
+          "cycles": 219,
+          "seconds": 2.203320298998733,
+          "changed_c0_cycles": 129,
+          "mean_absolute_c0_change_m": 0.037433836245882854,
+          "max_absolute_c0_change_m": 0.11000000000000032,
+          "increased_absolute_c0_cycles": 128,
+          "decreased_absolute_c0_cycles": 1,
+          "driver_input_percent": 9.936619705158035,
+          "baseline_peak_absolute_c0_m": 2.51,
+          "candidate_peak_absolute_c0_m": 2.51
+        }
+      }
+    },
+    "routea0": {
+      "cycles": 114582,
+      "eligible_cycles": 105382,
+      "same_validity": true,
+      "c1_exactly_unchanged": true,
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 229164,
+      "baseline_revision": "7e63449749d112f096c56cb848dd289054e5f85b",
+      "baseline_source_sha256": "7a226cf3cdf6dc8c3b15829078a7b93e486ba4722c4a1d93ceb55dd2e6c77b21",
+      "candidate_source_sha256": "213a4dfa586092c28d8e6e2c27a7dd3622c5862615db9afd5a4e2d38144b1af8",
+      "source_rlog_sha256": {
+        "84865544361f55cb_000000a0--5e86c30dae--0--rlog.zst": "57b8113783f70f9176e1f2703e70185f64df07b3283f739a6fd7b182a2c92417",
+        "84865544361f55cb_000000a0--5e86c30dae--1--rlog.zst": "a821ce7df80c6466110a6f6433b2c9482f5f3a5b4227e8459182c3ee9fb8c7a7",
+        "84865544361f55cb_000000a0--5e86c30dae--2--rlog.zst": "984cca8e311be8a61444ca2bc23bbf3937c6f00264303faefb7efb8f0a3e8aa2",
+        "84865544361f55cb_000000a0--5e86c30dae--3--rlog.zst": "c6ea25b226aa875a15a8f8356522ab3b12501d280f63ebe0438a9d50b2116279",
+        "84865544361f55cb_000000a0--5e86c30dae--4--rlog.zst": "99b129e2965674fb3406855d1b022c0876083984adf2f2ef51c07f1e699974ea",
+        "84865544361f55cb_000000a0--5e86c30dae--5--rlog.zst": "428e2ce9a1ac0ca7135c00221590c286ab32b4ce5cf24d376de1519b39b25fec",
+        "84865544361f55cb_000000a0--5e86c30dae--6--rlog.zst": "efca7fedd0aaed4be4500fd1a0c67f01d8f805cfdd681a0a9c0439061ec2f36f",
+        "84865544361f55cb_000000a0--5e86c30dae--7--rlog.zst": "1da238f57b63efbbf7dfd2b147637c1b846736e198999741817fc0646d4f60b7",
+        "84865544361f55cb_000000a0--5e86c30dae--8--rlog.zst": "b61209325da26751b98da85fe166dab65ab9eaaef129b859a59fe99e4de5ab65",
+        "84865544361f55cb_000000a0--5e86c30dae--9--rlog.zst": "cf555f1c57a9afdbed702b5934892acb5b2174ef48f1fcf494422aba9bd01321",
+        "84865544361f55cb_000000a0--5e86c30dae--10--rlog.zst": "13ff83c236cca64b57c11ffa05cb74511af1258cde3cf0554395922769a39683",
+        "84865544361f55cb_000000a0--5e86c30dae--11--rlog.zst": "848a1d26fd72bbe4e119339bd405aedb0b9d7e6b96329d0b868a27bd01b6b128",
+        "84865544361f55cb_000000a0--5e86c30dae--12--rlog.zst": "9e8bf8597942cd74d8d04c783148b8201f63f56e55b766f7887f27465de276f2",
+        "84865544361f55cb_000000a0--5e86c30dae--13--rlog.zst": "e37575ed1424d574015c20a3066754bcca6fdacbb104db2bedce782428fd48ef",
+        "84865544361f55cb_000000a0--5e86c30dae--14--rlog.zst": "b67c7ed7ddd957bf5dcbcdb0604ddc9c52cd7af9d6aa70c1eae3e7622cee160d",
+        "84865544361f55cb_000000a0--5e86c30dae--15--rlog.zst": "6baaf9fdf64c72a9be70d261580914e2489f363230d95a1291b8479bcc249ba2",
+        "84865544361f55cb_000000a0--5e86c30dae--16--rlog.zst": "f84803fa2a615caf59a6bd4e9b2848059dd9f4a7f0c01ac4ec7e2347e317b5e7",
+        "84865544361f55cb_000000a0--5e86c30dae--17--rlog.zst": "f0f5f36cb155c18f8f9f90f700e1e91e169ef0442fd70670323be4a1c658040e",
+        "84865544361f55cb_000000a0--5e86c30dae--18--rlog.zst": "8674d5822949bafaefc0d0f30b0e91603354b9981de7566419a2e5b780054e64",
+        "84865544361f55cb_000000a0--5e86c30dae--19--rlog.zst": "b5df55701d5bcfbfd750608a1dfbcba9b931a39fba7a82e87bc7915b8ca2333f"
+      },
+      "focus_cohorts": {
+        "bends_5min": {
+          "cycles": 3966,
+          "seconds": 39.99197329400005,
+          "changed_c0_cycles": 8,
+          "mean_absolute_c0_change_m": 1.950586094527418e-05,
+          "max_absolute_c0_change_m": 0.009999999999999787,
+          "increased_absolute_c0_cycles": 8,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 14.825647085260368,
+          "baseline_peak_absolute_c0_m": 0.3099999999999996,
+          "candidate_peak_absolute_c0_m": 0.3099999999999996
+        },
+        "bend_7min": {
+          "cycles": 893,
+          "seconds": 8.995078784999919,
+          "changed_c0_cycles": 0,
+          "mean_absolute_c0_change_m": 0.0,
+          "max_absolute_c0_change_m": 0.0,
+          "increased_absolute_c0_cycles": 0,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.20000000000000018,
+          "candidate_peak_absolute_c0_m": 0.20000000000000018
+        },
+        "bend_9min": {
+          "cycles": 895,
+          "seconds": 8.998970718999999,
+          "changed_c0_cycles": 0,
+          "mean_absolute_c0_change_m": 0.0,
+          "max_absolute_c0_change_m": 0.0,
+          "increased_absolute_c0_cycles": 0,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 0.22147398432951632,
+          "baseline_peak_absolute_c0_m": 0.2400000000000002,
+          "candidate_peak_absolute_c0_m": 0.2400000000000002
+        }
+      }
+    }
+  },
+  "routea0_ordinary_bend_warnings_v4_vs_v5": {
+    "definition": "Driver-clean +/-1s, speed>=8m/s, absolute desired wheel angle 3 to30deg. Proxy for ordinary bends, not map geometry.",
+    "cycles": 1578,
+    "seconds": 15.92979767199978,
+    "changed_c0_cycles": 0,
+    "changed_c1_cycles": 0,
+    "max_c0_change_m": 0.0
+  },
+  "source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "213a4dfa586092c28d8e6e2c27a7dd3622c5862615db9afd5a4e2d38144b1af8",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "be610c09e4bbd93e84d1920cfbb3e9609b3fbb7dca8eb229b8e67befb4b2ca84",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "4d2dffdfd81190a871bd41e28e5832502aa95d8aacf91daa7e68382085588aca",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "30cfffb86fcd3830320e9b2ec1f65cdfd219684a55d0bc0bb13f77966cc18b53",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "7f8b13c4d85217cbb6bd32ec193ef18bdb30bdf1f2b59fa7fa9d40988cb20bea",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_prediction.py": "eb50e1d6acdb5e7332fadc3dcfcfa1b3809545e607c47c54b0ba3de80b2df208",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py": "f826c6328f0abac2a61f1a0a6f8d119fdbab858e363cb466d84ba9a7783059cd",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_yaw.py": "d646f89e2c4d4e112d03e77c5c34b7541b0bdba800419d472a4a69753637c515",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "9697db696d5a01838ea5cdcf4f1771d813a647f5038cabe9f0460778a2936929",
+    "tools/ford_pscm_lab/__init__.py": "db4b8b7d2e317ed34ca0ec220bf9d53e7b80a23e766f4e1cb2c8224dada45f2e",
+    "tools/ford_pscm_lab/damping_replay.py": "d21565a29b9c2b4668fafe07f995c15e113888a1f4787f37d72d02d36998f9cf",
+    "tools/ford_pscm_lab/model_action_replay.py": "a90caed1c46c6f964fecb50bd0f531ae355505f1e84be9a1e37243d910b292b4",
+    "tools/ford_pscm_lab/stress_model_action.py": "123c6c0c9f53a5af2ab50bb568e47c0f8960b7d49fad2fb6ea74de5c63e03ad5",
+    "tools/ford_pscm_lab/test_model_action_replay.py": "bf1a1612474cb7308b3640cba535ee534999fdecdf055ae6d7bc83b9a8e66f63"
+  },
+  "artifact_sha256": {
+    ".cache/ford_no_yaw_damping/removal_red.txt": "4aa79e5af1066d66db65853faad3923f8645f5dff8383fa3106489a3e53cae18",
+    ".cache/ford_no_yaw_damping/suite_run.txt": "434d80389bc7d2859070c9f9df0cac1ac0f854151b5888dcedc2ce38df23d519",
+    ".cache/ford_no_yaw_damping/coverage.json": "d99baeb8729a1394b41d9a4739b538372aad27b56d379f4b9cefe682ed99d662",
+    ".cache/ford_no_yaw_damping/stress.json": "3e2c49dcc473d90ea03c95dc12905c2216ee573a691891b8a1f56ba82fd78251",
+    ".cache/ford_no_yaw_damping/route9b/report.json": "446371173fed1ff632ece4286e53d0d941491be590d467af960d1931431212ca",
+    ".cache/ford_no_yaw_damping/route9b/commands.npz": "d543a92991f06b56e20d0ed58daf6e4ef6a0021b79c027570b8d997658336b9d",
+    ".cache/ford_no_yaw_damping/route9e/report.json": "fb8ddd8d2c242a14a63993593c96604e7306dcd97219d073bbcfcbeba68af45b",
+    ".cache/ford_no_yaw_damping/route9e/commands.npz": "375a94227f3cb1a161a1295496908baa428d5ef0e0b79134a728f876477dbd6b",
+    ".cache/ford_no_yaw_damping/routea0/report.json": "7959b77af715016b4da45b9bc8f095872702f662950324f64a0f4d332c814eb0",
+    ".cache/ford_no_yaw_damping/routea0/commands.npz": "4f9dcde39ad9e20958ad04e53dbb32830c6a933dc7f743fdffe5a612252cb16b"
+  }
+}

docs/ford_model_action_prediction.md

@@ -0,0 +1,126 @@
+# Experimental Ford path prediction, v3
+
+This document and its counts describe archived v3. The
+[current v4 experiment](ford_model_action_full_prediction.md) removes the extra
+15 cm / 25% prediction adjustment cap.
+
+The latest driven route9e used v1 (`5fc16abc7`), before the v2 yaw damping.
+It often follows the requested steering angle closely, but some tight turns
+fall behind after a reasonable initial turn-in. The requested angle is replanned
+from the car's changing position; a large late request may partly be a recovery
+request after arriving wide. It is not proof that the original turn required
+that much steering. The generic PSCM limit flag does not identify a torque,
+rate or mechanical limit, and the miss starts before our C1 cap in the clearest
+left turn.
+
+Short measured-motion integrations against earlier frozen model paths are
+consistent with a growing miss, but model uncertainty, reference timing and
+some driver input prevent a conclusive causal attribution. V3 tests a bounded
+change to initial path demand. No counterfactual physical tracking score is
+claimed from replaying fixed logs.
+
+## Change and bounds
+
+Start with the current model's lateral offset at 7 m of path arc length, as in
+v1/v2. Advance the reference pose by speed × 0.15 s along the **selected,
+upstream-limited curvature**, and read the same model path 7 m beyond that
+advance, expressed in the predicted ego frame. Bound the change from the
+original offset to both ±0.15 m and ±25% of its magnitude. Prediction cannot
+reverse that target or create C0 from a zero offset.
+
+For advance `d`, selected curvature `k`, rotation `theta = k*d`, and model
+point `(x, y)` at arc station `7+d`, the predicted lateral coordinate is:
+
+```
+y_predicted = cos(theta)*y - sin(theta)*x + (1-cos(theta))/k
+```
+
+The code evaluates the last term continuously at zero curvature without
+cancellation. Available path horizon limits `d`; prediction tapers to zero as
+the horizon approaches 7 m. Nonfinite prediction falls back to the validated
+current offset. The existing endpoint hold remains for paths shorter than 7 m.
+
+A matched constant-radius path retains essentially the same C0, subject to
+sample interpolation. Developing and flattening bends can move the target
+earlier. This is a geometric hypothesis assuming motion along selected
+curvature, not an identified 150 ms actuator delay or a calibrated plant model.
+Errors in that assumption can increase or reduce useful steering demand.
+
+The predicted offset passes through the existing ±5.11 m clip, v2 excess-yaw
+damping, independent 4 m/s slew and 0.01 m quantization. C1 construction, clip,
+slew and quantization are unchanged. C2=C3=0. Input sanity, freshness, service
+health and startup selection gates are unchanged. There are still only two
+control states (C0 and C1 slew positions), plus three adapter timestamps.
+The module is 193 total lines, including 125 code lines excluding blanks,
+comments and docstrings (18 more code lines than v2). No model history, integral
+or turn state machine is added. The bounds above
+apply to the prediction target, not arbitrary differences between separately
+slewed controllers after different histories.
+
+## Offline results and tradeoff
+
+All four supplied routes run at their original controls timestamps. Routes90/95
+also reproduce the archived v1 command construction exactly. The newer routes
+compare immutable v2 code with v3 using identical measured yaw, selected
+curvature, exact consumed model and causal carState. Publication times proxy
+computation time; complete SubMaster health is unavailable. Neither v2 nor v3
+was driven on these recordings. V2-versus-recorded error is therefore not a
+reconstruction accuracy measurement.
+
+| Recorded interval | Command change versus v2 |
+| --- | --- |
+| route9e left entry, 173–175.4 s | Mean C0 magnitude +0.143 m; same 2.0 m level reached 0.203 s earlier |
+| route9e left peak, 175.4–178.3 s | Mean magnitude +0.095 m; prediction also increases some late demand |
+| route9e reversal, 728–734 s | Peak C0 magnitude 0.24 → 0.22 m |
+| route9b right exit, 642.7–643.852 s | Mean C0 +0.024 m, partially offsetting v2 damping |
+| Driver-clean low requests above 8 m/s, routes9b/9e | Mean absolute C0 change ≈0.0015 m; maximum 0.02 m |
+
+The entry C0 crossings at 0.5, 1.0, 1.5, 2.0 and 2.4 m move earlier by 61, 64,
+367, 203 and 90 ms respectively. These are command-level crossing times,
+not measured improvements in wheel response. Several entry/peak windows
+contain driver input, quantified in the validation record.
+
+On all 115 earlier right-exit cycles before strong intervention, v3 remains
+below the driven v1 reconstruction: mean C0 is 0.370 m for v1, 0.290 m for v2,
+and 0.314 m for v3. This tradeoff is retained explicitly; v3 does not improve
+every exit command relative to v2. There is no evidence yet that it reduces
+the late model request or the physical miss.
+
+Route9b now includes full rlogs 12/13, added after the archived v2 evaluation.
+Its 14-rlog totals therefore differ from the historical 12-rlog report. The
+focused segment-10 comparison uses identical timestamps and data.
+
+Validation passes 372 tests and 26 subtests, including the real extracted
+controlsd selection/limiter/publication path and downstream CAN builder, with
+100% controller statement and branch coverage. Four routes cover 299,604
+original cycles. Randomized testing adds 200,000 core updates plus their
+mirrors against an independent analytic geometry/damping/slew oracle; boundary
+and route checks total 817,346 Float32/CAN round trips. These checks establish
+command construction and retained gates, not closed-loop vehicle behavior.
+See `ford_model_action_prediction_validation.json` for provenance and counts.
+
+## Reproduce and select
+
+To reproduce the archived suite and stress results, use v3 commit
+`01f8d51c82b3e863f1012d383b5994813ef01b81` with the native dependencies and
+suite command in the [drive-test guide](ford_model_action_drive_test.md).
+Current replay tooling can select the immutable v3 source explicitly. New-route comparisons use
+the deployment opendbc pin `c21a9013700734dd20b09e05aa68329ad8cc20f9`:
+
+```sh
+python -m tools.ford_pscm_lab.damping_replay /path/to/route9e/rlogs --baseline v2 --candidate v3 --window left_entry 173 175.4 --window left_peak 175.4 178.3 --window left_exit 178.3 180.5 --window reversal 728 734 --window final_entry 822 825.5 --output /path/to/separate/route9e-results
+python -m tools.ford_pscm_lab.damping_replay /path/to/route9b/rlogs --baseline v2 --candidate v3 --window right_entry 637 640 --window right_exit_before_strong_input 642.7 643.852 --output /path/to/separate/route9b-results
+python -m tools.ford_pscm_lab.stress_model_action --cycles 200000 --seed 20260907 --opendbc-revision c21a9013700734dd20b09e05aa68329ad8cc20f9 --output /path/to/stress.json
+```
+
+Historical replay loads trusted controller source from immutable local Git
+commits; those objects must exist in the checkout. The original replay tool
+still requires its explicit historical opendbc pin. Neither tool downloads
+code or drives the car.
+
+The existing default-off Sunnylink **Selected-Action Path Tracking
+(Experimental)** toggle selects v3 on the CAN FD F-150 Lightning. Updating
+with that toggle already enabled selects v3 at the next controlsd startup.
+Diagnostics identify `model-action-c0-c1-prediction-v3` and keep
+`calibration_approved=false`. No new device installation, hardware build,
+physical calibration or device boot is part of this offline validation.

docs/ford_model_action_prediction_validation.json

@@ -0,0 +1,849 @@
+{
+  "date": "2026-09-07",
+  "baseline_commit": "744a97d9bc08d8743b250eceff7c88585b5480de",
+  "deployment_target": {
+    "repository": "sunnypilot/sunnypilot",
+    "branch": "hiimisaac-dev"
+  },
+  "hypothesis": "model-action-c0-c1-prediction-v3",
+  "scope": "Bounded geometric prediction; fixed-input offline evidence only.",
+  "calibration_approved": false,
+  "hardware_build_and_device_boot": "not performed",
+  "controller_size": {
+    "total_lines": 193,
+    "code_lines_excluding_blanks_comments_docstrings": 125,
+    "core_persistent_values": 2,
+    "adapter_timestamps": 3
+  },
+  "checks": {
+    "combined_ford_params_sunnylink_suite": "372 passed, 26 subtests passed; no skips",
+    "suite_log_sha256": "b9546bd4cac8349ab9a699f175ba569ed41fc8a9e85e79a0018c79d3af24c11d",
+    "coverage": {
+      "covered_lines": 116,
+      "num_statements": 116,
+      "percent_covered": 100.0,
+      "percent_covered_display": "100",
+      "missing_lines": 0,
+      "excluded_lines": 0,
+      "percent_statements_covered": 100.0,
+      "percent_statements_covered_display": "100",
+      "num_branches": 32,
+      "num_partial_branches": 0,
+      "covered_branches": 32,
+      "missing_branches": 0,
+      "percent_branches_covered": 100.0,
+      "percent_branches_covered_display": "100"
+    },
+    "ruff": "pass",
+    "ty_controller_and_lab": "pass",
+    "settings_compiler_check": "pass",
+    "prediction_red_probe": "Disabling prediction fails all four mirrored developing/flattening-bend cases (4 failed, 34 passed); all 38 pass with prediction.",
+    "standards_review_remaining_findings": 0,
+    "spec_review_remaining_findings": 0,
+    "review_scope": "Tracked and untracked v3 changes since 744a97d9bc08d8743b250eceff7c88585b5480de; final loader unit test checked separately.",
+    "test_portability": "Archive loader unit test mocks Git source retrieval; full offline route runs use the actual immutable commit sources."
+  },
+  "stress": {
+    "seed": 20260907,
+    "random_cycles": 200000,
+    "mirrored_core_updates": 200000,
+    "invalid_or_inactive_resets": 3537,
+    "field_boundary_cases": 18138,
+    "float32_can_round_trips": 218138,
+    "analytic_targets_scalar_slew_and_mirror_checks_pass": true,
+    "bounded_excess_yaw_damping_checked": true,
+    "bounded_geometric_prediction_checked": true,
+    "direct_raw_float32_packing_matches_host_output": true,
+    "max_continuous_step_c0_c1": [
+      0.40000000000015523,
+      0.05000000000000002
+    ],
+    "calibration_approved": false,
+    "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.",
+    "opendbc_import_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+    "source_sha256": {
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/stress_model_action.py": "be77689e5327a13b7b4d0e18a3fc1863b57cab76be714339efc609124400489f",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "a872cf4594e364988796394b09f531d18cc3d6732e0c92f45eed9fbf6f5cbee6"
+    }
+  },
+  "routes": {
+    "route90": {
+      "scope": "Command construction and adapter reconstruction only; no counterfactual closed-loop score.",
+      "calibration_approved": false,
+      "executes_live_selector": false,
+      "cycles": 78812,
+      "core_active_cycles": 73055,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 73055,
+      "adapter_status_counts": {
+        "inactive": 5757,
+        "active": 73055
+      },
+      "adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
+      "core_active_path_shorter_than_7m_cycles": 0,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 27264,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.16000000000000014,
+        0.0020000000000000018
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 157624,
+      "timing": "Original controls publication timestamps proxy computation time; repeated frames and gaps retained. No identified delay.",
+      "eligibility": "Adapter checks recorded services independently; full SubMaster health is unavailable. Core uses archived validity.",
+      "reference": "Recorded controlsState.desiredCurvature, already selected/limited. These two routes have no maneuver publications.",
+      "host_yaw": "Extract cs.yaw equals -carState.yawRate; current adapter uses it for bounded damping.",
+      "archived_core_revision": "5fc16abc7662020706e29f57d31a6d5e2bc1293a",
+      "archived_core_source_sha256": "8f3bc5d68e0051776f614a2ccffae84a88f7898dc95bdc12c23dcfe10dfe676a",
+      "cohorts": {
+        "small_request": {
+          "seconds": 475.24161910000026,
+          "core_c0_c1_rms": [
+            0.06125184365338734,
+            0.0057692154738982855
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.057970374224682646,
+            0.007060795713496332
+          ],
+          "adapter_eligible_seconds": 475.24161910000026,
+          "adapter_c0_c1_rms": [
+            0.06384544973437024,
+            0.0057692154738982855
+          ]
+        },
+        "turn": {
+          "seconds": 47.85871389900012,
+          "core_c0_c1_rms": [
+            0.18725251016049418,
+            0.046449510236699354
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.4773534845264775,
+            0.04315935101392594
+          ],
+          "adapter_eligible_seconds": 47.85871389900012,
+          "adapter_c0_c1_rms": [
+            0.20375259142277982,
+            0.046449510236699354
+          ]
+        },
+        "small_speed_2_8": {
+          "seconds": 38.696717235999785,
+          "core_c0_c1_rms": [
+            0.209264275431127,
+            0.014019080484620104
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.1593558816527209,
+            0.01823849401244768
+          ],
+          "adapter_eligible_seconds": 38.696717235999785,
+          "adapter_c0_c1_rms": [
+            0.21664870054721863,
+            0.014019080484620104
+          ]
+        },
+        "turn_speed_2_8": {
+          "seconds": 0.30159887100000304,
+          "core_c0_c1_rms": [
+            2.090789090165622,
+            0.21878942570759013
+          ],
+          "recorded_v8_c0_c1_rms": [
+            2.155728831937131,
+            0.38375302254248855
+          ],
+          "adapter_eligible_seconds": 0.30159887100000304,
+          "adapter_c0_c1_rms": [
+            2.2043400076988204,
+            0.21878942570759013
+          ]
+        },
+        "small_speed_8_15": {
+          "seconds": 79.2599167590001,
+          "core_c0_c1_rms": [
+            0.02250905681378238,
+            0.005843904113223639
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03477379324171293,
+            0.006545440153133947
+          ],
+          "adapter_eligible_seconds": 79.2599167590001,
+          "adapter_c0_c1_rms": [
+            0.026080258136122546,
+            0.005843904113223639
+          ]
+        },
+        "turn_speed_8_15": {
+          "seconds": 0.022264622000001566,
+          "core_c0_c1_rms": [
+            0.20999999999999996,
+            0.04349999999999998
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.25,
+            0.04050000011920929
+          ],
+          "adapter_eligible_seconds": 0.022264622000001566,
+          "adapter_c0_c1_rms": [
+            0.20999999999999996,
+            0.04349999999999998
+          ]
+        },
+        "small_speed_15_55": {
+          "seconds": 357.2849851050004,
+          "core_c0_c1_rms": [
+            0.011622097035118838,
+            0.003925571829874466
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03809717934501699,
+            0.0045587590980714285
+          ],
+          "adapter_eligible_seconds": 357.2849851050004,
+          "adapter_c0_c1_rms": [
+            0.013693372023947319,
+            0.003925571829874466
+          ]
+        },
+        "turn_speed_15_55": {
+          "seconds": 47.53485040600012,
+          "core_c0_c1_rms": [
+            0.08686835454709245,
+            0.043216347944464766
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.4471065308273131,
+            0.030663943784303503
+          ],
+          "adapter_eligible_seconds": 47.53485040600012,
+          "adapter_c0_c1_rms": [
+            0.10462899220936993,
+            0.043216347944464766
+          ]
+        },
+        "pose_quiet": {
+          "seconds": 427.99642020700026,
+          "core_c0_c1_rms": [
+            0.01261133542366393,
+            0.00422389167351435
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03724956081568198,
+            0.0046880581807907966
+          ],
+          "adapter_eligible_seconds": 427.99642020700026,
+          "adapter_c0_c1_rms": [
+            0.014639447114479471,
+            0.00422389167351435
+          ]
+        }
+      },
+      "workspace_head": "744a97d9bc08d8743b250eceff7c88585b5480de",
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+      "opendbc_import_path": "/Users/ibpersonal/.codex/worktrees/b926/sunnypilot/opendbc_repo",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "a872cf4594e364988796394b09f531d18cc3d6732e0c92f45eed9fbf6f5cbee6",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_path.py": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/route.npz": "51e8c26eedde253e171af47d704c1967ba45ae6825d883393bec1fb9e00251c1",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/metadata.json": "742afec55ed629155b22d387f376f878f7b2765221fe94f515a829052bdd916f",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/encoder_comparison.npz": "7a625d3ed5cbd8013d1028aa3bc421740551dcae5c3d60981208bd047af9794c",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/encoder_comparison.json": "64e1cc4be84394ac7ec408d383b99b48ada9b3fd129cdef2fd846e5fac620d66",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route90/pose_candidate/pose_replay.npz": "4457ccc0868354749da5b72c1dea0faf750f783dfdc87038101288fdcba1e707"
+      }
+    },
+    "route95": {
+      "scope": "Command construction and adapter reconstruction only; no counterfactual closed-loop score.",
+      "calibration_approved": false,
+      "executes_live_selector": false,
+      "cycles": 54738,
+      "core_active_cycles": 37614,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 37614,
+      "adapter_status_counts": {
+        "inactive": 17124,
+        "active": 37614
+      },
+      "adapter_matches_current_core_with_yaw_and_fresh_engagement_dt": true,
+      "core_active_path_shorter_than_7m_cycles": 44,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 20110,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.16999999999999993,
+        0.0010000000000000009
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 109476,
+      "timing": "Original controls publication timestamps proxy computation time; repeated frames and gaps retained. No identified delay.",
+      "eligibility": "Adapter checks recorded services independently; full SubMaster health is unavailable. Core uses archived validity.",
+      "reference": "Recorded controlsState.desiredCurvature, already selected/limited. These two routes have no maneuver publications.",
+      "host_yaw": "Extract cs.yaw equals -carState.yawRate; current adapter uses it for bounded damping.",
+      "archived_core_revision": "5fc16abc7662020706e29f57d31a6d5e2bc1293a",
+      "archived_core_source_sha256": "8f3bc5d68e0051776f614a2ccffae84a88f7898dc95bdc12c23dcfe10dfe676a",
+      "cohorts": {
+        "small_request": {
+          "seconds": 160.93427628100014,
+          "core_c0_c1_rms": [
+            0.027283284164810895,
+            0.006054573235641941
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.04189890174428264,
+            0.0075342351227490395
+          ],
+          "adapter_eligible_seconds": 160.93427628100014,
+          "adapter_c0_c1_rms": [
+            0.02970169836480558,
+            0.006054573235641941
+          ]
+        },
+        "turn": {
+          "seconds": 58.39837079799993,
+          "core_c0_c1_rms": [
+            0.5425346915921071,
+            0.09102985199238507
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.63700479987207,
+            0.09691236120507696
+          ],
+          "adapter_eligible_seconds": 58.39837079799993,
+          "adapter_c0_c1_rms": [
+            0.5622074838217578,
+            0.09102985199238507
+          ]
+        },
+        "small_speed_2_8": {
+          "seconds": 8.083969085000064,
+          "core_c0_c1_rms": [
+            0.09941192804175551,
+            0.016572084086884566
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.07597383189748476,
+            0.016403469943482765
+          ],
+          "adapter_eligible_seconds": 8.083969085000064,
+          "adapter_c0_c1_rms": [
+            0.1039594708778471,
+            0.016572084086884566
+          ]
+        },
+        "turn_speed_2_8": {
+          "seconds": 2.9056519089999995,
+          "core_c0_c1_rms": [
+            2.286598281178436,
+            0.3420570411209625
+          ],
+          "recorded_v8_c0_c1_rms": [
+            2.4371443775668484,
+            0.37360610108051345
+          ],
+          "adapter_eligible_seconds": 2.9056519089999995,
+          "adapter_c0_c1_rms": [
+            2.342145227147664,
+            0.3420570411209625
+          ]
+        },
+        "small_speed_8_15": {
+          "seconds": 32.021085868,
+          "core_c0_c1_rms": [
+            0.028387860150467858,
+            0.0067602044219821
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.036941169841952046,
+            0.007349671742584858
+          ],
+          "adapter_eligible_seconds": 32.021085868,
+          "adapter_c0_c1_rms": [
+            0.032473217223426755,
+            0.0067602044219821
+          ]
+        },
+        "turn_speed_8_15": {
+          "seconds": 22.160645306000006,
+          "core_c0_c1_rms": [
+            0.28668316323581683,
+            0.0671915746138579
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.3391022553367454,
+            0.07297678510653233
+          ],
+          "adapter_eligible_seconds": 22.160645306000006,
+          "adapter_c0_c1_rms": [
+            0.32040564199313326,
+            0.0671915746138579
+          ]
+        },
+        "small_speed_15_55": {
+          "seconds": 120.82922132800007,
+          "core_c0_c1_rms": [
+            0.010802222799973712,
+            0.004282512931613862
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03987958989535526,
+            0.0065793876512313535
+          ],
+          "adapter_eligible_seconds": 120.82922132800007,
+          "adapter_c0_c1_rms": [
+            0.013133007539405142,
+            0.004282512931613862
+          ]
+        },
+        "turn_speed_15_55": {
+          "seconds": 33.332073582999925,
+          "core_c0_c1_rms": [
+            0.07257996778378971,
+            0.03628926246224237
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.3416081588451539,
+            0.027322786376822172
+          ],
+          "adapter_eligible_seconds": 33.332073582999925,
+          "adapter_c0_c1_rms": [
+            0.08556161843867907,
+            0.03628926246224237
+          ]
+        },
+        "pose_quiet": {
+          "seconds": 145.24136911700015,
+          "core_c0_c1_rms": [
+            0.012566204405760934,
+            0.004614194153565963
+          ],
+          "recorded_v8_c0_c1_rms": [
+            0.03855743841849719,
+            0.006621274926345545
+          ],
+          "adapter_eligible_seconds": 145.24136911700015,
+          "adapter_c0_c1_rms": [
+            0.014652655970248427,
+            0.004614194153565963
+          ]
+        }
+      },
+      "workspace_head": "744a97d9bc08d8743b250eceff7c88585b5480de",
+      "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+      "opendbc_import_path": "/Users/ibpersonal/.codex/worktrees/b926/sunnypilot/opendbc_repo",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "a872cf4594e364988796394b09f531d18cc3d6732e0c92f45eed9fbf6f5cbee6",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_path.py": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/route.npz": "6e5438867ea618e53ca395b60ff4b9b146256f1bf6f07dc8e92d663286074154",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/metadata.json": "f7c95a102e0e084bbcc9938399219080c1d3b26274b4ea3c3a9514ff7dc8ea8d",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/encoder_comparison.npz": "23bd05c4b6400299844acaba1d97051c96c23c682bf17b46e89e7f2cca5fce38",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/encoder_comparison.json": "1ab24a796d14817b8672ea6dfd39711d65cb06744eaaef7dcc02a840c392c4d3",
+        "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/analysis/controller_search_20260904/route95/pose_candidate/pose_replay.npz": "9cfbb3c6f0b1fdb7e3d38e6b64b8c94cffbcc9fabe41f6344d84a9b657b6af9b"
+      }
+    },
+    "route9b": {
+      "scope": "Fixed-input command replay only; no physical improvement or stability claim.",
+      "calibration_approved": false,
+      "cycles": 82386,
+      "eligible_cycles": 70703,
+      "status_counts": {
+        "inactive": 11683,
+        "active": 70703
+      },
+      "c1_exactly_unchanged": true,
+      "same_validity": true,
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 164772,
+      "cohorts": {
+        "eligible": {
+          "cycles": 70703,
+          "seconds": 711.571703289,
+          "changed_c0_cycles": 20858,
+          "mean_absolute_c0_change_m": 0.006916798541216091,
+          "max_absolute_c0_change_m": 0.15000000000000124,
+          "increased_absolute_c0_cycles": 17700,
+          "decreased_absolute_c0_cycles": 3157,
+          "driver_input_percent": 3.0297623863274845,
+          "baseline_peak_absolute_c0_m": 3.89,
+          "candidate_peak_absolute_c0_m": 3.96
+        },
+        "driver_clean": {
+          "cycles": 63267,
+          "seconds": 636.7469998759999,
+          "changed_c0_cycles": 16024,
+          "mean_absolute_c0_change_m": 0.002923399405403562,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 13587,
+          "decreased_absolute_c0_cycles": 2437,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.16,
+          "candidate_peak_absolute_c0_m": 2.24
+        },
+        "driver_clean_low_request_above_8mps": {
+          "cycles": 40656,
+          "seconds": 409.13067679800054,
+          "changed_c0_cycles": 6007,
+          "mean_absolute_c0_change_m": 0.0014813245503935623,
+          "max_absolute_c0_change_m": 0.020000000000000462,
+          "increased_absolute_c0_cycles": 5099,
+          "decreased_absolute_c0_cycles": 908,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.08999999999999986,
+          "candidate_peak_absolute_c0_m": 0.11000000000000032
+        },
+        "turn": {
+          "cycles": 5459,
+          "seconds": 54.939139939000256,
+          "changed_c0_cycles": 3156,
+          "mean_absolute_c0_change_m": 0.007655026845650452,
+          "max_absolute_c0_change_m": 0.08999999999999986,
+          "increased_absolute_c0_cycles": 2938,
+          "decreased_absolute_c0_cycles": 218,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.16,
+          "candidate_peak_absolute_c0_m": 2.24
+        },
+        "right_entry": {
+          "cycles": 298,
+          "seconds": 2.9932484459999387,
+          "changed_c0_cycles": 298,
+          "mean_absolute_c0_change_m": 0.14871908015683724,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 298,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 20.395198628296086,
+          "baseline_peak_absolute_c0_m": 2.8000000000000003,
+          "candidate_peak_absolute_c0_m": 2.95
+        },
+        "right_exit_before_strong_input": {
+          "cycles": 115,
+          "seconds": 1.156770048999988,
+          "changed_c0_cycles": 115,
+          "mean_absolute_c0_change_m": 0.02362929150320746,
+          "max_absolute_c0_change_m": 0.040000000000000036,
+          "increased_absolute_c0_cycles": 115,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.7000000000000002,
+          "candidate_peak_absolute_c0_m": 0.7199999999999998
+        }
+      },
+      "baseline_commands_vs_recorded_publications": {
+        "paired_cycles": 70703,
+        "within_one_quantum_cycles": 69862,
+        "maximum_absolute_error_c0_c1": [
+          0.20999998569488554,
+          0.0005000143051147043
+        ]
+      },
+      "timing": "Publication-time proxy, causal carState, exact consumed model; full SubMaster health unavailable.",
+      "baseline_version": "v2",
+      "baseline_revision": "744a97d9bc08d8743b250eceff7c88585b5480de",
+      "candidate_version": "current",
+      "candidate_revision": "working_tree",
+      "focus_windows": [
+        [
+          "right_entry",
+          "637",
+          "640"
+        ],
+        [
+          "right_exit_before_strong_input",
+          "642.7",
+          "643.852"
+        ]
+      ],
+      "baseline_source_sha256": "59d66297a017557f3d4f28b115be3f6220b800566c11935e2284b3814783fb7e",
+      "candidate_source_sha256": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+      "source_rlog_sha256": {
+        "84865544361f55cb_0000009b--e4616dddaa--0--rlog.zst": "22746f7119109b73ed7f2c26ce8c99f87136e9124fb7fc14c9554409a28a7c3f",
+        "84865544361f55cb_0000009b--e4616dddaa--1--rlog.zst": "4c2e1d7083c31a2b37d0f8dd3be4d330898511b7e02c26f7d40ca9bc2779397d",
+        "84865544361f55cb_0000009b--e4616dddaa--2--rlog.zst": "62f3e049e220cd3681fadf386f2969537bd571998ae2f6ba2d08479428b5a28f",
+        "84865544361f55cb_0000009b--e4616dddaa--3--rlog.zst": "83bf0131b2d36b2ba7e5ba050bbc13c0a3350feb5c9b89dc9c87d3a37abebfb3",
+        "84865544361f55cb_0000009b--e4616dddaa--4--rlog.zst": "430985a80dd6e10f7abeb89457a17022e6bb6978617f415c905f584b1647603e",
+        "84865544361f55cb_0000009b--e4616dddaa--5--rlog.zst": "8c0c5ae6323ec33b3e14f84ca834f70cb56f6b29f471a350f1e3efc06b6ba553",
+        "84865544361f55cb_0000009b--e4616dddaa--6--rlog.zst": "db53dfa8156b9d66792c3eff0b2ce5d31b71ad41cc580dec85f528845593c184",
+        "84865544361f55cb_0000009b--e4616dddaa--7--rlog.zst": "91b0b3be10cb7d7d7f7dd2024d8f9ee99d1e9fd2204203a3a9a2f2f1c6e3fa03",
+        "84865544361f55cb_0000009b--e4616dddaa--8--rlog.zst": "687dbbfc49837efbfe8fa6bc091e40f7fad2908832234d7884f4616d1bc9ccff",
+        "84865544361f55cb_0000009b--e4616dddaa--9--rlog.zst": "a88ec4d25b04cdbf5844686fc77f6b28dca920c9b164e37ebf69844a3ae398fc",
+        "84865544361f55cb_0000009b--e4616dddaa--10--rlog.zst": "fe6b29580a6c94e1c236d13e18db4cd9f31cc1b25d52e1e6e19a5021125c9932",
+        "84865544361f55cb_0000009b--e4616dddaa--11--rlog.zst": "150d31b1944d7a1b8c562f3aee20b66cefa6c4e8d02660ec889907d835142f45",
+        "84865544361f55cb_0000009b--e4616dddaa--12--rlog.zst": "e105e5f703a70f36f1989c66fb46c35a65ff60a265b4332e10fa2e6875a2ced4",
+        "84865544361f55cb_0000009b--e4616dddaa--13--rlog.zst": "69304cf0c81401374f047c5022ca47d257ff9e59e30ec171325a2ee1e7ed13d4"
+      },
+      "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/damping_replay.py": "663674fa9686291228766c4fd2990dcfc620b155f6d74e2f957bbb33e5593a5d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0"
+      }
+    },
+    "route9e": {
+      "scope": "Fixed-input command replay only; no physical improvement or stability claim.",
+      "calibration_approved": false,
+      "cycles": 83668,
+      "eligible_cycles": 74669,
+      "status_counts": {
+        "inactive": 8999,
+        "active": 74669
+      },
+      "c1_exactly_unchanged": true,
+      "same_validity": true,
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 167336,
+      "cohorts": {
+        "eligible": {
+          "cycles": 74669,
+          "seconds": 751.5479497329998,
+          "changed_c0_cycles": 23282,
+          "mean_absolute_c0_change_m": 0.00617904439256301,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 19880,
+          "decreased_absolute_c0_cycles": 3402,
+          "driver_input_percent": 2.7324748055386094,
+          "baseline_peak_absolute_c0_m": 4.33,
+          "candidate_peak_absolute_c0_m": 4.38
+        },
+        "driver_clean": {
+          "cycles": 66321,
+          "seconds": 667.5291971840106,
+          "changed_c0_cycles": 18123,
+          "mean_absolute_c0_change_m": 0.003409158953346422,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 16136,
+          "decreased_absolute_c0_cycles": 1987,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.3600000000000003,
+          "candidate_peak_absolute_c0_m": 2.4699999999999998
+        },
+        "driver_clean_low_request_above_8mps": {
+          "cycles": 39635,
+          "seconds": 398.9613925449994,
+          "changed_c0_cycles": 5866,
+          "mean_absolute_c0_change_m": 0.0014778774629256156,
+          "max_absolute_c0_change_m": 0.020000000000000462,
+          "increased_absolute_c0_cycles": 5377,
+          "decreased_absolute_c0_cycles": 489,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.05999999999999961,
+          "candidate_peak_absolute_c0_m": 0.07000000000000028
+        },
+        "turn": {
+          "cycles": 6219,
+          "seconds": 62.59491144700405,
+          "changed_c0_cycles": 5129,
+          "mean_absolute_c0_change_m": 0.011069748090899225,
+          "max_absolute_c0_change_m": 0.10999999999999943,
+          "increased_absolute_c0_cycles": 5098,
+          "decreased_absolute_c0_cycles": 31,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 2.3600000000000003,
+          "candidate_peak_absolute_c0_m": 2.4699999999999998
+        },
+        "left_entry": {
+          "cycles": 238,
+          "seconds": 2.4048367620016506,
+          "changed_c0_cycles": 238,
+          "mean_absolute_c0_change_m": 0.14261645650107144,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 238,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 2.403816920672437,
+          "baseline_peak_absolute_c0_m": 2.46,
+          "candidate_peak_absolute_c0_m": 2.6100000000000003
+        },
+        "left_peak": {
+          "cycles": 288,
+          "seconds": 2.891819394000777,
+          "changed_c0_cycles": 288,
+          "mean_absolute_c0_change_m": 0.095420590363382,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 288,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 6.41034524444729,
+          "baseline_peak_absolute_c0_m": 2.79,
+          "candidate_peak_absolute_c0_m": 2.9400000000000004
+        },
+        "left_exit": {
+          "cycles": 219,
+          "seconds": 2.203320298998733,
+          "changed_c0_cycles": 185,
+          "mean_absolute_c0_change_m": 0.0422943841629376,
+          "max_absolute_c0_change_m": 0.09000000000000075,
+          "increased_absolute_c0_cycles": 56,
+          "decreased_absolute_c0_cycles": 129,
+          "driver_input_percent": 9.936619705158035,
+          "baseline_peak_absolute_c0_m": 2.4299999999999997,
+          "candidate_peak_absolute_c0_m": 2.51
+        },
+        "reversal": {
+          "cycles": 554,
+          "seconds": 5.570295188001182,
+          "changed_c0_cycles": 167,
+          "mean_absolute_c0_change_m": 0.007060409634059627,
+          "max_absolute_c0_change_m": 0.040000000000000036,
+          "increased_absolute_c0_cycles": 20,
+          "decreased_absolute_c0_cycles": 147,
+          "driver_input_percent": 0.0,
+          "baseline_peak_absolute_c0_m": 0.2400000000000002,
+          "candidate_peak_absolute_c0_m": 0.21999999999999975
+        },
+        "final_entry": {
+          "cycles": 348,
+          "seconds": 3.4958847329980927,
+          "changed_c0_cycles": 348,
+          "mean_absolute_c0_change_m": 0.11950442609746584,
+          "max_absolute_c0_change_m": 0.15000000000000036,
+          "increased_absolute_c0_cycles": 348,
+          "decreased_absolute_c0_cycles": 0,
+          "driver_input_percent": 3.556656282920533,
+          "baseline_peak_absolute_c0_m": 3.31,
+          "candidate_peak_absolute_c0_m": 3.4400000000000004
+        }
+      },
+      "baseline_commands_vs_recorded_publications": {
+        "paired_cycles": 74669,
+        "within_one_quantum_cycles": 74156,
+        "maximum_absolute_error_c0_c1": [
+          0.29000000834465034,
+          0.0010000016689301061
+        ]
+      },
+      "timing": "Publication-time proxy, causal carState, exact consumed model; full SubMaster health unavailable.",
+      "baseline_version": "v2",
+      "baseline_revision": "744a97d9bc08d8743b250eceff7c88585b5480de",
+      "candidate_version": "current",
+      "candidate_revision": "working_tree",
+      "focus_windows": [
+        [
+          "left_entry",
+          "173",
+          "175.4"
+        ],
+        [
+          "left_peak",
+          "175.4",
+          "178.3"
+        ],
+        [
+          "left_exit",
+          "178.3",
+          "180.5"
+        ],
+        [
+          "reversal",
+          "728",
+          "734"
+        ],
+        [
+          "final_entry",
+          "822",
+          "825.5"
+        ]
+      ],
+      "baseline_source_sha256": "59d66297a017557f3d4f28b115be3f6220b800566c11935e2284b3814783fb7e",
+      "candidate_source_sha256": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+      "source_rlog_sha256": {
+        "84865544361f55cb_0000009e--592f7dc149--0--rlog.zst": "de63532ae6aedf5dc7fd3ac8e47a2d96a8f065614f4aca79ae2f120ea00390c7",
+        "84865544361f55cb_0000009e--592f7dc149--1--rlog.zst": "67b58197158b3e8f0581643f6657d2a85c47b0bb75dd3e00306d9ffae790008b",
+        "84865544361f55cb_0000009e--592f7dc149--2--rlog.zst": "a4840f338f51b5f1864c79ce3a4f2b11dc13d58d52b8459961c9da57237f2cc2",
+        "84865544361f55cb_0000009e--592f7dc149--3--rlog.zst": "ff3ea8d948006ab19c4dbfeeff59a509de3f193da47b91c88faa27ecd0b3f1fc",
+        "84865544361f55cb_0000009e--592f7dc149--4--rlog.zst": "5ff1996c2336299128a11e657c32bc21921716b7e3bd401de69a2d49484aa223",
+        "84865544361f55cb_0000009e--592f7dc149--5--rlog.zst": "238763457e89896933afaf9a5df325469ee8c02ddc53550df252531bc94fc540",
+        "84865544361f55cb_0000009e--592f7dc149--6--rlog.zst": "2ee86de80cfd762be10cd2dfb2895ddbee6b813706e9c7261460203e09b9bc4d",
+        "84865544361f55cb_0000009e--592f7dc149--7--rlog.zst": "017801f080861c63d799f87aebe30ab57cdf82f078ac882e6187d3870c403538",
+        "84865544361f55cb_0000009e--592f7dc149--8--rlog.zst": "3729016bd1f00bb1077613b63fe25b21ba7112822b994e0e0aa2b4cd93bdb940",
+        "84865544361f55cb_0000009e--592f7dc149--9--rlog.zst": "c40b3c1f6eb9252b85f176fee32cae16c23eaad3db830f6bbf37a730034a5ccf",
+        "84865544361f55cb_0000009e--592f7dc149--10--rlog.zst": "e0ce8f231798073e5bbc34551fbd9493f169a06a068adc1957ef8cd914b71fbb",
+        "84865544361f55cb_0000009e--592f7dc149--11--rlog.zst": "90863353f7c862a05618b4d0761dda3fcedd3cf6b4234c21c6dcc2eee3ec20b2",
+        "84865544361f55cb_0000009e--592f7dc149--12--rlog.zst": "e064f8abde9615b2daf00f469ef36b438cd4b3a1637069467c84de91bee3bdf1",
+        "84865544361f55cb_0000009e--592f7dc149--13--rlog.zst": "790fc438ce2675e0690fdce3b07bf54fdf6d06b17bbe7f5ff447adfa71189754",
+        "84865544361f55cb_0000009e--592f7dc149--14--rlog.zst": "b98f5a5be660013b1fe11250fdc24ee4fe1d9dd1301b787ddfdff1f2bcd3e27f"
+      },
+      "opendbc_head": "c21a9013700734dd20b09e05aa68329ad8cc20f9",
+      "source_sha256": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/damping_replay.py": "663674fa9686291228766c4fd2990dcfc620b155f6d74e2f957bbb33e5593a5d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0"
+      }
+    }
+  },
+  "focus_comparison": {
+    "scope": "Same recorded inputs; crossing a C0 level earlier does not identify vehicle response time.",
+    "left_entry_crossings": [
+      {
+        "c0_magnitude_m": 0.5,
+        "v2_time_s": 173.04409075799958,
+        "v3_time_s": 172.98298211199835,
+        "earlier_ms": 61.10864600123023
+      },
+      {
+        "c0_magnitude_m": 1.0,
+        "v2_time_s": 173.41054691999852,
+        "v3_time_s": 173.3469171409979,
+        "earlier_ms": 63.629779000621056
+      },
+      {
+        "c0_magnitude_m": 1.5,
+        "v2_time_s": 174.09016323599826,
+        "v3_time_s": 173.7234842099988,
+        "earlier_ms": 366.6790259994741
+      },
+      {
+        "c0_magnitude_m": 2.0,
+        "v2_time_s": 174.69427409499804,
+        "v3_time_s": 174.49167145299907,
+        "earlier_ms": 202.60264199896483
+      },
+      {
+        "c0_magnitude_m": 2.4,
+        "v2_time_s": 175.08411706099832,
+        "v3_time_s": 174.99415714599854,
+        "earlier_ms": 89.95991499978118
+      }
+    ],
+    "right_exit": {
+      "mean_c0_v1_m": 0.3704272817752263,
+      "mean_c0_v2_m": 0.29049595970311604,
+      "mean_c0_v3_m": 0.31412525120632345,
+      "v3_below_driven_v1_every_cycle": true,
+      "cycles": 115
+    }
+  },
+  "total_original_route_cycles": 299604,
+  "total_float32_can_round_trips": 817346,
+  "source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "5b9f7946f29b5fcd94ca93e53be14a12b18ae74d7ca27380e92b90c8c599dce0",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "8851104f811757f6a31b5e23ec5933c7bfc18c17012af9a0a78f851217eca525",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "04daed84093330c05ae1669c155ed9c197f95ebb02fd4e7ca1c7a10ce70d03da",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_damping.py": "a7062d2c3c46b3abcd4c3facd97970644ce609fb55285daa0359681950948250",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_prediction.py": "c05ae173c809228a9f4782af32a174b1b928f506a76c454aa89a51ef0ed17bdd",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py": "f826c6328f0abac2a61f1a0a6f8d119fdbab858e363cb466d84ba9a7783059cd",
+    "openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py": "fd4095bc4f1363fd014c43a9e0776a2245105e8fb2703d87da367d0f33dc22dc",
+    "tools/ford_pscm_lab/__init__.py": "db4b8b7d2e317ed34ca0ec220bf9d53e7b80a23e766f4e1cb2c8224dada45f2e",
+    "tools/ford_pscm_lab/damping_replay.py": "663674fa9686291228766c4fd2990dcfc620b155f6d74e2f957bbb33e5593a5d",
+    "tools/ford_pscm_lab/model_action_replay.py": "a872cf4594e364988796394b09f531d18cc3d6732e0c92f45eed9fbf6f5cbee6",
+    "tools/ford_pscm_lab/stress_model_action.py": "be77689e5327a13b7b4d0e18a3fc1863b57cab76be714339efc609124400489f",
+    "tools/ford_pscm_lab/test_model_action_replay.py": "d2a3602e145db4f969ecf1402fe66684c3b1f884e3ed429e95f5fb5dcaddefb3",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "b5951a90cb01bfcd79b82705600ad2d165b3cc5f7e33c7404536eb989bc3ce39",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "6badb0080d8a999d63d063430c0a320f1a58884dfe0c390774f90a39cbf0e0e2"
+  },
+  "artifact_sha256": {
+    ".cache/ford_model_action_prediction/prediction_red.txt": "81e765edd1fc73783855ef30aa3c4aa8f111c001d7e9e0951d5791e18a860f4e",
+    ".cache/ford_model_action_prediction/stress.json": "e358a7caafe457a83637857cee7f0b1b2b60e0e4020f64b46630b8462e5edff5",
+    ".cache/ford_model_action_prediction/coverage.json": "333ed4414632adbb12a8a1f3ef2f9dc7a62dd9fde08bbc6b8a3d372cf9a6869b",
+    ".cache/ford_model_action_prediction/focus_comparison.json": "b0af320c0eb7e535420702ed78df30c43134f8f9e2a977e5f88f765ba0626c69",
+    ".cache/ford_model_action_prediction/route90/report.json": "b58f738269d9fe176672670f5bf4116f3226d319f43b5b407ac221c288aba174",
+    ".cache/ford_model_action_prediction/route95/report.json": "379a6f5bc9c98d8d1bccf687d61d46c54eda439284508e321d49b5312c1ca9f2",
+    ".cache/ford_model_action_prediction/route9b/report.json": "650ce4056b663124e2f72fae94cd1b66423ff74de42611a99501271c2c846bc2",
+    ".cache/ford_model_action_prediction/route9e/report.json": "196b3b242e3e056ed531cd6e657a235cc9bf8ed4357ce696b1bec1e5f0a49a39",
+    ".cache/ford_model_action_prediction/route9b/commands.npz": "5d82318935253d4fa8a1b8c30cc7f2d465c151ae1cb9109fa09a622d5be49871",
+    ".cache/ford_model_action_prediction/route9e/commands.npz": "2917bbe801a78711340fa4eae63d5bf1cb4a4464c45d8538c9a46a77b5511e06",
+    ".cache/ford_model_action_prediction/plot_commands.py": "6a5bd97fedeb9bd951925dddc656e508082f6d457dbc1a6c9d4db769afcdd921",
+    "/Users/ibpersonal/.codex/visualizations/2026/09/07/01a07c33-1f6c-7292-8030-ad4b01923034/ford-route9e/prediction_command_comparison.png": "86e68a0497490352065997f73cd5a798ff93dcabc93152028669ccfb36a4fefe",
+    "/Users/ibpersonal/.codex/visualizations/2026/09/07/01a07c33-1f6c-7292-8030-ad4b01923034/ford-route9e/prediction_command_comparison.svg": "2310b2978b60b97619886c2e10ecca88c73b5fde3477452347e6db0a5bbcd5b3"
+  }
+}

docs/ford_model_action_validation.json

@@ -0,0 +1,220 @@
+{
+  "date": "2026-09-07",
+  "baseline_commit": "c4b3c55c826fca1ce09618e418e95f0a24478d96",
+  "calibration_approved": false,
+  "production_selector_changed": false,
+  "vehicle_settings_changed": false,
+  "panda_safety_changed": false,
+  "scope": "Offline command construction, adapter integration, numerical fault probes and Ford regression tests. Physical response remains unvalidated.",
+  "controller_size": {
+    "total_lines": 131,
+    "code_lines_excluding_blanks_comments_docstrings": 86,
+    "core_persistent_values": 2,
+    "adapter_timestamps": 3
+  },
+  "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7",
+  "tests": {
+    "ford_suite": "264 passed, 150 subtests passed in 14.29s",
+    "new_core_adapter_tests": 107,
+    "new_replay_validator_tests": 13,
+    "controller_coverage": {
+      "statements": 78,
+      "missing_statements": 0,
+      "branches": 24,
+      "partial_branches": 0,
+      "percent": 100
+    },
+    "ruff": "pass",
+    "ty_controller_and_lab": "pass"
+  },
+  "routes": {
+    "route95": {
+      "cycles": 54738,
+      "core_active_cycles": 37614,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 37614,
+      "adapter_status_counts": {
+        "inactive": 17124,
+        "active": 37614
+      },
+      "adapter_exact_match_with_fresh_engagement_dt": true,
+      "core_active_path_shorter_than_7m_cycles": 44,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 59,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.010000000000000675,
+        0.0010000000000000009
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 109476,
+      "turn_speed_15_55": {
+        "seconds": 33.332073582999925,
+        "core_c0_c1_rms": [
+          0.07257996778378971,
+          0.03628926246224237
+        ],
+        "recorded_v8_c0_c1_rms": [
+          0.3416081588451539,
+          0.027322786376822172
+        ],
+        "adapter_eligible_seconds": 33.332073582999925,
+        "adapter_c0_c1_rms": [
+          0.07257996778378971,
+          0.03628926246224237
+        ]
+      },
+      "input_sha256": {
+        "route.npz": "6e5438867ea618e53ca395b60ff4b9b146256f1bf6f07dc8e92d663286074154",
+        "encoder_comparison.npz": "23bd05c4b6400299844acaba1d97051c96c23c682bf17b46e89e7f2cca5fce38",
+        "pose_replay.npz": "9cfbb3c6f0b1fdb7e3d38e6b64b8c94cffbcc9fabe41f6344d84a9b657b6af9b"
+      },
+      "report_sha256": "cf4e3804f2ebdb0e50f3c49636bf60a30e3c1180411b582826a115970ab972fc"
+    },
+    "route90": {
+      "cycles": 78812,
+      "core_active_cycles": 73055,
+      "core_exact_archived_match": true,
+      "cohorts_reproduced": true,
+      "adapter_active_cycles": 73055,
+      "adapter_status_counts": {
+        "inactive": 5757,
+        "active": 73055
+      },
+      "adapter_exact_match_with_fresh_engagement_dt": true,
+      "core_active_path_shorter_than_7m_cycles": 0,
+      "adapter_validity_differs_from_archive_cycles": 0,
+      "adapter_command_differs_from_archive_cycles": 19,
+      "adapter_max_absolute_command_difference_c0_c1": [
+        0.020000000000000462,
+        0.0020000000000000018
+      ],
+      "field_slew_zero_c2_c3_pass": true,
+      "float32_can_round_trips": 157624,
+      "turn_speed_15_55": {
+        "seconds": 47.53485040600012,
+        "core_c0_c1_rms": [
+          0.08686835454709245,
+          0.043216347944464766
+        ],
+        "recorded_v8_c0_c1_rms": [
+          0.4471065308273131,
+          0.030663943784303503
+        ],
+        "adapter_eligible_seconds": 47.53485040600012,
+        "adapter_c0_c1_rms": [
+          0.08686835454709245,
+          0.043216347944464766
+        ]
+      },
+      "input_sha256": {
+        "route.npz": "51e8c26eedde253e171af47d704c1967ba45ae6825d883393bec1fb9e00251c1",
+        "encoder_comparison.npz": "7a625d3ed5cbd8013d1028aa3bc421740551dcae5c3d60981208bd047af9794c",
+        "pose_replay.npz": "4457ccc0868354749da5b72c1dea0faf750f783dfdc87038101288fdcba1e707"
+      },
+      "report_sha256": "b95247bdf6bbec16e5dc4781eaf7a678aca787418251cd161f6438e3173ad490"
+    }
+  },
+  "stress": {
+    "seed": 20260907,
+    "random_cycles": 200000,
+    "mirrored_core_updates": 200000,
+    "invalid_or_inactive_resets": 3537,
+    "field_boundary_cases": 18138,
+    "float32_can_round_trips": 218138,
+    "analytic_targets_scalar_slew_and_mirror_checks_pass": true,
+    "direct_raw_float32_packing_matches_host_output": true,
+    "max_continuous_step_c0_c1": [
+      0.40000000000000147,
+      0.05000000000000002
+    ],
+    "calibration_approved": false,
+    "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.",
+    "opendbc_import_head": "72a775d35e54c21ff5c5798acef22016eedcc0a7"
+  },
+  "mutation_checks": {
+    "mutations": [
+      {
+        "mutation": "halve_heading",
+        "detected_by_tests": true,
+        "failed_tests": 9
+      },
+      {
+        "mutation": "cap_heading_preview",
+        "detected_by_tests": true,
+        "failed_tests": 9
+      },
+      {
+        "mutation": "erase_centering",
+        "detected_by_tests": true,
+        "failed_tests": 15
+      },
+      {
+        "mutation": "slow_c0_to_c1_rate",
+        "detected_by_tests": true,
+        "failed_tests": 11
+      },
+      {
+        "mutation": "retain_invalid_state",
+        "detected_by_tests": true,
+        "failed_tests": 18
+      },
+      {
+        "mutation": "ignore_model_freshness",
+        "detected_by_tests": true,
+        "failed_tests": 2
+      },
+      {
+        "mutation": "ignore_model_clock_rollback",
+        "detected_by_tests": true,
+        "failed_tests": 1
+      },
+      {
+        "mutation": "reverse_c0_wire_sign",
+        "detected_by_tests": true,
+        "failed_tests": 11
+      }
+    ],
+    "all_detected": true
+  },
+  "native_test_dependency": {
+    "scope": "Native dependency for inherited Params selection test only; copied existing local build, not rebuilt.",
+    "source_library": "/Users/ibpersonal/.codex/worktrees/3548/sunnypilot/openpilot/common/libparams_c.dylib",
+    "sha256": "ddde738108eab18b75f085c865c43aa197fd79a0384b390b1515ff90116e20e0",
+    "byte_identical_source_files": [
+      "openpilot/common/params.cc",
+      "openpilot/common/params.h",
+      "openpilot/common/params_c.cc",
+      "openpilot/common/params.py",
+      "openpilot/common/params_keys.h",
+      "openpilot/common/queue.h",
+      "openpilot/common/util.cc",
+      "openpilot/common/util.h",
+      "openpilot/common/hardware/hw.h"
+    ]
+  },
+  "review": {
+    "standards_findings": 0,
+    "spec_findings": 0,
+    "method": "Independent parallel read-only reviews; 120 focused tests independently passed."
+  },
+  "source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "cb6353f00f2f5c84df4e606c6b7e20650f8c1e908b9fd71890f72aa4a5e42592",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action.py": "c3b971622cc4041575aeec1826d45b76cebab9a2f77b2b9525c85d4184961295",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py": "5aaa29c1f11080b7df0165fd0202a29b065053809e5f9c4b9ed8abfcce47e41b",
+    "tools/ford_pscm_lab/__init__.py": "db4b8b7d2e317ed34ca0ec220bf9d53e7b80a23e766f4e1cb2c8224dada45f2e",
+    "tools/ford_pscm_lab/model_action_replay.py": "c114c479bd22e4fc61a3e8d3ee7fae5d71d1d80ec4b952faad8f8f3692fb1508",
+    "tools/ford_pscm_lab/stress_model_action.py": "a78a50eed1f801f3b096d694ab8c2fd70804b6c250465b4152f38d83770a982b",
+    "tools/ford_pscm_lab/test_model_action_replay.py": "09d024c59d44b83ec081d6416d0f946a7719a73f22213af1f4ddc03dc4e6f4ac"
+  },
+  "artifacts": {
+    "directory": ".cache/ford_model_action",
+    "route_reports": [
+      "route95/report.json",
+      "route90/report.json"
+    ],
+    "stress_report": "stress.json",
+    "mutation_report": "mutations/report.json",
+    "test_log": "ford_suite.txt"
+  }
+}

docs/ford_model_points.md

@@ -0,0 +1,134 @@
+# Ford model-point candidate v7
+
+This is the historical baseline at `4bd841ecc`. The driving branch now uses
+the [C1 early-release experiment](ford_model_release.md). Results below remain
+bound to their original sources and do not validate the newer request mapping.
+
+## Decision
+
+Use the model's own position and orientation at one shared path point. This
+implements the request to follow model geometry through C0/C1, with C2/C3 zero,
+without a fitted PSCM plant, new strength gain, yaw integral, or release mode.
+
+Let `s(t)` be cumulative planar arc distance along model position. Choose:
+
+```
+station = min(path_end, max(7 metres, s(1 second)))
+C0_target = model.position.y at station
+C1_target = unwrapped model.orientation.z at station
+C2 = C3 = 0
+```
+
+Use the published model timestamps, not `speed × 1 second`, so the point also
+follows the model's predicted acceleration/braking. The seven-metre floor keeps
+the existing low-speed preview distance; it can select a time beyond one second.
+If the entire path is shorter, both fields hold the same endpoint. Interpolation
+uses the same arc segment and weights for position and heading.
+
+One second keeps C1 near the old one-second heading scale in ordinary driving.
+Using that same point for C0 represents a meaningful change in faster bends.
+This is a chosen local approximation; Ford's expected reference point and
+preview are unknown. Matching metres/radians does not prove PSCM equivalence.
+
+The former 150 ms vehicle-pose forecast is removed. C1 no longer uses
+`max(7, speed) × selected desiredCurvature`. Measured yaw remains an input-health
+check only; calibrated yaw has no command role. Freshness, independent slew,
+packing, the shared startup toggle, 20 Hz sends, and Panda safety remain.
+The upstream scalar curvature is still logged but does not limit this geometry;
+C0/C1 retain their existing amplitude and slew limits. The scalar-only lateral
+maneuver test reference is explicitly unsupported and disengages this candidate.
+
+## Offline evidence
+
+The broad run passed **585 tests and 9,146 subtests**, with 178 inherited safety
+cases skipped as inapplicable. Tests of the removed yaw forecast were retired;
+new tests cover actual model clocks, a nonconstant-speed trajectory, shared-point
+sampling, the distance floor, endpoint holding, heading unwrap, source selection,
+invalid geometry, reset, independent slew, and actual CAN delivery.
+
+The stress run covers 200,000 random cycles plus mirrored turns, 18,138 field
+boundary cases, and 218,138 Float32/CAN round trips. An independent scalar oracle
+checks analytic model points and slew. No vehicle plant is simulated.
+
+Five recorded routes (a5, a2, a0, 9b, 9e) supply **379,718 controller cycles** and
+**76,294 model frames**. Original rlog hashes and position/orientation timestamps
+were checked. An independently implemented segment-weight oracle matches every
+sampled target and eligible slew state. Every cycle passed packing/bound checks;
+all **75,947** scheduled 20 Hz requests passed the actual unchanged Panda TX hook.
+This is TX acceptance with controlled eligibility, not a full Panda RX watchdog
+or vehicle-response replay. The [validation manifest](ford_model_points_validation.json)
+binds the results to their source and input hashes.
+
+On a5's 61.24-second driver-clean ordinary-bend cohort, mean absolute C0 changes
+from 0.075 m recorded to 0.167 m replayed; C1 changes from 0.02287 to 0.02224 rad.
+Neither field target clips in that cohort. These are command differences, not
+predicted changes in steering strength or tracking error.
+
+| a5 time | Recorded C0 / C1 | Candidate C0 / C1 | Selected station / time |
+| --- | --- | --- | --- |
+| 107.995 s, ordinary bend | +0.20 m / +0.068 rad | +0.55 m / +0.0685 rad | 13.71 m / 1.00 s |
+| 390.681 s, sustained bend | −0.52 m / −0.1235 rad | −0.75 m / −0.1215 rad | 9.37 m / 1.00 s |
+| 374.889 s, tight turn | −3.76 m / −0.50 rad | −3.46 m / −0.50 rad | 7.00 m / 1.49 s |
+| 17.607 s, exit overshoot | −0.01 m / −0.015 rad | −0.05 m / −0.012 rad | 7.00 m / 1.85 s |
+
+The exit example retains more C0 into the turn than v6; physical unwind behavior
+must be evaluated. Tight-turn C1 clipping remains (14.92 eligible seconds on a5).
+No root cause or physical fix is proven by frozen inputs. The latest a5 road
+logs used 100 Hz sends, whereas the immediately preceding code revision already
+changed to 20 Hz; a comparison against that drive also includes the cadence change.
+
+## Unwind comparison
+
+The follow-up a5 comparison includes all seven clearly separated tight turns and
+four completed ordinary bends identified in the full-route command plot. A fifth
+bend runs into the next turn and is excluded from the summary. All scored windows
+remain continuously paired-active, without control gaps over 30 ms. The tight
+turns contain driver input; this compares instructions on frozen inputs, not
+unassisted tracking or hypothetical truck motion.
+
+Measure the first time each instruction falls below the same fixed level on
+exit and stays below for 100 ms. All seven tight turns exceed these levels:
+
+| Instruction | Candidate minus recorded clearance time |
+| --- | --- |
+| C0 below 0.5 m into the turn | 0.10 s later median; five later by 0.01–0.46 s, two unchanged |
+| C1 below 0.1 rad into the turn | 0.10 s earlier median; all seven 0.04–0.17 s earlier |
+
+These are control-publication times. Actual packet timing also includes the
+20 Hz send phase; shifts of only a few tens of milliseconds should not be
+interpreted as equally precise changes at the PSCM.
+
+The report also compares half of each command's own peak: C1 reaches that level
+0.25 s earlier in the median tight turn and C0 0.12 s later. Those normalized
+crossings have different absolute thresholds when amplitudes differ. Comparing
+half the smaller peak at an identical level instead gives C0 earlier in one
+turn and later in six. No single threshold captures the whole release waveform.
+
+For the ordinary bend around 108 s, C0 reaches half of its own peak 0.20 s later;
+C1 reaches half-peak 0.24 s earlier. The larger C0 takes 0.95 s longer to fall
+below the same 0.05 m threshold. On the last tight turn, C0 clears 0.05 m 0.23 s
+later. Near-zero thresholds are sensitive to small residual model offsets; the
+complete report retains 90%, 50%, 10%, common-level and near-zero crossings
+rather than treating any one threshold as a physical success criterion.
+
+The plotted candidate target and command largely coincide during these exits:
+the longer C0 tail comes from the selected model point continuing to ask for
+lateral offset, rather than a retained yaw correction. C1 can release sooner
+because it now follows model orientation directly. These results do not show
+that every command unwinds earlier, or that the vehicle will unwind earlier.
+
+Ten additional end-to-end sender cases cover both signs and all five CAN send
+phases. After a full-cap turn, a zero model target starts reducing both states on
+the first control update and appears on the next scheduled CAN message, 0–40 ms
+later at nominal 100 Hz calculation. There is no additional release hold. The
+unchanged slew itself takes 1.28 s of updates to clear 5.11 m C0 and 1.00 s to
+clear 0.5 rad C1, plus scheduling to transmit zero. These are software timing
+checks, not measured PSCM dynamics. The [unwind report](ford_model_points_unwind.json)
+records the episode timings, method and source hashes.
+
+Older offline utilities now fail explicitly when their input lacks original
+model clocks or their historical unchanged-C1 comparison cannot support v7.
+This prevents missing model inputs from producing an all-invalid apparent match.
+
+`calibration_approved=false` remains. Installation instructions and restore
+behavior are in the [drive-test guide](ford_model_action_drive_test.md).

docs/ford_model_points_validation.json

@@ -0,0 +1,632 @@
+{
+  "baseline_root": "e1cd61166c529f39cb47c815db47e80de720a778",
+  "opendbc": "87ca78e6e641eefb2d654f260a6ab08df3058bd5",
+  "scope": "Model-point construction only; physical tracking not validated.",
+  "calibration_approved": false,
+  "tests": {
+    "passed": 585,
+    "subtests_passed": 9146,
+    "inapplicable_skips": 178
+  },
+  "source_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d",
+    "openpilot/selfdrive/controls/controlsd.py": "bea720b8ec68d6b8a6ad4376ae37324e2404833fb13097a8ab7710a1cedf37a5",
+    "tools/ford_pscm_lab/stress_model_action.py": "0221f85ea2f5757d3b54d077ed8be1faaa6a5d2ed8371de1137f3b6f3d70bf47",
+    "openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml": "ec5e0d9023f7260ae619a4cc6ec538d39053ac13e4515bd0de7d854b79d8abf6",
+    "openpilot/sunnypilot/sunnylink/settings_ui.json": "62e5178a8deee32ef5a88026094cedde359130741688e8a4bcfa82155ef4cca3",
+    "tools/ford_pscm_lab/model_action_replay.py": "95d546cc87c065fa7d581b41382e1ab78bacc4030931b81388cb69c61adf32f7",
+    "tools/ford_pscm_lab/damping_replay.py": "1c253b99342d8113739b0a0551ca66ec6ced9e0aa96338ce6cb42e09c04178a4",
+    "tools/ford_pscm_lab/test_model_action_replay.py": "7aa01cbf197155175215aa4494cfc909029526fbacad0a1e1a2a214583064c4b",
+    "openpilot/selfdrive/controls/tests/test_ford_model_action_cadence.py": "d647e33c83581257f2662c3fec725bb9cd245fb48d579c2e38388318592a2690"
+  },
+  "routes": {
+    "a5": {
+      "route": "a5",
+      "scope": "Frozen inputs; command construction and Panda TX acceptance only. No counterfactual vehicle response.",
+      "cycles": 38961,
+      "models": 7817,
+      "round_trips": 38961,
+      "panda_accepted_sends": 7793,
+      "source_model_clocks_verified": true,
+      "independent_point_and_slew_oracle_passed": true,
+      "cohorts": {
+        "eligible": {
+          "seconds": 359.186785903,
+          "old_mean_abs_c0": 0.34434291232781916,
+          "new_mean_abs_c0": 0.3706073530276498,
+          "old_mean_abs_c1": 0.05784747586810226,
+          "new_mean_abs_c1": 0.06183462577629167,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 14.919213676999902
+        },
+        "road_speed": {
+          "seconds": 261.730126543,
+          "old_mean_abs_c0": 0.10481662146430282,
+          "new_mean_abs_c0": 0.16319455033344274,
+          "old_mean_abs_c1": 0.025151988851597765,
+          "new_mean_abs_c1": 0.02471229299756163,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        },
+        "low_speed": {
+          "seconds": 97.45665935999997,
+          "old_mean_abs_c0": 0.9876160024355487,
+          "new_mean_abs_c0": 0.9276362872461182,
+          "old_mean_abs_c1": 0.14565465097969882,
+          "new_mean_abs_c1": 0.16153056158564297,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 14.919213676999902
+        },
+        "clean": {
+          "seconds": 249.91415584299997,
+          "old_mean_abs_c0": 0.08470877473478762,
+          "new_mean_abs_c0": 0.1191542994782463,
+          "old_mean_abs_c1": 0.017726187650852487,
+          "new_mean_abs_c1": 0.018543416153046232,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 1.056498005999913
+        },
+        "clean_ordinary_bends": {
+          "seconds": 61.23773978999962,
+          "old_mean_abs_c0": 0.07537829679720884,
+          "new_mean_abs_c0": 0.16659385323910988,
+          "old_mean_abs_c1": 0.022868833214909796,
+          "new_mean_abs_c1": 0.022235380107927534,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        }
+      },
+      "points": [
+        {
+          "t": 17.606574576,
+          "old_c0_c1": [
+            -0.009999999776482582,
+            -0.014999999664723873
+          ],
+          "new_c0_c1": [
+            -0.04999999999999982,
+            -0.01200000000000001
+          ],
+          "raw_target": [
+            -0.04535145975205448,
+            -0.012038111718131041
+          ],
+          "sample_station_m": 7.0,
+          "sample_time_s": 1.8525775632377612
+        },
+        {
+          "t": 107.99483939600002,
+          "old_c0_c1": [
+            0.20000000298023224,
+            0.06800000369548798
+          ],
+          "new_c0_c1": [
+            0.5499999999999998,
+            0.0685
+          ],
+          "raw_target": [
+            0.548033630847931,
+            0.06853501158101219
+          ],
+          "sample_station_m": 13.711669224561392,
+          "sample_time_s": 1.0
+        },
+        {
+          "t": 374.888921539,
+          "old_c0_c1": [
+            -3.759999990463257,
+            -0.5
+          ],
+          "new_c0_c1": [
+            -3.46,
+            -0.5
+          ],
+          "raw_target": [
+            -3.497294441592979,
+            -0.6981158781127829
+          ],
+          "sample_station_m": 7.0,
+          "sample_time_s": 1.490557194102553
+        },
+        {
+          "t": 390.68080671100006,
+          "old_c0_c1": [
+            -0.5199999809265137,
+            -0.12349999696016312
+          ],
+          "new_c0_c1": [
+            -0.75,
+            -0.12150000000000005
+          ],
+          "raw_target": [
+            -0.7522029416901727,
+            -0.1214686983398029
+          ],
+          "sample_station_m": 9.370794504969545,
+          "sample_time_s": 1.0
+        }
+      ],
+      "hashes": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/replay.py": "762ebaac3b31737ad609d5b9788514147c696e548fd1ee09458314a0eea30434",
+        ".cache/ford_routea5/route.npz": "815d1e248ff5c3e5e5cfc11dfbd0690d0d13ddafe1436890e239975a72dae9b8",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/a5_models.npz": "84cf4b6e0102d8d5a3f6c3e29bf332e929d7f28d0262eb46220fccc631b902aa"
+      }
+    },
+    "a2": {
+      "route": "a2",
+      "scope": "Frozen inputs; command construction and Panda TX acceptance only. No counterfactual vehicle response.",
+      "cycles": 71111,
+      "models": 14287,
+      "round_trips": 71111,
+      "panda_accepted_sends": 14223,
+      "source_model_clocks_verified": true,
+      "independent_point_and_slew_oracle_passed": true,
+      "cohorts": {
+        "eligible": {
+          "seconds": 437.0644468180001,
+          "old_mean_abs_c0": 0.2155311761983153,
+          "new_mean_abs_c0": 0.31863289785799315,
+          "old_mean_abs_c1": 0.04341179516514646,
+          "new_mean_abs_c1": 0.045887913019549746,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 11.38161301699995
+        },
+        "road_speed": {
+          "seconds": 328.02165902499996,
+          "old_mean_abs_c0": 0.06680861343584246,
+          "new_mean_abs_c0": 0.21392942512598495,
+          "old_mean_abs_c1": 0.025146318618058133,
+          "new_mean_abs_c1": 0.025146126320953914,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        },
+        "low_speed": {
+          "seconds": 109.04278779300012,
+          "old_mean_abs_c0": 0.6629172230811673,
+          "new_mean_abs_c0": 0.6336010633494195,
+          "old_mean_abs_c1": 0.09835785845941926,
+          "new_mean_abs_c1": 0.10828319309008405,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 11.38161301699995
+        },
+        "clean": {
+          "seconds": 332.6767952400001,
+          "old_mean_abs_c0": 0.07338523837198829,
+          "new_mean_abs_c0": 0.1946808147540538,
+          "old_mean_abs_c1": 0.023104103590461375,
+          "new_mean_abs_c1": 0.022903078905402382,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 2.902278352000053
+        },
+        "clean_ordinary_bends": {
+          "seconds": 111.90846122999926,
+          "old_mean_abs_c0": 0.07917757541079931,
+          "new_mean_abs_c0": 0.3589047266588909,
+          "old_mean_abs_c1": 0.03830142702025842,
+          "new_mean_abs_c1": 0.03807353528971872,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        }
+      },
+      "points": [],
+      "hashes": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/replay.py": "762ebaac3b31737ad609d5b9788514147c696e548fd1ee09458314a0eea30434",
+        ".cache/ford_routea2/route.npz": "af4128d278be6be0042f72a02255ddbec96a8a5aa435b9852c8202e4a5cb39e5",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/a2_models.npz": "c3a366796ca5fb27f8c0dc85732ccd6f0ec61b4c8646af3dfe55600e026c0810"
+      }
+    },
+    "a0": {
+      "route": "a0",
+      "scope": "Frozen inputs; command construction and Panda TX acceptance only. No counterfactual vehicle response.",
+      "cycles": 114582,
+      "models": 23038,
+      "round_trips": 114582,
+      "panda_accepted_sends": 22917,
+      "source_model_clocks_verified": true,
+      "independent_point_and_slew_oracle_passed": true,
+      "cohorts": {
+        "eligible": {
+          "seconds": 1061.1945591,
+          "old_mean_abs_c0": 0.16681197800600925,
+          "new_mean_abs_c0": 0.25477442195222594,
+          "old_mean_abs_c1": 0.036905620742387873,
+          "new_mean_abs_c1": 0.0381744756199198,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 14.894412850999998
+        },
+        "road_speed": {
+          "seconds": 901.0129238640001,
+          "old_mean_abs_c0": 0.038282177648169546,
+          "new_mean_abs_c0": 0.13938686570168032,
+          "old_mean_abs_c1": 0.016746723820478274,
+          "new_mean_abs_c1": 0.01614716305741802,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        },
+        "low_speed": {
+          "seconds": 160.18163523600006,
+          "old_mean_abs_c0": 0.889785064486564,
+          "new_mean_abs_c0": 0.9038231052276234,
+          "old_mean_abs_c1": 0.15029831167554059,
+          "new_mean_abs_c1": 0.1620769021843944,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 14.894412850999998
+        },
+        "clean": {
+          "seconds": 884.2159145080002,
+          "old_mean_abs_c0": 0.05133685312289168,
+          "new_mean_abs_c0": 0.13569127462997554,
+          "old_mean_abs_c1": 0.017060990972111164,
+          "new_mean_abs_c1": 0.01680535356441334,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 4.094436286000047
+        },
+        "clean_ordinary_bends": {
+          "seconds": 229.50590253700045,
+          "old_mean_abs_c0": 0.06277285849992259,
+          "new_mean_abs_c0": 0.29272144832989233,
+          "old_mean_abs_c1": 0.032915757288861924,
+          "new_mean_abs_c1": 0.03226126900978203,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        }
+      },
+      "points": [],
+      "hashes": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/replay.py": "762ebaac3b31737ad609d5b9788514147c696e548fd1ee09458314a0eea30434",
+        ".cache/ford_routea0/route.npz": "01ce8b0361c3154d07d5449525a650baa2cc261675095cb1f04ec9e37a9174d3",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/a0_models.npz": "72a72c950d244f5afcffe8cb1cd09b9731297f7565c93814197c728d4b25e8cb"
+      }
+    },
+    "9b": {
+      "route": "9b",
+      "scope": "Frozen inputs; command construction and Panda TX acceptance only. No counterfactual vehicle response.",
+      "cycles": 71396,
+      "models": 14346,
+      "round_trips": 71396,
+      "panda_accepted_sends": 14280,
+      "source_model_clocks_verified": true,
+      "independent_point_and_slew_oracle_passed": true,
+      "cohorts": {
+        "eligible": {
+          "seconds": 651.159063169,
+          "old_mean_abs_c0": 0.09686840630728118,
+          "new_mean_abs_c0": 0.16616909035237892,
+          "old_mean_abs_c1": 0.020769878239136775,
+          "new_mean_abs_c1": 0.02325818302804746,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 4.1034965929999885
+        },
+        "road_speed": {
+          "seconds": 549.860784016,
+          "old_mean_abs_c0": 0.025487998809066218,
+          "new_mean_abs_c0": 0.10707029577162376,
+          "old_mean_abs_c1": 0.011795500864550536,
+          "new_mean_abs_c1": 0.01141539602134342,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        },
+        "low_speed": {
+          "seconds": 101.29827915299992,
+          "old_mean_abs_c0": 0.484330929447169,
+          "new_mean_abs_c0": 0.48696535455636186,
+          "old_mean_abs_c1": 0.06948401455523531,
+          "new_mean_abs_c1": 0.08754243546441211,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 4.1034965929999885
+        }
+      },
+      "points": [],
+      "hashes": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/replay.py": "762ebaac3b31737ad609d5b9788514147c696e548fd1ee09458314a0eea30434",
+        ".cache/ford_route9b/route.npz": "8942b281b76eac012ee660f8ae21bbcad0df95d515291d2497ce5897d5738647",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/9b_models.npz": "fa444360a2775b0df38f274ab4c99568bbe47a3d2777c31ab0193705fce2b1fb"
+      }
+    },
+    "9e": {
+      "route": "9e",
+      "scope": "Frozen inputs; command construction and Panda TX acceptance only. No counterfactual vehicle response.",
+      "cycles": 83668,
+      "models": 16806,
+      "round_trips": 83668,
+      "panda_accepted_sends": 16734,
+      "source_model_clocks_verified": true,
+      "independent_point_and_slew_oracle_passed": true,
+      "cohorts": {
+        "eligible": {
+          "seconds": 751.5479497329998,
+          "old_mean_abs_c0": 0.09489001052394747,
+          "new_mean_abs_c0": 0.16056913479509022,
+          "old_mean_abs_c1": 0.02256825583285058,
+          "new_mean_abs_c1": 0.022300154628921095,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 7.398116462998587
+        },
+        "road_speed": {
+          "seconds": 624.6602415809975,
+          "old_mean_abs_c0": 0.023757802428838078,
+          "new_mean_abs_c0": 0.10203005034223132,
+          "old_mean_abs_c1": 0.01124063950870769,
+          "new_mean_abs_c1": 0.010770200237869725,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        },
+        "low_speed": {
+          "seconds": 126.88770815200223,
+          "old_mean_abs_c0": 0.4450694167092398,
+          "new_mean_abs_c0": 0.4487533818653312,
+          "old_mean_abs_c1": 0.07833340166635812,
+          "new_mean_abs_c1": 0.07906139809550021,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 7.398116462998587
+        },
+        "clean": {
+          "seconds": 639.296275887018,
+          "old_mean_abs_c0": 0.02396620128468532,
+          "new_mean_abs_c0": 0.09600545527466989,
+          "old_mean_abs_c1": 0.01021397220255641,
+          "new_mean_abs_c1": 0.010198642694698857,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        },
+        "clean_ordinary_bends": {
+          "seconds": 168.17771330400137,
+          "old_mean_abs_c0": 0.05087679617819314,
+          "new_mean_abs_c0": 0.2527786285635614,
+          "old_mean_abs_c1": 0.02663914589176354,
+          "new_mean_abs_c1": 0.026386933889696897,
+          "c0_target_capped_s": 0.0,
+          "c1_target_capped_s": 0.0
+        }
+      },
+      "points": [],
+      "hashes": {
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/replay.py": "762ebaac3b31737ad609d5b9788514147c696e548fd1ee09458314a0eea30434",
+        ".cache/ford_route9e/route.npz": "9f169338caf2b5a5377332d15914d1dde57be8b922e25c1bbe16264e0f1be6fb",
+        "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/.cache/ford_model_points/9e_models.npz": "acacf8f0d16fd986b6d74e4a02d704fa5b5b3327fe66dcbe149806f2a5bc3d65"
+      }
+    }
+  },
+  "stress": {
+    "seed": 20260908,
+    "random_cycles": 200000,
+    "mirrored_core_updates": 200000,
+    "invalid_or_inactive_resets": 3537,
+    "field_boundary_cases": 18138,
+    "float32_can_round_trips": 218138,
+    "analytic_targets_scalar_slew_and_mirror_checks_pass": true,
+    "valid_yaw_does_not_affect_targets_checked": true,
+    "shared_model_point_checked": true,
+    "direct_raw_float32_packing_matches_host_output": true,
+    "max_continuous_step_c0_c1": [
+      0.40000000000000147,
+      0.05000000000000002
+    ],
+    "calibration_approved": false,
+    "scope": "Numerical construction only; no PSCM response or closed-loop performance claims.",
+    "opendbc_import_head": "87ca78e6e641eefb2d654f260a6ab08df3058bd5",
+    "source_sha256": {
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/stress_model_action.py": "0221f85ea2f5757d3b54d077ed8be1faaa6a5d2ed8371de1137f3b6f3d70bf47",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/openpilot/selfdrive/controls/lib/ford_model_action.py": "e7502ab68d04aef52edcffcef4b1d42c04e9008ab6892789aac3a865c100cb6d",
+      "/Users/ibpersonal/.codex/worktrees/1a1c/sunnypilot/tools/ford_pscm_lab/model_action_replay.py": "95d546cc87c065fa7d581b41382e1ab78bacc4030931b81388cb69c61adf32f7"
+    }
+  },
+  "unwind": {
+    "report": "docs/ford_model_points_unwind.json",
+    "sha256": "7395a4e66a275fd72d744f5a8244f1b9c8840bb7efff2eec225f15b9c033cd6e",
+    "full_cap_release_send_phase_cases": 10,
+    "summary": {
+      "tight": {
+        "episodes": 7,
+        "c0": {
+          "below_90pct_s": {
+            "n": 7,
+            "median_s": 0.0,
+            "min_s": -0.12269046900000546,
+            "max_s": 0.04266988000000538,
+            "earlier": 2,
+            "later": 2
+          },
+          "below_50pct_s": {
+            "n": 7,
+            "median_s": 0.12129962300002717,
+            "min_s": 0.0848213179999675,
+            "max_s": 0.8246327600000001,
+            "earlier": 0,
+            "later": 7
+          },
+          "below_10pct_s": {
+            "n": 7,
+            "median_s": 0.1376267710000434,
+            "min_s": 0.0,
+            "max_s": 0.45046578099999124,
+            "earlier": 0,
+            "later": 6
+          },
+          "near_zero_s": {
+            "n": 5,
+            "median_s": 0.1399018539999588,
+            "min_s": -0.5224231490000193,
+            "max_s": 1.9510819790000085,
+            "earlier": 1,
+            "later": 4
+          },
+          "common_half_s": {
+            "n": 7,
+            "median_s": 0.07685077599995793,
+            "min_s": -0.2591174110000338,
+            "max_s": 0.5661551039999893,
+            "earlier": 1,
+            "later": 6
+          },
+          "fixed_clearance_s": {
+            "n": 7,
+            "median_s": 0.10080577899998389,
+            "min_s": 0.0,
+            "max_s": 0.4609723959999883,
+            "earlier": 0,
+            "later": 5
+          }
+        },
+        "c1": {
+          "below_90pct_s": {
+            "n": 7,
+            "median_s": -0.3856273459999784,
+            "min_s": -0.5246007750000103,
+            "max_s": 0.5545458859999997,
+            "earlier": 6,
+            "later": 1
+          },
+          "below_50pct_s": {
+            "n": 7,
+            "median_s": -0.2540558009999927,
+            "min_s": -0.46442294700000275,
+            "max_s": -0.21144918100003451,
+            "earlier": 7,
+            "later": 0
+          },
+          "below_10pct_s": {
+            "n": 6,
+            "median_s": -0.15774414349999688,
+            "min_s": -0.20159088100001554,
+            "max_s": 0.0,
+            "earlier": 5,
+            "later": 0
+          },
+          "near_zero_s": {
+            "n": 5,
+            "median_s": 0.0,
+            "min_s": -0.148710938000022,
+            "max_s": 1.5534367700000047,
+            "earlier": 2,
+            "later": 2
+          },
+          "common_half_s": {
+            "n": 7,
+            "median_s": -0.2528352759999848,
+            "min_s": -0.45502116000000115,
+            "max_s": -0.15168304499999863,
+            "earlier": 7,
+            "later": 0
+          },
+          "fixed_clearance_s": {
+            "n": 7,
+            "median_s": -0.10267424500000288,
+            "min_s": -0.1688626670000417,
+            "max_s": -0.040530361999969955,
+            "earlier": 7,
+            "later": 0
+          }
+        }
+      },
+      "ordinary": {
+        "episodes": 4,
+        "c0": {
+          "below_90pct_s": {
+            "n": 4,
+            "median_s": 0.49771416100000465,
+            "min_s": 0.2530698499999744,
+            "max_s": 3.195503137000003,
+            "earlier": 0,
+            "later": 4
+          },
+          "below_50pct_s": {
+            "n": 4,
+            "median_s": 0.3417062340000143,
+            "min_s": 0.19507780300000377,
+            "max_s": 0.4595077099999685,
+            "earlier": 0,
+            "later": 4
+          },
+          "below_10pct_s": {
+            "n": 4,
+            "median_s": 0.11352378200000146,
+            "min_s": 0.09310488600004874,
+            "max_s": 1.1537201169999776,
+            "earlier": 0,
+            "later": 4
+          },
+          "near_zero_s": {
+            "n": 4,
+            "median_s": 0.6988682280000234,
+            "min_s": 0.40513631699997177,
+            "max_s": 3.1885321799999815,
+            "earlier": 0,
+            "later": 4
+          },
+          "common_half_s": {
+            "n": 4,
+            "median_s": 1.0263607459999946,
+            "min_s": 0.7940629349999995,
+            "max_s": 2.846801904000017,
+            "earlier": 0,
+            "later": 4
+          },
+          "fixed_clearance_s": {
+            "n": 2,
+            "median_s": 1.97568246000003,
+            "min_s": 1.5486082510000188,
+            "max_s": 2.4027566690000413,
+            "earlier": 0,
+            "later": 2
+          }
+        },
+        "c1": {
+          "below_90pct_s": {
+            "n": 4,
+            "median_s": -0.0037014085000066643,
+            "min_s": -0.25255103900002496,
+            "max_s": 0.3999567310000316,
+            "earlier": 2,
+            "later": 1
+          },
+          "below_50pct_s": {
+            "n": 4,
+            "median_s": -0.17102558000001977,
+            "min_s": -0.2489269939999872,
+            "max_s": -0.046780566999984785,
+            "earlier": 4,
+            "later": 0
+          },
+          "below_10pct_s": {
+            "n": 4,
+            "median_s": -0.10130154349999998,
+            "min_s": -0.29764589699999533,
+            "max_s": 0.05121605200002932,
+            "earlier": 2,
+            "later": 1
+          },
+          "near_zero_s": {
+            "n": 4,
+            "median_s": -0.022888650000027155,
+            "min_s": -0.10325871399999187,
+            "max_s": 0.24679704999999785,
+            "earlier": 2,
+            "later": 1
+          },
+          "common_half_s": {
+            "n": 4,
+            "median_s": -0.22263683349999042,
+            "min_s": -0.2573995380000156,
+            "max_s": -0.09939024700003074,
+            "earlier": 4,
+            "later": 0
+          },
+          "fixed_clearance_s": {
+            "n": 2,
+            "median_s": -0.17009618150001415,
+            "min_s": -0.24998850800000127,
+            "max_s": -0.09020385500002703,
+            "earlier": 2,
+            "later": 0
+          }
+        }
+      }
+    }
+  }
+}

docs/ford_model_release.md

@@ -0,0 +1,101 @@
+# Ford C1 early-release experiment
+
+The a9 recording contains holds followed by rapid steering release while C0/C1
+requests were already decreasing. Several occur without a PSCM limit flag or
+detected driver input. The experiment changes C1 alone to ask for less turning
+when the model's path straightens ahead. It does not establish which internal
+PSCM mechanism caused those holds.
+
+## Request mapping
+
+Keep the v7 sample station `s`: model arc distance at one second, with the
+existing seven-metre minimum and endpoint hold. Let `psi` be unwrapped model
+heading at that station and `psi0` its initial heading. Using the same model
+segment enclosing that point:
+
+```
+terminal_curvature = change in heading / change in arc distance
+release_heading = psi0 + s * terminal_curvature
+C0_target = model lateral position at s
+C1_target = release_heading bounded between zero and psi
+C2 = C3 = 0
+```
+
+The C1 interval respects the sign of `psi`. The correction cannot amplify the
+raw heading target or manufacture an opposite-direction target. A real model
+sign reversal continues through the normal slew. At an exact model knot the
+incoming segment supplies the slope. A duplicate selected station, zero span,
+or nonfinite derived slope retains the original heading.
+
+Heading linear in the sampled cumulative arc distance, and constant heading,
+retain the original mapping. Arc distance uses model-point chords, so even a
+true circle can differ slightly under uneven sampling; exact preservation is
+a statement about the discrete heading slope, not every sampled physical arc.
+When the terminal curvature is gentler than the average over the preview, C1
+can decrease sooner; it can reach zero even while the future pose still has a
+nonzero heading. C0 continues requesting that future lateral position.
+
+There is no temporal turn-mode latch, new strength multiplier, measured-yaw
+correction, fitted PSCM model, or foreign firmware command cap. Only the two
+existing C0/C1 slew positions persist. Limits remain ±5.11 m / ±0.5 rad and
+4 m/s / 0.5 rad/s; transmission remains 20 Hz.
+
+## Physical question and tradeoff
+
+The question is whether unloading C1 sooner makes the measured wheel release
+sooner, without unacceptable loss of turning authority. Constant discrete
+heading slope is preserved; not every real turn-in is guaranteed unchanged. A model
+point can already lie on the straight after a corner while the truck still
+needs heading change to reach it. This rule can then remove useful C1. C0 and
+the PSCM's physical response determine the result, which passive replay cannot
+establish.
+
+The prior ML3V firmware's internal contribution limits are not treated as
+Lightning limits. `LimitReached` is not used as a release trigger. No claim is
+made that this fixes the a9 hang, tracking error, or stability across PSCMs.
+`calibration_approved=false` remains.
+
+## Recorded command comparison
+
+The a9 comparison uses 7,199 original-clock models and 35,775 control cycles
+from six recorded segments. Candidate C0 is exactly equal to baseline C0 at
+raw target, publication, and sampled send times. Raw C1 never exceeds the
+original heading magnitude or invents a reversal. Independent slew histories
+can let the candidate reach a genuine opposite-direction target sooner, so
+published C1 magnitude is not universally smaller on reversal.
+
+Entry and plateau changes are included, rather than scoring only exits:
+
+| Recorded window | Baseline mean absolute C1 | Candidate mean absolute C1 |
+| --- | ---: | ---: |
+| Right entry, 38:47–38:50 | 0.13643 rad | 0.12561 rad |
+| Large left entry, 40:01–40:05 | unchanged | unchanged |
+| Right plateau, 45:26–45:28.4 | 0.11442 rad | 0.10506 rad |
+| Left entry, 45:34–45:36 | 0.16580 rad | 0.15962 rad |
+
+C1 crosses a common 0.1 rad level about 0.80–0.85 seconds earlier in three
+selected exits. The level is a comparison marker, not a PSCM limit. On the
+right exits near 38:51 and 45:29, it stops requesting the original turn
+direction 0.454 and 0.356 seconds earlier. These are proposed command times
+on frozen inputs, not measured improvements in wheel release.
+
+The baseline replay agrees with recorded publication within one wire quantum
+after cache-boundary warmup; logged publication times do not provide every
+internal controller-entry time. Original clocks, eligibility, and that
+reconstruction tolerance are retained in the validation record.
+
+## Selection and verification
+
+Use the existing default-off Sunnylink **Selected-Action Path Tracking
+(Experimental)** toggle (`FordModelActionController`). An already enabled
+setting selects this revision after updating and an offroad-to-onroad cycle.
+The diagnostic identity is `model-pose-terminal-c1-v1`; active diagnostics also
+report `c1_release=terminal_spatial_curvature`. Turning the toggle off restores
+the previous nonexperimental selection after another offroad-to-onroad cycle.
+
+See the [drive-test guide](ford_model_action_drive_test.md) and the
+[validation record](ford_model_release_validation.json). Unit/integration checks
+cover command construction, bounds, reset, sender cadence, and encoding. Route
+comparisons report changes in commands, including entry attenuation; they do
+not predict wheel motion under the changed commands. The next recording must
+compare desired versus actual steering and release timing on the actual PSCM.

docs/ford_virtual_angle_experiment.md

@@ -0,0 +1,327 @@
+# Ford C2-free model-pose tracking with measured feedback
+
+This experiment is retired. Its implementation, setting and dedicated tests
+were removed from the selected-action drive-test branch. For current setup,
+see [Ford selected-action drive testing](ford_model_action_drive_test.md).
+The material below is historical; it does not describe an available toggle.
+
+Hypothesis `model-pose-c0-c1-feedback-v8` retains the model-pose C0/C1 base
+and adds two guarded release policies. When measured turning exceeds both
+current and delayed requests, a separate output guard prevents same-direction
+C0/C1 growth, including while feedback history rebuilds after driver input.
+When turning instead falls below both requests and is no longer increasing,
+bounded C1 tracking can use remaining release-entry command headroom.
+Existing opposing-bias recovery still stops at zero bias. Geometry, blending,
+feedback gain, slew rates and field limits are unchanged; C2/C3 remain zero.
+
+This is an experimental outer controller around the multivariable PSCM.
+Its geometry does not define a calibrated C0/C1-to-wheel mapping or an angle
+servo. V8 has offline validation only. Command replay cannot establish the
+truck's response, closed-loop stability, or an overshoot improvement.
+
+## Evidence and scope
+
+Route80 ran v3 and contains both sustained under-response and over-response.
+Representative eligible windows had median CAN response/request ratios of
+0.78, 1.77 and 0.69 with a declared 0.2-second comparison interval. These
+are descriptive tracking ratios, not identified controller gains.
+
+V4 replaced separate model-heading C1 with selected-curvature C1 and reduced
+heading demand in several large maneuvers. The user subsequently reported
+weak turning and steering repeatedly stopping near 85 degrees. Older logs
+contain larger wheel angles; the inspected host code has no fixed 85-degree
+wheel stop, although upstream curvature limits depend on speed.
+
+Route83 had the Sunnylink toggle on, but omitted EPS firmware responses.
+The former firmware gate selected the default `FordPathController`; replay
+reproduced its recorded C0/C1/C2 requests. Its favorable turns are evidence
+for the existing model-pose construction, not validation of v5 or v6.
+V6 reuses that construction while replacing its remaining C2 request with
+C0/C1 geometry. Removing C2 changes the request received by the PSCM, so
+matching large C0/C1 commands does not guarantee matching vehicle motion.
+
+Route8a ran v6 and was reported as the best drive. Route8e ran v7 throughout
+with the experiment enabled; it includes entry lag and excessive turning
+while requests release. Fixed-input v6/v7 replay produced identical commands
+in the main reversal and over-response examples, so the v7 recovery change
+does not directly explain their command behavior. In the over-response
+example, model C0/C1 grew while selected curvature fell and driver resets
+repeatedly removed feedback history. Another exit remained deficient after
+opposing bias reached zero. These observations motivate the v8 guards; they
+do not isolate an EPS transfer function or demonstrate the proposed response.
+
+## Base request
+
+controlsd selects valid `lateralManeuverPlan.desiredCurvature`, otherwise
+`modelV2.action.desiredCurvature`, after the existing curvature limiter.
+This action already includes upstream delay handling; it receives no extra
+response advance here.
+
+The model contribution uses the existing allocator's raw forward pose and
+bounded short-pose correction. `_model_pose` advances 0.1 seconds, retains
+the model's remaining forward geometry, and separately corrects the short
+pose using measured curvature and its recent change. Its offset preview is
+up to 7 m and its heading preview is up to max(7 m, speed × 1 s), bounded by
+available path length. This raw pose is not passed through a second model
+filter. The filtered, ego-aligned reference remains available for comparison
+and the existing geometry-validity checks.
+
+```text
+share(k) = clip((k - 0.006/m) / (0.012/m - 0.006/m), 0, 1)
+aligned = desired_curvature × model_forward_heading > 0
+model_share = min(share(abs(desired_curvature)), share(model_curvature_demand))
+              if aligned, otherwise 0
+model_pair = existing_pose_encoder(model_pose, model_share, C2=0)
+
+remaining_curvature = desired_curvature × (1 - model_share)
+L0 = max(8 m, speed × 1 s)
+L1 = max(7 m, speed × 1 s)
+curvature_C0 = 0.5 × remaining_curvature × L0²
+curvature_C1 = remaining_curvature × L1
+C0_base = clip(model_pair.C0 + curvature_C0, ±5.11 m)
+C1_base = clip(model_pair.C1 + curvature_C1, ±0.5 rad)
+```
+
+`model_curvature_demand` is the larger absolute curvature implied by the
+forward offset and heading previews. The share uses the existing allocator's
+0.006–0.012/m thresholds. Both model and action must request a substantial
+turn in the same direction before model pose supplies the full base.
+Small, flat, opposed or zero requests use the curvature contribution; zero
+action produces a zero base. Partial shares combine both contributions.
+The existing pose encoder retains its quantization and field-allocation rules.
+The residual-curvature lift is geometric, not a claim of EPS equivalence to C2.
+
+The inherited pose encoder allocates heading overflow using its asymmetric
+limits (+0.5235/−0.5 rad), before the symmetric final ±0.5 rad
+heading bound. On clipped tails, this can leave mirrored C0 requests differing
+by up to 0.0235 rad × 7 m = 0.1645 m. The favorable comparison anchors lie
+below that heading cap; full model-base odd symmetry is not claimed.
+
+## Measured feedback and limits
+
+```text
+past_request = selected curvature held at or before (measurement_time - delay)
+yaw_error = measured_speed × past_request - measured_yaw_rate
+bias_trial = released_bias + feedback_gain × yaw_error × measurement_dt
+C1_unconstrained = clip(C1_base + accepted_bias, ±0.5 rad)
+C1_target = temporary_backoff_ceiling(C1_unconstrained) if backoff_active
+            otherwise C1_unconstrained
+```
+
+Measured yaw is negated Ford CAN yaw, matching the control sign convention.
+The historical request uses zero-order hold; it never interpolates toward a
+future publication. Nominal comparison delay is `CP.steerActuatorDelay`
+(0.2 seconds on the source vehicle). Feedback compares against selected
+curvature, not curvature inferred from the model-pose coefficients.
+
+| Quantity | Value |
+|---|---:|
+| C0 / C1 final bounds | ±5.11 m / ±0.5 rad |
+| Independent C0 / C1 slew | 4 m/s / 0.5 rad/s |
+| Feedback integration scale | 1.0 |
+| Feedback minimum speed | 2 m/s |
+| Maximum PSCM/core input age | 150 ms |
+| Allowed timestamp lead | 5 ms |
+| Release comparison tolerance | one C1 wire quantum, 0.0005 rad |
+
+The integration scale, preview distances and blend thresholds are effective
+gains; none establishes stability. No wheel-response gain is fitted.
+Zero yaw error retains acquired bias while an eligible turn continues.
+Host anti-windup admits reachable correction within the combined C1 field
+and slew limits. Feedback overflow is not transferred into C0.
+
+The release logic scales bias as the bounded base decreases and resets on
+zero/reversal. When delayed curvature still represents a stronger or opposing
+request, or PSCM reports LimitReached, new integration is normally frozen.
+One exception permits measured-error backoff: measured turning must exceed
+both the delayed and current selected yaw requests in the base's direction,
+and total heading must still have the base's sign. Exceeding only an older,
+smaller request during turn-in does not qualify. The accepted increment may
+only reduce that existing total toward zero; it cannot grow the request or
+carry it through zero. Existing host field and slew limits still apply.
+
+The existing release-recovery exception requires fresh valid PSCM status with
+limit below 2, retained bias opposing the base, and both current and delayed
+requests aligned with that base. Measured turning must be below both requests
+in their direction. It then uses the current yaw deficit × the existing
+feedback gain × measurement interval to unwind only the opposing bias toward
+zero. The increment is clipped so recovery cannot cross zero bias or create
+demand beyond the existing base. Common host anti-windup still limits what
+can be accepted. A separate release-tracking exception is described below;
+other constrained cases remain frozen. PSCM limit 2 never permits either
+request-increasing exception.
+The no-new-bias restriction applies to `release_recovery`. It does not apply
+to the separate bounded `release_tracking` branch. Once release ends,
+ordinary eligible integration can add correction beyond the base as before;
+its existing limits and guards are unchanged.
+
+`release_recovery` and `feedback_recovery_active=true` indicate that the
+recovery branch actually changed bias on that update. If host anti-windup
+blocks the entire increment, the status remains `host_limit` and the flag is
+false. Recovery is evaluated only on fresh measurements; the flag is false
+on repeated-measurement updates and after reset.
+
+Diagnostics distinguish `release_backoff` and `pscm_backoff`; a release takes
+precedence when both conditions apply. While `feedback_backoff_active` is
+true, total C1 is also capped at the preceding continuous heading request in
+the current request direction and at zero in the opposite direction. This
+ceiling affects the output only: it is not stored or projected into bias.
+The measured-error increment can still update bias under the normal limits,
+but a changing model base does not create persistent integral suppression.
+The ceiling persists between repeated measurements; C1 cannot grow or reverse
+while it applies. The next fresh measurement clears it unless backoff is
+again warranted. It does not cap C0, and normal feedback has its own rules
+outside backoff. Independent slew remains 0.5 rad/s for C1 and 4 m/s for C0.
+Backoff still compares against the delayed reference, so response lag remains.
+Reducing a request does not demonstrate that physical overshoot is resolved.
+
+## V8 release guard and tracking
+
+`ReleaseGuard` retains selected-request history independently of feedback
+bias history. Driver-related feedback resets do not erase that reference,
+but the guard still requires current fresh valid PSCM status, no current
+driver override, and the existing input and speed eligibility. Invalid core
+input or disengagement resets its history with the controller.
+
+During release, measured yaw must exceed both the current and delay-matched
+requests in the requested turn direction. Only then does the guard cap
+same-direction C0/C1 growth at each preceding continuous request. Terms
+already reducing the turn, including an opposing C0 centering offset, remain
+available. The guard follows base allocation and C1 feedback, so changing
+model geometry cannot bypass it. Its ceilings affect outputs, never stored
+bias. No scalar-curvature cap replaces strong model geometry during turn-in
+or undertracking. Existing independent slew and field limits still apply.
+
+`release_tracking` addresses an eligible release deficit once bias is zero
+or already in the base's direction. Both current and delayed requests must
+align with that base, measured turning must be below both, and measured
+curvature must not be rising in the turn direction across the response
+interval by more than one C1 wire quantum after scaling by heading preview.
+Fresh valid PSCM status with limit below 2 is required. The current yaw deficit
+uses the existing integration gain and measurement interval;
+new C1 tracking increments are limited by command headroom captured at
+release entry, tapered with remaining desired curvature. The allowance is
+`max(0, entry_command_magnitude - abs(base)) × min(1, abs(desired) / entry_reference)`
+above the current base; any existing same-direction bias consumes it first.
+This limits new tracking integration, not the existing model base or bias.
+Only that additional allowance is tapered; strong model geometry remains
+available. A brief pause does not reacquire a higher entry
+ceiling; a full response interval without release ends the retained episode.
+Common host anti-windup, field and slew bounds still apply. Opposing bias
+continues through `release_recovery`, which stops at zero, before any separate
+tracking exception can be considered.
+
+Neither exception relaxes the PSCM LimitReached growth restriction. The
+reference delay and finite response time remain; these output policies are
+command-construction changes, not evidence of improved physical tracking.
+
+## PSCM status and driver handling
+
+card publishes `Lane_Assist_Data3_FD1` in `carStateSP.fordPscmStatus`, retaining
+the original CAN receipt timestamp. Republishing carStateSP or receiving
+unrelated frames cannot refresh it. The opendbc submodule is unchanged.
+
+Feedback requires valid fresh status, InProgress lateral state (2), capability
+LimitedModeAvailable or ExtendedModeAvailable (1 or 2), and no denial.
+Missing, malformed, stale, backward-timestamped, denied or unavailable status
+clears feedback bias/history and disables the separate release guard,
+leaving the base subject to its core validity gates.
+LimitReached (2) permits only the bounded request-reducing backoff described
+above and otherwise freezes integration. LimitWithDriverActive (3) clears
+feedback. Backoff still requires fresh, valid, InProgress status with an
+available capability and no denial. These generic PSCM reports do not identify
+a specific torque or rate limit.
+
+`steeringPressed`, raw torque above the existing Ford driver allowance, or
+nonfinite torque clear feedback. Below 2 m/s feedback also clears. A fresh
+feedback reference interval is required after override; the independent
+release guard can use retained valid request history once its current gates
+are satisfied. Base requests retain normal
+PSCM driver arbitration while lateral control remains authorized; an unset
+override flag cannot rule out subthreshold driver influence.
+
+## Gates and Sunnylink selection
+
+Core model/action/car-state freshness, finite-value, clock and speed checks
+remain in place. Invalid core inputs reset both commands and clear latActive.
+Raw model geometry is validated on every update, including repeated model
+timestamps; an invalid raw path cannot reuse the cached valid reference.
+Missing PSCM status disables feedback, not an otherwise valid base request.
+
+Vehicle → Ford → **C2-Free Path Tracking (Experimental)** retains the
+`FordVirtualAngleController` key, default-off setting and offroad/onroad cycle
+requirement. Enabled selects v8 on Ford CAN FD `FORD_F_150_LIGHTNING_MK1`
+regardless of missing or different EPS firmware-query results. Other platforms
+retain their existing controller. V8 takes priority over PSCM Coefficient
+Observer while selected; disabling and cycling offroad/onroad restores the
+previous selection. Controller selection does not force lateral engagement.
+
+The analyzed firmware is `RL38-14D003-AA`; removing the eligibility check
+is not validation of other firmware. No live device setting is changed.
+
+## Diagnostics and verification
+
+The 5 Hz `Ford C2-free path tracking` event keeps its name and identifies v8.
+`model_offset_base` / `model_heading_base` report the already weighted and
+encoded model contribution; `curvature_offset_base` / `curvature_heading_base`
+report the residual-curvature contribution. `model_share` and `base_guard`
+identify model-pose, blended, curvature-only, opposed-model and zero-request
+cases. `heading_base` is the bounded pre-feedback C1. `offset_target` and
+`heading_target` are the final targets after the independent release guard;
+`offset_target_unguarded` and `heading_target_unguarded` retain the inputs to
+that guard. The latter C1 already includes its normal feedback/backoff policy.
+
+The event retains source timestamps, measured curvature/yaw, final commands,
+slew scales, feedback bias/status/history, raw torque and PSCM status/age.
+`feedback_backoff_active` records the persistent heading ceiling, including
+cycles whose feedback status is `no_new_measurement`.
+`release_guard_active` and `release_guard_reference_curvature` expose the
+independent C0/C1 guard and its retained delayed reference.
+`feedback_release_tracking_active`, `feedback_release_ceiling` and
+`feedback_curvature_delta` identify accepted release
+tracking, the total-heading threshold used to admit new bias, and the
+measured-curvature change across the response interval (1/m). The tracking
+flag is true only when the branch accepts a bias change on a new measurement;
+it is false on repeated measurements. The ceiling/trend fields can describe
+an evaluated condition even when no increment is accepted.
+`feedback_recovery_active` records an accepted recovery increment on this
+update only; it does not persist between measurements.
+`feedback_yaw_error` retains its delayed-reference meaning. Recovery instead
+uses current error, reconstructed from logged `desired_curvature`,
+synchronized car-state speed and `yaw_rate`; those two errors can differ.
+During backoff or the independent release guard, `heading_target` can be lower in the request direction than
+the bounded sum of `heading_base` and `heading_bias`, because the temporary
+ceiling is not part of the stored bias.
+`model_heading_target` remains a filtered comparison reference; it is not the
+weighted model contribution. `angleState.saturated` is not an EPS-limit signal.
+
+Validation must cover large recorded maneuvers, flat-model centering, both
+turn directions, model/action disagreement, share transitions, release and
+reversal, release/limit backoff without growth or zero crossing, status/driver
+resets, reference causality, bounds, slew and CAN packing with C2/C3 zero.
+Recovery checks cover both directions, stopping at zero bias, repeated
+measurements, current-and-delayed agreement, and rejection at PSCM limit 2.
+Old v3/v4 command-equality expectations do not define
+v8 success. Guard checks also cover driver reset/history rebuilding,
+same-direction growth, opposing coefficients, repeated measurements,
+undertracking and invalid-status inhibition. Tracking checks cover delayed
+curvature trends and tapered release-entry headroom. Historical v5–v7 replay
+results remain historical observations.
+
+The v8 recorded-input fixture contains 15,273 cycles with 4,879 selected
+evidence samples. Base allocation and output eligibility match v7. In the
+clean deficient exit, median absolute C1 changes from 0.0665 to 0.0845 rad
+while C0 stays unchanged. The growth guard also acts while feedback history
+rebuilds; the largest over-growth witness includes nearby driver input and
+is excluded from the strict autonomous tracking score. Both good comparison
+curves in that fixture retain their median requests, and the older large-turn
+fixtures retain their required command scale.
+
+On the earlier good drive, one comparison curve retains extra C1 after
+eligible release tracking: median magnitude changes from 0.121 to 0.128 rad.
+In its 103–110 s interval, tracking increments occur only while measured
+turning falls short, with a median current response/request ratio of 0.895.
+Acquired bias can persist after matching, as with ordinary integral feedback.
+This collateral command change remains a reason to compare new vehicle logs.
+Replay fixes recorded motion and planner outputs, so enabled vehicle logs
+are still required to assess tracking error, oscillation and interventions.

launch_chffrplus.sh

@@ -24,7 +24,9 @@ function agnos_init {
     if $AGNOS_PY --verify $MANIFEST; then
       sudo reboot
     fi
-    $DIR/openpilot/common/hardware/comma/updater $AGNOS_PY $MANIFEST
+    while true; do
+      $DIR/openpilot/common/hardware/comma/updater $AGNOS_PY $MANIFEST
+    done
   fi
 }
 

launch_env.sh

@@ -16,7 +16,7 @@ export VECLIB_MAXIMUM_THREADS=1
 export QCOM_PRIORITY=12
 
 if [ -z "$AGNOS_VERSION" ]; then
-  export AGNOS_VERSION="19.6"
+  export AGNOS_VERSION="19.7"
 fi
 
 export STAGING_ROOT="/data/safe_staging"

openpilot/cereal/custom.capnp

@@ -131,6 +131,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
     downloaded @2;
     cached @3;
     failed @4;
+    verifying @5;
   }
 
   struct DownloadProgress {
@@ -352,6 +353,7 @@ struct OnroadEventSP @0xda96579883444c35 {
     speedLimitPending @22;
     e2eChime @23;
     laneChangeRoadEdge @24;
+    bigModelReady @25;
   }
 }
 
@@ -381,6 +383,7 @@ struct CarControlSP @0xa5cd762cd951a455 {
   leadOne @2 :LeadData;
   leadTwo @3 :LeadData;
   intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
+  fordLateralPath @5 :FordLateralPath;
 
   struct Param {
     key @0 :Text;
@@ -401,6 +404,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;
@@ -445,6 +456,16 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
 
 struct CarStateSP @0xb86e6369214c01c8 {
   speedLimit @0 :Float32;
+  fordPscmStatus @1 :FordPscmStatus;
+
+  struct FordPscmStatus {
+    valid @0 :Bool;
+    canMonoTime @1 :UInt64;  # Last accepted Lane_Assist_Data3_FD1 CAN receipt, not carStateSP publication time.
+    lateralState @2 :UInt8;  # LatCtlSte_D_Stat
+    limit @3 :UInt8;  # LatCtlLim_D_Stat: generic lateral limit, not a torque/rate diagnosis.
+    capability @4 :UInt8;  # LatCtlCpblty_D_Stat
+    denied @5 :Bool;  # LaActDeny_B_Actl
+  }
 }
 
 struct LiveMapDataSP @0xf416ec09499d9d19 {
@@ -468,7 +489,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 {

openpilot/cereal/log.capnp

@@ -725,6 +725,7 @@ struct ChestnutState {
   pcieLtssm @7 :UInt8;
   supplyVoltage @8 :UInt16;  # mV
   supplyCurrent @9 :Int16;  # mA
+  supplyFault @10 :Bool;
 }
 
 struct RadarState @0x9a185389d6fdd05f {
@@ -1004,6 +1005,7 @@ struct DrivingModelData {
   frameIdExtra @1 :UInt32;
   frameDropPerc @6 :Float32;
   modelExecutionTime @7 :Float32;
+  big @8 :Bool;
 
   action @2 :ModelDataV2.Action;
 
@@ -2640,7 +2642,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;

openpilot/cereal/services.py

@@ -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.),

openpilot/common/hardware/comma/agnos.json

@@ -56,29 +56,29 @@
   },
   {
     "name": "boot",
-    "url": "https://commadist.azureedge.net/agnosupdate/boot-b30f5eef65ec3878f3aa3dcaf2cc95c09e2c1e661cd3a38e94da37dee76f68bd.img.xz",
-    "hash": "b30f5eef65ec3878f3aa3dcaf2cc95c09e2c1e661cd3a38e94da37dee76f68bd",
-    "hash_raw": "b30f5eef65ec3878f3aa3dcaf2cc95c09e2c1e661cd3a38e94da37dee76f68bd",
+    "url": "https://commadist.azureedge.net/agnosupdate/boot-6ecf6f987cd11968104abcccabbe268485d329cdb73012dfd3c381a6b8deb27d.img.xz",
+    "hash": "6ecf6f987cd11968104abcccabbe268485d329cdb73012dfd3c381a6b8deb27d",
+    "hash_raw": "6ecf6f987cd11968104abcccabbe268485d329cdb73012dfd3c381a6b8deb27d",
     "size": 46897152,
     "sparse": false,
     "full_check": true,
     "has_ab": true,
-    "ondevice_hash": "6650e4c46df99ae6dfd6ee895a34b8a2a3cc490a8ce18e16cc3c451c3f822b6e"
+    "ondevice_hash": "d12e1e5b9455b62a1464558716493b33e470d7a7e88da1c4105a3b21d0961808"
   },
   {
     "name": "system",
-    "url": "https://commadist.azureedge.net/agnosupdate/system-5b6ce7965904a157fd3a134ccfcb854f9ca5c1cc2a26b7cb80a4fa4e1cc4aaa3.img.xz",
-    "hash": "b134fd04e9da27fa1d359ea0f2742c216fa21a08b5c47e9be22ab3b0563d9b9b",
-    "hash_raw": "5b6ce7965904a157fd3a134ccfcb854f9ca5c1cc2a26b7cb80a4fa4e1cc4aaa3",
+    "url": "https://commadist.azureedge.net/agnosupdate/system-3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f.img.xz",
+    "hash": "74ffc9c551e1f29cda897ace8a69080fe644f8039977c6885f2b48362e39b744",
+    "hash_raw": "3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f",
     "size": 4718592000,
     "sparse": true,
     "full_check": false,
     "has_ab": true,
-    "ondevice_hash": "91242772af771ae96fe2eebc105f2b80a7e1dbaaf6003c2574b62d51b806f468",
+    "ondevice_hash": "6a992680183685eea9db99d915219a37935f45989330d9b619e880450257f448",
     "alt": {
-      "hash": "5b6ce7965904a157fd3a134ccfcb854f9ca5c1cc2a26b7cb80a4fa4e1cc4aaa3",
-      "url": "https://commadist.azureedge.net/agnosupdate/system-5b6ce7965904a157fd3a134ccfcb854f9ca5c1cc2a26b7cb80a4fa4e1cc4aaa3.img",
+      "hash": "3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f",
+      "url": "https://commadist.azureedge.net/agnosupdate/system-3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f.img",
       "size": 4718592000
     }
   }
-]
+]

openpilot/common/hardware/comma/modem.py

@@ -5,6 +5,7 @@ import logging
 import os
 import select
 import signal
+import string
 import struct
 import subprocess
 import tempfile
@@ -354,7 +355,7 @@ class Modem:
       imei = ""
 
     iccid = (self._atv("AT+QCCID", "+QCCID:") or "").rstrip("F")
-    if not iccid.isdigit():
+    if not all(c in string.hexdigits for c in iccid):
       iccid = ""
 
     imsi = first_line("AT+CIMI")

openpilot/common/hardware/usb.py

@@ -4,11 +4,17 @@ from pathlib import Path
 CHESTNUT_FW_VERSION = "ed4e39b7"
 CHESTNUT_USB_IDS = ((0xADD1, 0x0001), (0x3801, 0x0001))
 CHESTNUT_ROM_USB_IDS = ((0x174C, 0x2464), (0x174C, 0x2463))
+CHESTNUT_USB_PRODUCT = f"custom {CHESTNUT_FW_VERSION}-CLEAN"
 USB_DEVICES_PATH = Path("/sys/bus/usb/devices")
 TYPEC_CC_ORIENTATION_PATH = Path("/sys/class/power_supply/usb/typec_cc_orientation")
 PRIMARY_USB_CONTROLLER = "a600000.ssusb"
 
 
+def is_chestnut_usb_id(vendor_id: int, product_id: int, include_bootloader: bool = False) -> bool:
+  ids = CHESTNUT_USB_IDS + CHESTNUT_ROM_USB_IDS if include_bootloader else CHESTNUT_USB_IDS
+  return (vendor_id, product_id) in ids
+
+
 def get_usb_topology() -> set[str]:
   try:
     return set(os.listdir(USB_DEVICES_PATH))
@@ -81,7 +87,7 @@ def set_usb_state(device_state, devices: list[dict]) -> None:
     entry.linkErrorCount = device["linkErrorCount"]
     entry.usb3Lane = device.get("usb3Lane", "unknown")
 
-    if (entry.vendorId, entry.productId) in CHESTNUT_USB_IDS:
+    if is_chestnut_usb_id(entry.vendorId, entry.productId):
       chestnut_present = True
 
   device_state.chestnutPresent = chestnut_present

openpilot/common/params.cc

@@ -97,6 +97,10 @@ Params::Params(const std::string &path) {
 }
 
 Params::~Params() {
+  flushNonBlockingWrites();
+}
+
+void Params::flushNonBlockingWrites() {
   if (future.valid()) {
     future.wait();
   }

openpilot/common/params.h

@@ -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");
   }

openpilot/common/params.py

@@ -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))
 

openpilot/common/params_c.cc

@@ -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);
@@ -162,12 +168,15 @@ ParamsBuffer params_key_at(ParamsHandle *handle, size_t index) noexcept {
 
 size_t params_keys_by_flag(ParamsHandle *handle, uint32_t flag, ParamsBuffer *out, size_t out_size) noexcept {
   return translate_exceptions(size_t{0}, [&]() {
-    auto filtered = handle->params.allKeys(static_cast<ParamKeyFlag>(flag));
-    size_t count = std::min(filtered.size(), out_size);
-    for (size_t i = 0; i < count; i++) {
-      out[i] = return_string(filtered[i]);
+    size_t count = 0;
+    for (const auto &key : handle->keys) {
+      if (flag == ALL || (handle->params.getKeyFlag(key) & flag)) {
+        // Each buffer borrows a different string, stable for the handle's lifetime.
+        if (count < out_size) out[count] = {key.data(), key.size()};
+        ++count;
+      }
     }
-    return filtered.size();
+    return count;
   });
 }
 

openpilot/common/params_keys.h

@@ -59,7 +59,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"IsDriverViewEnabled", {CLEAR_ON_MANAGER_START, BOOL}},
     {"IsEngaged", {PERSISTENT, BOOL}},
     {"IsLdwEnabled", {PERSISTENT | BACKUP, BOOL}},
-    {"IsLiveStreaming", {CLEAR_ON_MANAGER_START, BOOL}},
+    {"IsLiveStreaming", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, BOOL}},
     {"IsMetric", {PERSISTENT | BACKUP, BOOL}},
     {"IsOffroad", {CLEAR_ON_MANAGER_START, BOOL}},
     {"IsRhdDetected", {PERSISTENT, BOOL}},
@@ -92,6 +92,12 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"ObdMultiplexingEnabled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
     {"Offroad_CarUnrecognized", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
     {"Offroad_ChestnutBranch", {CLEAR_ON_MANAGER_START, JSON}},
+    {"Offroad_ChestnutNotDetected", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
+    {"Offroad_ChestnutOverheated", {CLEAR_ON_MANAGER_START, JSON}},
+    {"Offroad_ChestnutPcieUnavailable", {CLEAR_ON_MANAGER_START, JSON}},
+    {"Offroad_ChestnutUncompiled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
+    {"Offroad_ChestnutUpdateFailed", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
+    {"Offroad_ChestnutUsbSlow", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
     {"Offroad_ConnectivityNeeded", {CLEAR_ON_MANAGER_START, JSON}},
     {"Offroad_ConnectivityNeededPrompt", {CLEAR_ON_MANAGER_START, JSON}},
     {"Offroad_ExcessiveActuation", {PERSISTENT, JSON}},
@@ -130,12 +136,15 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"UpdaterLastFetchTime", {PERSISTENT, TIME}},
     {"UptimeOffroad", {PERSISTENT, FLOAT, "0.0"}},
     {"UptimeOnroad", {PERSISTENT, FLOAT, "0.0"}},
-    {"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}},
+    {"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}},
     {"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 +165,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 +175,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,14 +207,16 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
 
     // Model Manager params
     {"ModelManager_ActiveBundle", {PERSISTENT, JSON}},
-    {"ModelManager_ActiveJson", {CLEAR_ON_MANAGER_START, STRING}},
+    {"ModelManager_ActiveBundleUSBGPU", {PERSISTENT, JSON}}, //TODO-SP: kept for migration, remove on next sync?
+    {"ModelManager_ActiveBundleChestnut", {PERSISTENT, JSON}},
+    {"ModelManager_ActiveJson", {CLEAR_ON_MANAGER_START, JSON}},
     {"ModelManager_ClearCache", {CLEAR_ON_MANAGER_START, BOOL}},
-    {"ModelManager_DownloadIndex", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, INT}},
+    {"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_USBGPU", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}},
+    {"ModelManager_LastSyncTime_Chestnut", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}},
     {"ModelManager_ModelsCache", {PERSISTENT | BACKUP, JSON}},
-    {"ModelManager_ModelsCache_USBGPU", {PERSISTENT | BACKUP, JSON}},
+    {"ModelManager_ModelsCache_Chestnut", {PERSISTENT | BACKUP, JSON}},
 
     // Neural Network Lateral Control
     {"NeuralNetworkLateralControl", {PERSISTENT | BACKUP, BOOL, "0"}},
@@ -223,6 +237,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"BackupManager_RestoreVersion", {PERSISTENT, STRING}},
 
     // sunnypilot car specific params
+    {"FordPscmObserver", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"FordModelActionController", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}},
     {"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}},
@@ -245,6 +261,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
 
     // mapd
     {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}},
+    {"Mapd_ClearCache", {CLEAR_ON_MANAGER_START, BOOL}},
     {"MapdVersion", {PERSISTENT, STRING}},
     {"MapSpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT, "0.0"}},
     {"NextMapSpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, JSON}},

openpilot/common/prefix.h

@@ -27,14 +27,14 @@ public:
     auto param_path = Params().getParamPath();
     if (util::file_exists(param_path)) {
       std::string real_path = util::readlink(param_path);
-      util::check_system(util::string_format("rm %s -rf", real_path.c_str()));
+      util::check_system(util::string_format("rm -rf %s", real_path.c_str()));
       unlink(param_path.c_str());
     }
     if (getenv("COMMA_CACHE") == nullptr) {
-      util::check_system(util::string_format("rm %s -rf", Path::download_cache_root().c_str()));
+      util::check_system(util::string_format("rm -rf %s", Path::download_cache_root().c_str()));
     }
-    util::check_system(util::string_format("rm %s -rf", Path::comma_home().c_str()));
-    util::check_system(util::string_format("rm %s -rf", msgq_path.c_str()));
+    util::check_system(util::string_format("rm -rf %s", Path::comma_home().c_str()));
+    util::check_system(util::string_format("rm -rf %s", msgq_path.c_str()));
     unsetenv("OPENPILOT_PREFIX");
   }
 

openpilot/common/tests/test_params.py

@@ -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()
 
@@ -126,6 +133,16 @@ class TestParams(OpenpilotTestCase):
     assert self.params.get("LiveParametersV2") is None
     assert self.params.get("LiveParametersV2", return_default=True) is None
 
+  def test_filtered_keys_are_distinct_registered_strings(self):
+    registered = set(self.params.all_keys())
+    for flag in (ParamKeyFlag.PERSISTENT, ParamKeyFlag.BACKUP, ParamKeyFlag.CLEAR_ON_MANAGER_START):
+      filtered = self.params.all_keys(flag)
+      assert len(filtered) > 1
+      assert len(filtered) == len(set(filtered))
+      assert set(filtered) <= registered
+      assert all(key.decode('utf-8') for key in filtered)
+      assert self.params.all_keys(flag) == filtered
+
   def test_params_get_type(self):
     # json
     self.params.put("ApiCache_FirehoseStats", {"a": 0}, block=True)

openpilot/selfdrive/assets/icons_mici/chestnut_orange.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:845c40ff0d37612e8f2f482a36845744b5ae91ce2fcfc8117990d7d278b59820
+size 13079

openpilot/selfdrive/assets/icons_mici/egpu_crossed.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:8a8c5fece2a1c7587feb41cbe04c6aee08e768ecd9b5d00da6af9832a4ccc842
-size 2034

openpilot/selfdrive/assets/icons_mici/egpu_gray.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:7409c53d7c72681c24982fd83b56ce70f80797c9c0f936d9296a5c18557ac472
-size 7279

openpilot/selfdrive/assets/icons_mici/egpu_orange.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:58bd6155433f623b1f75d134bd8ca4745d9aa71f6767eb807cdbcf7deb3089a1
-size 10876

openpilot/selfdrive/assets/icons_mici/usb.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:07bda2fe5d6be0b2854044053c384fe002e96406da119863a443b9344258b500
+size 1544

openpilot/selfdrive/car/card.py

@@ -21,6 +21,7 @@ from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
 from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
 from openpilot.selfdrive.car.cruise import VCruiseHelper
 from openpilot.selfdrive.car.helpers import convert_carControlSP, convert_to_capnp
+from openpilot.selfdrive.car.ford_pscm_status import populate_ford_pscm_status
 
 from openpilot.sunnypilot.mads.helpers import set_alternative_experience, set_car_specific_params
 from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
@@ -198,6 +199,7 @@ class Car:
     # Update carState from CAN
     CS, CS_SP = self.CI.update(can_list)
     CS_SP = convert_to_capnp(CS_SP)
+    populate_ford_pscm_status(self.CP, self.CI.can_parsers, CS_SP, CS.canValid)
 
     # Update radar tracks from CAN
     RD: structs.RadarDataT | None = self.RI.update(can_list)

openpilot/selfdrive/car/ford_pscm_status.py

@@ -0,0 +1,36 @@
+"""Publish the Ford PSCM's actual CAN status without changing opendbc structs."""
+import math
+
+from opendbc.car import Bus
+from opendbc.car.ford.values import FordFlags
+
+
+MESSAGE = 'Lane_Assist_Data3_FD1'
+SIGNALS = ('LatCtlSte_D_Stat', 'LatCtlLim_D_Stat', 'LatCtlCpblty_D_Stat', 'LaActDeny_B_Actl')
+
+
+def populate_ford_pscm_status(CP, can_parsers, CS_SP, can_valid):
+  if CP.brand != 'ford' or not CP.flags & FordFlags.CANFD:
+    return
+  status = CS_SP.init('fordPscmStatus')
+  parser = can_parsers.get(Bus.pt)
+  if parser is None:
+    return
+  values = parser.vl.get(MESSAGE, {})
+  timestamps = parser.ts_nanos.get(MESSAGE, {})
+  if any(signal not in values or signal not in timestamps for signal in SIGNALS):
+    return
+  received = timestamps[SIGNALS[0]]
+  if received <= 0 or any(timestamps[signal] != received for signal in SIGNALS):
+    return
+  decoded = [values[signal] for signal in SIGNALS]
+  if any(not math.isfinite(value) or int(value) != value or not 0 <= value <= maximum
+         for value, maximum in zip(decoded, (7, 3, 3, 1), strict=True)):
+    return
+  status.canMonoTime = received
+  status.lateralState, status.limit, status.capability = map(int, decoded[:3])
+  status.denied = bool(decoded[3])
+  # CI.update already checked all parser validity. Reading can_valid again here
+  # would advance the parser's invalid-message counter a second time per tick.
+  # Age is evaluated by the feedback consumer using this original CAN timestamp.
+  status.valid = bool(can_valid)

openpilot/selfdrive/car/helpers.py

@@ -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

openpilot/selfdrive/car/tests/test_ford_pscm_status.py

@@ -0,0 +1,109 @@
+import ast
+from pathlib import Path
+from types import SimpleNamespace
+import unittest
+
+from openpilot.cereal import custom
+from openpilot.selfdrive.car.ford_pscm_status import MESSAGE, SIGNALS, populate_ford_pscm_status
+from openpilot.selfdrive.car.helpers import convert_to_capnp
+from opendbc.can import CANPacker, CANParser
+from opendbc.car import Bus, structs
+from opendbc.car.ford.values import FordFlags
+
+
+class TestFordPscmStatus(unittest.TestCase):
+  def setUp(self):
+    self.cp = SimpleNamespace(brand='ford', flags=FordFlags.CANFD)
+    self.packer = CANPacker('ford_lincoln_base_pt')
+    self.parser = CANParser('ford_lincoln_base_pt', [(MESSAGE, 33), ('Yaw_Data_FD1', 100)], 0)
+
+  def update_status(self, timestamp, *, lateral_state=2, limit=0, capability=2, denied=False):
+    status = self.packer.make_can_msg(MESSAGE, 0, dict(zip(SIGNALS, (lateral_state, limit, capability, denied), strict=True)))
+    yaw = self.packer.make_can_msg('Yaw_Data_FD1', 0, {'VehYaw_W_Actl': 0.1})
+    self.parser.update([(timestamp, [status, yaw])])
+
+  def publish(self, *, can_valid=True):
+    state_sp = convert_to_capnp(structs.CarStateSP(speedLimit=13.5))
+    populate_ford_pscm_status(self.cp, {Bus.pt: self.parser}, state_sp, can_valid)
+    return state_sp
+
+  def test_decodes_status_and_preserves_receipt_time_across_other_can_messages(self):
+    self.update_status(1_000_000_000, limit=2, capability=1, denied=True)
+    original = self.publish()
+    self.assertEqual(original.speedLimit, 13.5)
+    status = original.fordPscmStatus
+    self.assertTrue(status.valid)
+    self.assertEqual(status.canMonoTime, 1_000_000_000)
+    self.assertEqual((status.lateralState, status.limit, status.capability, status.denied), (2, 2, 1, True))
+
+    # carStateSP may publish at 100 Hz while this 33 Hz message is absent. New
+    # unrelated CAN must not freshen the timestamp of an old PSCM status.
+    yaw = self.packer.make_can_msg('Yaw_Data_FD1', 0, {'VehYaw_W_Actl': .2})
+    self.parser.update([(1_080_000_000, [yaw])])
+    copied = self.publish().fordPscmStatus
+    self.assertEqual(copied.canMonoTime, 1_000_000_000)
+    self.assertEqual((copied.limit, copied.capability, copied.denied), (2, 1, True))
+
+    self.update_status(1_090_000_000, lateral_state=3, limit=3, capability=2)
+    next_state = self.publish()
+    with custom.CarStateSP.from_bytes(next_state.to_bytes()) as decoded:
+      latest = decoded.fordPscmStatus
+      self.assertTrue(latest.valid)
+      self.assertEqual(latest.canMonoTime, 1_090_000_000)
+      self.assertEqual((latest.lateralState, latest.limit, latest.capability, latest.denied), (3, 3, 2, False))
+
+  def test_absent_parser_unseen_message_and_invalid_can_do_not_claim_valid_status(self):
+    state = custom.CarStateSP.new_message()
+    populate_ford_pscm_status(self.cp, {}, state, True)
+    self.assertFalse(state.fordPscmStatus.valid)
+    self.assertEqual(state.fordPscmStatus.canMonoTime, 0)
+    self.assertFalse(self.publish().fordPscmStatus.valid)
+    self.update_status(1_000_000_000)
+    invalid = self.publish(can_valid=False).fordPscmStatus
+    self.assertFalse(invalid.valid)
+    self.assertEqual(invalid.canMonoTime, 1_000_000_000)
+
+  def test_mixed_timestamps_or_malformed_status_cannot_enable_feedback(self):
+    self.update_status(1_000_000_000)
+    self.parser.ts_nanos[MESSAGE][SIGNALS[-1]] = 990_000_000
+    self.assertFalse(self.publish().fordPscmStatus.valid)
+    self.parser.ts_nanos[MESSAGE][SIGNALS[-1]] = 1_000_000_000
+    for value in (float('nan'), -1, 1.5, 4):
+      self.parser.vl[MESSAGE]['LatCtlLim_D_Stat'] = value
+      self.assertFalse(self.publish().fordPscmStatus.valid)
+
+  def test_other_vehicles_and_legacy_messages_default_to_unavailable(self):
+    for cp in (SimpleNamespace(brand='toyota'), SimpleNamespace(brand='ford', flags=0)):
+      state = custom.CarStateSP.new_message(speedLimit=10.)
+      populate_ford_pscm_status(cp, {}, state, True)
+      self.assertFalse(state.fordPscmStatus.valid)
+      self.assertEqual(state.fordPscmStatus.canMonoTime, 0)
+      self.assertEqual(state.speedLimit, 10.)
+    # Old recordings/readers have no appended status pointer; defaults must
+    # remain unavailable rather than interpreting zeroed enums as fresh data.
+    self.assertFalse(custom.CarStateSP.new_message().fordPscmStatus.valid)
+
+  def test_actual_card_update_populates_status_after_dataclass_conversion(self):
+    self.update_status(1_000_000_000, limit=1)
+    source_path = Path(__file__).resolve().parents[1] / 'card.py'
+    source = ast.parse(source_path.read_text())
+    car_class = next(n for n in source.body if isinstance(n, ast.ClassDef) and n.name == 'Car')
+    method = next(n for n in car_class.body if isinstance(n, ast.FunctionDef) and n.name == 'state_update')
+    statements = method.body
+    first = next(i for i, n in enumerate(statements) if isinstance(n, ast.Assign) and ast.unparse(n.value) == 'self.CI.update(can_list)')
+    last = next(i for i, n in enumerate(statements) if isinstance(n, ast.Expr) and isinstance(n.value, ast.Call)
+                and isinstance(n.value.func, ast.Name) and n.value.func.id == 'populate_ford_pscm_status')
+    self.assertGreater(last, first)
+    code = compile(ast.Module(body=statements[first:last + 1], type_ignores=[]), str(source_path), 'exec')
+    ci = SimpleNamespace(update=lambda _: (SimpleNamespace(canValid=True), structs.CarStateSP(speedLimit=11.)),
+                         can_parsers={Bus.pt: self.parser})
+    environment = {'self': SimpleNamespace(CP=self.cp, CI=ci), 'can_list': [], 'convert_to_capnp': convert_to_capnp,
+                   'populate_ford_pscm_status': populate_ford_pscm_status}
+    exec(code, environment)
+    self.assertTrue(environment['CS_SP'].fordPscmStatus.valid)
+    self.assertEqual(environment['CS_SP'].fordPscmStatus.canMonoTime, 1_000_000_000)
+    self.assertEqual(environment['CS_SP'].fordPscmStatus.limit, 1)
+
+
+if __name__ == '__main__':
+  unittest.main()

openpilot/selfdrive/controls/controlsd.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 import math
+import time
 from numbers import Number
 
 from openpilot.cereal import log
@@ -11,8 +12,11 @@ from openpilot.common.realtime import config_realtime_process, DT_CTRL, Priority
 from openpilot.common.swaglog import cloudlog
 
 from opendbc.car.car_helpers import interfaces
+from opendbc.car.ford.values import FordFlags, FordFlagsSP
 from opendbc.car.vehicle_model import VehicleModel
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, select_model_action_controller
+from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController, FordPscmObserverPathController
 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
@@ -44,7 +48,7 @@ class Controls(ControlsExt):
     self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP)
 
     self.sm = messaging.SubMaster(['lateralDelay', 'vehicleParameters', 'lateralTorqueParameters', 'modelV2', 'selfdriveState',
-                                   'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput',
+                                   'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carStateSP', 'carOutput',
                                    'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + self.sm_services_ext,
                                   poll='selfdriveState')
     self.pm = messaging.PubMaster(['carControl', 'controlsState'] + self.pm_services_ext)
@@ -52,6 +56,15 @@ class Controls(ControlsExt):
     self.steer_limited_by_safety = False
     self.curvature = 0.0
     self.desired_curvature = 0.0
+    self.ford_pscm_observer = (self.CP.brand == "ford" and self.CP.flags & FordFlags.CANFD and
+                               self.params.get_bool("FordPscmObserver"))
+    self.ford_path_controller = FordPscmObserverPathController() if self.ford_pscm_observer else FordPathController()
+    self.ford_path_controller = select_model_action_controller(self.CP, bool(self.CP_SP.flags & FordFlagsSP.MODEL_ACTION),
+                                                              self.ford_path_controller)
+    self.ford_model_action = isinstance(self.ford_path_controller, FordModelActionController)
+    if self.CP.brand == "ford":
+      cloudlog.event("Ford path controller selected", controller=type(self.ford_path_controller).__name__)
+    self.ford_path = FordPath()
 
     self.pose_calibrator = PoseCalibrator()
     self.calibrated_pose: Pose | None = None
@@ -155,6 +168,38 @@ class Controls(ControlsExt):
       actuators.curvature = float(lateral_output)
     else:
       actuators.steeringAngleDeg = float(lateral_output)
+    if self.CP.brand == "ford":
+      ford_model = model_v2 if self.sm.valid['modelV2'] else None
+      if self.ford_model_action:
+        assert isinstance(self.ford_path_controller, FordModelActionController)
+        reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2'
+        now = time.monotonic()
+        self.ford_path = self.ford_path_controller.update(
+          ford_model, self.desired_curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=now,
+          measurement_time=self.sm.logMonoTime['carState'] * 1e-9,
+          model_time=self.sm.logMonoTime['modelV2'] * 1e-9,
+          reference_time=self.sm.logMonoTime[reference_service] * 1e-9,
+          reference_source=reference_service,
+          active=CC.latActive, valid=CS.canValid and self.sm.all_checks(['carState', 'vehicleParameters', 'modelV2', reference_service]),
+        )
+        if not self.ford_path.valid:
+          CC.latActive = False
+        if self.sm.frame % 20 == 0:
+          cloudlog.event("Ford C2-free path tracking", model_mono_time=self.sm.logMonoTime['modelV2'],
+                         measurement_mono_time=self.sm.logMonoTime['carState'],
+                         reference_service=reference_service, reference_mono_time=self.sm.logMonoTime[reference_service],
+                         measured_curvature=self.curvature,
+                         **self.ford_path_controller.diagnostics)
+      elif self.ford_pscm_observer:
+        assert isinstance(self.ford_path_controller, FordPscmObserverPathController)
+        self.ford_path = self.ford_path_controller.update(ford_model, self.desired_curvature,
+                                                          current_curvature=self.curvature, v_ego=CS.vEgo,
+                                                          v_ego_raw=CS.vEgoRaw, active=CC.latActive)
+      else:
+        self.ford_path = self.ford_path_controller.update(ford_model, self.desired_curvature,
+                                                          current_curvature=self.curvature, v_ego=CS.vEgo,
+                                                          active=CC.latActive)
+      actuators.curvature = float(self.ford_path.curvature)
     # Ensure no NaNs/Infs
     for p in ACTUATOR_FIELDS:
       attr = getattr(actuators, p)

openpilot/selfdrive/controls/lib/ford_model_action.py

@@ -0,0 +1,163 @@
+"""Experimental Ford model-point controller with earlier C1 release.
+
+The one-second preview and seven-metre minimum are engineering choices,
+not identified Ford reference points or PSCM calibration.
+"""
+import math
+import struct
+
+import numpy as np
+
+from opendbc.car.ford.values import FordFlags
+from openpilot.selfdrive.controls.lib.ford_path import FordPath, _model_path
+
+
+MIN_STATION_M = 7.0
+PREVIEW_TIME_S = 1.0
+CALIBRATION_APPROVED = False
+
+
+def _packed(value, resolution, offset):
+  """Mirror Float32 carControlSP and sign-reversed CANPacker rounding."""
+  value = struct.unpack("f", struct.pack("f", value))[0]
+  return -(math.floor((-value - offset) / resolution + 0.5) * resolution + offset)
+
+
+def _finite(*values):
+  try:
+    return all(math.isfinite(value) for value in values)
+  except (TypeError, ValueError, OverflowError):
+    return False
+
+
+def encode_model_action(model, desired_curvature, speed):
+  """Sample a model pose and unload C1 when its path is straightening ahead.
+
+  Use the model's distance at one second, with a seven-metre minimum and an
+  endpoint hold. Terminal spatial curvature bounds the heading contribution
+  toward zero. A constant heading slope in sampled arc distance retains C1;
+  decreasing curvature can release C1 before that heading returns to zero.
+  This is an experimental request mapping, not an identified PSCM response.
+  Selected curvature remains a health/diagnostic input only.
+  """
+  if not _finite(desired_curvature, speed) or not .3 <= speed <= 55 or abs(desired_curvature) > 1:
+    return FordPath()
+  try:
+    path = _model_path(model)
+    times = [float(t) for t in model.position.t]
+    heading_times = [float(t) for t in model.orientation.t]
+  except (AttributeError, TypeError, ValueError, OverflowError):
+    return FordPath()
+  if path is None or not all(_finite(*values) for values in path):
+    return FordPath()
+  station, _, lateral, heading = path
+  if (len(times) != len(station) or not times or times[0] != 0. or times != heading_times or
+      not _finite(*times) or any(b <= a for a, b in zip(times, times[1:], strict=False))):
+    return FordPath()
+  sample_station = min(station[-1], max(MIN_STATION_M, float(np.interp(PREVIEW_TIME_S, times, station))))
+  c0 = float(np.interp(sample_station, station, lateral))
+  c1 = float(np.interp(sample_station, station, heading))
+  # Use the same enclosing model segment as the pose interpolation. At an
+  # exact knot use its incoming segment; an ambiguous duplicate keeps C1.
+  upper = max(1, int(np.searchsorted(station, sample_station, side='left')))
+  span = station[upper] - station[upper - 1]
+  duplicate = (upper + 1 < len(station) and station[upper] == sample_station == station[upper + 1])
+  if span > 0. and not duplicate:
+    terminal_heading = heading[0] + sample_station * ((heading[upper] - heading[upper - 1]) / span)
+    if _finite(terminal_heading):
+      # One-sided: never amplify C1 or invent a reversal ahead of the model.
+      direction = math.copysign(1., c1)
+      c1 = direction * float(np.clip(direction * terminal_heading, 0., abs(c1)))
+  return FordPath(True, c0, c1, 0., 0.) if _finite(c0, c1) else FordPath()
+
+
+class ModelActionController:
+  """Only two states: independently slewed C0/C1 model-point requests."""
+  __slots__ = ('c0', 'c1')
+
+  def __init__(self):
+    self.reset()
+
+  def reset(self):
+    self.c0 = self.c1 = 0.
+
+  def update(self, model, desired_curvature, *, speed, dt, yaw_rate=0., active=True, valid=True):
+    # Raw Ford yaw remains an input-health check, not a pose measurement.
+    if not active or not valid or not _finite(dt, yaw_rate) or not .002 <= dt <= .1 or abs(yaw_rate) > 3:
+      self.reset()
+      return FordPath()
+    target = encode_model_action(model, desired_curvature, speed)
+    if not target.valid:
+      self.reset()
+      return FordPath()
+    c0 = float(np.clip(target.path_offset, -5.11, 5.11))
+    c1 = float(np.clip(target.path_angle, -.5, .5))
+    self.c0 += float(np.clip(c0-self.c0, -4.*dt, 4.*dt))
+    self.c1 += float(np.clip(c1-self.c1, -.5*dt, .5*dt))
+    return FordPath(True, _packed(self.c0, .01, -5.12), _packed(self.c1, .0005, -.5), 0., 0.)
+
+
+class FordModelActionController:
+  """Freshness, engagement and reference checks for model-point tracking.
+
+  Scalar-only maneuver references cannot supply this controller's model pose.
+  Reject them explicitly rather than silently following a different reference.
+  Measured yaw checks input health only; it never modifies valid geometry.
+  """
+  def __init__(self):
+    self.core = ModelActionController()
+    self.reset()
+
+  def reset(self, status='inactive'):
+    self.core.reset()
+    self.last_time = self.last_measurement_time = self.last_model_time = None
+    self.diagnostics = {'status': status, 'hypothesis': 'model-pose-terminal-c1-v1',
+                        'calibration_approved': CALIBRATION_APPROVED, 'command': (0., 0., 0., 0.)}
+
+  def update(self, model, desired_curvature, *, yaw_rate, speed, now, measurement_time, model_time, reference_time,
+             active, valid=True, reference_source="modelV2"):
+    reason = None
+    if not active:
+      reason = 'inactive'
+    elif reference_source != 'modelV2':
+      reason = 'unsupported_reference'
+    elif not valid:
+      reason = 'invalid_service'
+    elif not _finite(desired_curvature, yaw_rate, speed, now, measurement_time, model_time, reference_time):
+      reason = 'nonfinite'
+    elif not all(-.005 <= now - timestamp <= .15 for timestamp in (measurement_time, model_time, reference_time)):
+      reason = 'stale_input'
+    elif not .3 <= speed <= 55 or abs(yaw_rate) > 3 or abs(desired_curvature) > 1:
+      reason = 'input_range'
+    if reason is not None:
+      self.reset(reason)
+      return FordPath()
+
+    dt = .01 if self.last_time is None else now - self.last_time
+    if not .002 <= dt <= .1 or (self.last_measurement_time is not None and measurement_time < self.last_measurement_time) or (
+      self.last_model_time is not None and model_time < self.last_model_time
+    ):
+      self.reset('timing_reset')
+      return FordPath()
+    command = self.core.update(model, desired_curvature, speed=speed, dt=dt, yaw_rate=yaw_rate)
+    if not command.valid:
+      self.reset('invalid_path')
+      return command
+    self.last_time, self.last_measurement_time, self.last_model_time = now, measurement_time, model_time
+    self.diagnostics = {'status': 'active', 'hypothesis': 'model-pose-terminal-c1-v1',
+                        'calibration_approved': CALIBRATION_APPROVED, 'desired_curvature': desired_curvature,
+                        'yaw_rate': yaw_rate, 'pose_source': 'model',
+                        'preview_time_s': PREVIEW_TIME_S, 'minimum_station_m': MIN_STATION_M,
+                        'c1_release': 'terminal_spatial_curvature',
+                        'model_age': now - model_time, 'measurement_age': now - measurement_time, 'reference_age': now - reference_time,
+                        'dt': dt, 'offset_request': self.core.c0, 'heading_request': self.core.c1,
+                        'command': (command.path_offset, command.path_angle, 0., 0.)}
+    return command
+
+
+def select_model_action_controller(CP, enabled, previous_controller):
+  """The separate default-off toggle takes priority on the CAN FD Lightning."""
+  compatible = CP.brand == 'ford' and CP.flags & FordFlags.CANFD and CP.carFingerprint == 'FORD_F_150_LIGHTNING_MK1'
+  if enabled and compatible:
+    return FordModelActionController()
+  return previous_controller

openpilot/selfdrive/controls/lib/ford_path.py

@@ -0,0 +1,368 @@
+from collections import deque
+from dataclasses import dataclass
+import math
+
+import numpy as np
+
+from opendbc.car.ford.values import CarControllerParams
+
+
+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)
+
+DBC_OFFSET_RESOLUTION = 0.01
+DBC_ANGLE_RESOLUTION = 0.0005
+DBC_CURVATURE_RESOLUTION = 0.00002
+DBC_CURVATURE_RATE_RESOLUTION = 0.000001
+_PATH_MIN_LOOKAHEAD = 7.0
+_POSE_PREDICTION_TIME = 0.1
+_POSE_BLEND_CURVATURE = (0.006, 0.012)
+_PATH_OFFSET_RATE = 4.0
+_PATH_ANGLE_RATE = 1.0
+
+_PSCM_DT = 0.004
+_PSCM_C0_RATE = 1.5
+_PSCM_C1_RATE = 0.100006103515625
+_PSCM_C2_RATE = 0.0030059814453125
+_PSCM_SPEED_KPH = (0.0, 15.0, 40.0, 70.0, 100.0, 150.0, 200.0, 250.0)
+_PSCM_SPEED_GAIN = (32.0, 32.0, 32.0, 30.0, 30.0, 24.0, 12.0, 0.0)
+_PSCM_C0_EFFECTIVE_LIMIT = 1.0
+_PSCM_C1_EFFECTIVE_LIMIT = 0.349609375 / 10.0
+
+
+@dataclass(frozen=True)
+class FordPath:
+  valid: bool = False
+  path_offset: float = 0.0
+  path_angle: float = 0.0
+  curvature: float = 0.0
+  curvature_rate: float = 0.0
+
+
+@dataclass(frozen=True)
+class FordPscmState:
+  path_offset: float = 0.0
+  path_angle: float = 0.0
+  curvature: float = 0.0
+
+
+@dataclass(frozen=True)
+class FordModelPose:
+  path_offset: float
+  path_angle: float
+  offset_horizon: float
+  curvature_demand: float
+  forward_angle: float
+
+
+def _finite(value: float) -> float:
+  return float(value) if math.isfinite(value) else 0.0
+
+
+def _sample(distance: float, distances: list[float], values: list[float]) -> float:
+  return float(np.interp(distance, distances, values))
+
+
+def _blend_share(demand: float) -> float:
+  lower, upper = _POSE_BLEND_CURVATURE
+  return float(np.clip((demand - lower) / (upper - lower), 0.0, 1.0))
+
+
+def _model_path(model) -> tuple[list[float], list[float], list[float], 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, x, y, unwrapped_heading
+
+
+def _predicted_pose(distance: float, current_curvature: float,
+                    curvature_delta: float) -> tuple[float, float, float]:
+  curvature = current_curvature + 0.5 * curvature_delta
+  heading = curvature * distance
+  if abs(curvature) < 1e-9:
+    return distance, 0.0, 0.0
+  return math.sin(heading) / curvature, (1.0 - math.cos(heading)) / curvature, heading
+
+
+def _relative_pose(target_distance: float, path: tuple[list[float], list[float], list[float], list[float]],
+                   vehicle_pose: tuple[float, float, float]) -> tuple[float, float]:
+  distance, x, y, heading = path
+  vehicle_x, vehicle_y, vehicle_heading = vehicle_pose
+  dx = _sample(target_distance, distance, x) - vehicle_x
+  dy = _sample(target_distance, distance, y) - vehicle_y
+  cosine = math.cos(vehicle_heading)
+  sine = math.sin(vehicle_heading)
+  offset = -sine * dx + cosine * dy
+  angle = math.atan2(math.sin(_sample(target_distance, distance, heading) - vehicle_heading),
+                     math.cos(_sample(target_distance, distance, heading) - vehicle_heading))
+  return offset, angle
+
+
+def _path_pose(target_distance: float,
+               path: tuple[list[float], list[float], list[float], list[float]]) -> tuple[float, float, float]:
+  distance, x, y, heading = path
+  return (_sample(target_distance, distance, x), _sample(target_distance, distance, y),
+          _sample(target_distance, distance, heading))
+
+
+def _bounded_feedback(feedforward: float, feedback: float, resolution: float, zero_path_limit: float) -> float:
+  quantization_threshold = 0.5 * resolution
+  limit = max(abs(feedforward) - resolution, 0.0) if abs(feedforward) >= quantization_threshold else zero_path_limit
+  return float(np.clip(feedback, -limit, limit))
+
+
+def _model_pose(path: tuple[list[float], list[float], list[float], list[float]],
+                current_curvature: float, curvature_delta: float, v_ego: float) -> FordModelPose:
+  distance, _, _, _ = path
+  advance = min(v_ego * _POSE_PREDICTION_TIME, distance[-1])
+  offset_horizon = min(_PATH_MIN_LOOKAHEAD, distance[-1] - advance)
+  angle_horizon = min(max(v_ego, _PATH_MIN_LOOKAHEAD), distance[-1] - advance)
+
+  # Keep the model's remaining path as feedforward. Measured vehicle motion is
+  # a separate, short delay-aligned correction, so catching the requested
+  # curvature cannot erase a turn that is still present in the model path.
+  model_pose = _path_pose(advance, path)
+  model_offset, _ = _relative_pose(advance + offset_horizon, path, model_pose)
+  _, model_angle = _relative_pose(advance + angle_horizon, path, model_pose)
+  vehicle_pose = _predicted_pose(advance, current_curvature, curvature_delta)
+  feedback_offset, feedback_angle = _relative_pose(advance, path, vehicle_pose)
+  gentle_curvature = _POSE_BLEND_CURVATURE[0]
+  feedback_offset = _bounded_feedback(model_offset, feedback_offset, DBC_OFFSET_RESOLUTION,
+                                       0.5 * gentle_curvature * advance ** 2)
+  feedback_angle = _bounded_feedback(model_angle, feedback_angle, DBC_ANGLE_RESOLUTION,
+                                      gentle_curvature * advance)
+
+  offset_curvature = 2.0 * model_offset / max(offset_horizon, 1e-3) ** 2
+  angle_curvature = model_angle / max(angle_horizon, 1e-3)
+  return FordModelPose(model_offset + feedback_offset, model_angle + feedback_angle, offset_horizon,
+                       max(abs(offset_curvature), abs(angle_curvature)), model_angle)
+
+
+def _encode_pose(pose: FordModelPose, pose_share: float, curvature: float) -> FordPath:
+  path_offset = pose_share * pose.path_offset
+  path_angle = pose_share * pose.path_angle
+  if abs(path_offset) < 0.5 * DBC_OFFSET_RESOLUTION:
+    path_offset = 0.0
+  if abs(path_angle) < 0.5 * DBC_ANGLE_RESOLUTION:
+    path_angle = 0.0
+  limited_path_angle = float(np.clip(path_angle, *DBC_ANGLE))
+  path_offset += (path_angle - limited_path_angle) * pose.offset_horizon
+  return FordPath(
+    valid=True,
+    path_offset=float(np.clip(path_offset, *DBC_OFFSET)),
+    path_angle=limited_path_angle,
+    curvature=float(np.clip(curvature, *DBC_CURVATURE)),
+    curvature_rate=0.0,
+  )
+
+
+def _encode_path(path: tuple[list[float], list[float], list[float], list[float]], desired_curvature: float,
+                 current_curvature: float, curvature_delta: float, v_ego: float) -> FordPath:
+  pose = _model_pose(path, current_curvature, curvature_delta, v_ego)
+  pose_share = _blend_share(max(pose.curvature_demand, abs(desired_curvature)))
+
+  # Match upstream's C2-only normal driving, then continuously transfer the
+  # command to the model pose for larger maneuvers. An opposing/finished model
+  # path must unload sticky C2 and retain the fast pose needed to unwind it.
+  c2_opposes_path = desired_curvature != 0.0 and desired_curvature * pose.forward_angle <= 0.0
+  if c2_opposes_path:
+    pose_share = 1.0
+    curvature = 0.0
+  else:
+    curvature = desired_curvature * (1.0 - pose_share)
+
+  return _encode_pose(pose, pose_share, curvature)
+
+
+class FordPathController:
+  """Blend normal C2 following into the model's forward C0/C1 pose."""
+
+  def __init__(self, dt: float = 0.01):
+    self.dt = dt
+    self._last_path = FordPath(valid=True)
+    self._curvature_history = deque(maxlen=max(round(_POSE_PREDICTION_TIME / dt) + 1, 2))
+
+  def _limit(self, target: FordPath) -> FordPath:
+    offset_delta = target.path_offset - self._last_path.path_offset
+    angle_delta = target.path_angle - self._last_path.path_angle
+    scale = min(
+      1.0,
+      _PATH_OFFSET_RATE * self.dt / abs(offset_delta) if offset_delta else 1.0,
+      _PATH_ANGLE_RATE * self.dt / abs(angle_delta) if angle_delta else 1.0,
+    )
+    self._last_path = FordPath(
+      True,
+      self._last_path.path_offset + scale * offset_delta,
+      self._last_path.path_angle + scale * angle_delta,
+      self._last_path.curvature + scale * (target.curvature - self._last_path.curvature),
+      0.0,
+    )
+    return self._last_path
+
+  def update(self, model, desired_curvature: float, *, current_curvature: float = 0.0,
+             v_ego: float = 0.0, active: bool = True) -> FordPath:
+    if not active:
+      self._last_path = FordPath(valid=True)
+      self._curvature_history.clear()
+      return FordPath()
+    current_curvature = _finite(current_curvature)
+    self._curvature_history.append(current_curvature)
+    curvature_delta = (current_curvature - self._curvature_history[0]
+                       if len(self._curvature_history) == self._curvature_history.maxlen else 0.0)
+    path = _model_path(model) if model is not None else None
+    if path is None:
+      return self._limit(FordPath(valid=True))
+    return self._limit(_encode_path(path, _finite(desired_curvature), current_curvature, curvature_delta,
+                                    max(_finite(v_ego), 0.0)))
+
+
+def _pscm_slew(value: float, target: float, rate: float, ticks: int) -> float:
+  step = rate * _PSCM_DT * ticks
+  return float(np.clip(target, value - step, value + step))
+
+
+def _pscm_speed_gain(v_ego: float) -> float:
+  return float(np.interp(max(v_ego, 0.0) * 3.6, _PSCM_SPEED_KPH, _PSCM_SPEED_GAIN))
+
+
+def _wire_path(path: FordPath) -> FordPath:
+  return FordPath(
+    valid=path.valid,
+    path_offset=round(path.path_offset / DBC_OFFSET_RESOLUTION) * DBC_OFFSET_RESOLUTION,
+    path_angle=round(path.path_angle / DBC_ANGLE_RESOLUTION) * DBC_ANGLE_RESOLUTION,
+    curvature=round(path.curvature / DBC_CURVATURE_RESOLUTION) * DBC_CURVATURE_RESOLUTION,
+    curvature_rate=round(path.curvature_rate / DBC_CURVATURE_RATE_RESOLUTION) * DBC_CURVATURE_RATE_RESOLUTION,
+  )
+
+
+def _pscm_contributions(state: FordPscmState, v_ego: float) -> tuple[float, float, float]:
+  gain = _pscm_speed_gain(v_ego)
+  return (
+    float(np.clip(0.5 * gain * state.path_offset, -0.5 * gain, 0.5 * gain)),
+    float(np.clip(10.0 * gain * state.path_angle, -0.349609375 * gain, 0.349609375 * gain)),
+    float(np.clip(0.30078125 * gain * state.curvature * v_ego ** 2, -0.5 * gain, 0.5 * gain)),
+  )
+
+
+class FordPscmObserver:
+  """Mirror the firmware's held-command coefficient states at its 250 Hz step."""
+
+  def __init__(self):
+    self.state = FordPscmState()
+    self.command = FordPath(valid=True)
+    self._phase = 0.0
+
+  def reset(self) -> None:
+    self.state = FordPscmState()
+    self.command = FordPath(valid=True)
+    self._phase = 0.0
+
+  def advance(self, elapsed: float) -> None:
+    self._phase += max(elapsed, 0.0)
+    ticks = int((self._phase + 1e-12) / _PSCM_DT)
+    self._phase -= ticks * _PSCM_DT
+    if ticks == 0:
+      return
+    self.state = FordPscmState(
+      _pscm_slew(self.state.path_offset, self.command.path_offset, _PSCM_C0_RATE, ticks),
+      _pscm_slew(self.state.path_angle, self.command.path_angle, _PSCM_C1_RATE, ticks),
+      _pscm_slew(self.state.curvature, self.command.curvature + 10.0 * self.command.curvature_rate,
+                  _PSCM_C2_RATE, ticks),
+    )
+
+  def set_command(self, command: FordPath) -> None:
+    self.command = _wire_path(command)
+
+
+class FordPscmObserverPathController:
+  """Compensate model-path commands for the PSCM coefficient state it still carries."""
+
+  def __init__(self, dt: float = 0.01):
+    self.dt = dt
+    self._last_path = FordPath(valid=True)
+    self._curvature_history = deque(maxlen=max(round(_POSE_PREDICTION_TIME / dt) + 1, 2))
+    self.observer = FordPscmObserver()
+    self._sent_c2 = 0.0
+
+  def _reset(self) -> None:
+    self._last_path = FordPath(valid=True)
+    self._curvature_history.clear()
+    self.observer.reset()
+    self._sent_c2 = 0.0
+
+  def _command_for_state(self, target: FordPath, v_ego: float) -> FordPath:
+    # The target describes the desired fully-settled PSCM contribution. C0 keeps
+    # the remaining C1-saturated residual. C1 supplies the primary contribution
+    # that the known slow C2 state does not yet provide, without a guessed gain.
+    target_state = FordPscmState(target.path_offset, target.path_angle, target.curvature)
+    target_contribution = sum(_pscm_contributions(target_state, v_ego))
+    _, _, observed_c2 = _pscm_contributions(self.observer.state, v_ego)
+    gain = _pscm_speed_gain(v_ego)
+    required_fast = target_contribution - observed_c2
+    c1_contribution = float(np.clip(required_fast, -0.349609375 * gain, 0.349609375 * gain))
+    c0_contribution = required_fast - c1_contribution
+    path_offset = c0_contribution / (0.5 * gain) if gain > 0.0 else 0.0
+    path_angle = c1_contribution / (10.0 * gain) if gain > 0.0 else 0.0
+    return FordPath(
+      valid=True,
+      path_offset=float(np.clip(path_offset, -_PSCM_C0_EFFECTIVE_LIMIT, _PSCM_C0_EFFECTIVE_LIMIT)),
+      path_angle=float(np.clip(path_angle, -_PSCM_C1_EFFECTIVE_LIMIT, _PSCM_C1_EFFECTIVE_LIMIT)),
+      curvature=target.curvature,
+      curvature_rate=target.curvature_rate,
+    )
+
+  def _limit(self, target: FordPath, v_ego_raw: float) -> FordPath:
+    path_offset = float(np.clip(target.path_offset,
+                                self._last_path.path_offset - _PATH_OFFSET_RATE * self.dt,
+                                self._last_path.path_offset + _PATH_OFFSET_RATE * self.dt))
+    path_angle = float(np.clip(target.path_angle,
+                               self._last_path.path_angle - _PATH_ANGLE_RATE * self.dt,
+                               self._last_path.path_angle + _PATH_ANGLE_RATE * self.dt))
+    curvature = CarControllerParams.CURVATURE_LIMITS.apply_limits(
+      target.curvature, self._sent_c2, v_ego_raw, 0.0, True, CarControllerParams.LMC2_STEP,
+    )
+    self._sent_c2 = curvature
+    self._last_path = FordPath(True, path_offset, path_angle, curvature, target.curvature_rate)
+    self.observer.set_command(self._last_path)
+    return self._last_path
+
+  def update(self, model, desired_curvature: float, *, current_curvature: float = 0.0,
+             v_ego: float = 0.0, v_ego_raw: float = 0.0, active: bool = True) -> FordPath:
+    if not active:
+      self._reset()
+      return FordPath()
+
+    self.observer.advance(self.dt)
+    current_curvature = _finite(current_curvature)
+    self._curvature_history.append(current_curvature)
+    curvature_delta = (current_curvature - self._curvature_history[0]
+                       if len(self._curvature_history) == self._curvature_history.maxlen else 0.0)
+    path = _model_path(model) if model is not None else None
+    if path is None:
+      target = FordPath(valid=True)
+    else:
+      target = _encode_path(path, _finite(desired_curvature), current_curvature, curvature_delta,
+                            max(_finite(v_ego), 0.0))
+    v_ego_raw = max(_finite(v_ego_raw), 0.0)
+    command = self._command_for_state(target, v_ego_raw)
+    return self._limit(command, v_ego_raw)

openpilot/selfdrive/controls/tests/fixtures/ford_c2_free_path_routes.json

@@ -0,0 +1,229 @@
+{
+  "description": "Curvature-driven C0 and full-heading C1 command regression; does not predict counterfactual wheel response. Contains geometry and control signals only, no GPS.",
+  "fixture_sha256": "12782ac1b0d0637945f729a46ad03af16cd58188872b6a65f104e32c4db70e9b",
+  "episodes": [
+    {
+      "name": "left_large",
+      "route": "84865544361f55cb_00000077--4b55791ce6",
+      "range_seconds": [
+        809.5,
+        815.0
+      ],
+      "evidence_seconds": [
+        812.1,
+        814.0
+      ],
+      "samples": 532
+    },
+    {
+      "name": "right_large",
+      "route": "84865544361f55cb_00000077--4b55791ce6",
+      "range_seconds": [
+        866.5,
+        872.0
+      ],
+      "evidence_seconds": [
+        869.5,
+        871.0
+      ],
+      "samples": 547
+    },
+    {
+      "name": "left_very_large",
+      "route": "84865544361f55cb_00000077--4b55791ce6",
+      "range_seconds": [
+        880.0,
+        884.0
+      ],
+      "evidence_seconds": [
+        882.9,
+        883.32
+      ],
+      "samples": 397
+    },
+    {
+      "name": "right_plateau",
+      "route": "84865544361f55cb_00000077--4b55791ce6",
+      "range_seconds": [
+        964.0,
+        970.0
+      ],
+      "evidence_seconds": [
+        967.2,
+        968.93
+      ],
+      "samples": 583
+    },
+    {
+      "name": "oscillation",
+      "route": "84865544361f55cb_00000078--349f5b8695",
+      "range_seconds": [
+        29.0,
+        37.0
+      ],
+      "evidence_seconds": [
+        32.5,
+        36.1
+      ],
+      "samples": 795
+    },
+    {
+      "name": "weak_first",
+      "route": "84865544361f55cb_0000007a--5a95fc717e",
+      "range_seconds": [
+        90.5,
+        95.2
+      ],
+      "evidence_seconds": [
+        93.5,
+        95.08
+      ],
+      "samples": 467
+    },
+    {
+      "name": "weak_second",
+      "route": "84865544361f55cb_0000007a--5a95fc717e",
+      "range_seconds": [
+        119.5,
+        125.3
+      ],
+      "evidence_seconds": [
+        122.5,
+        125.2
+      ],
+      "samples": 576
+    }
+  ],
+  "sources": {
+    "84865544361f55cb_00000077--4b55791ce6": [
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--0--rlog.zst",
+        "bytes": 10342855,
+        "sha256": "2f072eff3076f4d32dbc1a077f24fc85818abeafd13a4b970b1ba558a0d2ca52"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--1--rlog.zst",
+        "bytes": 11688902,
+        "sha256": "bbbbf7fc79b1c7133b83678a5202ac41cd8b3dcdd0bb358843d43fdb38f25e1e"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--2--rlog.zst",
+        "bytes": 11875525,
+        "sha256": "18d7e0224a61d068aeeef0e176da071f5e67d77fcb7ebaa7b5854756d4438add"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--3--rlog.zst",
+        "bytes": 12489313,
+        "sha256": "7869f95c87849018df07c680e0584145a31c407608f96f2fc77f35b729f02d2c"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--4--rlog.zst",
+        "bytes": 12301118,
+        "sha256": "2c2125fb2320b9bc6620fc586cedb6558b33e8475d0cce3fb361db4eea6a1c28"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--5--rlog.zst",
+        "bytes": 12976655,
+        "sha256": "b18c448786daf46cfa05bf0352396ce5851a09603d34ee5b989e096da5ee5180"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--6--rlog.zst",
+        "bytes": 13223857,
+        "sha256": "8c08b8d47ebca38c70aa94a1cdda25443ed3be94727bb487406b518471c88dd1"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--7--rlog.zst",
+        "bytes": 13043701,
+        "sha256": "638948d7e5853046773f82df8a531c518c42883c77f060e61ff683ccb59d52af"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--8--rlog.zst",
+        "bytes": 12569024,
+        "sha256": "41e5c83d2205964889579cf24967712dd0340da5f30f213409f2b1e04e6eb78a"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--9--rlog.zst",
+        "bytes": 11926213,
+        "sha256": "6034a90e817424c02755c2c0d9bdbad088c4286edfb887d9f2acbd60d7818da7"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--10--rlog.zst",
+        "bytes": 13010961,
+        "sha256": "23958a1ac8277977952c73e889fbfd9245bc2cf82b3af58233c245dbf1e76545"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--11--rlog.zst",
+        "bytes": 13204208,
+        "sha256": "f78d65b7b5927a8570f52d765f9af45431f8ab72260987f718321ad1b03c70af"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--12--rlog.zst",
+        "bytes": 12562994,
+        "sha256": "485621d1cf71605fccc8c679cb146b1f0954078849db7c74b1b65b166d741025"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--13--rlog.zst",
+        "bytes": 12610836,
+        "sha256": "0b4b0c01caae39a7dc4ff2219ab168029ea12c43b1966b8c204972743627c24b"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--14--rlog.zst",
+        "bytes": 13114068,
+        "sha256": "f18769800bd08c7614280916b50ec0274eca9b4765ad26d6549b72912321dac0"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--15--rlog.zst",
+        "bytes": 13113162,
+        "sha256": "abf0adc702db6f5fdd78a145709504c7061255bc9b9a61329a575707d5bd2f74"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--16--rlog.zst",
+        "bytes": 13029294,
+        "sha256": "a76ed74b889bda60e2d929123186df83653591d1e058fbd1497551fbbf23941e"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--17--rlog.zst",
+        "bytes": 12429846,
+        "sha256": "51d3aff2afa1a7ce3c5372a499b14394f22ac730e952273a69496c65ef726ed2"
+      },
+      {
+        "name": "84865544361f55cb_00000077--4b55791ce6--18--rlog.zst",
+        "bytes": 9241845,
+        "sha256": "2a1646227f9cdb1ab7eaf79fa653b7f4443a629a703577ff6c38a715dbced3a4"
+      }
+    ],
+    "84865544361f55cb_00000078--349f5b8695": [
+      {
+        "name": "84865544361f55cb_00000078--349f5b8695--0--rlog.zst",
+        "bytes": 10919702,
+        "sha256": "2d35f6c9ac9b09f8b86b3fe2fbe864e4af0d50c411e3dd56572b5aa113bf1973"
+      },
+      {
+        "name": "84865544361f55cb_00000078--349f5b8695--1--rlog.zst",
+        "bytes": 10600112,
+        "sha256": "6cabb48ea0caeb2fddd58be35b5c7e7c42faf87fc01991c4603573bffe33eaeb"
+      }
+    ],
+    "84865544361f55cb_0000007a--5a95fc717e": [
+      {
+        "name": "84865544361f55cb_0000007a--5a95fc717e--0--rlog.zst",
+        "bytes": 10841486,
+        "sha256": "bfc9e3308e5241e76cba57f2441043710fe8b18ade314221ee5e423f640c211a"
+      },
+      {
+        "name": "84865544361f55cb_0000007a--5a95fc717e--1--rlog.zst",
+        "bytes": 12567837,
+        "sha256": "ab9eb05c5805e286a6ec639bbbbc1cf086bfcf1b440801ad000713db95dfa7fc"
+      },
+      {
+        "name": "84865544361f55cb_0000007a--5a95fc717e--2--rlog.zst",
+        "bytes": 11015857,
+        "sha256": "af4f87f39be37f0a5b23b58de50c2ed8801bdbda6544cd6cd292525fc3bd4fb3"
+      }
+    ]
+  },
+  "pairing": "controlsState cycle time; causal carState speed/yaw/pressed; exact consumed model timestamp and geometry; nearest same-cycle carControl and carControlSP within 5ms.",
+  "yaw_rate": "Negative carState.yawRate, matching the model/control curvature coordinate sign; no wheel-to-curvature conversion.",
+  "desired_curvature": "Exact controlsState.desiredCurvature from the matching controlsState cycle. This is the post-selection, post-limiting request consumed by controlsd; it is not a wheel-angle-to-curvature fit.",
+  "reference_time": "Exact consumed modelV2 publication time, in the same relative seconds as each episode. The extraction cache does not retain consumed lateralManeuverPlan timestamps or validity; model time is an explicit replay assumption and cannot verify alternate-reference freshness."
+}

openpilot/selfdrive/controls/tests/fixtures/ford_curvature_heading_route80.json

@@ -0,0 +1,67 @@
+{
+  "description": "Real route80 turn-command regressions. Signal-only fixture; no GPS. Counterfactual commands do not predict physical vehicle response.",
+  "route": "84865544361f55cb_00000080--1643deea7e",
+  "source_commit": "98662df401217a00ec9fc8e73b16857b6c220150",
+  "frozen_v3_controller_sha256": "576f4ec6f2dbc93f7e6c93a69839f69447eb5a0c2f834bd48b24f84a163dc2eb",
+  "fixture_sha256": "c1460e2cf1d3fd52b1a036d923fec7835a7d361126ee0c2decbc3f101ee6653c",
+  "episodes": [
+    {
+      "name": "under_333_339",
+      "range_seconds": [
+        331.5,
+        339.0
+      ],
+      "evidence_seconds": [
+        333.0,
+        339.0
+      ],
+      "samples": 745
+    },
+    {
+      "name": "over_417_420",
+      "range_seconds": [
+        415.5,
+        420.0
+      ],
+      "evidence_seconds": [
+        417.0,
+        420.0
+      ],
+      "samples": 447
+    },
+    {
+      "name": "under_430_435",
+      "range_seconds": [
+        428.5,
+        435.0
+      ],
+      "evidence_seconds": [
+        430.0,
+        435.0
+      ],
+      "samples": 646
+    }
+  ],
+  "sources": [
+    {
+      "name": "84865544361f55cb_00000080--1643deea7e--5--rlog.zst",
+      "bytes": 12531711,
+      "sha256": "059482830794cb0eabe6069b75a9610b900bf2a93d7a6624f53c575cef997157"
+    },
+    {
+      "name": "84865544361f55cb_00000080--1643deea7e--6--rlog.zst",
+      "bytes": 12560505,
+      "sha256": "147276789f5b14913adc4cd16db18f3d4bd27ce8497c9ff96fdf0315c219339f"
+    },
+    {
+      "name": "84865544361f55cb_00000080--1643deea7e--7--rlog.zst",
+      "bytes": 12660797,
+      "sha256": "b311b6ace75819db52b9618154d68c7d12e2751d5046b6d174adb89ef87a223c"
+    }
+  ],
+  "pairing": "Exact controlsState desiredCurvature and consumed model publication timestamp; causal carState speed, negative CAN yaw, and steeringPressed; nearest same-cycle carControl/carControlSP within 5 ms.",
+  "reference_time": "Consumed modelV2 publication time. Controller audit confirms route80 used modelV2 as reference throughout.",
+  "preroll": "Each episode starts from reset 1.5 s before evidence; v3_replay stores those exact cold-start commands and gates, while recorded stores original live path fields.",
+  "benchmark_clean": "Existing route80 benchmark mask: whole interval request minus 0.5 s through response (0.2 s) plus 0.25 s active, unpressed, valid, fresh, and speed >= 2 m/s.",
+  "expected_common_c1": "Independent shadow: clip(desiredCurvature * max(7 m, vEgo * 1 s), +/-0.5 rad), independently slewed at 0.5 rad/s and packed to Float32/sign-reversed CAN semantics. No subtraction of measured curvature."
+}

openpilot/selfdrive/controls/tests/fixtures/ford_heading_feedback_route80_status.json

@@ -0,0 +1,13 @@
+{
+  "description": "PSCM status and raw driver-torque overlay for the existing three route80 request windows. No GPS. No counterfactual vehicle response.",
+  "fixture_sha256": "a9defdc5abdf26724358d606beb16becbdf30faa972974d49b179a9e004d7629",
+  "base_fixture": "ford_curvature_heading_route80.npz",
+  "base_fixture_sha256": "c1460e2cf1d3fd52b1a036d923fec7835a7d361126ee0c2decbc3f101ee6653c",
+  "source_route": "84865544361f55cb_00000080--1643deea7e",
+  "source_commit": "98662df401217a00ec9fc8e73b16857b6c220150",
+  "samples": 1838,
+  "source_cache_sha256": "1cd3e0c00805869ace1c5954dc682644f36f5eddb71785b69e4cb9da40f7f04f",
+  "pairing": "Latest actual bus-0 EPS 972 frame at or before each controlsState cycle; raw steering torque from the exact causal carState used by the base fixture.",
+  "timestamp_policy": "Actual CAN event logMonoTime in route-relative seconds, not the benchmark response-shifted status. The old route predates the new carStateSP status telemetry; source CAN timestamps are an explicit replay approximation.",
+  "validity": "Replay validity uses the paired carState valid and canValid values; enum validity, availability and age are checked by the production feedback controller."
+}

openpilot/selfdrive/controls/tests/fixtures/ford_heading_recovery_requests.json

@@ -0,0 +1,40 @@
+{
+  "description": "Signal-only v6 turn-exit recovery regression; no location, device identity, or predicted new vehicle response.",
+  "recorded_controller_revision": "61dac4977bf9c36504398e8a4959dfed79cf6f05",
+  "baseline_revision": "61dac4977bf9c36504398e8a4959dfed79cf6f05",
+  "response_delay": 0.20000000298023224,
+  "samples": 9134,
+  "models": 1843,
+  "fixture_sha256": "41d5e3efcee03a9e02fcaf7bf456c050c6a671a5b7f7fc9706ddf4d27bad71b8",
+  "windows": [
+    {
+      "name": "overturn_then_underturn",
+      "range_s": [
+        19.99615067150053,
+        29.48070058550053
+      ],
+      "samples": 521
+    },
+    {
+      "name": "well_tracked_curve_a",
+      "range_s": [
+        56.19474309950053,
+        63.68808603150053
+      ],
+      "samples": 729
+    },
+    {
+      "name": "well_tracked_curve_b",
+      "range_s": [
+        101.19780691350051,
+        116.33885445450052
+      ],
+      "samples": 810
+    }
+  ],
+  "selection": "One previously identified overturn-then-underturn event and two previously reported well-tracked curves; selected before recovery implementation.",
+  "mask": "Whole t-0.5 through t+0.65 interval active, valid, fresh, unpressed, raw driver torque magnitude <=1 Nm; requested |curvature|*speed\u00b2 >=.5 m/s\u00b2.",
+  "timing": "Exact consumed model publication; causal CAN/PSCM at estimated control computation time. Subtract observed median computation-to-publication delay; unsampled tick timing remains approximate.",
+  "context": "At least 20 seconds prior context or the available start, extended before the latest observed reset. Overlapping episodes are merged.",
+  "coordinates": "Times are local elapsed seconds; models contain only relative position.x/y and orientation.z arrays."
+}

openpilot/selfdrive/controls/tests/fixtures/ford_large_turn_requests_route83.json

@@ -0,0 +1,247 @@
+{
+  "description": "Signal-only historical fallback evidence and frozen-v5 comparison; no GPS or inferred counterfactual vehicle response.",
+  "route": "route83",
+  "recorded_commit": "79a4caa1f6b71488949108aee9ae6ae6566347b1",
+  "fixture_sha256": "d00312c430ace47000c05b8284ee8d56df56ec24bb17a9ea8f4dce83133527c3",
+  "samples": 11744,
+  "model_count": 2367,
+  "source_cache_sha256": "53d786aff2e0b6338e1991320145305fda3101f7b76e50bd2929adbcaea95b28",
+  "response_delay": 0.20000000298023224,
+  "episodes": [
+    [
+      1861.2933736250002,
+      1874.756970279
+    ],
+    [
+      1878.07372132,
+      1892.07372132
+    ],
+    [
+      1950.874232409,
+      1964.874232409
+    ],
+    [
+      2440.9020600930003,
+      2456.964158177
+    ],
+    [
+      2580.722658577,
+      2611.364366768
+    ],
+    [
+      2734.478264791,
+      2764.574374172
+    ]
+  ],
+  "windows": [
+    {
+      "name": "successful_large_early",
+      "role": "authority_target",
+      "range_s": [
+        1866.722720383,
+        1874.756970279
+      ],
+      "samples": 426,
+      "substantial_demand_required": true,
+      "recorded_can_ratio_02s_median": 1.0233371460413845,
+      "published_median_abs_c0_c1": [
+        1.6002928018569946,
+        0.2796146124601364
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        1.6002928018569946,
+        0.2796146124601364
+      ],
+      "phase_samples": {
+        "phase_turn_in": 15,
+        "phase_held": 122,
+        "phase_release": 402,
+        "phase_reversal": 0
+      }
+    },
+    {
+      "name": "centering_reversal_positive_to_negative",
+      "role": "reversal",
+      "range_s": [
+        1888.07372132,
+        1892.07372132
+      ],
+      "samples": 396,
+      "substantial_demand_required": false,
+      "recorded_can_ratio_02s_median": null,
+      "published_median_abs_c0_c1": [
+        0.0,
+        0.0
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        0.0,
+        0.0
+      ],
+      "phase_samples": {
+        "phase_turn_in": 283,
+        "phase_held": 48,
+        "phase_release": 104,
+        "phase_reversal": 21
+      }
+    },
+    {
+      "name": "centering_reversal_negative_to_positive",
+      "role": "reversal",
+      "range_s": [
+        1960.874232409,
+        1964.874232409
+      ],
+      "samples": 397,
+      "substantial_demand_required": false,
+      "recorded_can_ratio_02s_median": null,
+      "published_median_abs_c0_c1": [
+        0.0,
+        0.0
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        0.0,
+        0.0
+      ],
+      "phase_samples": {
+        "phase_turn_in": 154,
+        "phase_held": 0,
+        "phase_release": 183,
+        "phase_reversal": 21
+      }
+    },
+    {
+      "name": "clean_release",
+      "role": "release",
+      "range_s": [
+        2453.714158177,
+        2456.964158177
+      ],
+      "samples": 323,
+      "substantial_demand_required": false,
+      "recorded_can_ratio_02s_median": null,
+      "published_median_abs_c0_c1": [
+        0.0,
+        0.0
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        0.0,
+        0.0
+      ],
+      "phase_samples": {
+        "phase_turn_in": 4,
+        "phase_held": 0,
+        "phase_release": 305,
+        "phase_reversal": 17
+      }
+    },
+    {
+      "name": "successful_smaller_positive",
+      "role": "sign_coverage_only",
+      "range_s": [
+        2590.722658577,
+        2600.918740146
+      ],
+      "samples": 175,
+      "substantial_demand_required": true,
+      "recorded_can_ratio_02s_median": 1.0960646334373787,
+      "published_median_abs_c0_c1": [
+        0.42173025012016296,
+        0.1222948431968689
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        0.42173025012016296,
+        0.1222948431968689
+      ],
+      "phase_samples": {
+        "phase_turn_in": 170,
+        "phase_held": 61,
+        "phase_release": 0,
+        "phase_reversal": 0
+      }
+    },
+    {
+      "name": "large_under_response",
+      "role": "under_response_challenge",
+      "range_s": [
+        2604.2254721,
+        2611.364366768
+      ],
+      "samples": 128,
+      "substantial_demand_required": true,
+      "recorded_can_ratio_02s_median": 0.7322859508492778,
+      "published_median_abs_c0_c1": [
+        2.4204851388931274,
+        0.42145511507987976
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        2.4204851388931274,
+        0.42145511507987976
+      ],
+      "phase_samples": {
+        "phase_turn_in": 68,
+        "phase_held": 96,
+        "phase_release": 56,
+        "phase_reversal": 0
+      }
+    },
+    {
+      "name": "successful_large_181deg",
+      "role": "authority_target",
+      "range_s": [
+        2744.478264791,
+        2750.573209708
+      ],
+      "samples": 207,
+      "substantial_demand_required": true,
+      "recorded_can_ratio_02s_median": 1.0087938914780248,
+      "published_median_abs_c0_c1": [
+        2.1044259071350098,
+        0.3815947473049164
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        2.1044259071350098,
+        0.3815947473049164
+      ],
+      "phase_samples": {
+        "phase_turn_in": 137,
+        "phase_held": 94,
+        "phase_release": 64,
+        "phase_reversal": 0
+      }
+    },
+    {
+      "name": "large_over_response_290deg",
+      "role": "over_response_challenge_not_target",
+      "range_s": [
+        2760.493612962,
+        2764.574374172
+      ],
+      "samples": 181,
+      "substantial_demand_required": true,
+      "recorded_can_ratio_02s_median": 1.2515789463064766,
+      "published_median_abs_c0_c1": [
+        4.737145900726318,
+        0.5235000252723694
+      ],
+      "send_clamped_median_abs_c0_c1": [
+        4.737145900726318,
+        0.5
+      ],
+      "phase_samples": {
+        "phase_turn_in": 139,
+        "phase_held": 90,
+        "phase_release": 41,
+        "phase_reversal": 0
+      }
+    }
+  ],
+  "selection": "Authority targets require automatic turn windows with >=1 second strict torque eligibility, eligible |wheel|>=150 degrees, and whole-window CAN response ratio median 0.90..1.10 at fixed 0.2 s. No positive-request large turn qualifies.",
+  "non_targets": "Positive smaller turn supplies sign coverage only. Under/over response and release/reversal windows are regression challenges, not authority targets.",
+  "context": "At least 10 s pre-roll or available route start, extended to include the preceding feedback reset/sign reversal. Overlapping intervals are merged. First episode begins at the partial route boundary with unobserved earlier history.",
+  "phase_policy": "Held means request curvature range over +/-0.25 s times speed squared <0.15 m/s2 at demand>=0.5. Turn-in/release compare current absolute curvature with the historical held request at measurement_time-delay, scaled by max(7,speed), using +/-0.0005 rad. These masks can overlap held; reversal means opposing delayed/current signs.",
+  "wire_policy": "Published coefficients preserve Float32 values. Send-clamped copy caps C0 to +/-5.11 and C1 to +/-0.5 before packing. Actual decoded wire is normalized to controller sign, nearest within 15 ms; wire_time/fresh/mode expose timing approximation.",
+  "model_schema": "models[model_index] contains position.x, position.y, orientation.z; Float32 conversion preserves the original model payload precision.",
+  "v5_reference": "Frozen full sequential replay from command_replay.npz, whose source hash and limitations are recorded in command_replay.json.",
+  "frozen_v5_revision": "09acf8ec2f327769f00ee53563ad2dd9225e37a7",
+  "preroll_validation": "Compact reset replay exactly matches full sequential frozen-v5 C0/C1, gates and bias on all 2233 evidence samples."
+}

openpilot/selfdrive/controls/tests/fixtures/ford_turn_exit_requests.json

@@ -0,0 +1,156 @@
+{
+  "description": "Anonymous recorded-input turn-exit regression fixture; command construction only, not simulated vehicle response.",
+  "baseline_revision": "dfcfddb91ce2409511f5b2dbce25d06d5056b3d6",
+  "baseline_hypothesis": "model-pose-c0-c1-feedback-v7",
+  "baseline_source_hashes": {
+    "controller_sha256": "4951a6352d89fcd66277bbfe682bd22e935a31b5a4db33e617ad21189b6705fd",
+    "allocator_sha256": "383538fc7cdae3bc28dffb71fe12ac5f3f9866ffbe6adfb7457f3593e9fc903a"
+  },
+  "fixture_sha256": "87a030c309061b7dc218715d05440c2077e465a8138079b46e8e8cee94201e54",
+  "source_fixture_sha256": "d476110b83dc628ffbd094220e464d6d3114b709bda2977813c3217964d41086",
+  "response_delay": 0.20000000298023224,
+  "publication_latency_estimate_s": 0.0015483515003040793,
+  "samples": 15273,
+  "model_count": 3078,
+  "evidence_samples": 4879,
+  "context_policy": "At least twenty seconds prior context, extended before the last observed reset. Overlapping intervals are merged.",
+  "provenance": "Selected from a recorded drive running the pinned baseline; request, model, driver and PSCM observations stay fixed during replay.",
+  "baseline_policy": "Stored commands, validity and bias exactly match the complete baseline replay on evidence samples. Context outside evidence initializes state and is not an exact-output target.",
+  "compact_full_baseline_evidence_parity": {
+    "commands": {
+      "exact": true,
+      "max_difference": 0.0
+    },
+    "valid": {
+      "exact": true,
+      "max_difference": 0.0
+    },
+    "heading_bias": {
+      "exact": true,
+      "max_difference": 0.0
+    }
+  },
+  "measurement_policy": "Controller computation time is estimated from publication time using the recorded median latency; exact vehicle motion under changed commands is unknown.",
+  "clean_policy": "Every sample from request time minus 0.5 s through plus 0.65 s is active, valid, fresh, unpressed and within 1 Nm raw driver torque. Demand is absolute desired curvature times current speed squared; substantial means at least 0.5 m/s2.",
+  "driver_policy": "All replay inputs retain driver interference; only comparison metrics use the clean mask. History-reset failures intentionally retain nearby driver context.",
+  "coordinates": "Elapsed seconds shifted to the first fixture control cycle; model x/y/heading are vehicle-relative, not global position.",
+  "retained_fields": [
+    "t",
+    "episode",
+    "model_index",
+    "models",
+    "desired_curvature",
+    "yaw_rate",
+    "speed",
+    "measurement_time",
+    "model_time",
+    "reference_time",
+    "active",
+    "valid",
+    "pressed",
+    "steering_torque",
+    "pscm_timestamp",
+    "pscm_valid",
+    "pscm_lateral_state",
+    "pscm_limit",
+    "pscm_capability",
+    "pscm_denied",
+    "clean_rawtorque",
+    "demand",
+    "window_masks",
+    "evidence",
+    "baseline_commands",
+    "baseline_valid",
+    "baseline_heading_base",
+    "baseline_heading_target",
+    "baseline_heading_bias",
+    "baseline_feedback_yaw_error",
+    "baseline_feedback_reference_curvature",
+    "baseline_status",
+    "baseline_offset_target"
+  ],
+  "omitted_data": "No route/device identifiers, VIN, GPS, private paths, raw wheel angle, wheel rate, EPS torque, or absolute clock origins.",
+  "baseline_status_meaning": "feedback_status from the pinned baseline",
+  "windows": [
+    {
+      "name": "good_curve_a",
+      "role": "comparison",
+      "range_s": [
+        20.0002130975003,
+        25.0002130975003
+      ],
+      "samples": 496,
+      "clean_substantial_samples": 259
+    },
+    {
+      "name": "first_reversal",
+      "role": "reversal",
+      "range_s": [
+        83.0002130975003,
+        92.7002130975003
+      ],
+      "samples": 964,
+      "clean_substantial_samples": 167
+    },
+    {
+      "name": "good_curve_b",
+      "role": "comparison",
+      "range_s": [
+        121.0002130975003,
+        128.0002130975003
+      ],
+      "samples": 695,
+      "clean_substantial_samples": 308
+    },
+    {
+      "name": "second_reversal",
+      "role": "reversal",
+      "range_s": [
+        133.5002130975003,
+        138.9002130975003
+      ],
+      "samples": 537,
+      "clean_substantial_samples": 191
+    },
+    {
+      "name": "large_turn_driver_context_a",
+      "role": "driver_context",
+      "range_s": [
+        150.0002130975003,
+        157.0002130975003
+      ],
+      "samples": 695,
+      "clean_substantial_samples": 0
+    },
+    {
+      "name": "over_growth",
+      "role": "over_response",
+      "range_s": [
+        182.0002130975003,
+        191.0002130975003
+      ],
+      "samples": 897,
+      "clean_substantial_samples": 66
+    },
+    {
+      "name": "large_turn_driver_context_b",
+      "role": "driver_context",
+      "range_s": [
+        199.0002130975003,
+        205.0002130975003
+      ],
+      "samples": 595,
+      "clean_substantial_samples": 281
+    },
+    {
+      "name": "zero_bias_release",
+      "role": "under_response",
+      "range_s": [
+        202.0002130975003,
+        205.0002130975003
+      ],
+      "samples": 297,
+      "clean_substantial_samples": 279
+    }
+  ]
+}

openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py

@@ -0,0 +1,64 @@
+import ast
+import io
+import json
+import logging
+from pathlib import Path
+from types import SimpleNamespace
+import unittest
+
+from openpilot.common.logging_extra import SwagFormatter, SwagLogger
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
+from openpilot.selfdrive.controls.lib.ford_path import FordPathController, FordPscmObserverPathController
+from openpilot.selfdrive.controls.tests.test_ford_model_action import circle
+
+
+class TestFordControlsLogging(unittest.TestCase):
+  def emit_controls_event(self, event, controls):
+    # Execute the actual controlsd call with the real logger and formatter,
+    # without launching hardware-dependent Controls or opening logging IPC.
+    source_path = Path(__file__).resolve().parents[1] / 'controlsd.py'
+    source = ast.parse(source_path.read_text())
+    calls = [node for node in ast.walk(source) if isinstance(node, ast.Call)
+             and isinstance(node.func, ast.Attribute) and isinstance(node.func.value, ast.Name)
+             and node.func.value.id == 'cloudlog' and node.args
+             and isinstance(node.args[0], ast.Constant) and node.args[0].value == event]
+    self.assertEqual(len(calls), 1)
+    logger = SwagLogger()
+    logger.setLevel(logging.INFO)  # disabled INFO logging would hide this crash
+    stream = io.StringIO()
+    handler = logging.StreamHandler(stream)
+    handler.setFormatter(SwagFormatter(logger))
+    logger.addHandler(handler)
+    try:
+      expression = ast.Expression(body=calls[0])
+      eval(compile(expression, str(source_path), 'eval'), {'cloudlog': logger, 'self': controls, 'reference_service': 'modelV2'})
+      record = json.loads(stream.getvalue())
+    finally:
+      handler.close()
+    self.assertEqual(record['level'], 'INFO')
+    self.assertEqual(record['msg']['event'], event)
+    return record['msg']
+
+  def test_startup_logs_selected_controller_without_crashing(self):
+    for controller in (FordPathController(), FordPscmObserverPathController(), FordModelActionController()):
+      with self.subTest(controller=type(controller).__name__):
+        record = self.emit_controls_event('Ford path controller selected', SimpleNamespace(ford_path_controller=controller))
+        self.assertEqual(record['controller'], type(controller).__name__)
+
+  def test_candidate_diagnostics_identify_the_experiment_and_do_not_claim_calibration(self):
+    controller = FordModelActionController()
+    for active, valid in ((False, True), (True, True), (True, False)):
+      controller.update(circle(.01), .005, yaw_rate=.05, speed=20., now=1.,
+                        measurement_time=1., model_time=1., reference_time=1., active=active, valid=valid)
+      controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.005, curvature=.0025,
+                                 sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000}))
+      record = self.emit_controls_event('Ford C2-free path tracking', controls)
+      self.assertEqual(record['hypothesis'], 'model-pose-terminal-c1-v1')
+      self.assertIs(record['calibration_approved'], False)
+      self.assertEqual(record['command'][2:], [0., 0.])
+      self.assertEqual(record['status'], controller.diagnostics['status'])
+      if active and valid:
+        self.assertEqual(record['pose_source'], 'model')
+        self.assertEqual(record['preview_time_s'], 1.)
+        self.assertEqual(record['minimum_station_m'], 7.)
+        self.assertEqual(record['c1_release'], 'terminal_spatial_curvature')

openpilot/selfdrive/controls/tests/test_ford_model_action.py

@@ -0,0 +1,194 @@
+import math
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from opendbc.can import CANPacker, CANParser
+from opendbc.car.ford.fordcan import CanBus, create_lat_ctl2_msg
+from openpilot.cereal import custom
+from openpilot.selfdrive.controls.lib.ford_path import FordPath
+from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController, encode_model_action
+
+
+def make_model(x, y, heading):
+  times = np.linspace(0., 3., len(x))
+  return SimpleNamespace(position=SimpleNamespace(t=times, x=x, y=y), orientation=SimpleNamespace(t=times, z=heading))
+
+
+def circle(curvature):
+  s = np.linspace(0., 60., 601)
+  return make_model(np.sin(curvature*s)/curvature, (1-np.cos(curvature*s))/curvature, curvature*s)
+
+
+def straight(offset=0., heading=0.):
+  x = np.linspace(0., 60., 121)
+  return make_model(x*np.cos(heading), offset+x*np.sin(heading), np.full_like(x, heading))
+
+
+def test_model_heading_is_used_even_when_scalar_action_differs():
+  model = circle(.02)
+  for desired in (0., -.004, .004):
+    target = encode_model_action(model, desired, 20.)
+    assert target.path_angle == pytest.approx(.4)
+    assert target.path_offset == pytest.approx((1-math.cos(.4))/.02)
+
+
+def test_straight_centering_and_matched_model_circles():
+  for speed in (2., 7., 20., 35.):
+    assert encode_model_action(straight(.4), 0., speed) == FordPath(True, .4, 0., 0., 0.)
+  for sign in (-1, 1):
+    target = encode_model_action(circle(sign*.01), sign*.01, 20.)
+    assert target.path_offset == pytest.approx(sign*(1-math.cos(.2))/.01)
+    assert target.path_angle == pytest.approx(sign*.2)
+
+
+def test_two_actuator_positions_are_sufficient_for_every_next_output():
+  controller = ModelActionController()
+  assert not hasattr(controller, '__dict__')
+  for i in range(300):
+    copied = ModelActionController()
+    copied.c0, copied.c1 = controller.c0, controller.c1
+    model = straight(.2*math.sin(i*.1))
+    kwargs = {'speed': 20., 'dt': .01}
+    desired = .005*math.cos(i*.03)
+    assert controller.update(model, desired, **kwargs) == copied.update(model, desired, **kwargs)
+
+
+def test_held_turn_releases_using_new_model_geometry_without_retained_bias():
+  for sign in (-1., 1.):
+    controller = ModelActionController()
+    for _ in range(400):
+      out = controller.update(circle(sign*.01), sign*.01, speed=20., dt=.01)
+    assert out.path_angle == pytest.approx(sign*.2)
+    previous = np.array([controller.c0, controller.c1])
+    for _ in range(100):
+      out = controller.update(straight(), sign*.01, speed=20., dt=.01)
+      expected = previous+np.clip(-previous, [-.04, -.005], [.04, .005])
+      values = np.array([controller.c0, controller.c1])
+      np.testing.assert_allclose(values, expected, atol=1e-10)
+      previous = values
+    assert out == FordPath(True, 0., 0., 0., 0.)
+
+
+def test_current_model_replacement_leaves_only_independent_actuator_slew():
+  controller = ModelActionController()
+  for _ in range(150):
+    controller.update(straight(1.-20*math.sin(.4), .4), .04, speed=20., dt=.01)
+  for _ in range(25):
+    out = controller.update(straight(), 0., speed=20., dt=.01)
+  assert out.path_offset == pytest.approx(0.)
+  assert out.path_angle > 0.  # C1 cannot hold C0 during its longer release.
+  for _ in range(75):
+    out = controller.update(straight(), 0., speed=20., dt=.01)
+  assert out == FordPath(True, 0., 0., 0., 0.)
+
+
+@pytest.mark.parametrize('overrides', [{'active': False}, {'valid': False}, {'dt': .2}, {'speed': math.nan}])
+def test_invalid_or_inactive_input_clears_state_before_reengagement(overrides):
+  controller = ModelActionController()
+  for _ in range(100):
+    controller.update(straight(.5), .01, speed=20., dt=.01)
+  kwargs = {'speed': 20., 'dt': .01, 'active': True, 'valid': True}
+  kwargs.update(overrides)
+  assert controller.update(straight(), 0., **kwargs) == FordPath()
+  assert (controller.c0, controller.c1) == (0., 0.)
+  assert controller.update(straight(), 0., speed=20., dt=.01) == FordPath(True, 0., 0., 0., 0.)
+
+
+def test_malformed_geometry_and_nonfinite_action_never_create_an_active_command():
+  for model, desired in ((None, 0.), (straight(), math.nan), (straight(), math.inf)):
+    assert not encode_model_action(model, desired, 20.).valid
+
+
+def test_selected_core_reversal_through_float32_and_wire_keeps_sign_and_zero_c2():
+  controller = ModelActionController()
+  packer = CANPacker('ford_lincoln_base_pt')
+  parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], 0)
+  bus = CanBus(fingerprint={0: {}})
+  previous = np.zeros(2)
+  for i in range(600):
+    sign = 1. if i < 300 else -1.
+    out = controller.update(straight(sign*8., sign*.8), sign*.1, speed=30., dt=.01)
+    fields = np.array([out.path_offset, out.path_angle])
+    assert (abs(fields) <= [5.1100001, .5000001]).all()
+    assert (abs(fields-previous) <= [.0500001, .0055001]).all()
+    previous = fields
+    message = custom.CarControlSP.new_message()
+    message.fordLateralPath.pathOffset = out.path_offset
+    message.fordLateralPath.pathAngle = out.path_angle
+    packet = create_lat_ctl2_msg(packer, bus, 2, -message.fordLateralPath.pathOffset,
+                                -message.fordLateralPath.pathAngle, out.curvature, out.curvature_rate, i % 16)
+    parser.update([i*10_000_000, [packet]])
+    decoded = parser.vl['LateralMotionControl2']
+    assert decoded['LatCtlPathOffst_L_Actl'] == pytest.approx(-out.path_offset)
+    assert decoded['LatCtlPath_An_Actl'] == pytest.approx(-out.path_angle)
+    assert decoded['LatCtlCurv_No_Actl'] == decoded['LatCtlCrv_NoRate2_Actl'] == 0.
+
+
+def test_short_path_holds_available_endpoint_without_extrapolation():
+  model = make_model([0., 1.], [0., .1], [0., 0.])
+  assert encode_model_action(model, .01, 20.) == FordPath(True, .1, 0., 0., 0.)
+
+
+def test_overflowing_arc_resets_instead_of_publishing_invalid_geometry():
+  model = make_model([0., 1e308, -1e308], [0., 0., 0.], [0., 0., 0.])
+  controller = ModelActionController()
+  controller.update(straight(.4), .01, speed=20., dt=.01)
+  assert controller.update(model, .01, speed=20., dt=.01) == FordPath()
+  assert (controller.c0, controller.c1) == (0., 0.)
+
+
+@pytest.mark.parametrize('value', [None, 'bad', 10**400])
+@pytest.mark.parametrize('field', ['dt', 'speed', 'desired_curvature'])
+def test_malformed_numeric_input_resets_without_throwing(field, value):
+  controller = ModelActionController()
+  kwargs = {'speed': 20., 'dt': .01, 'desired_curvature': .01}
+  controller.update(straight(.4), **kwargs)
+  kwargs[field] = value
+  assert controller.update(straight(.4), **kwargs) == FordPath()
+  assert (controller.c0, controller.c1) == (0., 0.)
+
+
+@pytest.mark.parametrize('model', [
+  make_model([], [], []), make_model([0.], [0.], [0.]),
+  make_model([0., 10.], [0.], [0., 0.]), make_model([0., 10.], [0., 0.], [0.]),
+  make_model([0., 0.], [0., 0.], [0., 0.]),
+  make_model([0., 10.], [0., math.nan], [0., 0.]), make_model([0., math.inf], [0., 0.], [0., 0.]),
+  make_model([0., 10.], [0., 0.], [0., math.inf]),
+  make_model([0., 10**400], [0., 0.], [0., 0.]),
+  make_model([0., 10.], [0., 0.], [1e308, -1e308]),
+])
+def test_malformed_model_arrays_cannot_reuse_a_previous_valid_command(model):
+  controller = ModelActionController()
+  controller.update(straight(.4), .01, speed=20., dt=.01)
+  assert controller.update(model, .01, speed=20., dt=.01) == FordPath()
+  assert (controller.c0, controller.c1) == (0., 0.)
+
+
+@pytest.mark.parametrize('field,value,valid', [
+  ('speed', .2999, False), ('speed', .3, True), ('speed', 55., True), ('speed', 55.0001, False),
+  ('desired_curvature', -1., True), ('desired_curvature', 1., True), ('desired_curvature', -1.0001, False),
+  ('dt', .001999, False), ('dt', .002, True), ('dt', .1, True), ('dt', .100001, False), ('dt', 0., False),
+])
+def test_domain_and_elapsed_time_boundaries(field, value, valid):
+  kwargs = {'speed': 20., 'desired_curvature': .01, 'dt': .01}
+  kwargs[field] = value
+  assert ModelActionController().update(straight(.4), **kwargs).valid == valid
+
+
+def test_arc_station_floor_not_forward_x_determines_offset():
+  x = np.array([0., 6., 12.])
+  y = .4+x*.75
+  target = encode_model_action(make_model(x, y, [.4, .4, .4]), -.01, 20.)
+  # At one second arc station is 5 m; the 7 m minimum gives x=5.6, y=4.6.
+  assert target.path_offset == pytest.approx(4.6)
+  assert target.path_angle == pytest.approx(.4)
+
+
+def test_duplicate_stations_keep_valid_geometry_and_first_cycle_slew():
+  model = make_model([0., 0., 10.], [.4, .4, .4], [0., 0., 0.])
+  assert encode_model_action(model, 0., 20.) == FordPath(True, .4, 0., 0., 0.)
+  out = ModelActionController().update(model, .01, speed=20., dt=.002)
+  assert out.path_offset == pytest.approx(.01)
+  assert out.path_angle == 0.

openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py

@@ -0,0 +1,269 @@
+"""Exercise the candidate through existing selection, publication and CAN code.
+
+Tests enable the candidate through controlsd's real startup selection.
+No hardware, IPC or CAN transmission is involved.
+"""
+import ast
+from collections import defaultdict
+import json
+import math
+from pathlib import Path
+from types import SimpleNamespace
+
+import pytest
+
+from opendbc.can import CANParser
+from opendbc.car import Bus, structs
+from opendbc.car.ford.carcontroller import CarController
+from opendbc.car.ford.values import FordFlags
+from openpilot.cereal import custom
+from openpilot.selfdrive.car.helpers import convert_carControlSP
+from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
+from openpilot.selfdrive.controls.lib.ford_path import FordPath
+from openpilot.selfdrive.controls.tests.test_ford_model_action import circle, straight
+from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup
+
+
+def update(controller, now=1., **overrides):
+  kwargs = {'model': straight(.4, .1), 'desired_curvature': .01, 'speed': 20., 'yaw_rate': 0., 'now': now,
+                'model_time': now, 'measurement_time': now, 'reference_time': now, 'active': True}
+  kwargs.update(overrides)
+  return controller.update(**kwargs)
+
+
+@pytest.mark.parametrize('field', ['model_time', 'measurement_time', 'reference_time'])
+@pytest.mark.parametrize('age', [.151, -.006])
+def test_stale_or_future_service_clears_commands_and_reengages_from_zero(field, age):
+  controller = FordModelActionController()
+  update(controller)
+  assert update(controller, 1.01, **{field: 1.01-age}) == FordPath()
+  assert controller.diagnostics['status'] == 'stale_input'
+  assert update(controller, 1.02).path_offset == pytest.approx(.04)
+
+
+@pytest.mark.parametrize('change,reason', [
+  ({'now': 1.}, 'timing_reset'),
+  ({'now': .99}, 'timing_reset'),
+  ({'now': 1.001}, 'timing_reset'),
+  ({'now': 1.101}, 'timing_reset'),
+  ({'model_time': .999}, 'timing_reset'),
+  ({'measurement_time': .999}, 'timing_reset'),
+  ({'active': False}, 'inactive'),
+  ({'valid': False}, 'invalid_service'),
+  ({'model': None}, 'invalid_path'),
+  ({'yaw_rate': math.nan}, 'nonfinite'),
+  ({'yaw_rate': 3.01}, 'input_range'),
+  ({'speed': 55.01}, 'input_range'),
+  ({'desired_curvature': 1.01}, 'input_range'),
+])
+def test_invalid_cycle_never_keeps_a_previous_active_request(change, reason):
+  controller = FordModelActionController()
+  update(controller)
+  now = change.get('now', 1.01)
+  assert update(controller, **dict(change, now=now)) == FordPath()
+  assert controller.diagnostics['status'] == reason
+  assert (controller.core.c0, controller.core.c1) == (0., 0.)
+  assert update(controller, now+1.).path_angle == pytest.approx(.005)
+
+
+@pytest.mark.parametrize('field', ['now', 'measurement_time', 'model_time', 'reference_time', 'speed', 'yaw_rate', 'desired_curvature'])
+@pytest.mark.parametrize('value', [math.nan, math.inf, -math.inf, None])
+def test_nonfinite_input_never_raises_or_leaks_into_diagnostics(field, value):
+  controller = FordModelActionController()
+  update(controller)
+  assert update(controller, **{field: value}) == FordPath()
+  assert controller.diagnostics['status'] == 'nonfinite'
+  json.dumps(controller.diagnostics, allow_nan=False)
+
+
+def test_repeated_measurements_do_not_freeze_slew_or_cache_invalid_model_geometry():
+  controller = FordModelActionController()
+  for i in range(10):
+    result = update(controller, 1.+i*.01, measurement_time=1., model_time=1., reference_time=1.)
+  assert result.path_offset == pytest.approx(.4)
+  assert result.path_angle == pytest.approx(.05)
+  broken = straight(.4)
+  broken.position.y[5] = math.nan
+  assert update(controller, 1.1, model=broken, model_time=1., measurement_time=1.) == FordPath()
+  assert controller.diagnostics['status'] == 'invalid_path'
+
+
+def test_yaw_offset_does_not_change_the_base():
+  controllers = [FordModelActionController() for _ in range(3)]
+  variants = [{}, {'yaw_rate': .0072}, {'yaw_rate': -.0072}]
+  for i in range(100):
+    outputs = [update(c, 1.+i*.01, **kwargs) for c, kwargs in zip(controllers, variants, strict=True)]
+    assert all(out == outputs[0] for out in outputs)
+  assert outputs[0].path_angle == pytest.approx(.1)
+
+
+def test_reference_source_can_change_to_an_older_but_fresh_publication():
+  controller = FordModelActionController()
+  update(controller, reference_time=.99)
+  assert update(controller, 1.01, reference_time=.98).valid
+
+
+def test_current_model_geometry_controls_both_fields_independently_of_scalar_action():
+  for sign in (-1., 1.):
+    controller = FordModelActionController()
+    for i in range(100):
+      before = update(controller, 1.+i*.01, model=circle(sign*.01), desired_curvature=sign*.005)
+    for i in range(100):
+      after = update(controller, 2.+i*.01, model=circle(sign*.02), desired_curvature=sign*.004)
+    assert abs(after.path_offset) > abs(before.path_offset)
+    assert abs(after.path_angle) > abs(before.path_angle)
+    for i in range(100):
+      released = update(controller, 3.+i*.01, model=circle(sign*.02), desired_curvature=0.)
+    assert released == after  # A scalar reference change does not fabricate a different model pose.
+
+
+def _method(filename, class_name, method):
+  tree = ast.parse(filename.read_text())
+  cls = next(node for node in tree.body if isinstance(node, ast.ClassDef) and node.name == class_name)
+  return next(node for node in cls.body if isinstance(node, ast.FunctionDef) and node.name == method)
+
+
+@pytest.fixture
+def pipeline():
+  root = Path(__file__).resolve().parents[3]
+  controls_file = root/'selfdrive/controls/controlsd.py'
+  body = _method(controls_file, 'Controls', 'state_control').body
+  # Execute the actual source choice, upstream limiter and Ford integration.
+  selection = next(n for n in body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.sm.valid['lateralManeuverPlan']")
+  limiter = next(n for n in body if isinstance(n, ast.Assign) and isinstance(n.value, ast.Call) and
+                 isinstance(n.value.func, ast.Name) and n.value.func.id == 'clip_curvature')
+  branch = next(n for n in body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.CP.brand == 'ford'")
+  call = compile(ast.Module(body=[selection, limiter, branch], type_ignores=[]), str(controls_file), 'exec')
+  publication_file = root/'sunnypilot/selfdrive/controls/controlsd_ext.py'
+  body = _method(publication_file, 'ControlsExt', 'state_control_ext').body
+  publish = [n for n in body if (isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'ford_path') or
+             (isinstance(n, ast.If) and ast.unparse(n.test) == 'ford_path is not None')]
+  assert len(publish) == 2
+  publication = compile(ast.Module(body=publish, type_ignores=[]), str(publication_file), 'exec')
+  return call, publication
+
+
+class Subscriptions:
+  frame = 1
+
+  def __init__(self, maneuver):
+    self.valid = {'lateralManeuverPlan': maneuver, 'modelV2': True}
+    self.logMonoTime = {'carState': 995_000_000, 'modelV2': 980_000_000, 'lateralManeuverPlan': 990_000_000,
+                       'deviceMotion': 980_000_000, 'extrinsicsCalibration': 750_000_000}
+    self.failed = set()
+    self.messages = {'carStateSP': custom.CarStateSP.new_message(), 'lateralManeuverPlan': SimpleNamespace(desiredCurvature=-.1),
+                     'deviceMotion': SimpleNamespace(angularVelocityDevice=SimpleNamespace(valid=True), sensorsOK=True, inputsOK=True,
+                                                     timestamp=970_000_000)}
+
+  def __getitem__(self, service):
+    return self.messages[service]
+
+  def all_checks(self, services):
+    return not self.failed.intersection(services) and all(self.valid.get(s, True) for s in services)
+
+
+@pytest.mark.parametrize('maneuver', [False, True])
+@pytest.mark.parametrize('host_yaw', [.0072, .3])
+@pytest.mark.parametrize('initial_curvature', [0., .005])
+def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipeline, maneuver, host_yaw, initial_curvature):
+  call, publication = pipeline
+  sm = Subscriptions(maneuver)
+  controls = startup()
+  controller = controls.ford_path_controller
+  initial_curvature *= -1 if maneuver else 1
+  controls.sm, controls.desired_curvature, controls.curvature = sm, initial_curvature, 0.
+  if initial_curvature:
+    # Start at the old target so startup slew cannot hide prediction on the real call path.
+    controller.core.c0, controller.core.c1 = .4, .1
+  model = straight(.4, .1)
+  model.action = SimpleNamespace(desiredCurvature=.1)
+  cc = structs.CarControl(latActive=True)
+  cs = SimpleNamespace(vEgo=20., yawRate=-host_yaw, canValid=True, steeringPressed=False, steeringTorque=0.)
+  environment = {'FordModelActionController': FordModelActionController, 'self': controls, 'CS': cs, 'CC': cc,
+                     'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
+                     'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)}
+  exec(call, environment)
+  expected_curvature = initial_curvature+(-1 if maneuver else 1)*.000125
+  assert controls.desired_curvature == pytest.approx(expected_curvature)
+  if maneuver:
+    assert controls.ford_path == FordPath() and not cc.latActive
+    assert controller.diagnostics['status'] == 'unsupported_reference'
+  else:
+    assert controls.ford_path.path_offset == pytest.approx(.44 if initial_curvature else .04)
+    assert controls.ford_path.path_angle == pytest.approx(.1 if initial_curvature else .005)
+    assert controller.diagnostics['yaw_rate'] == host_yaw
+    assert controller.diagnostics['pose_source'] == 'model'
+    assert cc.latActive and cc.actuators.curvature == 0.
+    assert controller.diagnostics['reference_age'] == pytest.approx(.02)
+
+  cp = structs.CarParams(flags=int(FordFlags.CANFD), carFingerprint='FORD_F_150_LIGHTNING_MK1')
+  downstream = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, controls.CP_SP)
+  vehicle = SimpleNamespace(out=structs.CarState(vEgo=20., vEgoRaw=20.), acc_tja_status_stock_values=defaultdict(int),
+                            lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
+  parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 20)], downstream.CAN.main)
+  for i, fail in enumerate((False, True)):
+    if fail:
+      sm.failed.add('modelV2')
+      exec(call, environment)
+      assert not cc.latActive and controls.ford_path == FordPath()
+    msg = custom.CarControlSP.new_message()
+    exec(publication, {'self': controls, 'CC_SP': msg})
+    for tick in range(5 if fail else 1):
+      now_nanos = (i + tick + 1) * 10_000_000
+      _, packets = downstream.update(cc.as_reader(), convert_carControlSP(msg.as_reader()), vehicle, now_nanos)
+      lateral = [p for p in packets if p[0] == 0x3d6]
+      assert len(lateral) == int(not fail or tick == 4)
+      parser.update([now_nanos, packets])
+    wire = parser.vl['LateralMotionControl2']
+    assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-controls.ford_path.path_offset)
+    assert wire['LatCtlPath_An_Actl'] == pytest.approx(-controls.ford_path.path_angle)
+    assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
+    assert wire['LatCtl_D2_Rq'] == (0 if fail or maneuver else 2)
+
+
+@pytest.mark.parametrize('maneuver', [False, True])
+@pytest.mark.parametrize('failed', ['carState', 'modelV2', 'vehicleParameters', 'lateralManeuverPlan'])
+def test_actual_controlsd_service_gates(pipeline, maneuver, failed):
+  sm = Subscriptions(maneuver)
+  sm.failed.add(failed)
+  controls = startup()
+  controls.sm, controls.desired_curvature, controls.curvature = sm, 0., 0.
+  cc = structs.CarControl(latActive=True)
+  cs = SimpleNamespace(vEgo=20., yawRate=0., canValid=True, steeringPressed=False, steeringTorque=0.)
+  model = straight()
+  model.action = SimpleNamespace(desiredCurvature=.1)
+  exec(pipeline[0], {'FordModelActionController': FordModelActionController, 'self': controls, 'CS': cs, 'CC': cc,
+                     'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
+                         'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)})
+  assert controls.ford_path.valid == cc.latActive == (failed == 'lateralManeuverPlan' and not maneuver)
+
+
+@pytest.mark.parametrize('service', ['deviceMotion', 'extrinsicsCalibration'])
+def test_optional_pose_services_do_not_modify_model_point_requests(pipeline, service):
+  sm = Subscriptions(False)
+  sm.failed.add(service)
+  controls = startup()
+  controls.sm, controls.desired_curvature, controls.curvature = sm, .01, 0.
+  controls.calibrated_pose = None
+  controls.ford_path_controller.core.c0, controls.ford_path_controller.core.c1 = .4, .1
+  cc = structs.CarControl(latActive=True)
+  cs = SimpleNamespace(vEgo=20., yawRate=-.3, canValid=True, steeringPressed=False, steeringTorque=0.)
+  model = straight(.4, .1)
+  model.action = SimpleNamespace(desiredCurvature=.01)
+  exec(pipeline[0], {'FordModelActionController': FordModelActionController, 'self': controls, 'CS': cs, 'CC': cc,
+                    'actuators': cc.actuators, 'model_v2': model, 'lp': SimpleNamespace(roll=0.),
+                    'clip_curvature': clip_curvature, 'time': SimpleNamespace(monotonic=lambda: 1.)})
+  assert cc.latActive and controls.ford_path.valid
+  assert controls.ford_path.path_offset == pytest.approx(.44)
+  assert controls.ford_path.path_angle == pytest.approx(.1)
+  assert controls.ford_path_controller.diagnostics['pose_source'] == 'model'
+
+
+def test_maneuver_reference_clears_existing_model_point_requests():
+  controller = FordModelActionController()
+  update(controller)
+  assert update(controller, 1.01, reference_source='lateralManeuverPlan') == FordPath()
+  assert controller.diagnostics['status'] == 'unsupported_reference'
+  assert (controller.core.c0, controller.core.c1) == (0., 0.)
+  assert update(controller, 1.02).path_angle == pytest.approx(.005)

openpilot/selfdrive/controls/tests/test_ford_model_action_cadence.py

@@ -0,0 +1,146 @@
+"""Exercise 100Hz calculation and 20Hz transmission through the real CAN sender."""
+from collections import defaultdict
+from types import SimpleNamespace
+
+import pytest
+
+from opendbc.can import CANParser
+from opendbc.car import Bus, structs
+from opendbc.car.ford.carcontroller import CarController
+from opendbc.car.ford.fordcan import calculate_lat_ctl2_checksum
+from opendbc.car.ford.values import FordFlags, FordFlagsSP, FordSafetyFlags
+from opendbc.safety.tests.libsafety import libsafety_py
+from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController
+from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
+
+
+def sender(canfd=True, selected=True):
+  cp = structs.CarParams(flags=int(FordFlags.CANFD) if canfd else 0, carFingerprint='FORD_F_150_LIGHTNING_MK1',
+                         safetyConfigs=[structs.CarParams.SafetyConfig()])
+  cp_sp = structs.CarParamsSP(flags=int(FordFlagsSP.MODEL_ACTION) if selected else 0)
+  controller = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp, cp_sp)
+  cs = SimpleNamespace(out=structs.CarState(vEgo=20., vEgoRaw=20.), acc_tja_status_stock_values=defaultdict(int),
+                       lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int))
+  return controller, cs
+
+
+@pytest.mark.parametrize('canfd,selected,step', [(True, True, 5), (True, False, 1), (False, True, 5), (False, False, 5)])
+def test_send_intervals_latest_sample_counter_and_checksum(canfd, selected, step):
+  controller, cs = sender(canfd, selected)
+  cc, sp = structs.CarControl(latActive=True), structs.CarControlSP()
+  sp.fordLateralPath.valid = True
+  name = 'LateralMotionControl2' if canfd else 'LateralMotionControl'
+  address = 0x3d6 if canfd else 0x3d3
+  parser = CANParser('ford_lincoln_base_pt', [(name, 0)], controller.CAN.main)
+  sent = []
+  for frame in range(1000):
+    sp.fordLateralPath.pathOffset = (frame % 101 - 50) * .01
+    sp.fordLateralPath.pathAngle = (frame % 101 - 50) * .0005
+    _, packets = controller.update(cc.as_reader(), sp, cs, frame * 10_000_000)
+    lateral = [p for p in packets if p[0] == address]
+    assert len(lateral) == int(frame % step == 0)
+    if not lateral:
+      continue
+    sent.append(frame)
+    parser.update([frame * 10_000_000, lateral])
+    wire = parser.vl[name]
+    assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-sp.fordLateralPath.pathOffset)
+    assert wire['LatCtlPath_An_Actl'] == pytest.approx(-sp.fordLateralPath.pathAngle)
+    if canfd:
+      counter = (len(sent) - 1) % 16
+      assert wire['LatCtlPath_No_Cnt'] == counter
+      assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, counter, lateral[0][1])
+  assert sent == list(range(0, 1000, step))
+
+
+@pytest.mark.parametrize('failure_frame', range(1, 6))
+@pytest.mark.parametrize('disengage', [False, True])
+def test_next_scheduled_frame_clears_invalid_or_inactive_path(failure_frame, disengage):
+  controller, cs = sender()
+  cc, sp = structs.CarControl(latActive=True), structs.CarControlSP()
+  sp.fordLateralPath.valid = True
+  sp.fordLateralPath.pathOffset, sp.fordLateralPath.pathAngle = .4, .1
+  parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 0)], controller.CAN.main)
+  for frame in range(11):
+    if frame == failure_frame:
+      if disengage:
+        cc.latActive = False
+      else:
+        sp.fordLateralPath.valid = False
+    _, packets = controller.update(cc.as_reader(), sp, cs, frame * 10_000_000)
+    lateral = [p for p in packets if p[0] == 0x3d6]
+    assert len(lateral) == int(frame % 5 == 0)
+    if lateral and frame >= failure_frame:
+      parser.update([frame * 10_000_000, lateral])
+      wire = parser.vl['LateralMotionControl2']
+      assert wire['LatCtl_D2_Rq'] == (0 if disengage else 2)
+      assert all(wire[k] == 0. for k in ('LatCtlPathOffst_L_Actl', 'LatCtlPath_An_Actl', 'LatCtlCurv_No_Actl', 'LatCtlCrv_NoRate2_Actl'))
+
+
+def test_core_slew_per_second_and_actual_panda_acceptance():
+  controller, cs = sender()
+  core = ModelActionController()
+  cc, sp = structs.CarControl(latActive=True), structs.CarControlSP()
+  parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 0)], controller.CAN.main)
+  safety = libsafety_py.libsafety
+  assert safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, FordSafetyFlags.CANFD) == 0
+  safety.init_tests()
+  safety.set_controls_allowed(True)
+  frames = []
+  for frame in range(100):
+    command = core.update(straight(10., 1.), .1, speed=20., dt=.01)
+    assert core.c0 == pytest.approx((frame + 1) * .04)
+    assert core.c1 == pytest.approx((frame + 1) * .005)
+    sp.fordLateralPath.valid = command.valid
+    sp.fordLateralPath.pathOffset, sp.fordLateralPath.pathAngle = command.path_offset, command.path_angle
+    _, packets = controller.update(cc.as_reader(), sp, cs, frame * 10_000_000)
+    for address, data, bus in packets:
+      if address != 0x3d6:
+        continue
+      frames.append(frame)
+      safety.set_timer(frame * 10_000)
+      assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data))
+      parser.update([frame * 10_000_000, [(address, data, bus)]])
+      wire = parser.vl['LateralMotionControl2']
+      assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-command.path_offset)
+      assert wire['LatCtlPath_An_Actl'] == pytest.approx(-command.path_angle)
+      assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
+  assert frames == list(range(0, 100, 5))
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('phase', range(5))
+def test_saturated_turn_unwinds_on_first_update_and_next_scheduled_can_frame(sign, phase):
+  controller, cs = sender()
+  core = ModelActionController()
+  cc, sp = structs.CarControl(latActive=True), structs.CarControlSP()
+  parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 0)], controller.CAN.main)
+  change_frame = 200 + phase
+  first_unwind = first_zero_c0 = first_zero_c1 = None
+  turn, released = straight(sign*10., sign), straight()
+  for frame in range(change_frame+135):
+    command = core.update(turn if frame < change_frame else released, sign*.1, speed=20., dt=.01)
+    if frame >= change_frame:
+      elapsed = (frame-change_frame+1)*.01
+      assert sign*core.c0 == pytest.approx(max(0., 5.11-4.*elapsed), abs=1e-10)
+      assert sign*core.c1 == pytest.approx(max(0., .5-.5*elapsed), abs=1e-10)
+    sp.fordLateralPath.valid = command.valid
+    sp.fordLateralPath.pathOffset, sp.fordLateralPath.pathAngle = command.path_offset, command.path_angle
+    _, packets = controller.update(cc.as_reader(), sp, cs, frame*10_000_000)
+    lateral = [p for p in packets if p[0] == 0x3d6]
+    if not lateral or frame < change_frame:
+      continue
+    parser.update([frame*10_000_000, lateral])
+    wire = parser.vl['LateralMotionControl2']
+    c0, c1 = -sign*wire['LatCtlPathOffst_L_Actl'], -sign*wire['LatCtlPath_An_Actl']
+    assert 0. <= c0 < 5.11 and 0. <= c1 < .5
+    assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0.
+    if first_unwind is None:
+      first_unwind = frame
+    if c0 == 0. and first_zero_c0 is None:
+      first_zero_c0 = frame
+    if c1 == 0. and first_zero_c1 is None:
+      first_zero_c1 = frame
+  assert first_unwind == ((change_frame+4)//5)*5
+  assert first_zero_c0 == ((change_frame+127+4)//5)*5
+  assert first_zero_c1 == ((change_frame+99+4)//5)*5

openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py

@@ -0,0 +1,125 @@
+"""Exercise real startup selection and Sunnylink writes without starting hardware."""
+import ast
+import base64
+import itertools
+from pathlib import Path
+from types import SimpleNamespace
+
+import pytest
+
+from opendbc.car import structs
+from opendbc.car.ford.values import FordFlags, FordFlagsSP
+from openpilot.cereal import custom
+from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType
+from openpilot.selfdrive.car.helpers import convert_to_capnp
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, select_model_action_controller
+from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController, FordPscmObserverPathController
+from openpilot.sunnypilot.mads.helpers import set_car_specific_params
+
+
+def car_params(**overrides):
+  return SimpleNamespace(**({'brand': 'ford', 'flags': FordFlags.CANFD, 'carFingerprint': 'FORD_F_150_LIGHTNING_MK1',
+                             'carFw': []} | overrides))
+
+
+def startup(cp=None, params=None, cp_sp=None):
+  filename = Path(__file__).resolve().parents[1]/'controlsd.py'
+  tree = ast.parse(filename.read_text())
+  cls = next(n for n in tree.body if isinstance(n, ast.ClassDef) and n.name == 'Controls')
+  body = next(n for n in cls.body if isinstance(n, ast.FunctionDef) and n.name == '__init__').body
+  start = next(i for i, n in enumerate(body) if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'self.ford_pscm_observer')
+  end = next(i for i, n in enumerate(body) if isinstance(n, ast.Assign) and ast.unparse(n.targets[0]) == 'self.ford_path')
+  if params is None:
+    params = SimpleNamespace(get_bool=lambda key: key == 'FordModelActionController')
+  cp = cp or car_params()
+  # card snapshots the toggle into CarParamsSP before controlsd starts.
+  if cp_sp is None:
+    cp_sp = structs.CarParamsSP()
+    if cp.brand == 'ford':
+      set_car_specific_params(cp, cp_sp, params)
+  controls = SimpleNamespace(CP=cp, CP_SP=cp_sp, params=params, calibrated_pose=None,
+                             pose_calibrator=SimpleNamespace(calib_valid=False))
+  environment = {'self': controls, 'FordFlags': FordFlags, 'FordFlagsSP': FordFlagsSP, 'FordPath': FordPath,
+                 'FordPathController': FordPathController, 'FordPscmObserverPathController': FordPscmObserverPathController,
+                 'FordModelActionController': FordModelActionController,
+                 'select_model_action_controller': select_model_action_controller,
+                 'cloudlog': SimpleNamespace(event=lambda *args, **kwargs: None)}
+  exec(compile(ast.Module(body=body[start:end+1], type_ignores=[]), str(filename), 'exec'), environment)
+  return controls
+
+
+@pytest.mark.parametrize('candidate,observer', list(itertools.product((False, True), repeat=2)))
+def test_actual_startup_priority(candidate, observer):
+  settings = {'FordModelActionController': candidate, 'FordPscmObserver': observer}
+  selected = startup(params=SimpleNamespace(get_bool=settings.__getitem__))
+  previous = FordPscmObserverPathController if observer else FordPathController
+  expected = FordModelActionController if candidate else previous
+  assert type(selected.ford_path_controller) is expected
+  assert selected.ford_model_action == candidate
+  assert bool(selected.CP_SP.flags & FordFlagsSP.MODEL_ACTION) == candidate
+  assert selected.ford_path == FordPath()
+
+
+@pytest.mark.parametrize('selected', [False, True])
+def test_controller_and_sender_share_card_snapshot_when_stored_toggle_changes(selected):
+  cp, cp_sp = car_params(), structs.CarParamsSP(flags=128)
+  set_car_specific_params(cp, cp_sp, SimpleNamespace(get_bool=lambda key: selected))
+  with custom.CarParamsSP.from_bytes(convert_to_capnp(cp_sp).to_bytes()) as snapshot:
+    controls = startup(cp, SimpleNamespace(get_bool=lambda key: not selected), snapshot)
+  assert controls.ford_model_action == selected
+  assert bool(controls.CP_SP.flags & FordFlagsSP.MODEL_ACTION) == selected
+  assert controls.CP_SP.flags & 128
+  set_car_specific_params(cp, cp_sp, SimpleNamespace(get_bool=lambda key: False))
+  assert cp_sp.flags == 128
+
+
+@pytest.mark.parametrize('overrides', [{'brand': 'tesla'}, {'flags': 0}, {'carFingerprint': 'FORD_F_150_MK14'}])
+@pytest.mark.parametrize('observer', [False, True])
+def test_other_vehicles_keep_their_previous_selection(overrides, observer):
+  settings = {'FordModelActionController': False, 'FordPscmObserver': observer}
+  params = SimpleNamespace(get_bool=settings.__getitem__)
+  before = startup(car_params(**overrides), params)
+  settings['FordModelActionController'] = True
+  after = startup(car_params(**overrides), params)
+  assert type(after.ford_path_controller) is type(before.ford_path_controller)
+  assert not after.ford_model_action
+
+
+@pytest.mark.parametrize('firmware', [[], [SimpleNamespace(ecu='eps', fwVersion=b'other')]])
+def test_candidate_does_not_depend_on_eps_firmware_query(firmware):
+  assert isinstance(startup(car_params(carFw=firmware)).ford_path_controller, FordModelActionController)
+
+
+@pytest.mark.parametrize('observer', [False, True])
+def test_sunnylink_write_takes_effect_on_restart_and_restores_stored_selection(tmp_path, monkeypatch, observer):
+  from openpilot.sunnypilot.sunnylink import utils
+
+  params = Params(str(tmp_path))
+  monkeypatch.setattr(utils, 'Params', lambda: params)
+  assert params.get_default_value('FordModelActionController') is False
+  assert params.get_type('FordModelActionController') == ParamKeyType.BOOL
+  assert b'FordModelActionController' in params.all_keys(ParamKeyFlag.PERSISTENT)
+  assert b'FordModelActionController' in params.all_keys(ParamKeyFlag.BACKUP)
+  params.put_bool('FordPscmObserver', observer, block=True)
+  old = startup(params=params)
+  assert not isinstance(old.ford_path_controller, FordModelActionController)
+  utils.save_param_from_base64_encoded_string('FordModelActionController', base64.b64encode(b'true').decode())
+  enabled = startup(params=params)
+  assert isinstance(enabled.ford_path_controller, FordModelActionController)
+  assert not isinstance(old.ford_path_controller, FordModelActionController)
+  utils.save_param_from_base64_encoded_string('FordModelActionController', base64.b64encode(b'false').decode())
+  assert isinstance(enabled.ford_path_controller, FordModelActionController)
+  assert type(startup(params=params).ford_path_controller) is type(old.ford_path_controller)
+  assert params.get_bool('FordPscmObserver') == observer
+
+
+def test_stored_retired_toggle_cannot_enable_the_candidate(tmp_path):
+  params = Params(str(tmp_path))
+  Path(params.get_param_path('FordVirtualAngleController')).write_text('1')
+  assert b'FordVirtualAngleController' not in params.all_keys()
+  assert params.get_bool('FordModelActionController') is False
+  assert type(startup(params=params).ford_path_controller) is FordPathController
+  params.put_bool('FordModelActionController', True, block=True)
+  params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START)
+  assert not Path(params.get_param_path('FordVirtualAngleController')).exists()
+  assert params.get_bool('FordModelActionController') is True

openpilot/selfdrive/controls/tests/test_ford_model_action_yaw.py

@@ -0,0 +1,29 @@
+"""Raw Ford yaw gates input health but cannot change path demand."""
+import math
+
+import pytest
+
+from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController
+from openpilot.selfdrive.controls.lib.ford_path import FordPath
+from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('yaw', [-3., -.2, -.008, 0., .008, .2, 3.])
+def test_valid_yaw_cannot_change_commands_during_entry_release_or_reversal(sign, yaw):
+  reference, measured = ModelActionController(), ModelActionController()
+  for i in range(400):
+    offset, desired = ((.4, .02), (.4, .001), (.12, -.0004078), (-.4, -.02))[i//100]
+    model = straight(sign*offset)
+    expected = reference.update(model, sign*desired, speed=10., dt=.01)
+    actual = measured.update(model, sign*desired, speed=10., dt=.01, yaw_rate=yaw)
+    assert actual == expected
+    assert actual.curvature == actual.curvature_rate == 0.
+
+
+@pytest.mark.parametrize('yaw', [math.nan, math.inf, -math.inf, None, 'bad', 3.001, -3.001])
+def test_invalid_yaw_still_resets_core(yaw):
+  controller = ModelActionController()
+  controller.update(straight(.4), .01, speed=10., dt=.01)
+  assert controller.update(straight(.4), .01, speed=10., dt=.01, yaw_rate=yaw) == FordPath()
+  assert controller.c0 == controller.c1 == 0.

openpilot/selfdrive/controls/tests/test_ford_model_points.py

@@ -0,0 +1,56 @@
+"""Model geometry contract, independent of any PSCM response model."""
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.controls.lib.ford_model_action import encode_model_action
+
+
+def model_points(t, x, y, heading):
+  return SimpleNamespace(position=SimpleNamespace(t=t, x=x, y=y), orientation=SimpleNamespace(t=t, z=heading))
+
+
+def test_both_fields_sample_the_same_model_time_without_constant_speed_assumption():
+  # Accelerating plan: one second is 12 m along this straight inclined path.
+  s = np.array([0., 4., 12., 30.])
+  heading = .1
+  m = model_points([0., .5, 1., 2.], s*np.cos(heading), .3+s*np.sin(heading), np.full(4, heading))
+  out = encode_model_action(m, -.01, 20.)
+  assert out.path_offset == pytest.approx(.3+12*np.sin(heading))
+  assert out.path_angle == pytest.approx(.1)
+  assert out.curvature == out.curvature_rate == 0.
+  assert encode_model_action(m, .01, 30.) == out
+
+
+def test_low_speed_floor_uses_one_shared_seven_metre_station():
+  m = model_points([0., 1., 2.], [0., 3., 9.], [0., 0., 0.], [0., .03, .09])
+  out = encode_model_action(m, .01, 3.)
+  assert out.path_offset == 0.
+  assert out.path_angle == pytest.approx(.07)
+
+
+def test_short_plan_holds_both_endpoint_values_without_extrapolation():
+  m = model_points([0., .5], [0., 2.], [0., .4], [0., .2])
+  out = encode_model_action(m, -.01, 20.)
+  assert out.path_offset == .4
+  assert out.path_angle == pytest.approx(.2)
+
+
+@pytest.mark.parametrize('times', [[], [0.], [0., 0., 1.], [0., 1., .5], [0., float('nan'), 1.], [.1, .5, 1.]])
+def test_invalid_model_clock_cannot_publish_an_active_path(times):
+  m = model_points(times, [0., 10., 20.], [0., .1, .4], [0., .02, .04])
+  assert not encode_model_action(m, .01, 20.).valid
+
+
+def test_orientation_and_position_must_describe_the_same_times():
+  m = model_points([0., .5, 1.], [0., 10., 20.], [0., .1, .4], [0., .02, .04])
+  m.orientation.t = [0., .6, 1.]
+  assert not encode_model_action(m, .01, 20.).valid
+
+
+def test_model_heading_unwraps_before_interpolation_and_terminal_slope():
+  m = model_points([0., .5, 1.5], [0., 10., 30.], [0., 0., 0.], [3., 3.1, -3.1])
+  # Heading at 20 m is pi, but the terminal slope is gentler than the
+  # average slope. Its release bound is 3 + 20 * ((2*pi - 3.1) - 3.1)/20.
+  assert encode_model_action(m, 0., 20.).path_angle == pytest.approx(2*np.pi - 3.2)

openpilot/selfdrive/controls/tests/test_ford_model_release.py

@@ -0,0 +1,148 @@
+"""C1 release geometry checks; none predicts a PSCM or vehicle response."""
+import math
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.controls.lib.ford_model_action import ModelActionController, encode_model_action
+
+
+def model_path(headings, *, origin=0., offset=.3, sign=1.):
+  # Known chord lengths make spatial heading profiles independent of speed.
+  station = (0., 6., 18., 30.)
+  heading = [sign*(origin+value) for value in headings]
+  x, y = [0.], [sign*offset]
+  for i in range(1, len(station)):
+    direction = (heading[i-1]+heading[i])/2
+    distance = station[i]-station[i-1]
+    x.append(x[-1]+distance*math.cos(direction))
+    y.append(y[-1]+distance*math.sin(direction))
+  times = [0., .5, 1.5, 2.5]
+  return SimpleNamespace(position=SimpleNamespace(t=times, x=x, y=y),
+                         orientation=SimpleNamespace(t=times, z=heading))
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('origin', [0., .15])
+def test_constant_spatial_curvature_preserves_original_heading(sign, origin):
+  model = model_path([0., .12, .36, .60], origin=origin, sign=sign)
+  out = encode_model_action(model, 0., 20.)
+  # The selected point is halfway from 6 m to 18 m: psi = psi0 + .02*12.
+  assert out.valid
+  assert out.path_angle == pytest.approx(sign*(origin+.24))
+  assert out.curvature == out.curvature_rate == 0.
+
+
+@pytest.mark.parametrize('heading', [-.3, .3])
+def test_constant_heading_line_keeps_its_nonzero_heading(heading):
+  out = encode_model_action(model_path([0.]*4, origin=heading), 0., 20.)
+  assert out.valid
+  assert out.path_angle == pytest.approx(heading)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_increasing_spatial_curvature_cannot_amplify_c1(sign):
+  # Selected heading .24; terminal curvature .03/m would yield .36 rad.
+  model = model_path([0., .06, .42, .90], sign=sign)
+  out = encode_model_action(model, 0., 20.)
+  assert out.path_angle == pytest.approx(sign*.24)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('origin', [0., .15])
+def test_decreasing_spatial_curvature_unloads_c1_without_erasing_origin(sign, origin):
+  # Selected heading is psi0+.36; terminal curvature .02/m gives psi0+.24.
+  model = model_path([0., .24, .48, .54], origin=origin, sign=sign)
+  out = encode_model_action(model, 0., 20.)
+  assert out.path_angle == pytest.approx(sign*(origin+.24))
+  assert abs(out.path_angle) < origin+.36
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_opposite_terminal_curvature_releases_without_inventing_a_reversal(sign):
+  model = model_path([0., .30, .10, -.30], sign=sign)
+  out = encode_model_action(model, 0., 20.)
+  # The selected model heading is still sign*.20, though its slope has reversed.
+  assert out.valid
+  assert out.path_angle == 0.
+
+
+def test_exact_model_knot_uses_incoming_segment():
+  model = SimpleNamespace(position=SimpleNamespace(t=[0., .5, 1., 2.], x=[0., 6., 12., 24.], y=[.3]*4),
+                          orientation=SimpleNamespace(t=[0., .5, 1., 2.], z=[0., .24, .48, .48]))
+  assert encode_model_action(model, 0., 20.).path_angle == pytest.approx(.48)
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_duplicate_selected_station_retains_original_c1(sign):
+  times = [0., .5, 1., 2.]
+  model = SimpleNamespace(position=SimpleNamespace(t=times, x=[0., 7., 7., 14.], y=[sign*.4]*4),
+                          orientation=SimpleNamespace(t=times, z=[0., sign*.3, sign*.2, sign*.2]))
+  out = encode_model_action(model, 0., 20.)
+  assert out.valid
+  assert out.path_offset == sign*.4
+  assert out.path_angle == pytest.approx(sign*.2)
+
+
+def test_actual_model_reversal_uses_existing_slew_and_reaches_opposite_c1():
+  controller = ModelActionController()
+  positive = model_path([0., .12, .36, .60])
+  negative = model_path([0., .12, .36, .60], sign=-1.)
+  for _ in range(50):
+    controller.update(positive, 0., speed=20., dt=.01)
+  before = controller.c1
+  first = controller.update(negative, 0., speed=20., dt=.01)
+  assert controller.c1 == pytest.approx(before-.005)
+  assert first.path_angle > 0.
+  for _ in range(100):
+    final = controller.update(negative, 0., speed=20., dt=.01)
+  assert final.path_angle == pytest.approx(-.24)
+
+
+def test_release_preserves_c0_exactly_through_target_slew_and_packing():
+  actual, reference = ModelActionController(), ModelActionController()
+  for i in range(240):
+    model = model_path([0., .24, .48, .54], offset=8.*math.sin(i*.04), sign=1. if i < 120 else -1.)
+    # C0's reference has byte-for-byte identical position and clocks, but no C1.
+    zero_heading = SimpleNamespace(position=model.position,
+                                   orientation=SimpleNamespace(t=model.orientation.t, z=[0.]*4))
+    target = encode_model_action(model, 0., 20.)
+    c0_target = encode_model_action(zero_heading, 0., 20.)
+    assert target.path_offset == c0_target.path_offset
+    output = actual.update(model, 0., speed=20., dt=.01)
+    c0_output = reference.update(zero_heading, 0., speed=20., dt=.01)
+    assert actual.c0 == reference.c0
+    assert output.path_offset == c0_output.path_offset
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_large_geometry_keeps_existing_field_caps_and_zero_c2_c3(sign):
+  controller = ModelActionController()
+  model = model_path([0.]*4, origin=.8, offset=20., sign=sign)
+  for _ in range(150):
+    out = controller.update(model, 0., speed=20., dt=.01)
+    assert out.valid
+    assert abs(out.path_offset) <= 5.11+1e-12
+    assert abs(out.path_angle) <= .5+1e-12
+    assert out.curvature == out.curvature_rate == 0.
+  assert out.path_offset == pytest.approx(sign*5.11)
+  assert out.path_angle == pytest.approx(sign*.5)
+
+
+@pytest.mark.parametrize('field', ['x', 'y', 'z'])
+def test_nonfinite_geometry_cannot_publish_a_release_request(field):
+  model = model_path([0., .24, .48, .54])
+  values = getattr(model.orientation if field == 'z' else model.position, field)
+  values[2] = math.nan
+  assert not encode_model_action(model, 0., 20.).valid
+
+
+def test_unrepresentable_terminal_slope_falls_back_to_finite_original_heading():
+  times = [0., .5, 1.]
+  model = SimpleNamespace(position=SimpleNamespace(t=times, x=[0., 5e-320, 1e-319], y=[0.]*3),
+                          orientation=SimpleNamespace(t=times, z=[0., .2, .4]))
+  out = encode_model_action(model, 0., 20.)
+  assert out.valid
+  assert np.isfinite(out.path_angle)
+  assert out.path_angle == pytest.approx(.4)

openpilot/selfdrive/controls/tests/test_ford_path.py

@@ -0,0 +1,422 @@
+import math
+from types import SimpleNamespace
+
+import numpy as np
+
+from openpilot.cereal import custom
+from openpilot.selfdrive.car.helpers import convert_carControlSP
+from openpilot.selfdrive.controls.lib.ford_path import (DBC_ANGLE, DBC_CURVATURE, DBC_OFFSET, FordPath, FordPathController,
+                                                        FordPscmObserver, FordPscmObserverPathController, FordPscmState,
+                                                        _bounded_feedback, _encode_path, _model_path, _predicted_pose,
+                                                        _pscm_contributions, _relative_pose)
+
+
+def _path(curvature: float, speed: float = 8.0):
+  t = np.linspace(0.0, 3.0, 61)
+  distance = speed * t
+  heading = curvature * distance
+  x = np.zeros_like(distance)
+  y = np.zeros_like(distance)
+  for i in range(1, len(distance)):
+    ds = distance[i] - distance[i - 1]
+    average_heading = 0.5 * (heading[i] + heading[i - 1])
+    x[i] = x[i - 1] + ds * math.cos(average_heading)
+    y[i] = y[i - 1] + ds * math.sin(average_heading)
+  return SimpleNamespace(
+    position=SimpleNamespace(t=t.tolist(), x=x.tolist(), y=y.tolist()),
+    orientation=SimpleNamespace(z=heading.tolist()),
+  )
+
+
+def _changing_path(start_curvature: float, end_curvature: float, speed: float = 8.0):
+  t = np.linspace(0.0, 3.0, 61)
+  distance = speed * t
+  curvature = np.interp(distance, [distance[0], min(distance[-1], 7.0)], [start_curvature, end_curvature])
+  heading = np.zeros_like(distance)
+  x = np.zeros_like(distance)
+  y = np.zeros_like(distance)
+  for i in range(1, len(distance)):
+    ds = distance[i] - distance[i - 1]
+    heading[i] = heading[i - 1] + 0.5 * (curvature[i] + curvature[i - 1]) * ds
+    average_heading = 0.5 * (heading[i] + heading[i - 1])
+    x[i] = x[i - 1] + ds * math.cos(average_heading)
+    y[i] = y[i - 1] + ds * math.sin(average_heading)
+  return SimpleNamespace(
+    position=SimpleNamespace(t=t.tolist(), x=x.tolist(), y=y.tolist()),
+    orientation=SimpleNamespace(z=heading.tolist()),
+  )
+
+
+def _command(model, desired_curvature: float, *, current_curvature: float = 0.0, v_ego: float = 8.0):
+  return FordPathController(dt=1.0).update(model, desired_curvature, current_curvature=current_curvature, v_ego=v_ego)
+
+
+def _equivalent_curvature(command) -> float:
+  return 2.0 * command.path_offset / 7.0 ** 2 + 2.0 * command.path_angle / 7.0 + command.curvature
+
+
+def test_gentle_path_uses_only_c2():
+  command = _command(_path(0.004, speed=20.0), 0.004, current_curvature=0.004, v_ego=20.0)
+  assert command.valid
+  assert command.path_offset == 0.0
+  assert command.path_angle == 0.0
+  assert np.isclose(command.curvature, 0.004, atol=1e-6)
+  assert command.curvature_rate == 0.0
+
+
+def test_gentle_path_uses_only_c2_when_model_and_action_disagree():
+  command = _command(_path(0.005), 0.002, current_curvature=0.005)
+  assert command.path_offset == 0.0
+  assert command.path_angle == 0.0
+  assert np.isclose(command.curvature, 0.002, atol=1e-6)
+
+
+def test_spatially_growing_path_adds_fast_pose_before_action_becomes_large():
+  controller = FordPathController(dt=1.0)
+  command = controller.update(_changing_path(0.0, 0.04), 0.012, current_curvature=0.0, v_ego=8.0)
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+  assert command.curvature < 0.012
+  assert command.curvature_rate == 0.0
+
+
+def test_growing_model_pose_adds_authority_but_c3_is_never_transmitted():
+  constant = _command(_path(0.012), 0.012)
+  growing = _command(_changing_path(0.0, 0.04), 0.012)
+  assert _equivalent_curvature(growing) > _equivalent_curvature(constant)
+  assert constant.curvature_rate == 0.0
+  assert growing.curvature_rate == 0.0
+
+
+def test_local_tracking_error_corrects_without_replacing_forward_pose():
+  model = _changing_path(0.0, 0.04)
+  local_curvature = 0.5 * 0.04 * 2.0 / 7.0
+  aligned = _command(model, 0.012, current_curvature=local_curvature)
+  under = _command(model, 0.012, current_curvature=0.0)
+  assert aligned.path_offset > 0.0
+  assert aligned.path_angle > 0.0
+  assert under.path_offset > aligned.path_offset
+  assert under.path_angle > aligned.path_angle
+
+
+def test_large_maneuver_uses_fast_pose_and_zeros_c2():
+  command = _command(_path(0.04), 0.04)
+  assert command.path_offset > 0.5
+  assert command.path_angle > 0.2
+  assert command.curvature == 0.0
+  assert command.curvature_rate == 0.0
+
+
+def test_model_pose_can_trigger_maneuver_when_action_is_late():
+  command = _command(_path(0.04), 0.002)
+  assert command.path_offset > 0.5
+  assert command.path_angle > 0.2
+  assert command.curvature == 0.0
+
+
+def test_gentle_model_pose_does_not_replace_a_collapsed_action():
+  command = _command(_path(0.005), 0.0, current_curvature=0.005)
+  assert command.path_offset == 0.0
+  assert command.path_angle == 0.0
+  assert command.curvature == 0.0
+
+
+def test_changing_gentle_curve_keeps_upstream_strength_c2():
+  command = _command(_changing_path(0.0, 0.008), 0.004, current_curvature=0.0)
+  assert np.isclose(command.curvature, 0.004)
+  assert command.path_offset == 0.0
+  assert command.path_angle == 0.0
+
+
+def test_action_only_maneuver_cannot_invent_large_model_pose():
+  command = _command(_path(0.002), 0.04)
+  assert 0.0 < command.path_offset < 0.1
+  assert 0.0 < command.path_angle < 0.03
+  assert command.curvature == 0.0
+
+
+def test_nearby_demands_blend_continuously_without_a_mode_threshold():
+  low = _command(_path(0.0119), 0.0119)
+  high = _command(_path(0.0121), 0.0121)
+  assert abs(high.path_offset - low.path_offset) < 0.05
+  assert abs(high.path_angle - low.path_angle) < 0.03
+  assert abs(high.curvature - low.curvature) < 0.001
+
+
+def test_leaving_c2_normal_band_does_not_drop_total_authority():
+  normal = _command(_path(0.006), 0.006)
+  transition = _command(_path(0.0061), 0.0061)
+  assert transition.curvature <= normal.curvature
+  assert _equivalent_curvature(transition) >= _equivalent_curvature(normal)
+
+
+def test_low_speed_still_uses_available_model_pose():
+  command = _command(_path(0.04, speed=2.0), 0.04, v_ego=2.0)
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+
+
+def test_higher_speed_advances_predicted_pose_and_extends_heading_horizon():
+  model = _changing_path(0.0, 0.015, speed=20.0)
+  slow = _command(model, 0.012, v_ego=7.0)
+  fast = _command(model, 0.012, v_ego=20.0)
+  assert fast.path_offset > slow.path_offset
+  assert fast.path_angle > slow.path_angle
+
+
+def test_short_model_uses_available_endpoint():
+  model = _path(0.04, speed=1.0)
+  command = _command(model, 0.04, v_ego=1.0)
+  assert command.valid
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+
+
+def test_turn_entry_coordinates_c2_release_with_fast_pose_attack():
+  controller = FordPathController(dt=0.01)
+  for _ in range(20):
+    assert controller.update(_path(0.004), 0.004, v_ego=8.0).curvature > 0.0
+  outputs = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)]
+  assert 0.0 < outputs[0].curvature < 0.004
+  assert outputs[0].path_offset > 0.0
+  assert outputs[0].path_angle > 0.0
+  assert outputs[-1].curvature == 0.0
+
+
+def test_turn_exit_allows_c2_to_take_over_while_fast_pose_drains():
+  controller = FordPathController(dt=0.01)
+  for _ in range(20):
+    controller.update(_path(0.04), 0.04, current_curvature=0.02, v_ego=8.0)
+  outputs = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)]
+  assert 0.0 < outputs[0].curvature < 0.004
+  assert outputs[0].path_offset != 0.0 or outputs[0].path_angle != 0.0
+  assert outputs[-1].path_offset == 0.0
+  assert outputs[-1].path_angle == 0.0
+
+
+def test_100hz_handoff_preserves_total_authority_without_entry_drop_or_exit_overshoot():
+  controller = FordPathController(dt=0.01)
+  normal = controller.update(_path(0.006), 0.006, current_curvature=0.006, v_ego=8.0)
+  entries = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)]
+  entry_authority = np.asarray([_equivalent_curvature(command) for command in entries])
+  assert np.all(np.diff(entry_authority) >= -1e-9)
+  assert entry_authority[0] >= _equivalent_curvature(normal)
+
+  exits = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)]
+  exit_authority = np.asarray([_equivalent_curvature(command) for command in exits])
+  assert np.all(np.diff(exit_authority) <= 1e-9)
+  assert np.all(exit_authority >= 0.004 - 1e-9)
+
+
+def test_measured_tracking_error_closes_bidirectionally_without_abandoning_the_turn():
+  model = _path(0.04)
+  under = _command(model, 0.04, current_curvature=0.005)
+  on_target = _command(model, 0.04, current_curvature=0.04)
+  over = _command(model, 0.04, current_curvature=0.05)
+  assert under.path_offset > on_target.path_offset
+  assert under.path_angle > on_target.path_angle
+  assert 0.0 < over.path_offset < on_target.path_offset
+  assert 0.0 < over.path_angle < on_target.path_angle
+
+
+def test_gentle_curve_does_not_add_fast_tracking_trim():
+  model = _path(0.004)
+  under = _command(model, 0.004, current_curvature=0.002)
+  on_target = _command(model, 0.004, current_curvature=0.004)
+  over = _command(model, 0.004, current_curvature=0.006)
+  assert under.path_offset == on_target.path_offset == over.path_offset == 0.0
+  assert under.path_angle == on_target.path_angle == over.path_angle == 0.0
+  assert np.allclose([under.curvature, on_target.curvature, over.curvature], 0.004, atol=2e-6)
+
+
+def test_overshoot_trim_cannot_erase_a_modeled_turn():
+  model = _path(0.04)
+  on_target = _command(model, 0.04, current_curvature=0.04)
+  over = _command(model, 0.04, current_curvature=0.06)
+  assert over.path_offset > 0.95 * on_target.path_offset
+  assert over.path_angle > 0.9 * on_target.path_angle
+
+
+def test_corrupt_measured_curvature_cannot_reverse_a_modeled_turn():
+  command = _command(_path(0.04), 0.04, current_curvature=0.5)
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+  assert command.curvature == 0.0
+
+
+def test_feedback_preserves_half_lsb_feedforward_direction():
+  for feedforward, resolution in ((0.006, 0.01), (0.0004, 0.0005)):
+    result = feedforward + _bounded_feedback(feedforward, -1.0, resolution, 1.0)
+    assert result >= 0.5 * resolution
+
+
+def test_recent_curvature_trend_advances_vehicle_pose_without_a_response_gain():
+  model = _model_path(_path(0.04))
+  assert model is not None
+  constant = _encode_path(model, 0.04, current_curvature=0.02, curvature_delta=0.0, v_ego=8.0)
+  rising = _encode_path(model, 0.04, current_curvature=0.02, curvature_delta=0.01, v_ego=8.0)
+  assert 0.0 < rising.path_offset < constant.path_offset
+  assert 0.0 < rising.path_angle < constant.path_angle
+
+
+def test_model_path_exit_zeros_lingering_c2_and_countersteers():
+  command = _command(_path(0.0), 0.004, current_curvature=0.006)
+  assert command.path_offset <= 0.0
+  assert command.path_angle < 0.0
+  assert command.curvature == 0.0
+
+
+def test_model_path_reversal_zeros_opposing_lingering_c2():
+  command = _command(_path(-0.004), 0.004, current_curvature=0.002)
+  assert command.path_offset < 0.0
+  assert command.path_angle < 0.0
+  assert command.curvature == 0.0
+
+
+def test_s_turn_reverses_model_pose_without_slow_c2():
+  controller = FordPathController(dt=0.05)
+  for _ in range(10):
+    controller.update(_path(0.04), 0.04, v_ego=8.0)
+  outputs = [controller.update(_path(-0.04), -0.04, v_ego=8.0) for _ in range(10)]
+  assert all(command.curvature == 0.0 for command in outputs)
+  assert np.all(np.diff([command.path_offset for command in outputs]) < 0.0)
+  assert np.all(np.diff([command.path_angle for command in outputs]) < 0.0)
+  assert outputs[-1].path_offset < 0.0
+  assert outputs[-1].path_angle < 0.0
+
+
+def test_output_limits_and_rates_are_bounded():
+  controller = FordPathController()
+  outputs = [controller.update(_path(0.2), 0.2, v_ego=8.0) for _ in range(100)]
+  assert all(DBC_OFFSET[0] <= command.path_offset <= DBC_OFFSET[1] for command in outputs)
+  assert all(DBC_ANGLE[0] <= command.path_angle <= DBC_ANGLE[1] for command in outputs)
+  assert all(DBC_CURVATURE[0] <= command.curvature <= DBC_CURVATURE[1] for command in outputs)
+  assert np.max(np.abs(np.diff([command.path_offset for command in outputs]))) <= 0.04 + 1e-9
+  assert np.max(np.abs(np.diff([command.path_angle for command in outputs]))) <= 0.01 + 1e-9
+
+
+def test_clipped_path_angle_uses_available_offset_to_preserve_endpoint():
+  horizon = 7.0
+  for curvature, angle_limit in ((-0.1, DBC_ANGLE[0]), (0.1, DBC_ANGLE[1])):
+    model = _path(curvature)
+    command = _command(model, curvature, current_curvature=curvature, v_ego=horizon)
+    path = _model_path(model)
+    assert path is not None
+    advance = 0.1 * horizon
+    model_offset, model_angle = _relative_pose(advance + horizon, path,
+                                                _predicted_pose(advance, curvature, 0.0))
+
+    assert command.path_angle == angle_limit
+    assert np.isclose(command.path_offset + horizon * command.path_angle,
+                      model_offset + horizon * model_angle)
+
+
+def test_invalid_model_ramps_pose_to_zero_and_inactive_resets():
+  controller = FordPathController(dt=0.01)
+  for _ in range(20):
+    active = controller.update(_path(0.04), 0.04, v_ego=8.0)
+  invalid = controller.update(None, 0.0, v_ego=8.0)
+  assert invalid.valid
+  assert abs(invalid.path_offset) < abs(active.path_offset)
+  assert abs(invalid.path_angle) < abs(active.path_angle)
+  assert not controller.update(_path(0.0), 0.0, v_ego=8.0, active=False).valid
+
+
+def test_sunnypilot_path_message_round_trip():
+  message = custom.CarControlSP.new_message()
+  message.fordLateralPath.pathOffset = 0.3
+  message.fordLateralPath.pathAngle = -0.2
+  message.fordLateralPath.curvature = 0.008
+  message.fordLateralPath.curvatureRate = -0.0004
+  message.fordLateralPath.valid = True
+  path = convert_carControlSP(message.as_reader()).fordLateralPath
+  assert np.isclose(path.pathOffset, 0.3)
+  assert np.isclose(path.pathAngle, -0.2)
+  assert np.isclose(path.curvature, 0.008)
+  assert np.isclose(path.curvatureRate, -0.0004)
+  assert path.valid
+
+
+def test_pscm_observer_mirrors_exact_250hz_slew_and_c3_target():
+  observer = FordPscmObserver()
+  observer.set_command(FordPath(True, 1.0, 0.5, 0.0, 0.001))
+  observer.advance(1.0)
+  assert np.isclose(observer.state.path_offset, 1.0)
+  assert np.isclose(observer.state.path_angle, 0.100006103515625)
+  assert np.isclose(observer.state.curvature, 0.0030059814453125)
+
+
+def test_pscm_observer_tracks_wire_quantized_commands():
+  observer = FordPscmObserver()
+  observer.set_command(FordPath(True, 0.006, 0.0004, 0.000011, 0.0))
+  assert observer.command.path_offset == 0.01
+  assert observer.command.path_angle == 0.0005
+  assert observer.command.curvature == 0.00002
+
+
+def test_pscm_c2_contribution_is_speed_scheduled():
+  state = FordPscmObserver().state
+  state = type(state)(curvature=0.004)
+  low = _pscm_contributions(state, 5.0)[2]
+  high = _pscm_contributions(state, 20.0)[2]
+  assert high > low * 10.0
+
+
+def test_pscm_observer_fills_missing_gentle_c2_with_fast_fields():
+  controller = FordPscmObserverPathController(dt=0.01)
+  command = controller.update(_path(0.004, speed=20.0), 0.004, current_curvature=0.004,
+                              v_ego=20.0, v_ego_raw=20.0)
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+  assert command.curvature > 0.0
+
+
+def test_pscm_observer_uses_c0_only_after_c1_reaches_its_effective_limit():
+  controller = FordPscmObserverPathController(dt=0.01)
+  small = controller._command_for_state(FordPath(True, 0.2, 0.0, 0.0, 0.0), 8.0)
+  large = controller._command_for_state(FordPath(True, 1.0, 0.5, 0.0, 0.0), 8.0)
+  assert small.path_offset == 0.0
+  assert small.path_angle > 0.0
+  assert large.path_offset > 0.0
+  assert large.path_angle == 0.349609375 / 10.0
+
+
+def test_pscm_observer_preserves_c2_residual_across_c0_c1_headroom():
+  controller = FordPscmObserverPathController(dt=0.01)
+  target = FordPath(True, 0.0, 0.0, 0.004, 0.0)
+  command = controller._command_for_state(target, 20.0)
+  target_contribution = sum(_pscm_contributions(FordPscmState(curvature=target.curvature), 20.0))
+  command_contributions = _pscm_contributions(FordPscmState(command.path_offset, command.path_angle), 20.0)
+  assert np.isclose(sum(command_contributions), target_contribution)
+
+  controller.observer.state = FordPscmState(curvature=0.004)
+  unwind = controller._command_for_state(FordPath(valid=True), 20.0)
+  unwind_contributions = _pscm_contributions(FordPscmState(unwind.path_offset, unwind.path_angle), 20.0)
+  lingering_c2 = _pscm_contributions(controller.observer.state, 20.0)[2]
+  assert np.isclose(sum(unwind_contributions) + lingering_c2, 0.0)
+
+
+def test_pscm_observer_unloads_fast_residual_as_c2_loads():
+  controller = FordPscmObserverPathController(dt=0.01)
+  outputs = [controller.update(_path(0.004, speed=20.0), 0.004, current_curvature=0.004,
+                               v_ego=20.0, v_ego_raw=20.0) for _ in range(200)]
+  assert outputs[0].path_angle > outputs[-1].path_angle >= 0.0
+  assert controller.observer.state.curvature > 0.003
+
+
+def test_pscm_observer_counters_lingering_c2_during_model_exit():
+  controller = FordPscmObserverPathController(dt=0.01)
+  for _ in range(200):
+    controller.update(_path(0.004, speed=20.0), 0.004, current_curvature=0.004,
+                      v_ego=20.0, v_ego_raw=20.0)
+  command = controller.update(_path(0.0, speed=20.0), 0.0, current_curvature=0.004,
+                              v_ego=20.0, v_ego_raw=20.0)
+  assert command.path_angle < 0.0
+  assert command.curvature < controller.observer.state.curvature
+
+
+def test_pscm_observer_avoids_ineffective_c0_c1_windup():
+  controller = FordPscmObserverPathController(dt=1.0)
+  command = controller.update(_path(0.2), 0.2, v_ego=8.0, v_ego_raw=8.0)
+  assert abs(command.path_offset) <= 1.0
+  assert abs(command.path_angle) <= 0.349609375 / 10.0

openpilot/selfdrive/modeld/SConscript

@@ -7,14 +7,9 @@ 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, usbgpu_present, modeld_pkl_path
+from openpilot.selfdrive.modeld.helpers import TG_INPUT_DEVICES_PATH, chestnut_present, modeld_pkl_path
 
 
-CAMERA_CONFIGS = [
-  (_ar_ox_fisheye.width, _ar_ox_fisheye.height),  # tici: 1928x1208
-  (_os_fisheye.width, _os_fisheye.height),        # mici: 1344x760
-]
-
 Import('env', 'arch')
 chunker_file = File("#openpilot/common/file_chunker.py")
 lenv = env.Clone()
@@ -24,30 +19,32 @@ tinygrad_files = ["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + "
                   if 'pycache' not in x and os.path.isfile(os.path.join(tinygrad_root, x))]
 
 def estimate_pickle_max_size(onnx_size):
-  return 1.2 * onnx_size  + 10 * 1024 * 1024  # 20% + 10MB is plenty
+  # QCOM programs for models with spatial recurrent features can approach 2x
+  # the ONNX size. Overestimating only adds an empty trailing chunk.
+  return 2.0 * onnx_size + 10 * 1024 * 1024
 
 if arch == 'comma_arm64':
+  from openpilot.common.hardware import HARDWARE
+  camera = _os_fisheye if HARDWARE.get_device_type() == "mici" else _ar_ox_fisheye
+  camera_configs = [(camera.width, camera.height)]
   tg_backend = 'QCOM'
   tg_flags = f'DEV={tg_backend} IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1'
 else:
+  camera_configs = [(c.width, c.height) for c in (_ar_ox_fisheye, _os_fisheye)]
   tg_backend = 'CPU'
   tg_flags = f'DEV=CPU' if arch == 'Darwin' else 'DEV=CPU:LLVM'
 
 tg_devices = { # which device to put jit inputs to at runtime
-  'openpilot.selfdrive.modeld.modeld': {
-    'default': {'WARP_DEV': tg_backend, 'QUEUE_DEV': tg_backend},
-    'usbgpu': {'WARP_DEV': tg_backend, 'QUEUE_DEV': 'AMD'}
-  },
   'openpilot.selfdrive.modeld.dmonitoringmodeld': {
     'default': {'DEV': tg_backend}
   },
 }
 
-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'
+CHESTNUT = chestnut_present()
+if CHESTNUT:
+  chestnut_tg_flags = 'DEBUG=1 DEV=USB+AMD:LLVM FRAME_DEV=CPU FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2 TC_OCCUPANCY_OPT=1'
   # the USB+AMD GPU takes an exclusive flock; serialize all targets that touch it
-  usbgpu_lock = File("models/.usb_gpu.lock").abspath
+  chestnut_lock = File("models/.chestnut.lock").abspath
 
 def write_tg_devices(target, source, env):
   with open(str(target[0]), "w") as f:
@@ -73,44 +70,44 @@ compile_modeld_script = [
 model_w, model_h = MEDMODEL_INPUT_SIZE
 frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
 
-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.
-  taskset = 'taskset -c 7 ' if arch == 'comma_arm64' else ''
-  cmd = (f'{cmd_flags} {mac_brew_string} {taskset}python3 {modeld_dir}/compile_modeld.py '
-        f'--model-size {model_w}x{model_h} '
-        f'--camera-resolutions {camera_res_args} '
-        f'--onnx {File(f"models/{file_prefix}driving_supercombo.onnx").abspath} '
-        f'--output {target_pkl_path} --frame-skip {frame_skip}')
-  onnx_sizes_sum = sum(os.path.getsize(f) for f in driving_onnx_deps)
-  chunk_targets = get_chunk_targets(target_pkl_path, estimate_pickle_max_size(onnx_sizes_sum))
-  def do_compile(target, source, env, command=cmd, pkl=target_pkl_path, chunks=chunk_targets):
-    from openpilot.system.hardware.chestnut.flash import link_up
-    # chestnut can enumerate before its PCIe link is up due to varying 12V power behavior across cars
-    for _ in range(10):
-      if link_up():
-        break
-      time.sleep(1)
-    else:
-      print("Chestnut not ready, skipping big model build")
-      return
-    if ret := env.Execute(command):
-      return ret
-    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, "  [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 usbgpu:
-    lenv.SideEffect(usbgpu_lock, node)
+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
+    file_prefix, cmd_flags = ('big_', chestnut_tg_flags) if chestnut 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.
+    taskset = 'taskset -c 7 ' if arch == 'comma_arm64' else ''
+    cmd = (f'{cmd_flags} {mac_brew_string} {taskset}python3 {modeld_dir}/compile_modeld.py '
+          f'--model-size {model_w}x{model_h} '
+          f'--camera-resolutions {camera_res_args} '
+          f'--onnx {File(f"models/{file_prefix}driving_supercombo.onnx").abspath} '
+          f'--output {target_pkl_path} --frame-skip {frame_skip}')
+    onnx_sizes_sum = sum(os.path.getsize(f) for f in driving_onnx_deps)
+    chunk_targets = get_chunk_targets(target_pkl_path, estimate_pickle_max_size(onnx_sizes_sum))
+    def do_compile(target, source, env, command=cmd, pkl=target_pkl_path, chunks=chunk_targets):
+      from openpilot.system.hardware.chestnut.flash import link_up
+      # chestnut can enumerate before its PCIe link is up due to varying 12V power behavior across cars
+      for _ in range(10):
+        if link_up():
+          break
+        time.sleep(1)
+      else:
+        print("Chestnut not ready, skipping big model build")
+        return
+      if ret := env.Execute(command):
+        return ret
+      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")]
+    node = lenv.Command(
+      chunk_targets,
+      tinygrad_files + compile_modeld_script + driving_onnx_deps + [Value(camera_res_args), Value(chunk_targets), chunker_file],
+      actions,
+    )
+    if chestnut:
+      lenv.SideEffect(chestnut_lock, node)
 
 # get model metadata
 fn = File(f"models/dmonitoring_model").abspath
@@ -120,7 +117,7 @@ lenv.Command(fn + "_metadata.pkl", [fn + ".onnx"] + tinygrad_files + script_file
 
 dm_w, dm_h = DM_INPUT_SIZE
 compile_dm_warp_script = [File(f"{modeld_dir}/compile_dm_warp.py")]
-for cam_w, cam_h in CAMERA_CONFIGS:
+for cam_w, cam_h in camera_configs:
   dm_pkl_path = File(f"models/dm_warp_{cam_w}x{cam_h}_tinygrad.pkl").abspath
   cmd = (f'{tg_flags} {mac_brew_string} python3 {modeld_dir}/compile_dm_warp.py '
          f'--camera-resolution {cam_w}x{cam_h} --warp-to {dm_w}x{dm_h} '

openpilot/selfdrive/modeld/compile_modeld.py

@@ -37,17 +37,12 @@ from tinygrad.engine.jit import TinyJit
 
 
 NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size'])
-WARP_INPUTS = ['tfm', 'big_tfm']
-POLICY_INPUTS = ['img_q', 'big_img_q', 'feat_q', 'desire_q', 'packed_npy_inputs']
-
-UV_SCALE_MATRIX = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 1]], dtype=np.float32)
-UV_SCALE_MATRIX_INV = np.linalg.inv(UV_SCALE_MATRIX)
-
-WARP_DEV = os.getenv('WARP_DEV')
+MODELD_INPUTS = ['img_q', 'big_img_q', 'feat_q', 'desire_q', 'packed_npy_inputs']
 
 
-def make_random_images(keys, shape, device=None):
-  return {k: Tensor.randint(shape, low=0, high=256, dtype='uint8', device=device).realize() for k in keys}
+def nv12_copy_size(stride: int, y_height: int, uv_height: int) -> int:
+  # Retain the padded Y and UV plane storage, but skip the trailing kernel/guard allocation.
+  return stride * (y_height + uv_height)
 
 
 def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad, border_fill_val=None):
@@ -99,7 +94,7 @@ def make_frame_prepare(nv12: NV12Frame, model_w, model_h):
 
   def frame_prepare_tinygrad(input_frame, M_inv):
     # UV_SCALE @ M_inv @ UV_SCALE_INV simplifies to elementwise scaling
-    M_inv_uv = M_inv * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=WARP_DEV)
+    M_inv_uv = M_inv * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=Device.DEFAULT)
     # deinterleave NV12 UV plane (UVUV... -> separate U, V)
     uv = input_frame[uv_offset:uv_offset + uv_height * stride].reshape(uv_height, stride)
     with Context(SPLIT_REDUCEOP=0):
@@ -118,49 +113,43 @@ def make_frame_prepare(nv12: NV12Frame, model_w, model_h):
   return frame_prepare_tinygrad
 
 
-def make_warp_input_queues(vision_input_shapes, frame_skip, device):
-  img = vision_input_shapes['img']  # (1, 12, 128, 256)
-  n_frames = img[1] // 6
-  img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
-
-  npy = {
-    'tfm': np.zeros((3, 3), dtype=np.float32),
-    'big_tfm': np.zeros((3, 3), dtype=np.float32),
-  }
-  input_queues = {
-    'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    **{k: Tensor(v, device='NPY').realize() for k, v in npy.items()},
-  }
-  return input_queues, npy
-
-
 def get_policy_npy_shapes(input_shapes):
   dp = input_shapes['desire_pulse']  # (1, 25, 8)
   tc = input_shapes['traffic_convention']  # (1, 2)
   at = input_shapes['action_t']  # (1, 2)
-  fb = input_shapes['features_buffer']  # (1, 24, 512)
+  fb = input_shapes['features_buffer']  # (1, T-1, ...) e.g. (1, 24, 32, 512) with spatial features
+  feat_dim = math.prod(fb[2:])
   # TODO prev_feat shouldn't exist and be handled inside the JIT, but corrupt on QCOM for now
-  shapes = {'desire': (dp[2],), 'traffic_convention': tuple(tc), 'action_t': tuple(at), 'prev_feat': (fb[0], fb[2])}
+  shapes = {'desire': (dp[2],), 'traffic_convention': tuple(tc), 'action_t': tuple(at), 'prev_feat': (fb[0], feat_dim)}
   return shapes, [math.prod(s) for s in shapes.values()]
 
 
-def make_input_queues(input_shapes, frame_skip, device):
-  input_queues, npy = make_warp_input_queues(input_shapes, frame_skip, device)
-
-  fb = input_shapes['features_buffer']  # (1, 24, 512), past features only; the model appends the current frame's feature
+def make_input_queues(input_shapes, frame_skip, device, frame_copy_size):
+  img = input_shapes['img']  # (1, 12, 128, 256)
+  fb = input_shapes['features_buffer']  # (1, T-1, ...), past features only; the model appends the current frame's feature
+  feat_dim = math.prod(fb[2:])
   dp = input_shapes['desire_pulse']  # (1, 25, 8)
+  n_frames = img[1] // 6
+  img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
 
-  shapes, sizes = get_policy_npy_shapes(input_shapes)
-  packed_npy_inputs = np.zeros(sum(sizes), dtype=np.float32)
+  policy_shapes, _ = get_policy_npy_shapes(input_shapes)
+  shapes = {'tfm': (3, 3), 'big_tfm': (3, 3)} | policy_shapes
+  sizes = [math.prod(s) for s in shapes.values()]
+  packed_npy_size = sum(sizes) * np.dtype(np.float32).itemsize
+  packed_input = np.zeros(packed_npy_size + 2 * frame_copy_size, dtype=np.uint8)
+  packed_npy_inputs = packed_input[:packed_npy_size].view(np.float32)
+  frames = packed_input[packed_npy_size:]
+  frame_views = {'img': frames[:frame_copy_size], 'big_img': frames[frame_copy_size:]}
   # views into the packed inputs, to be refilled at runtime
-  npy.update({k: v.reshape(s) for (k, s), v in zip(shapes.items(), np.split(packed_npy_inputs, np.cumsum(sizes[:-1])), strict=True)})
-  input_queues.update({
-    'feat_q': Tensor(np.zeros((frame_skip * fb[1], fb[0], fb[2]), dtype=np.float32), device=device).contiguous().realize(),
+  npy = {k: v.reshape(s) for (k, s), v in zip(shapes.items(), np.split(packed_npy_inputs, np.cumsum(sizes[:-1])), strict=True)}
+  input_queues = {
+    'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+    'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+    'feat_q': Tensor(np.zeros((frame_skip * fb[1], fb[0], feat_dim), dtype=np.float32), device=device).contiguous().realize(),
     'desire_q': Tensor(np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32), device=device).contiguous().realize(),
-    'packed_npy_inputs': Tensor(packed_npy_inputs, device='NPY').realize(),
-  })
-  return input_queues, npy
+    'packed_npy_inputs': Tensor(packed_input, device='NPY').realize(),
+  }
+  return input_queues, npy, frame_views
 
 
 def shift_and_sample(buf, new_val, sample_fn):
@@ -176,13 +165,15 @@ def sample_desire(buf, frame_skip):
   return buf.reshape(-1, frame_skip, *buf.shape[1:]).max(1).flatten(0, 1).unsqueeze(0)
 
 
-def make_warp(nv12, model_w, model_h, frame_skip):
+def make_warp(nv12, model_w, model_h):
   frame_prepare = make_frame_prepare(nv12, model_w, model_h)
 
   def warp(tfm, big_tfm, frame, big_frame):
-    tfm = tfm.to(WARP_DEV)
-    big_tfm = big_tfm.to(WARP_DEV)
-    Tensor.realize(tfm, big_tfm)
+    tfm = tfm.to(Device.DEFAULT)
+    big_tfm = big_tfm.to(Device.DEFAULT)
+    frame = frame.to(Device.DEFAULT)
+    big_frame = big_frame.to(Device.DEFAULT)
+    Tensor.realize(tfm, big_tfm, frame, big_frame)
 
     warped_frame = frame_prepare(frame, tfm).unsqueeze(0)
     warped_big_frame = frame_prepare(big_frame, big_tfm).unsqueeze(0)
@@ -195,10 +186,10 @@ def make_run_policy(model_runner, model_metadata, frame_skip):
   sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
   sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
   npy_shapes, npy_sizes = get_policy_npy_shapes(model_metadata['input_shapes'])
+  model_input_dtypes = {name: spec.dtype for name, spec in model_runner.graph_inputs.items()}
 
   def run_policy(warped, img_q, big_img_q, feat_q, desire_q, packed_npy_inputs):
     packed_npy_inputs = packed_npy_inputs.to(Device.DEFAULT)
-    warped = warped.to(Device.DEFAULT)
     Tensor.realize(packed_npy_inputs, warped)
 
     img = shift_and_sample(img_q, warped[0:1], sample_skip_fn)
@@ -211,33 +202,50 @@ def make_run_policy(model_runner, model_metadata, frame_skip):
     inputs = {
       'img': img,
       'big_img': big_img,
-      'features_buffer': feat_buf,
+      'features_buffer': feat_buf.reshape(model_metadata['input_shapes']['features_buffer']),
       'desire_pulse': desire_buf,
       'traffic_convention': traffic_convention,
       'action_t': action_t,
     }
+    inputs = {name: value.cast(model_input_dtypes[name]) for name, value in inputs.items()}
     out = next(iter(model_runner(inputs).values())).cast('float32')
     return out,
   return run_policy
 
 
-def compile_jit(jit, make_random_inputs, input_keys, make_queues):
-  SEED = 42
-  def random_inputs_run(fn, seed, test_val=None, test_buffers=None, expect_match=True):
-    input_queues, npy = make_queues(Device.DEFAULT)
-    rng = np.random.default_rng(seed)
-    Tensor.manual_seed(seed)
+def make_run_model(warp, run_policy, model_metadata, frame_copy_size):
+  _, policy_sizes = get_policy_npy_shapes(model_metadata['input_shapes'])
+  packed_npy_size = (18 + sum(policy_sizes)) * np.dtype(np.float32).itemsize
 
-    testing = test_val is not None or test_buffers is not None
-    n_runs = 1 if testing else 3
+  def run_model(img_q, big_img_q, feat_q, desire_q, packed_npy_inputs):
+    packed_input = packed_npy_inputs.to(Device.DEFAULT)
+    Tensor.realize(packed_input)
+    packed_npy_inputs = packed_input[:packed_npy_size].bitcast('float32')
+    frame = packed_input[packed_npy_size:packed_npy_size + frame_copy_size]
+    big_frame = packed_input[packed_npy_size + frame_copy_size:]
+    tfm, big_tfm, policy_inputs = packed_npy_inputs.split([9, 9, sum(policy_sizes)])
+    warped = warp(tfm.reshape(3, 3), big_tfm.reshape(3, 3), frame, big_frame)
+    return run_policy(warped, img_q, big_img_q, feat_q, desire_q, policy_inputs)
+  return run_model
+
+
+def compile_jit(jit, input_keys, make_queues, benchmark_runs):
+  if benchmark_runs < 1:
+    raise ValueError("benchmark_runs must be at least 1")
+
+  SEED = 42
+  def random_inputs_run(fn, seed, n_runs, test_val=None, test_buffers=None, expect_match=True):
+    input_queues, npy, frame_views = make_queues(Device.DEFAULT)
+    rng = np.random.default_rng(seed)
 
     for i in range(n_runs):
       for v in npy.values():
         v[:] = rng.standard_normal(v.shape).astype(v.dtype)
+      for v in frame_views.values():
+        v[:] = rng.integers(0, 256, size=v.shape, dtype=np.uint8)
       Device.default.synchronize()
-      random_inputs = make_random_inputs()
       st = time.perf_counter()
-      outs = fn(**{k: input_queues[k] for k in input_keys}, **random_inputs)
+      outs = fn(**{k: input_queues[k] for k in input_keys})
       mt = time.perf_counter()
       Device.default.synchronize()
       et = time.perf_counter()
@@ -256,14 +264,15 @@ def compile_jit(jit, make_random_inputs, input_keys, make_queues):
     return val, buffers
 
   print('capture + replay')
-  test_val, test_buffers = random_inputs_run(jit, SEED)
-  print('pickle round trip')
+  test_val, test_buffers = random_inputs_run(jit, SEED, 3)
+  print(f'pickle round trip ({benchmark_runs} runs per seed)')
   with tempfile.TemporaryFile(dir=".") as f:
     dump_oob(jit, f)
     f.seek(0)
-    jit = load_oob(f)
-  random_inputs_run(jit, SEED, test_val, test_buffers, expect_match=True)
-  random_inputs_run(jit, SEED+1, test_val, test_buffers, expect_match=False)
+    loaded_jit = load_oob(f)
+  random_inputs_run(loaded_jit, SEED, benchmark_runs, test_val, test_buffers, expect_match=True)
+  random_inputs_run(loaded_jit, SEED+1, benchmark_runs, test_val, test_buffers, expect_match=False)
+  # Keep the original so per-resolution JITs share model weight buffers in the final pickle.
   return jit
 
 
@@ -292,27 +301,31 @@ if __name__ == "__main__":
   p.add_argument('--onnx', required=True)
   p.add_argument('--output', required=True)
   p.add_argument('--frame-skip', type=int, required=True)
+  p.add_argument('--benchmark-runs', type=int, default=1,
+                 help='timed loaded-JIT runs for each correctness seed')
   args = p.parse_args()
 
   model_path = read_file_chunked_to_disk(args.onnx)
   model_w, model_h = args.model_size
 
   model_runner = OnnxRunner(model_path)
-  out = {'metadata': make_metadata_dict(model_path)}
+  out = {
+    'metadata': make_metadata_dict(model_path),
+    'input_devices': {'model': Device.DEFAULT},
+    'run_model': {},
+  }
 
-  run_policy_jit = TinyJit(make_run_policy(model_runner, out['metadata'], args.frame_skip), prune=True)
-
-  make_policy_queues = partial(make_input_queues, out['metadata']['input_shapes'], args.frame_skip)
-  make_random_model_inputs = partial(make_random_images, keys=['warped'], shape=(2, 6, *out['metadata']['input_shapes']['img'][2:]), device=WARP_DEV)
-  out['run_policy'] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS,
-                                  make_policy_queues)
+  run_policy = make_run_policy(model_runner, out['metadata'], args.frame_skip)
 
   for cam_w, cam_h in args.camera_resolutions:
     nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-    make_random_warp_inputs = partial(make_random_images, keys=['frame', 'big_frame'], shape=nv12.size, device=WARP_DEV)
-    warp = TinyJit(make_warp(nv12, model_w, model_h, args.frame_skip), prune=True)
-    make_warp_queues = partial(make_warp_input_queues, out['metadata']['input_shapes'], args.frame_skip)
-    out[(cam_w,cam_h)] = compile_jit(warp, make_random_warp_inputs, WARP_INPUTS, make_warp_queues)
+    frame_copy_size = nv12_copy_size(nv12.stride, nv12.y_height, nv12.uv_height)
+    make_model_queues = partial(make_input_queues, out['metadata']['input_shapes'], args.frame_skip,
+                                frame_copy_size=frame_copy_size)
+    warp = make_warp(nv12, model_w, model_h)
+    run_model_jit = TinyJit(make_run_model(warp, run_policy, out['metadata'], frame_copy_size), prune=True)
+    out['run_model'][(cam_w,cam_h)] = compile_jit(run_model_jit, MODELD_INPUTS, make_model_queues,
+                                                  args.benchmark_runs)
 
   with open(args.output, "wb") as f:
     dump_oob(out, f)

openpilot/selfdrive/modeld/dmonitoringmodeld.py

@@ -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, usbgpu=False)['DEV']
+    self.DEV = get_tg_input_devices(PROCESS_NAME, chestnut=False)['DEV']
     with open(METADATA_PATH, 'rb') as f:
       model_metadata = pickle.load(f)
       self.input_shapes = model_metadata['input_shapes']

openpilot/selfdrive/modeld/fill_model_msg.py

@@ -64,6 +64,7 @@ def fill_driving_model_data(msg: capnp._DynamicStructBuilder, modelv2_send: capn
   driving_model_data.frameIdExtra = modelV2.frameIdExtra
   driving_model_data.frameDropPerc = modelV2.frameDropPerc
   driving_model_data.modelExecutionTime = modelV2.modelExecutionTime
+  driving_model_data.big = modelV2.big
   driving_model_data.action = modelV2.action
   driving_model_data.meta.laneChangeState = modelV2.meta.laneChangeState
   driving_model_data.meta.laneChangeDirection = modelV2.meta.laneChangeDirection

openpilot/selfdrive/modeld/helpers.py

@@ -7,18 +7,20 @@ import tempfile
 from pathlib import Path
 
 from openpilot.common.file_chunker import get_manifest_path
-from openpilot.common.hardware.usb import CHESTNUT_FW_VERSION, CHESTNUT_USB_IDS, USB_DEVICES_PATH
+from openpilot.common.hardware.usb import CHESTNUT_USB_PRODUCT, USB_DEVICES_PATH, is_chestnut_usb_id
 
 MODELS_DIR = Path(__file__).resolve().parent / 'models'
 TG_INPUT_DEVICES_PATH = MODELS_DIR / 'tg_input_devices.json'
+CHESTNUT_POWERED_VOLTAGE = 5000
+CHESTNUT_PCIE_READY = 0x78
 
 
-def get_tg_input_devices(process_name: str, usbgpu: bool):
+def get_tg_input_devices(process_name: str, chestnut: bool):
   with open(TG_INPUT_DEVICES_PATH) as f:
-    return json.load(f)[process_name]['default' if not usbgpu else 'usbgpu']
+    return json.load(f)[process_name]['default' if not chestnut else 'chestnut']
 
-def modeld_pkl_path(usbgpu: bool):
-  prefix = 'big_' if usbgpu else ''
+def modeld_pkl_path(chestnut: bool):
+  prefix = 'big_' if chestnut else ''
   return MODELS_DIR / f'{prefix}driving_tinygrad.pkl'
 
 def dump_oob(obj, f):
@@ -45,16 +47,20 @@ def load_oob(f):
       yield pb
   return pickle.load(io.BytesIO(opcodes), buffers=buffers())
 
-def usbgpu_present() -> bool:
+def chestnut_present() -> bool:
   for d in USB_DEVICES_PATH.glob("*"):
     try:
       usb_id = (int((d / "idVendor").read_text(), 16), int((d / "idProduct").read_text(), 16))
       product = (d / "product").read_text().strip()
-      if usb_id in CHESTNUT_USB_IDS and product == f"custom {CHESTNUT_FW_VERSION}-CLEAN":
+      if is_chestnut_usb_id(*usb_id) and product == CHESTNUT_USB_PRODUCT:
         return True
     except Exception:
       pass
   return False
 
-def usbgpu_compiled() -> bool:
-  return Path(get_manifest_path(modeld_pkl_path(usbgpu=True))).is_file()
+def chestnut_compiled() -> bool:
+  return Path(get_manifest_path(modeld_pkl_path(chestnut=True))).is_file()
+
+
+def chestnut_ready(state) -> bool:
+  return state.supplyVoltage >= CHESTNUT_POWERED_VOLTAGE and not state.supplyFault and state.pcieLtssm == CHESTNUT_PCIE_READY

openpilot/selfdrive/modeld/modeld.py

@@ -1,9 +1,11 @@
 #!/usr/bin/env python3
+from collections.abc import Callable
+import ctypes
 from functools import cached_property
 import os
-os.environ['GMMU'] = '0' # for usbgpu fast loading, noop for qcom
-from tinygrad.tensor import Tensor
+os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom
 from tinygrad.device import Device
+import usb1
 import struct
 import threading
 import time
@@ -26,17 +28,17 @@ from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, should_stop, smooth_value, get_curvature_from_plan
 from openpilot.selfdrive.modeld.parse_model_outputs import Parser
-from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, WARP_INPUTS, POLICY_INPUTS
+from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, nv12_copy_size, MODELD_INPUTS
 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.common.hardware.usb import CHESTNUT_USB_IDS
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
-from openpilot.selfdrive.modeld.helpers import usbgpu_present, usbgpu_compiled, modeld_pkl_path, get_tg_input_devices, load_oob
+from openpilot.selfdrive.modeld.helpers import chestnut_present, chestnut_compiled, chestnut_ready, modeld_pkl_path, load_oob
 
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
-PROCESS_NAME = "openpilot.selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
 LAT_SMOOTH_SECONDS = 0.0
@@ -81,6 +83,37 @@ class ChestnutState:
     self.valid = True
     self.sends = 0
     self.metrics = {}
+    self._asm_usb = None
+
+  def _close_asm_usb(self) -> None:
+    if self._asm_usb is not None:
+      self._asm_usb.close()
+      self._asm_usb = None
+
+  def _open_asm_usb(self):
+    context = usb1.USBContext()
+    for vendor_id, product_id in CHESTNUT_USB_IDS:
+      if (handle := context.openByVendorIDAndProductID(vendor_id, product_id, skip_on_error=True)) is not None:
+        return handle
+    context.close()
+
+  def _read_ina(self) -> tuple[int, int, bool]:
+    if "AMD" in Device._opened_devices and self._asm_usb is None:
+      try:
+        raw = Device["AMD"].iface.pci_dev.usb.usb.control_read(0xC0, 5)
+        return struct.unpack('<Hh?', bytes(raw))
+      except Exception:
+        pass
+    if self._asm_usb is None:
+      self._asm_usb = self._open_asm_usb()
+    if self._asm_usb is None:
+      raise usb1.USBErrorNoDevice
+    try:
+      raw = self._asm_usb.controlRead(0xC0, 0xC0, 0, 0, 5, timeout=100)
+    except usb1.USBError:
+      self._close_asm_usb()
+      raise
+    return struct.unpack('<Hh?', bytes(raw))
 
   @cached_property
   def power_limit(self) -> int:
@@ -94,8 +127,10 @@ 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 = smu.read_table(smu.smu_mod.SmuMetricsExternal_t, smu.smu_mod.TABLE_SMU_METRICS).SmuMetrics
+        metrics_buf = bytearray(smu.adev.vram.view(smu.driver_table_paddr, ctypes.sizeof(metrics_t))[:])
+        metrics = metrics_t.from_buffer(metrics_buf).SmuMetrics
         self.metrics = {'tempC': metrics.AvgTemperature[smu.smu_mod.TEMP_HOTSPOT],
                         'memoryTempC': metrics.AvgTemperature[smu.smu_mod.TEMP_MEM],
                         'powerDrawW': metrics.AverageSocketPower,
@@ -114,13 +149,15 @@ class ChestnutState:
         setattr(state, k, v)
 
     asm_valid = False
+    try:
+      # ASM runs on USB-C power, these still read without a gpu
+      state.supplyVoltage, state.supplyCurrent, state.supplyFault = self._read_ina()
+      asm_valid = True
+    except Exception:
+      pass
     if "AMD" in Device._opened_devices:
       try:
-        # ASM runs on USB-C power, these still read without a gpu
-        asm = Device["AMD"].iface.pci_dev.usb
-        state.pcieLtssm = asm.read(0xB450, 1)[0]
-        state.supplyVoltage, state.supplyCurrent = struct.unpack('<Hh', bytes(asm.usb.control_read(0xC0, 5))[:4])
-        asm_valid = True
+        state.pcieLtssm = Device["AMD"].iface.pci_dev.usb.read(0xB450, 1)[0]
       except Exception:
         pass
 
@@ -141,42 +178,34 @@ 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, usbgpu: bool):
+  def __init__(self, cam_w: int, cam_h: int, chestnut: bool):
     ModelStateBase.__init__(self)
-    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(usbgpu)))
+    jits = load_oob(open_file_chunked(modeld_pkl_path(chestnut)))
+    input_devices = jits['input_devices']
+    self.model_device = input_devices['model']
     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.usbgpu = usbgpu
+    self.chestnut = chestnut
 
     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)
-    self.full_frames: dict[str, Tensor] = {}
-    self._blob_cache: dict[tuple[str, int], Tensor] = {}
+    self.frame_copy_size = nv12_copy_size(*get_nv12_info(cam_w, cam_h)[:3])
+    self.input_queues, self.npy, self.frame_views = make_input_queues(
+      self.input_shapes, self.frame_skip, device=self.model_device, frame_copy_size=self.frame_copy_size)
     self.parser = Parser()
-    self.frame_buf_params = {k: get_nv12_info(cam_w, cam_h) for k in ('img', 'big_img')}
-    self.run_policy = jits['run_policy']
-    self.warp = jits[(cam_w,cam_h)]
+    self.run_model = jits['run_model'][(cam_w,cam_h)]
 
   def slice_outputs(self, model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]:
     parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()}
     return parsed_model_outputs
 
   def run(self, bufs: dict[str, VisionBuf], transforms: 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]
-      # There is a ringbuffer of imgs, just cache tensors pointing to all of them
-      cache_key = (key, ptr)
-      if cache_key not in self._blob_cache:
-        self._blob_cache[cache_key] = Tensor.from_blob(ptr, (yuv_size,), dtype='uint8', device=self.WARP_DEV)
-      self.full_frames[key] = self._blob_cache[cache_key]
+          inputs: dict[str, np.ndarray], after_enqueue: Callable[[], None] | None = None) -> dict[str, np.ndarray]:
+    for key, buf in bufs.items():
+      np.copyto(self.frame_views[key], np.frombuffer(buf.data, dtype=np.uint8, count=self.frame_copy_size))
 
     # Model decides when action is completed, so desire input is just a pulse triggered on rising edge
     inputs['desire_pulse'][0] = 0
@@ -187,16 +216,12 @@ class ModelState(ModelStateBase):
     self.npy['tfm'][:,:] = transforms['img'][:,:]
     self.npy['big_tfm'][:,:] = transforms['big_img'][:,:]
 
-    warped = self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=self.full_frames['img'], big_frame=self.full_frames['big_img'])
-
-    outs, = self.run_policy(
-      **{k: self.input_queues[k] for k in POLICY_INPUTS if k in self.input_queues}, warped=warped
-    )
+    outs, = self.run_model(**{k: self.input_queues[k] for k in MODELD_INPUTS})
+    if after_enqueue is not None:
+      after_enqueue()
     model_output = outs.numpy()[0]
-    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
+    if self.chestnut and not np.all(np.isfinite(model_output)):
+      raise RuntimeError("model output not finite")
     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']]
 
@@ -205,25 +230,37 @@ class ModelState(ModelStateBase):
     return outputs_dict
 
   def warmup(self) -> None:
-    dummy_frames = {k: np.zeros(self.frame_buf_params[k][3], dtype=np.uint8) for k in self.vision_input_names}
+    dummy_frames = {k: np.zeros(self.frame_copy_size, dtype=np.uint8) for k in self.vision_input_names}
     eye = np.eye(3, dtype=np.float32)
     dims = {'desire_pulse': ModelConstants.DESIRE_LEN, 'traffic_convention': 2, 'action_t': 2}
     self.run(dummy_frames, dict.fromkeys(self.vision_input_names, eye), {k: np.zeros(v, dtype=np.float32) for k, v in dims.items()})
-    self.input_queues, self.npy = make_input_queues(self.input_shapes, self.frame_skip, device=self.QUEUE_DEV)
+    self.input_queues, self.npy, self.frame_views = make_input_queues(
+      self.input_shapes, self.frame_skip, device=self.model_device, frame_copy_size=self.frame_copy_size)
     self.prev_desire[:] = 0
-    self.full_frames.clear()
-    self._blob_cache.clear()
 
 
 def main(demo=False):
   cloudlog.warning("modeld init")
 
-  USBGPU = usbgpu_present() and usbgpu_compiled()
-  if USBGPU:
+  chestnut_available = chestnut_present() and chestnut_compiled()
+  CHESTNUT = False
+  if chestnut_available:
+    poller = messaging.Poller()
+    sock = messaging.sub_sock("chestnutState", poller=poller, conflate=True)
+    deadline = time.monotonic() + 4. / SERVICE_LIST['deviceState'].frequency
+    while not CHESTNUT and (remaining := deadline - time.monotonic()) > 0.:
+      if not poller.poll(round(remaining * 1000)):
+        break
+      msg = messaging.recv_one_or_none(sock)
+      CHESTNUT = msg is not None and msg.valid and chestnut_ready(msg.chestnutState)
+  if CHESTNUT:
     os.environ['HCQDEV_WAIT_TIMEOUT_MS'] = '3000'
   params = Params()
-  params.put_bool("UsbGpuLoading", USBGPU)
-  params.remove("UsbGpuActive")
+  params.put_bool("ChestnutLoading", CHESTNUT)
+  if chestnut_available and not CHESTNUT:
+    params.put_bool("ChestnutActive", False)
+  else:
+    params.remove("ChestnutActive")
 
   config_realtime_process(7, 54)
 
@@ -253,7 +290,7 @@ def main(demo=False):
   st = time.monotonic()
   cloudlog.warning("loading model")
   model = None
-  if USBGPU:
+  if CHESTNUT:
     big_model = None
     def load_big():
       nonlocal big_model
@@ -267,23 +304,27 @@ def main(demo=False):
     loader.start()
     loader.join(BIG_MODEL_TIMEOUT)
     model = big_model
-    params.put_bool("UsbGpuActive", model is not None)
+    if model is None:
+      params.put_bool("ChestnutModelError", True)
+    params.put_bool("ChestnutActive", model is not None)
+    if model is not None:
+      params.remove("ChestnutModelError")
 
-  small_model = ModelState(vipc_client_main.width, vipc_client_main.height, False) if model is None or USBGPU else None
+  small_model = ModelState(vipc_client_main.width, vipc_client_main.height, False) if model is None or CHESTNUT else None
   if model is None:
     model = small_model
-  params.put_bool("UsbGpuLoading", False)
+  params.put_bool("ChestnutLoading", 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 USBGPU else [])
+  pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutState"] if CHESTNUT else [])
   pm = PubMaster(pub_socks)
   sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"])
 
   publish_state = PublishState()
   params = Params()
-  chestnut_state = ChestnutState(pm, model.usbgpu) if USBGPU else None
+  chestnut_state = ChestnutState(pm, model.chestnut) if CHESTNUT else None
 
   # setup filter to track dropped frames
   frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ)
@@ -393,13 +434,16 @@ def main(demo=False):
 
     mt1 = time.perf_counter()
     try:
-      model_output = model.run(bufs, transforms, inputs)
+      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)
     except Exception:
-      if not params.get_bool("UsbGpuActive"):
+      if not params.get_bool("ChestnutActive"):
         raise
       # fallback to small model
       cloudlog.exception("big model failed, fall back to small")
-      params.put_bool("UsbGpuActive", False)
+      params.put_bool("ChestnutModelError", True)
+      params.put_bool("ChestnutActive", False)
       assert small_model is not None
       model = small_model
       if chestnut_state is not None:
@@ -419,18 +463,17 @@ 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.usbgpu
+      modelv2_send.modelV2.big = model.chestnut
 
       desire_state = modelv2_send.modelV2.meta.desireState
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
       r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
       lane_change_prob = l_lane_change_prob + r_lane_change_prob
-      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
-      modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
-      modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
-
       mdv2sp_send = messaging.new_message('modelDataV2SP')
       left_edge, right_edge = RELC.update_and_fill(modelv2_send.modelV2, mdv2sp_send.modelDataV2SP, v_ego)
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, left_edge, right_edge)
+      modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
+      modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
 
       fill_driving_model_data(drivingdata_send, modelv2_send)
@@ -441,10 +484,6 @@ 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

openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:a501760a9d1d5fef0eab2b8c5d122d06124fc26dc8e0782e0aa94b82a208f0ff
-size 1757355221
+oid sha256:1791d5940b2c048d0639813426dd2cf1d6f2a6727ed51e17c8bcea8bbe754123
+size 765950064

openpilot/selfdrive/pandad/pandad.cc

@@ -123,22 +123,22 @@ void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::Panda
   ps.setUptime(health.uptime_pkt);
   ps.setSafetyTxBlocked(health.safety_tx_blocked_pkt);
   ps.setSafetyRxInvalid(health.safety_rx_invalid_pkt);
-  ps.setIgnitionLine(health.ignition_line_pkt);
-  ps.setIgnitionCan(health.ignition_can_pkt);
-  ps.setControlsAllowed(health.controls_allowed_pkt);
+  ps.setIgnitionLine((health.flags_pkt & HEALTH_FLAG_IGNITION_LINE) != 0U);
+  ps.setIgnitionCan((health.flags_pkt & HEALTH_FLAG_IGNITION_CAN) != 0U);
+  ps.setControlsAllowed((health.flags_pkt & HEALTH_FLAG_CONTROLS_ALLOWED) != 0U);
   ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt);
   ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt);
   ps.setPandaType(hw_type);
   ps.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_mode_pkt));
   ps.setSafetyParam(health.safety_param_pkt);
   ps.setFaultStatus(cereal::PandaState::FaultStatus(health.fault_status_pkt));
-  ps.setPowerSaveEnabled((bool)(health.power_save_enabled_pkt));
-  ps.setHeartbeatLost((bool)(health.heartbeat_lost_pkt));
+  ps.setPowerSaveEnabled((health.flags_pkt & HEALTH_FLAG_POWER_SAVE_ENABLED) != 0U);
+  ps.setHeartbeatLost((health.flags_pkt & HEALTH_FLAG_HEARTBEAT_LOST) != 0U);
   ps.setAlternativeExperience(health.alternative_experience_pkt);
   ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt));
-  ps.setInterruptLoad(health.interrupt_load_pkt);
+  ps.setInterruptLoad(health.interrupt_load_pkt / 255.0f);
   ps.setFanPower(health.fan_power);
-  ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid_pkt));
+  ps.setSafetyRxChecksInvalid((health.flags_pkt & HEALTH_FLAG_SAFETY_RX_CHECKS_INVALID) != 0U);
   ps.setSpiErrorCount(health.spi_error_count_pkt);
   ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f);
   ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f);
@@ -198,10 +198,10 @@ std::optional<bool> send_panda_states(PubMaster *pm, Panda *panda, bool is_onroa
   }
 
   if (spoofing_started) {
-    health.ignition_line_pkt = 1;
+    health.flags_pkt |= HEALTH_FLAG_IGNITION_LINE;
   }
 
-  bool ignition_local = ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0)) && !always_offroad;
+  bool ignition_local = ((health.flags_pkt & (HEALTH_FLAG_IGNITION_LINE | HEALTH_FLAG_IGNITION_CAN)) != 0U) && !always_offroad;
 
   // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
   if (health.safety_mode_pkt == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
@@ -209,7 +209,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, Panda *panda, bool is_onroa
   }
 
   bool power_save_desired = !ignition_local;
-  if (health.power_save_enabled_pkt != power_save_desired) {
+  if (((health.flags_pkt & HEALTH_FLAG_POWER_SAVE_ENABLED) != 0U) != power_save_desired) {
     panda->set_power_saving(power_save_desired);
   }
 

openpilot/selfdrive/selfdrived/alerts_offroad.json

@@ -19,6 +19,30 @@
   },
   "Offroad_ChestnutBranch": {
     "text": "Chestnut detected! Switch to the %1 branch to use chestnut-class models.",
+    "severity": -1
+  },
+  "Offroad_ChestnutNotDetected": {
+    "text": "Chestnut not detected. Check USB and 12V connections.",
+    "severity": 0
+  },
+  "Offroad_ChestnutOverheated": {
+    "text": "Chestnut overheated. Ensure good airflow. Current GPU temperature is %1.",
+    "severity": 0
+  },
+  "Offroad_ChestnutPcieUnavailable": {
+    "text": "%1",
+    "severity": 0
+  },
+  "Offroad_ChestnutUncompiled": {
+    "text": "Chestnut model not compiled. Keep ignition on and reboot the comma.",
+    "severity": 0
+  },
+  "Offroad_ChestnutUpdateFailed": {
+    "text": "Chestnut update failed. Check the USB cable.",
+    "severity": 0
+  },
+  "Offroad_ChestnutUsbSlow": {
+    "text": "Chestnut USB link is slow. Check the USB cable. The current speed is %1.",
     "severity": 0
   },
   "Offroad_UnregisteredHardware": {

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -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,18 @@ class SelfdriveD(CruiseHelper):
       self.events.add(EventName.joystickDebug)
       self.startup_event = None
 
-    loading = self.params.get_bool("UsbGpuLoading")
+    loading = self.params.get_bool("ChestnutLoading")
     if self.big_model_loading and not loading:
       self.big_model_ready_t = time.monotonic()
+      self.events_sp.add(custom.OnroadEventSP.EventName.bigModelReady)
     self.big_model_loading = loading
     if self.big_model_loading:
       self.events.add(EventName.bigModelLoading)
 
-    big_active = self.params.get("UsbGpuActive")
-    usbgpu_present = self.sm['deviceState'].chestnutPresent
+    big_active = self.params.get("ChestnutActive")
+    chestnut_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 usbgpu_present)
+    big_failed = big_active is False or model_unavailable or (self.big_model_active and not chestnut_present)
     if big_failed and not self.big_model_failed:
       self.events.add(EventName.bigModelFailed)
     self.big_model_failed = big_failed
@@ -527,6 +542,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 +662,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 +696,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 +730,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 +744,7 @@ class SelfdriveD(CruiseHelper):
         self.step()
         self.rk.monitor_time()
     finally:
+      self.assisted_driving_milestones.close()
       e.set()
       t.join()
 

openpilot/selfdrive/test/process_replay/migration.py

@@ -152,7 +152,7 @@ def migrate_drivingModelData(msgs):
   add_ops = []
   for _, msg in msgs:
     dmd = messaging.new_message('drivingModelData', valid=msg.valid, logMonoTime=msg.logMonoTime)
-    for field in ["frameId", "frameIdExtra", "frameDropPerc", "modelExecutionTime", "action"]:
+    for field in ["frameId", "frameIdExtra", "frameDropPerc", "modelExecutionTime", "big", "action"]:
       setattr(dmd.drivingModelData, field, getattr(msg.modelV2, field))
     for meta_field in ["laneChangeState", "laneChangeState"]:
       setattr(dmd.drivingModelData.meta, meta_field, getattr(msg.modelV2.meta, meta_field))

openpilot/selfdrive/test/process_replay/model_replay.py

@@ -33,9 +33,9 @@ MODEL_REPLAY_BUCKET="model_replay_master"
 GITHUB = GithubUtils(API_TOKEN, DATA_TOKEN)
 
 EXEC_TIMINGS = [
-  # model, instant max, average max
-  ("modelV2", 0.05, 0.028),
-  ("driverStateV2", 0.05, 0.018),
+  # model, instant max, average max, chestnut average max
+  ("modelV2", 0.05, 0.03, 0.05),
+  ("driverStateV2", 0.05, 0.018, 0.018),
 ]
 
 def get_log_fn(test_route, ref="master"):
@@ -169,11 +169,13 @@ def model_replay(lr, frs):
   dmonitoringmodeld_msgs = replay_process(dmonitoringmodeld, dmodeld_logs, frs)
 
   msgs = modeld_msgs + dmonitoringmodeld_msgs
+  chestnut = any(m.modelV2.big for m in modeld_msgs if m.which() == "modelV2")
 
   header = ['model', 'max instant', 'max instant allowed', 'average', 'max average allowed', 'test result']
   rows = []
   timings_ok = True
-  for (s, instant_max, avg_max) in EXEC_TIMINGS:
+  for (s, instant_max, avg_max, chestnut_avg_max) in EXEC_TIMINGS:
+    avg_max = chestnut_avg_max if chestnut else avg_max
     ts = [getattr(m, s).modelExecutionTime for m in msgs if m.which() == s]
     # TODO some init can happen in first iteration
     ts = ts[1:]

openpilot/selfdrive/ui/body/layouts/onroad.py

@@ -1,7 +1,7 @@
 import time
 import pyray as rl
 
-from openpilot.system.ui.lib.application import gui_app, FontWeight
+from openpilot.system.ui.lib.application import gui_app, FontWeight, TextAlignment, TextAlignmentVertical
 from openpilot.system.ui.widgets import Widget
 from openpilot.system.ui.widgets.label import UnifiedLabel
 from openpilot.selfdrive.ui.ui_state import ui_state
@@ -26,8 +26,8 @@ class BodyLayout(Widget):
     self._last_input_time = time.monotonic()
     self._was_active = False
     self._offroad_label = UnifiedLabel("turn on ignition to use", 95 if gui_app.big_ui() else 45, FontWeight.DISPLAY,
-                                       alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
-                                       alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE)
+                                       alignment=TextAlignment.CENTER,
+                                       alignment_vertical=TextAlignmentVertical.MIDDLE)
 
   def draw_dot_grid(self, rect: rl.Rectangle, dots: list[tuple[int, int]], color: rl.Color):
     spacing = min(rect.height / GRID_ROWS, rect.width / GRID_COLS)

openpilot/selfdrive/ui/layouts/home.py

@@ -8,7 +8,7 @@ from openpilot.selfdrive.ui.widgets.exp_mode_button import ExperimentalModeButto
 from openpilot.selfdrive.ui.widgets.prime import PrimeWidget
 from openpilot.selfdrive.ui.widgets.setup import SetupWidget
 from openpilot.system.ui.lib.text_measure import measure_text_cached
-from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
+from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos, TextAlignment
 from openpilot.system.ui.lib.multilang import tr, trn
 from openpilot.system.ui.widgets.label import gui_label
 from openpilot.system.ui.widgets import Widget
@@ -178,7 +178,7 @@ class HomeLayout(Widget):
 
     version_rect = rl.Rectangle(self.header_rect.x + self.header_rect.width - version_text_width, self.header_rect.y,
                                 version_text_width, self.header_rect.height)
-    gui_label(version_rect, self._version_text, 48, rl.WHITE, alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT)
+    gui_label(version_rect, self._version_text, 48, rl.WHITE, alignment=TextAlignment.RIGHT)
 
   def _render_home_content(self):
     self._render_left_column()

openpilot/selfdrive/ui/layouts/onboarding.py

@@ -5,7 +5,7 @@ from enum import IntEnum
 
 import pyray as rl
 from openpilot.common.basedir import BASEDIR
-from openpilot.system.ui.lib.application import FontWeight, gui_app
+from openpilot.system.ui.lib.application import FontWeight, TextAlignment, gui_app
 from openpilot.system.ui.lib.multilang import tr
 from openpilot.system.ui.widgets import Widget
 from openpilot.system.ui.widgets.button import Button, ButtonStyle
@@ -115,9 +115,9 @@ class TermsPage(Widget):
     self._on_accept = on_accept
     self._on_decline = on_decline
 
-    self._title = Label(tr("Welcome to sunnypilot"), font_size=90, font_weight=FontWeight.BOLD, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT)
+    self._title = Label(tr("Welcome to sunnypilot"), font_size=90, font_weight=FontWeight.BOLD, text_alignment=TextAlignment.LEFT)
     self._desc = Label(tr("You must accept the Terms of Service to use sunnypilot. Read the latest terms at https://sunnypilot.ai/terms before continuing."),
-                       font_size=90, font_weight=FontWeight.MEDIUM, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT)
+                       font_size=90, font_weight=FontWeight.MEDIUM, text_alignment=TextAlignment.LEFT)
 
     self._decline_btn = Button(tr("Decline"), click_callback=on_decline)
     self._accept_btn = Button(tr("Agree"), button_style=ButtonStyle.PRIMARY, click_callback=on_accept)
@@ -150,7 +150,7 @@ class DeclinePage(Widget):
   def __init__(self, back_callback=None):
     super().__init__()
     self._text = Label(tr("You must accept the Terms of Service in order to use sunnypilot."),
-                       font_size=90, font_weight=FontWeight.MEDIUM, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT)
+                       font_size=90, font_weight=FontWeight.MEDIUM, text_alignment=TextAlignment.LEFT)
     self._back_btn = Button(tr("Back"), click_callback=back_callback)
     self._uninstall_btn = Button(tr("Decline, uninstall sunnypilot"), button_style=ButtonStyle.DANGER,
                                  click_callback=self._on_uninstall_clicked)

openpilot/selfdrive/ui/layouts/settings/software.py

@@ -199,6 +199,9 @@ class SoftwareLayout(Widget):
         selection = self._branch_dialog.selection
         ui_state.params.put("UpdaterTargetBranch", selection, block=True)
         self._branch_btn.action_item.set_value(selection)
+        self._download_btn.action_item.set_enabled(False)
+        self._waiting_for_updater = True
+        self._waiting_start_ts = time.monotonic()
         subprocess.run("pkill -SIGUSR1 -f openpilot.system.updated.updated", shell=True)
       self._branch_dialog = None
 

openpilot/selfdrive/ui/layouts/sidebar.py

@@ -168,9 +168,16 @@ class Sidebar(Widget, SidebarSP):
     # Home/Flag button
     flag_pressed = mouse_down and rl.check_collision_point_rec(mouse_pos, HOME_BTN)
     button_img = self._flag_img if ui_state.started else self._home_img
+    button_pos = rl.Vector2(HOME_BTN.x, HOME_BTN.y)
+    icon_opacity = 1.0
+
+    if gui_app.sunnypilot_ui():
+      button_img, button_pos, icon_opacity = SidebarSP._get_home_icon(self, button_img)
 
     tint = Colors.BUTTON_PRESSED if (ui_state.started and flag_pressed) else Colors.BUTTON_NORMAL
-    rl.draw_texture_ex(button_img, rl.Vector2(HOME_BTN.x, HOME_BTN.y), 0.0, 1.0, tint)
+    if icon_opacity < 1.0:
+      tint = rl.Color(tint[0], tint[1], tint[2], int(255 * icon_opacity))
+    rl.draw_texture_ex(button_img, button_pos, 0.0, 1.0, tint)
 
     # Microphone button
     if self._recording_audio:

openpilot/selfdrive/ui/mici/layouts/home.py

@@ -1,4 +1,5 @@
 import datetime
+import math
 import time
 
 from openpilot.cereal import log
@@ -8,8 +9,8 @@ from openpilot.system.ui.widgets import Widget
 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
+from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos, TextAlignment, TextAlignmentVertical
+from openpilot.selfdrive.ui.ui_state import ui_state, ChestnutState
 from openpilot.common.version import RELEASE_BRANCHES
 
 HEAD_BUTTON_FONT_SIZE = 40
@@ -69,8 +70,8 @@ class AlertsPill(Widget):
 
       count_rect = rl.Rectangle(self.rect.x + self.COUNT_OFFSET, self.rect.y, pill_w - self.COUNT_OFFSET, pill_h)
       gui_label(count_rect, str(alert_count), font_size=36,
-                alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
-                alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE)
+                alignment=TextAlignment.CENTER,
+                alignment_vertical=TextAlignmentVertical.MIDDLE)
 
 
 class NetworkIcon(Widget):
@@ -139,8 +140,10 @@ class MiciHomeLayout(Widget):
     self._version_text = self._get_version_text()
 
     self._experimental_icon = IconWidget("icons_mici/experimental_mode.png", (48, 48))
-    self._egpu_icon = IconWidget("icons_mici/egpu_green.png", (50, 37))
-    self._egpu_icon_gray = IconWidget("icons_mici/egpu_gray.png", (50, 37))
+    self._usb_icon = IconWidget("icons_mici/usb.png", (62, 40))
+    self._chestnut_icon = IconWidget("icons_mici/chestnut_green.png", (68, 40))
+    self._chestnut_loading_icon = IconWidget("icons_mici/chestnut.png", (68, 40))
+    self._chestnut_failed_icon = IconWidget("icons_mici/chestnut_orange.png", (68, 40))
     self._mic_icon = IconWidget("icons_mici/microphone.png", (32, 46))
     self._body_icon = IconWidget("icons_mici/body.png", (54, 37))
 
@@ -150,13 +153,15 @@ class MiciHomeLayout(Widget):
       IconWidget("icons_mici/settings.png", (48, 48), opacity=0.9),
       NetworkIcon(),
       self._experimental_icon,
-      self._egpu_icon,
-      self._egpu_icon_gray,
+      self._usb_icon,
+      self._chestnut_icon,
+      self._chestnut_loading_icon,
+      self._chestnut_failed_icon,
       self._body_icon,
       self._mic_icon,
     ], spacing=18)
 
-    self._openpilot_label = UnifiedLabel("sunnypilot", font_size=96, font_weight=FontWeight.DISPLAY, max_width=480, wrap_text=False)
+    self._openpilot_label = UnifiedLabel("openpilot", font_size=96, font_weight=FontWeight.DISPLAY, max_width=480, wrap_text=False)
     self._version_label = UnifiedLabel("", font_size=36, font_weight=FontWeight.ROMAN, max_width=480, wrap_text=False)
     self._large_version_label = UnifiedLabel("", font_size=64, text_color=rl.GRAY, font_weight=FontWeight.ROMAN, max_width=480, wrap_text=False)
     self._date_label = UnifiedLabel("", font_size=36, text_color=rl.GRAY, font_weight=FontWeight.ROMAN, max_width=480, wrap_text=False)
@@ -247,9 +252,20 @@ class MiciHomeLayout(Widget):
         self._version_commit_label.render()
 
     # ***** Center-aligned bottom section icons *****
+    usb_connected = ui_state.usb_connected
+    usb_unknown = ui_state.usb_unknown
+    chestnut_state = ui_state.chestnut_state
     self._experimental_icon.set_visible(ui_state.experimental_mode)
-    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)
+    if gui_app.sunnypilot_ui():
+      self._set_chestnut_visibility()
+    else:
+      self._usb_icon.set_visible(usb_connected and usb_unknown)
+      self._chestnut_icon.set_visible(not usb_unknown and chestnut_state not in
+                                      (ChestnutState.LOADING, ChestnutState.UNCOMPILED, ChestnutState.FAILED) and
+                                      (usb_connected or chestnut_state in (ChestnutState.READY, ChestnutState.ACTIVE)))
+      self._chestnut_loading_icon.set_visible(not usb_unknown and chestnut_state == ChestnutState.LOADING)
+      self._chestnut_loading_icon.set_opacity(0.35 + 0.65 * (0.5 - 0.5 * math.cos(rl.get_time() * 6.0)))
+      self._chestnut_failed_icon.set_visible(not usb_unknown and chestnut_state in (ChestnutState.UNCOMPILED, ChestnutState.FAILED))
     self._mic_icon.set_visible(ui_state.recording_audio)
     self._body_icon.set_visible(bool(ui_state.is_body))