Ford: restore large-turn path demand with bounded heading backoff

Reuse the existing model-pose allocator for aligned large maneuvers while encoding remaining selected curvature as C0/C1 and keeping C2/C3 zero. Permit measured heading backoff during release or PSCM limits without turning model-base changes into stored bias. Validate with 127 tests and 67 subtests, including recorded large-turn retention, release and reversal, repeated-measurement backoff, CAN packing, logging, and Sunnylink schema checks. Replay checks command behavior; enabled vehicle tracking remains unvalidated.
Ford: honor C2-free toggle without EPS firmware gate
2026-09-06 05:33:42 +08:00 · 2026-09-05 13:28:56 -04:00 · 2026-09-05 12:08:21 -04:00 · 2026-09-05 09:46:32 -04:00 · 2026-09-05 07:56:40 -04:00 · 2026-09-04 21:15:17 -04:00
204 changed files with 7261 additions and 3058 deletions
@@ -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
@@ -0,0 +1,11 @@
+*       @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
@@ -78,7 +78,6 @@ 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
@@ -30,7 +30,6 @@ jobs:
    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 }}
@@ -65,9 +64,7 @@ jobs:
            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
@@ -138,7 +135,7 @@ jobs:

      - name: Prepare output
        env:
-          MODEL_NAME: ${{ needs.resolve.outputs.safe_model_name }}
+          MODEL_NAME: ${{ needs.resolve.outputs.model_name }}
        run: |
          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
          export PYTHONPATH=${{ github.workspace }}
@@ -161,13 +158,13 @@ jobs:
      - name: Upload small model artifact
        uses: actions/upload-artifact@v4
        with:
-          name: model-${{ needs.resolve.outputs.safe_model_name }}-${{ github.run_number }}
+          name: model-${{ needs.resolve.outputs.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 }}
+          name: artifact-name-${{ needs.resolve.outputs.model_name }}
          path: ${{ github.workspace }}/small_output/artifact_name.txt

      - name: Re-enable powersave
@@ -257,7 +254,7 @@ jobs:

      - name: Prepare output
        env:
-          MODEL_NAME: ${{ needs.resolve.outputs.safe_model_name }}
+          MODEL_NAME: ${{ needs.resolve.outputs.model_name }}
        run: |
          source ${UV_PROJECT_ENVIRONMENT}/bin/activate
          export PYTHONPATH=${{ github.workspace }}
@@ -280,13 +277,13 @@ jobs:
      - name: Upload big model artifact
        uses: actions/upload-artifact@v4
        with:
-          name: model-${{ needs.resolve.outputs.safe_model_name }}-${{ github.run_number }}
+          name: model-${{ needs.resolve.outputs.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 }}
+          name: artifact-name-${{ needs.resolve.outputs.model_name }}
          path: ${{ github.workspace }}/big_output/artifact_name.txt

      - name: Re-enable powersave
@@ -321,7 +318,7 @@ jobs:
        if: ${{ inputs.target == 'small' || inputs.target == 'big' }}
        uses: actions/download-artifact@v4
        with:
-          name: artifact-name-${{ needs.resolve.outputs.safe_model_name }}
+          name: artifact-name-${{ needs.resolve.outputs.model_name }}
          path: artifact_name

      - name: Read artifact name
@@ -341,7 +338,7 @@ jobs:
      - name: Upload model to HF
        if: ${{ inputs.target == 'small' || inputs.target == 'big' }}
        env:
-          HF_TOKEN: ${{ secrets.HF_TOKEN }}
+          HF_OIDC_RESOURCE: datasets/${{ env.HF_REPO }}
          ARTIFACT_NAME: ${{ steps.artifact.outputs.artifact_name }}
        run: |
          rm -f output/artifact_name.txt
@@ -367,7 +364,7 @@ jobs:
      - name: Generate DM metadata and upload to HF
        if: ${{ inputs.target == 'dm' }}
        env:
-          HF_TOKEN: ${{ secrets.HF_TOKEN }}
+          HF_OIDC_RESOURCE: datasets/${{ env.HF_REPO }}
        run: |
          export PYTHONPATH=$(pwd)
          python3 -c "
@@ -484,29 +481,11 @@ jobs:
          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
@@ -146,7 +146,7 @@ jobs:

      - name: Validate hf_repo and JSON version
        env:
-          HF_TOKEN: ${{ secrets.HF_TOKEN }}
+          HF_OIDC_RESOURCE: datasets/${{ inputs.hf_repo }}
        run: |
          if [ ! -f "$JSON_FILE" ]; then
            echo "JSON file $JSON_FILE does not exist!"
@@ -155,8 +155,13 @@ jobs:
          python3 -c "
          import sys
          from huggingface_hub import HfApi
-          HfApi().repo_info(repo_id=sys.argv[1], repo_type='dataset')
-          print(f'Success: Repo {sys.argv[1]} exists.')
+          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)
          " "${{ inputs.hf_repo }}"

      - name: Download artifact name file
@@ -187,7 +192,7 @@ jobs:

      - name: Upload to Hugging Face
        env:
-          HF_TOKEN: ${{ secrets.HF_TOKEN }}
+          HF_OIDC_RESOURCE: datasets/${{ inputs.hf_repo }}
          ARTIFACT_NAME: ${{ steps.read-artifact-name.outputs.artifact_name }}
        run: |
          hf upload ${{ inputs.hf_repo }} \
@@ -46,13 +46,6 @@ runs:
            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
@@ -188,7 +188,7 @@ jobs:
          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"
+            TG_FLAGS="DEBUG=1 DEV=USB+AMD:LLVM WARP_DEV=QCOM FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2"
            OUTPUT_PKL="${{ env.MODELS_DIR }}/big_driving_tinygrad.pkl"
          else
            echo "QCOM build"
@@ -216,9 +216,6 @@ 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:
@@ -231,10 +228,8 @@ jobs:
        run: |
          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)
+          ONNX_HASH=$(gh api "repos/${GH_REPO}/contents/openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx?ref=${REF}" --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')
@@ -243,7 +238,7 @@ jobs:
          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
+            DEFAULTS=$(curl -fsSL "$JSON_URL" 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)
@@ -257,35 +252,18 @@ jobs:

          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..."
+          echo "Polling HF for big model availability..."
          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
+            if check_defaults; then
+              echo "Big model available on HF after $((i * 30))s"
+              exit 0
            fi
+            echo "Poll $i/90: not yet available"
          done

-          echo "::error::Build run did not complete within 45 minutes"
+          echo "::error::Big model not available on HF after 45 minutes"
          exit 1
        env:
          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
@@ -299,9 +277,6 @@ jobs:
  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:
@@ -314,10 +289,8 @@ jobs:
        run: |
          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)
+          DRIVING_HASH=$(gh api "repos/${GH_REPO}/contents/openpilot/selfdrive/modeld/models/driving_supercombo.onnx?ref=${REF}" --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')
@@ -326,7 +299,7 @@ jobs:
          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
+            DEFAULTS=$(curl -fsSL "$JSON_URL" 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)
@@ -340,35 +313,18 @@ jobs:

          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..."
+          echo "Polling HF for model availability..."
          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
+            if check_defaults; then
+              echo "Model available on HF after $((i * 30))s"
+              exit 0
            fi
+            echo "Poll $i/60: not yet available"
          done

-          echo "::error::Small model build did not complete within 30 minutes"
+          echo "::error::Small driving model not available on HF after 30 minutes"
          exit 1
        env:
          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
@@ -382,9 +338,6 @@ jobs:
  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:
@@ -397,10 +350,8 @@ jobs:
        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)
+          DM_HASH=$(gh api "repos/${GH_REPO}/contents/openpilot/selfdrive/modeld/models/dmonitoring_model.onnx?ref=${REF}" --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')
@@ -409,7 +360,7 @@ jobs:
          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
+            DEFAULTS=$(curl -fsSL "$JSON_URL" 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)
@@ -423,35 +374,18 @@ jobs:

          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..."
+          echo "Polling HF for DM model availability..."
          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
+            if check_defaults; then
+              echo "DM model available on HF after $((i * 30))s"
+              exit 0
            fi
+            echo "Poll $i/60: not yet available"
          done

-          echo "::error::DM model build did not complete within 30 minutes"
+          echo "::error::DM model not available on HF after 30 minutes"
          exit 1
        env:
          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
@@ -0,0 +1,208 @@
+# Ford C2-free model-pose tracking with measured feedback
+
+Hypothesis `model-pose-c0-c1-feedback-v6` restores the existing allocator's
+model-path C0/C1 demand for large turns when model geometry and selected
+curvature agree. The remaining selected curvature becomes C0/C1 centering
+and turn demand; C2/C3 stay zero. Selected desired curvature remains the
+measured-yaw feedback target, even when model geometry supplies the base.
+
+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. 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.
+
+## 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_target = 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 v6 applies 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. Other error directions remain frozen, and existing
+host field and slew limits still apply.
+
+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.
+
+## 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 bias/history, leaving the new 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
+reference interval is required after override. 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 v6 on Ford CAN FD `FORD_F_150_LIGHTNING_MK1`
+regardless of missing or different EPS firmware-query results. Other platforms
+retain their existing controller. V6 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 v6.
+`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. `offset_target` is the final bounded C0 target, `heading_base` the
+bounded pre-feedback C1, and `heading_target` the corrected C1 target.
+
+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`.
+During backoff, `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.
+Old v3/v4 command-equality expectations do not define
+v6 success. Historical v5 replay results remain historical observations.
+Replay fixes recorded motion and planner outputs, so enabled vehicle logs
+are still required to assess tracking error, oscillation and interventions.
@@ -16,7 +16,7 @@ export VECLIB_MAXIMUM_THREADS=1
 export QCOM_PRIORITY=12

 if [ -z "$AGNOS_VERSION" ]; then
-  export AGNOS_VERSION="19.7"
+  export AGNOS_VERSION="19.6"
 fi

 export STAGING_ROOT="/data/safe_staging"
@@ -383,6 +383,7 @@ struct CarControlSP @0xa5cd762cd951a455 {
  leadOne @2 :LeadData;
  leadTwo @3 :LeadData;
  intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
+  fordLateralPath @5 :FordLateralPath;

  struct Param {
    key @0 :Text;
@@ -403,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;
@@ -447,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 {
@@ -470,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 {
@@ -725,7 +725,6 @@ struct ChestnutState {
  pcieLtssm @7 :UInt8;
  supplyVoltage @8 :UInt16;  # mV
  supplyCurrent @9 :Int16;  # mA
-  supplyFault @10 :Bool;
 }

 struct RadarState @0x9a185389d6fdd05f {
@@ -1005,7 +1004,6 @@ struct DrivingModelData {
  frameIdExtra @1 :UInt32;
  frameDropPerc @6 :Float32;
  modelExecutionTime @7 :Float32;
-  big @8 :Bool;

  action @2 :ModelDataV2.Action;

@@ -2642,7 +2640,7 @@ struct Event {
    carStateSP @114 :Custom.CarStateSP;
    liveMapDataSP @115 :Custom.LiveMapDataSP;
    modelDataV2SP @116 :Custom.ModelDataV2SP;
-    customReserved10 @136 :Custom.CustomReserved10;
+    assistedDrivingMilestoneState @136 :Custom.AssistedDrivingMilestoneState;
    customReserved11 @137 :Custom.CustomReserved11;
    customReserved12 @138 :Custom.CustomReserved12;
    customReserved13 @139 :Custom.CustomReserved13;
@@ -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.),
@@ -56,29 +56,29 @@
  },
  {
    "name": "boot",
-    "url": "https://commadist.azureedge.net/agnosupdate/boot-6ecf6f987cd11968104abcccabbe268485d329cdb73012dfd3c381a6b8deb27d.img.xz",
-    "hash": "6ecf6f987cd11968104abcccabbe268485d329cdb73012dfd3c381a6b8deb27d",
-    "hash_raw": "6ecf6f987cd11968104abcccabbe268485d329cdb73012dfd3c381a6b8deb27d",
+    "url": "https://commadist.azureedge.net/agnosupdate/boot-b30f5eef65ec3878f3aa3dcaf2cc95c09e2c1e661cd3a38e94da37dee76f68bd.img.xz",
+    "hash": "b30f5eef65ec3878f3aa3dcaf2cc95c09e2c1e661cd3a38e94da37dee76f68bd",
+    "hash_raw": "b30f5eef65ec3878f3aa3dcaf2cc95c09e2c1e661cd3a38e94da37dee76f68bd",
    "size": 46897152,
    "sparse": false,
    "full_check": true,
    "has_ab": true,
-    "ondevice_hash": "d12e1e5b9455b62a1464558716493b33e470d7a7e88da1c4105a3b21d0961808"
+    "ondevice_hash": "6650e4c46df99ae6dfd6ee895a34b8a2a3cc490a8ce18e16cc3c451c3f822b6e"
  },
  {
    "name": "system",
-    "url": "https://commadist.azureedge.net/agnosupdate/system-3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f.img.xz",
-    "hash": "74ffc9c551e1f29cda897ace8a69080fe644f8039977c6885f2b48362e39b744",
-    "hash_raw": "3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f",
+    "url": "https://commadist.azureedge.net/agnosupdate/system-5b6ce7965904a157fd3a134ccfcb854f9ca5c1cc2a26b7cb80a4fa4e1cc4aaa3.img.xz",
+    "hash": "b134fd04e9da27fa1d359ea0f2742c216fa21a08b5c47e9be22ab3b0563d9b9b",
+    "hash_raw": "5b6ce7965904a157fd3a134ccfcb854f9ca5c1cc2a26b7cb80a4fa4e1cc4aaa3",
    "size": 4718592000,
    "sparse": true,
    "full_check": false,
    "has_ab": true,
-    "ondevice_hash": "6a992680183685eea9db99d915219a37935f45989330d9b619e880450257f448",
+    "ondevice_hash": "91242772af771ae96fe2eebc105f2b80a7e1dbaaf6003c2574b62d51b806f468",
    "alt": {
-      "hash": "3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f",
-      "url": "https://commadist.azureedge.net/agnosupdate/system-3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f.img",
+      "hash": "5b6ce7965904a157fd3a134ccfcb854f9ca5c1cc2a26b7cb80a4fa4e1cc4aaa3",
+      "url": "https://commadist.azureedge.net/agnosupdate/system-5b6ce7965904a157fd3a134ccfcb854f9ca5c1cc2a26b7cb80a4fa4e1cc4aaa3.img",
      "size": 4718592000
    }
  }
-]
+]
@@ -5,7 +5,6 @@ import logging
 import os
 import select
 import signal
-import string
 import struct
 import subprocess
 import tempfile
@@ -355,7 +354,7 @@ class Modem:
      imei = ""

    iccid = (self._atv("AT+QCCID", "+QCCID:") or "").rstrip("F")
-    if not all(c in string.hexdigits for c in iccid):
+    if not iccid.isdigit():
      iccid = ""

    imsi = first_line("AT+CIMI")
@@ -4,17 +4,11 @@ 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))
@@ -87,7 +81,7 @@ def set_usb_state(device_state, devices: list[dict]) -> None:
    entry.linkErrorCount = device["linkErrorCount"]
    entry.usb3Lane = device.get("usb3Lane", "unknown")

-    if is_chestnut_usb_id(entry.vendorId, entry.productId):
+    if (entry.vendorId, entry.productId) in CHESTNUT_USB_IDS:
      chestnut_present = True

  device_state.chestnutPresent = chestnut_present
@@ -97,6 +97,10 @@ Params::Params(const std::string &path) {
 }

 Params::~Params() {
+  flushNonBlockingWrites();
+}
+
+void Params::flushNonBlockingWrites() {
  if (future.valid()) {
    future.wait();
  }
@@ -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");
  }
@@ -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))

@@ -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);
@@ -92,12 +92,6 @@ 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}},
@@ -138,11 +132,12 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"UptimeOnroad", {PERSISTENT, FLOAT, "0.0"}},
    {"ChestnutActive", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}},
    {"ChestnutLoading", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}},
-    {"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
@@ -163,6 +158,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
    {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
    {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
+    {"FullAssistDrivenDistanceMeters", {PERSISTENT, FLOAT, "0.0"}},
    {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
    {"HasAcceptedTermsSP", {PERSISTENT, STRING, "0"}},
@@ -172,7 +168,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
    {"IsReleaseSpBranch", {CLEAR_ON_MANAGER_START, BOOL}},
    {"LastGPSPositionLLK", {PERSISTENT, STRING}},
+    {"LastDriveAssistedDrivingSummary", {PERSISTENT, JSON, "{}"}},
    {"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"MadsDrivenDistanceMeters", {PERSISTENT, FLOAT, "0.0"}},
    {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
    {"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}},
    {"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}},
@@ -232,6 +230,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"BackupManager_RestoreVersion", {PERSISTENT, STRING}},

    // sunnypilot car specific params
+    {"FordPscmObserver", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"FordVirtualAngleController", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}},
    {"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}},
    {"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}},
@@ -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 -rf %s", real_path.c_str()));
+      util::check_system(util::string_format("rm %s -rf", real_path.c_str()));
      unlink(param_path.c_str());
    }
    if (getenv("COMMA_CACHE") == nullptr) {
-      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::download_cache_root().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()));
+    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()));
    unsetenv("OPENPILOT_PREFIX");
  }

@@ -106,6 +106,13 @@ class TestParams(OpenpilotTestCase):
    assert q.get("CarParams") is None
    assert q.get("CarParams", True) == b"1"

+  def test_flush_non_blocking_writes(self):
+    self.params.put("DongleId", "first")
+    self.params.put("DongleId", "last")
+    self.params.flush()
+
+    assert self.params.get("DongleId") == "last"
+
  def test_params_all_keys(self):
    keys = Params().all_keys()

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:07bda2fe5d6be0b2854044053c384fe002e96406da119863a443b9344258b500
-size 1544
@@ -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)
@@ -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)
@@ -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
@@ -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()
@@ -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
 from opendbc.car.vehicle_model import VehicleModel
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController, FordPscmObserverPathController
+from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController, PscmStatus, select_virtual_angle_controller
 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_virtual_angle_controller(self.CP, self.params.get_bool("FordVirtualAngleController"),
+                                                               self.ford_path_controller)
+    self.ford_virtual_angle = isinstance(self.ford_path_controller, FordVirtualAngleController)
+    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,39 @@ 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_virtual_angle:
+        reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2'
+        pscm = self.sm['carStateSP'].fordPscmStatus
+        pscm_status = PscmStatus(timestamp=pscm.canMonoTime * 1e-9, lateral_state=pscm.lateralState,
+                                 limit=pscm.limit, capability=pscm.capability, denied=pscm.denied,
+                                 valid=pscm.valid and self.sm.all_checks(['carStateSP']))
+        self.ford_path = self.ford_path_controller.update(
+          ford_model, self.desired_curvature, yaw_rate=-CS.yawRate, speed=CS.vEgo, now=time.monotonic(),
+          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,
+          active=CC.latActive, valid=CS.canValid and self.sm.all_checks(['carState', 'vehicleParameters', 'modelV2', reference_service]),
+          steering_pressed=CS.steeringPressed, steering_torque=CS.steeringTorque, pscm_status=pscm_status,
+        )
+        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:
+        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)
@@ -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)
@@ -0,0 +1,346 @@
+"""C2-free curvature requests with bounded yaw tracking for the Lightning RL38 PSCM.
+
+The historical Virtual Angle name/key is retained for settings compatibility.
+C0/C1 remain path geometry, never a fitted wheel-angle or torque command.
+"""
+from collections import deque
+from dataclasses import dataclass
+import math
+import struct
+import numpy as np
+
+from openpilot.selfdrive.controls.lib.ford_path import FordPath, _blend_share, _encode_pose, _model_path, _model_pose, _relative_pose, _predicted_pose
+from opendbc.car.ford.values import CarControllerParams, FordFlags
+
+
+FEEDBACK_MIN_SPEED = 2.0
+HEADING_RESOLUTION = .0005
+
+
+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)
+
+
+@dataclass(frozen=True)
+class PathTuning:
+  filter_time: float = 0.3
+  offset_horizon: float = 8.0
+  heading_horizon: float = 7.0
+  heading_time: float = 1.0
+  offset_rate: float = 4.0
+  heading_rate: float = 0.5
+  feedback_gain: float = 1.0
+
+
+@dataclass(frozen=True)
+class PscmStatus:
+  timestamp: float
+  lateral_state: int
+  limit: int
+  capability: int
+  denied: bool
+  valid: bool = True
+
+  def invalid_reason(self, now):
+    if not self.valid or not math.isfinite(self.timestamp) or any(v not in (0, 1, 2, 3) for v in (
+      self.lateral_state, self.limit, self.capability,
+    )):
+      return 'invalid_pscm'
+    if not -.005 <= now - self.timestamp <= .15:
+      return 'stale_pscm'
+    if self.denied or self.lateral_state != 2 or self.capability not in (1, 2):
+      return 'unavailable_pscm'
+    return None
+
+
+class HeadingFeedback:
+  """Bound a heading correction using measured yaw error, not an EPS gain fit.
+
+  The nominal response interval, integration gain and low-speed policy remain
+  experimental. A downstream limit report cannot identify motor effort from
+  the sign of C1 while C0 and the PSCM's own controller are also acting.
+  """
+  def __init__(self, delay, tuning):
+    self.delay, self.tuning = delay, tuning
+    self.reset()
+
+  def reset(self, status='inactive'):
+    self.history = deque()
+    self.bias = 0.
+    self.previous_base = None
+    self.last_measurement_time = self.last_pscm_time = None
+    self.backoff_active = False
+    self.diagnostics = {'heading_bias': 0., 'feedback_status': status, 'feedback_reference_time': None,
+                        'feedback_reference_curvature': None, 'feedback_yaw_error': None, 'feedback_backoff_active': False}
+
+  def update(self, base, desired, *, yaw_rate, speed, now, measurement_time, dt, previous_command, heading_horizon, driver_override, pscm_status):
+    reason = ('missing_pscm' if pscm_status is None else pscm_status.invalid_reason(now))
+    if reason is None and self.last_pscm_time is not None and pscm_status.timestamp < self.last_pscm_time:
+      reason = 'pscm_timing'
+    if reason is None:
+      reason = ('driver_override' if driver_override or pscm_status.limit == 3 else 'low_speed' if speed < FEEDBACK_MIN_SPEED else
+                'zero_request' if base == 0. else 'disabled' if self.tuning.feedback_gain == 0. else None)
+    if reason is not None:
+      self.reset(reason)
+      return base
+
+    if self.previous_base is not None and base * self.previous_base < 0.:
+      self.reset('reversal')
+    elif self.previous_base and abs(base) < abs(self.previous_base):
+      # Releasing a clipped base, rather than raw curvature, avoids increasing
+      # total C1 by shrinking a negative correction while the base stays capped.
+      self.bias *= abs(base / self.previous_base)
+    self.previous_base = base
+    self.last_pscm_time = pscm_status.timestamp
+    self.history.append((now, desired))
+    while len(self.history) > 2 and self.history[1][0] < now - self.delay - .25:
+      self.history.popleft()
+
+    status = 'no_new_measurement'
+    reference_time = reference_curvature = yaw_error = None
+    if measurement_time != self.last_measurement_time:
+      self.backoff_active = False
+      measurement_dt = 0. if self.last_measurement_time is None else measurement_time - self.last_measurement_time
+      self.last_measurement_time = measurement_time
+      target_time = measurement_time - self.delay
+      # Use the command actually held at the historical instant. Interpolating
+      # toward a later publication would compare against a different request.
+      reference = next((sample for sample in reversed(self.history) if sample[0] <= target_time), None)
+      if reference is None:
+        status = 'history'
+      elif not .002 <= measurement_dt <= .1:
+        status = 'measurement_timing'
+      else:
+        reference_time, reference_curvature = reference
+        yaw_error = speed * reference_curvature - yaw_rate
+        releasing = reference_curvature * desired <= 0. or (abs(reference_curvature) - abs(desired)) * heading_horizon > HEADING_RESOLUTION
+        constrained = releasing or pscm_status.limit >= 2
+        heading_before = base + self.bias
+        # Do not brake turn-in merely for exceeding an older, smaller request:
+        # measured turning must also exceed the current selected action.
+        current_yaw_error = speed * desired - yaw_rate
+        backoff = constrained and yaw_error * base < 0. and current_yaw_error * base < 0. and heading_before * base > 0.
+        if constrained and not backoff:
+          status = 'release' if releasing else 'pscm_limit'
+        else:
+          increment = self.tuning.feedback_gain * yaw_error * measurement_dt
+          if backoff:
+            # A release/limit may still reduce an excessive same-direction
+            # heading request. It cannot grow that request or cross through
+            # zero. This does not identify the PSCM's limiting mechanism or
+            # equate C1 with motor effort; all other status/driver gates apply.
+            reduced = float(np.clip(heading_before + increment, min(0., heading_before), max(0., heading_before)))
+            increment = reduced - heading_before
+          proposed = base + self.bias + increment
+          field_limited = float(np.clip(proposed, -.5, .5))
+          host_limited = previous_command + float(np.clip(field_limited - previous_command,
+                                                          -self.tuning.heading_rate * dt, self.tuning.heading_rate * dt))
+          # Admit the reachable portion of an outward increment, rather than
+          # freezing forever when a large/batched error exceeds one tick's slew.
+          # A base transition must not fabricate a correction opposite the error.
+          if yaw_error * (proposed - host_limited) > 1e-12:
+            self.bias += float(np.clip(host_limited - (base + self.bias), min(0., increment), max(0., increment)))
+            status = 'host_limit'
+          else:
+            self.bias += increment
+            status = 'integrating'
+          if backoff:
+            self.backoff_active = True
+            status = 'release_backoff' if releasing else 'pscm_backoff'
+    self.bias = float(np.clip(self.bias, -.5 - base, .5 - base))
+    target = float(np.clip(base + self.bias, -.5, .5))
+    if self.backoff_active:
+      # A rising geometry base or an unfinished slew must not outweigh
+      # backoff and increase the sent heading, even between measurements.
+      # Keep this temporary ceiling out of the integral: a new model base
+      # is not measured yaw error and must not create persistent suppression.
+      ceiling = max(0., math.copysign(1., base) * previous_command)
+      target = float(np.clip(target, -ceiling if base < 0. else 0., ceiling if base > 0. else 0.))
+    self.diagnostics = {'heading_bias': self.bias, 'feedback_status': status, 'feedback_reference_time': reference_time,
+                        'feedback_reference_curvature': reference_curvature, 'feedback_yaw_error': yaw_error,
+                        'feedback_backoff_active': self.backoff_active}
+    return target
+
+
+class PathReference:
+  """Retain model geometry in the current ego frame between model messages."""
+  def __init__(self, tuning):
+    self.tuning = tuning
+    self.path = None
+    self.model_time = None
+
+  def reset(self):
+    self.path = None
+    self.model_time = None
+
+  @staticmethod
+  def advance(path, distance, curvature):
+    stations, x, y, heading = path
+    dx, dy, yaw = _predicted_pose(distance, curvature, 0.0)
+    cosine, sine = math.cos(yaw), math.sin(yaw)
+    return stations - distance, cosine * (x - dx) + sine * (y - dy), -sine * (x - dx) + cosine * (y - dy), heading - yaw
+
+  def update(self, model, *, model_time, now, dt, speed, curvature):
+    if self.path is not None:
+      self.path = self.advance(self.path, speed * dt, curvature)
+    if model_time == self.model_time:
+      return self.path
+    raw = _model_path(model)
+    if raw is None or not all(np.isfinite(a).all() for a in raw):
+      self.reset()
+      return None
+    new = self.advance(tuple(np.array(a) for a in raw), speed * max(now - model_time, 0.0), curvature)
+    if self.path is not None:
+      # Both paths now describe the same ego frame and traveled arc. Only the
+      # model innovation is filtered; measured ego motion is accounted for at
+      # every control tick. Never average two unaligned vehicle-frame paths.
+      elapsed = model_time - self.model_time
+      alpha = elapsed / (self.tuning.filter_time + elapsed)
+      old = self.path
+      values = [new[0]]
+      for index in (1, 2, 3):
+        prior = np.interp(new[0], old[0], old[index])
+        delta = new[index] - prior
+        if index == 3:
+          delta = (delta + np.pi) % (2 * np.pi) - np.pi
+        values.append(prior + alpha * delta)
+      # Do not invent reference history beyond the previous path's coverage.
+      outside = (new[0] < old[0][0]) | (new[0] > old[0][-1])
+      for index in (1, 2, 3):
+        values[index][outside] = new[index][outside]
+      new = tuple(values)
+    self.path = new
+    self.model_time = model_time
+    return self.path
+
+
+class FordVirtualAngleController:
+  """Retain the Ford model-pose turn request and encode centering without C2.
+
+  The selected curvature gates model anticipation and remains the measured
+  tracking target. A bounded yaw-error integral corrects C1 when fresh PSCM
+  status permits; no fixed EPS gain is assumed.
+  """
+  def __init__(self, response_delay=.2, tuning: PathTuning | None = None):
+    self.tuning = tuning if tuning is not None else PathTuning()
+    if not math.isfinite(response_delay) or not .05 <= response_delay <= .5:
+      raise ValueError("response delay must be within 0.05..0.5 seconds")
+    if not all(math.isfinite(v) and v >= 0 for v in vars(self.tuning).values()) or min(
+      self.tuning.offset_horizon, self.tuning.heading_horizon, self.tuning.heading_time, self.tuning.offset_rate, self.tuning.heading_rate,
+    ) <= 0:
+      raise ValueError("invalid path tuning")
+    self.delay = response_delay
+    self.reference = PathReference(self.tuning)
+    self.feedback = HeadingFeedback(self.delay, self.tuning)
+    self.reset()
+
+  def reset(self):
+    self.reference.reset()
+    self.feedback.reset()
+    self.command = FordPath()
+    self.last_time = None
+    self.last_measurement_time = None
+    self.curvature_history = deque()
+    self.offset_request = self.heading_request = 0.0
+    self.diagnostics = {'status': 'inactive', 'hypothesis': 'model-pose-c0-c1-feedback-v6', 'command': (0., 0., 0., 0.),
+                        **self.feedback.diagnostics}
+
+  def update(self, model, desired_curvature, *, yaw_rate, speed, now, measurement_time, model_time, reference_time,
+             active, valid=True, steering_pressed=False, steering_torque=0., pscm_status: PscmStatus | None = None):
+    finite = all(math.isfinite(v) for v in (desired_curvature, yaw_rate, speed, now, measurement_time, model_time, reference_time))
+    fresh = finite and all(-.005 <= now - timestamp <= .15 for timestamp in (measurement_time, model_time, reference_time))
+    if not active or not valid or model is None or not fresh or not .3 <= speed <= 55 or abs(yaw_rate) > 3 or abs(desired_curvature) > 1:
+      self.reset()
+      self.diagnostics['status'] = 'inactive' if not active else 'invalid_input'
+      self.diagnostics['reason'] = ('inactive' if not active else 'invalid_service' if not valid else 'missing_model' if model is None else
+                                    'nonfinite' if not finite else 'stale_input' if not fresh else 'speed' if not .3 <= speed <= 55 else
+                                    'yaw_rate' if abs(yaw_rate) > 3 else 'desired_curvature')
+      return self.command
+    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.reference.model_time is not None and model_time < self.reference.model_time
+    ):
+      self.reset()
+      self.diagnostics['status'] = 'timing_reset'
+      return self.command
+    current_curvature = yaw_rate / speed
+    self.last_time = now
+    self.last_measurement_time = measurement_time
+    path = self.reference.update(model, model_time=model_time, now=now, dt=dt, speed=speed, curvature=current_curvature)
+    raw_path = _model_path(model)
+    if path is None or raw_path is None or path[0][-1] <= 0:
+      self.reset()
+      self.diagnostics['status'] = 'invalid_path'
+      return self.command
+    advance = min(speed * self.delay, path[0][-1])
+    offset_horizon = max(self.tuning.offset_horizon, speed * self.tuning.heading_time)
+    heading_horizon = max(speed * self.tuning.heading_time, self.tuning.heading_horizon)
+    model_heading_horizon = min(heading_horizon, max(path[0][-1] - advance, 0.0))
+    ego = _predicted_pose(advance, current_curvature, 0.)
+    _, model_heading = _relative_pose(advance + model_heading_horizon, path, ego)
+    self.curvature_history.append((now, current_curvature))
+    while len(self.curvature_history) > 2 and self.curvature_history[1][0] <= now - .1:
+      self.curvature_history.popleft()
+    curvature_delta = current_curvature - self.curvature_history[0][1] if now - self.curvature_history[0][0] >= .1 else 0.
+    # Reuse the working allocator's raw forward geometry and bounded short-pose
+    # correction. Filtering that geometry again would delay the turn request.
+    pose = _model_pose(raw_path, current_curvature, curvature_delta, speed)
+    aligned = desired_curvature * pose.forward_angle > 0.
+    model_share = min(_blend_share(abs(desired_curvature)), _blend_share(pose.curvature_demand)) if aligned else 0.
+    model_base = _encode_pose(pose, model_share, 0.)
+    residual_curvature = desired_curvature * (1. - model_share)
+    curvature_offset = .5 * residual_curvature * offset_horizon ** 2
+    curvature_heading = residual_curvature * heading_horizon
+    # This geometric lift replaces the remaining C2 request. It is not an EPS
+    # transfer-function equivalence or a fitted coefficient-to-wheel mapping.
+    target_offset = float(np.clip(model_base.path_offset + curvature_offset, -5.11, 5.11))
+    base_heading = float(np.clip(model_base.path_angle + curvature_heading, -.5, .5))
+    base_guard = ('zero_request' if desired_curvature == 0. else 'opposed_model' if not aligned else
+                  'curvature_only' if model_share == 0. else 'model_pose' if model_share == 1. else 'blended')
+    driver_override = steering_pressed or not math.isfinite(steering_torque) or abs(steering_torque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
+    target_heading = self.feedback.update(base_heading, desired_curvature, yaw_rate=yaw_rate, speed=speed, now=now,
+                                          measurement_time=measurement_time, dt=dt, previous_command=self.heading_request,
+                                          heading_horizon=heading_horizon, driver_override=driver_override, pscm_status=pscm_status)
+    delta_offset = target_offset - self.offset_request
+    delta_heading = target_heading - self.heading_request
+    # A slow C1 transition must not hold a C0 correction after action releases it.
+    offset_scale = min(1., self.tuning.offset_rate * dt / abs(delta_offset)) if delta_offset else 1.
+    heading_scale = min(1., self.tuning.heading_rate * dt / abs(delta_heading)) if delta_heading else 1.
+    self.offset_request += offset_scale * delta_offset
+    self.heading_request += heading_scale * delta_heading
+    offset = _packed(self.offset_request, .01, -5.12)
+    heading = _packed(self.heading_request, .0005, -.5)
+    self.command = FordPath(True, offset, heading, 0., 0.)
+    self.diagnostics = {'status': 'driver_override' if driver_override else 'active', 'hypothesis': 'model-pose-c0-c1-feedback-v6',
+                        'desired_curvature': desired_curvature, 'offset_target': target_offset, 'heading_target': target_heading,
+                        'model_offset_base': model_base.path_offset, 'model_heading_base': model_base.path_angle,
+                        'curvature_offset_base': curvature_offset, 'curvature_heading_base': curvature_heading,
+                        'model_share': model_share, 'base_guard': base_guard,
+                        'heading_base': base_heading, 'feedback_gain': self.tuning.feedback_gain, 'feedback_min_speed': FEEDBACK_MIN_SPEED,
+                        'steering_torque': steering_torque if math.isfinite(steering_torque) else None,
+                        'pscm_valid': pscm_status.valid if pscm_status is not None else False,
+                        'pscm_timestamp': pscm_status.timestamp if pscm_status is not None and math.isfinite(pscm_status.timestamp) else None,
+                        'pscm_age': now - pscm_status.timestamp if pscm_status is not None and math.isfinite(pscm_status.timestamp) else None,
+                        'pscm_limit': pscm_status.limit if pscm_status is not None else None,
+                        'pscm_capability': pscm_status.capability if pscm_status is not None else None,
+                        'pscm_lateral_state': pscm_status.lateral_state if pscm_status is not None else None,
+                        'pscm_denied': pscm_status.denied if pscm_status is not None else None,
+                        **self.feedback.diagnostics,
+                        'model_heading_target': float(np.clip(model_heading, -.5, .5)), 'model_heading_horizon': model_heading_horizon,
+                        'offset_slew_scale': offset_scale, 'heading_slew_scale': heading_scale,
+                        'measurement_age': now - measurement_time, 'model_age': now - model_time, 'reference_age': now - reference_time,
+                        'response_delay': self.delay, 'reference_filter_time': self.tuning.filter_time, 'yaw_rate': yaw_rate,
+                        'offset_horizon': offset_horizon, 'heading_horizon': heading_horizon,
+                        'command': (offset, heading, 0., 0.)}
+    return self.command
+
+
+def select_virtual_angle_controller(CP, enabled, previous_controller):
+  # The Sunnylink toggle selects this controller on the Lightning even when
+  # the startup firmware query omits EPS identification.
+  compatible = CP.brand == 'ford' and CP.flags & FordFlags.CANFD and CP.carFingerprint == 'FORD_F_150_LIGHTNING_MK1'
+  if enabled and compatible:
+    return FordVirtualAngleController(CP.steerActuatorDelay)
+  return previous_controller
@@ -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."
+}
@@ -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."
+}
@@ -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."
+}
@@ -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."
+}
@@ -0,0 +1,236 @@
+import ast
+import io
+import json
+import logging
+from pathlib import Path
+from types import SimpleNamespace
+import unittest
+from unittest.mock import Mock
+
+from openpilot.cereal import custom
+from openpilot.common.logging_extra import SwagFormatter, SwagLogger
+from openpilot.selfdrive.controls.lib.ford_path import FordPathController, FordPscmObserverPathController
+from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController, PscmStatus
+from openpilot.selfdrive.controls.tests.test_ford_path_reference 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(), FordVirtualAngleController()):
+      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_periodic_diagnostics_log_without_crashing(self):
+    controller = FordVirtualAngleController()
+    for active, valid, pressed in ((False, True, False), (True, True, False), (True, True, True), (True, False, False)):
+      controller.reset()
+      controller.update(circle(.01), .01, yaw_rate=.05, speed=10.0, now=1.0,
+                        measurement_time=1.0, model_time=1.0, reference_time=1.0, active=active,
+                        valid=valid, steering_pressed=pressed)
+      controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.01, curvature=.005,
+                                 sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000}))
+      record = self.emit_controls_event('Ford C2-free path tracking', controls)
+      self.assertEqual(record['model_mono_time'], 123456789)
+      self.assertEqual(record['measurement_mono_time'], 123450000)
+      self.assertEqual(record['reference_service'], 'modelV2')
+      self.assertEqual(record['reference_mono_time'], 123456789)
+      self.assertEqual(record['status'], controller.diagnostics['status'])
+      self.assertEqual(record['hypothesis'], 'model-pose-c0-c1-feedback-v6')
+      self.assertEqual(record['command'], list(controller.diagnostics['command']))
+      self.assertIs(record['feedback_backoff_active'], False)
+      if active and valid:
+        self.assertEqual(record['response_delay'], 0.2)
+        self.assertEqual(record['desired_curvature'], 0.01)
+        self.assertEqual(record['measured_curvature'], 0.005)
+        self.assertEqual(record['base_guard'], 'blended')
+        self.assertGreater(record['model_share'], 0.)
+        self.assertLess(record['model_share'], 1.)
+        self.assertEqual(record['heading_target'], record['heading_base'])  # missing PSCM status leaves the base intact
+        self.assertTrue(all(key in record for key in ('offset_target', 'heading_target', 'model_heading_target', 'model_heading_horizon',
+                                                    'model_age', 'reference_age', 'reference_filter_time', 'model_offset_base', 'model_heading_base',
+                                                    'curvature_offset_base', 'curvature_heading_base', 'model_share', 'base_guard')))
+
+  def test_periodic_diagnostics_distinguish_model_curvature_and_blended_bases(self):
+    for desired, geometry, guard, share in ((.02, .02, 'model_pose', 1.), (.002, .002, 'curvature_only', 0.),
+                                           (.01, .01, 'blended', 2 / 3), (-.02, .02, 'opposed_model', 0.),
+                                           (.002, 0., 'opposed_model', 0.), (0., .02, 'zero_request', 0.)):
+      with self.subTest(desired=desired, geometry=geometry):
+        controller = FordVirtualAngleController()
+        controller.update(circle(geometry), desired, yaw_rate=0., speed=10., now=1., measurement_time=1.,
+                          model_time=1., reference_time=1., active=True)
+        controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=desired, curvature=0.,
+                                   sm=SimpleNamespace(logMonoTime={'modelV2': 1_000_000_000, 'carState': 1_000_000_000}))
+        record = self.emit_controls_event('Ford C2-free path tracking', controls)
+        self.assertEqual(record['base_guard'], guard)
+        self.assertAlmostEqual(record['model_share'], share)
+        for key in ('model_offset_base', 'model_heading_base', 'curvature_offset_base', 'curvature_heading_base'):
+          self.assertEqual(record[key], controller.diagnostics[key])
+        self.assertAlmostEqual(record['offset_target'], record['model_offset_base'] + record['curvature_offset_base'])
+        self.assertAlmostEqual(record['heading_base'], record['model_heading_base'] + record['curvature_heading_base'])
+        self.assertEqual(record['feedback_status'], 'missing_pscm')
+        self.assertEqual(record['heading_bias'], 0.)
+        self.assertEqual(record['command'][2:], [0., 0.])
+        if share == 0.:
+          self.assertEqual((record['model_offset_base'], record['model_heading_base']), (0., 0.))
+        if share == 1.:
+          self.assertEqual((record['curvature_offset_base'], record['curvature_heading_base']), (0., 0.))
+
+  def test_periodic_diagnostics_log_backoff_between_measurements(self):
+    controller = FordVirtualAngleController()
+    for i in range(50):
+      now = 1. + i * .01
+      controller.update(circle(.02), .02, yaw_rate=.2, speed=10., now=now, measurement_time=now,
+                        model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, 0, 2, False))
+    cases = ((1.5, .02, 1.5, 'pscm_backoff', True), (1.51, .02, 1.5, 'no_new_measurement', True),
+             (1.52, .05, 1.52, 'pscm_limit', False))
+    for now, desired, measurement, expected_status, backoff in cases:
+      controller.update(circle(.03), desired, yaw_rate=.5, speed=10., now=now, measurement_time=measurement,
+                        model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, 2, 2, False))
+      controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=desired, curvature=.05,
+                                 sm=SimpleNamespace(logMonoTime={'modelV2': int(now * 1e9), 'carState': int(measurement * 1e9)}))
+      record = self.emit_controls_event('Ford C2-free path tracking', controls)
+      self.assertEqual(record['feedback_status'], expected_status)
+      self.assertIs(record['feedback_backoff_active'], backoff)
+      self.assertEqual(record['heading_bias'], controller.diagnostics['heading_bias'])
+      if backoff:
+        # The output ceiling is observable separately from the stored integral.
+        self.assertLess(record['heading_target'], record['heading_base'] + record['heading_bias'])
+
+  def test_actual_ford_branch_uses_selected_reference_and_disables_invalid_output(self):
+    source_path = Path(__file__).resolve().parents[1] / 'controlsd.py'
+    source = ast.parse(source_path.read_text())
+    controls_class = next(n for n in source.body if isinstance(n, ast.ClassDef) and n.name == 'Controls')
+    state_control = next(n for n in controls_class.body if isinstance(n, ast.FunctionDef) and n.name == 'state_control')
+    branch = next(n for n in state_control.body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.CP.brand == 'ford'")
+    code = compile(ast.Module(body=[branch], type_ignores=[]), str(source_path), 'exec')
+
+    class Subscriptions:
+      frame = 1  # periodic logging is covered separately
+      valid = {'lateralManeuverPlan': False, 'modelV2': True, 'carStateSP': True}
+      logMonoTime = {'carState': 995_000_000, 'modelV2': 980_000_000, 'lateralManeuverPlan': 990_000_000, 'carStateSP': 998_000_000}
+      failed_checks = set()
+
+      def __init__(self):
+        self.state_sp = custom.CarStateSP.new_message()
+        self.state_sp.fordPscmStatus = {'valid': True, 'canMonoTime': 970_000_000, 'lateralState': 2,
+                                      'limit': 1, 'capability': 2, 'denied': False}
+
+      def __getitem__(self, service):
+        if service == 'carStateSP':
+          return self.state_sp
+        raise KeyError(service)
+
+      def all_checks(self, services):
+        return all(self.valid.get(service, True) and service not in self.failed_checks for service in services)
+
+    for maneuver in (False, True):
+      sm = Subscriptions()
+      sm.valid = dict(sm.valid, lateralManeuverPlan=maneuver)
+      controller = FordVirtualAngleController()
+      controller.update = Mock(wraps=controller.update)
+      controls = SimpleNamespace(CP=SimpleNamespace(brand='ford'), sm=sm, ford_virtual_angle=True, ford_path_controller=controller,
+                                 desired_curvature=0.007, curvature=0.002, steer_limited_by_safety=True)
+      cs = SimpleNamespace(vEgo=8.0, yawRate=-.015, canValid=True, steeringPressed=False, steeringTorque=.75)
+      cc = SimpleNamespace(latActive=True)
+      actuator = SimpleNamespace(curvature=0.007)
+      environment = {'self': controls, 'CS': cs, 'CC': cc, 'actuators': actuator, 'model_v2': circle(.007),
+                     'time': SimpleNamespace(monotonic=lambda: 1.0), 'PscmStatus': PscmStatus}
+      exec(code, environment)
+      self.assertTrue(controls.ford_path.valid)
+      self.assertTrue(cc.latActive)
+      self.assertIs(controller.update.call_args.args[0], environment['model_v2'])
+      self.assertEqual(controller.update.call_args.args[1], controls.desired_curvature)
+      args = controller.update.call_args.kwargs
+      self.assertEqual(args['yaw_rate'], .015)
+      self.assertEqual(args['steering_torque'], .75)
+      status = args['pscm_status']
+      self.assertAlmostEqual(status.timestamp, .97)
+      self.assertEqual((status.lateral_state, status.limit, status.capability, status.denied, status.valid), (2, 1, 2, False, True))
+      self.assertAlmostEqual(args['measurement_time'], 0.995)
+      self.assertAlmostEqual(args['model_time'], 0.98)
+      reference_service = 'lateralManeuverPlan' if maneuver else 'modelV2'
+      self.assertAlmostEqual(args['reference_time'], sm.logMonoTime[reference_service] * 1e-9)
+      self.assertEqual(actuator.curvature, 0.0)
+
+      # C0 needs the selected action service; C1 independently needs modelV2.
+      # Reject stale/failed selected services rather than silently fall back or
+      # transmit an active zero path. A non-selected maneuver service is ignored.
+      for stale_service in {reference_service, 'modelV2'}:
+        with self.subTest(maneuver=maneuver, stale_service=stale_service):
+          controller.reset()
+          cc.latActive = True
+          sm.logMonoTime = dict(Subscriptions.logMonoTime, **{stale_service: 500_000_000})
+          exec(code, environment)
+          self.assertFalse(controls.ford_path.valid)
+          self.assertFalse(cc.latActive)
+          self.assertIsNone(controller.reference.path)
+
+      for failed_service in {reference_service, 'modelV2', 'carState', 'vehicleParameters'}:
+        with self.subTest(maneuver=maneuver, failed_service=failed_service):
+          controller.reset()
+          cc.latActive = True
+          sm.logMonoTime = Subscriptions.logMonoTime.copy()
+          sm.failed_checks = {failed_service}
+          exec(code, environment)
+          self.assertFalse(controller.update.call_args.kwargs['valid'])
+          self.assertFalse(controls.ford_path.valid)
+          self.assertFalse(cc.latActive)
+          self.assertIsNone(controller.reference.path)
+
+      if not maneuver:
+        controller.reset()
+        cc.latActive = True
+        sm.logMonoTime = dict(Subscriptions.logMonoTime, lateralManeuverPlan=500_000_000)
+        sm.failed_checks = {'lateralManeuverPlan'}
+        exec(code, environment)
+        self.assertTrue(controls.ford_path.valid)
+        self.assertTrue(cc.latActive)
+
+      # A missing, stale or invalid optional PSCM status must not disable the
+      # existing feedforward request. Feedback receives its own validity/age.
+      for fault in ('service', 'missing', 'stale_can'):
+        with self.subTest(maneuver=maneuver, pscm_fault=fault):
+          controller.reset()
+          cc.latActive = True
+          sm.logMonoTime = Subscriptions.logMonoTime.copy()
+          sm.failed_checks = {'carStateSP'} if fault == 'service' else set()
+          sm.state_sp.fordPscmStatus.valid = fault != 'missing'
+          sm.state_sp.fordPscmStatus.canMonoTime = 500_000_000 if fault == 'stale_can' else 970_000_000
+          exec(code, environment)
+          status = controller.update.call_args.kwargs['pscm_status']
+          self.assertEqual(status.valid, fault == 'stale_can')
+          self.assertAlmostEqual(status.timestamp, .5 if fault == 'stale_can' else .97)
+          self.assertTrue(controller.update.call_args.kwargs['valid'])
+          self.assertTrue(controls.ford_path.valid)
+          self.assertTrue(cc.latActive)
+
+
+if __name__ == '__main__':
+  unittest.main()
@@ -0,0 +1,94 @@
+"""Action-to-C0 regressions; these do not simulate PSCM/vehicle response."""
+import math
+import unittest
+
+from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController
+from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController
+from openpilot.selfdrive.controls.tests.test_ford_path_reference import circle
+
+
+def step(controller, t, desired, model=None, speed=8., yaw_rate=0., **kwargs):
+  inputs = {'yaw_rate': yaw_rate, 'speed': speed, 'now': t, 'measurement_time': t,
+            'model_time': math.floor((t + 1e-6) / .05) * .05, 'reference_time': t, 'active': True}
+  inputs.update(kwargs)
+  return controller.update(circle() if model is None else model, desired, **inputs)
+
+
+class TestFordCurvatureC0(unittest.TestCase):
+  def test_centering_action_survives_an_ego_anchored_model(self):
+    for sign in (-1, 1):
+      controller = FordVirtualAngleController()
+      for i in range(200):
+        # Action can request recovery even when the short model preview is flat.
+        path = step(controller, i * .01, sign * .002, speed=20.)
+      self.assertAlmostEqual(path.path_offset, sign * .4, delta=.0051)
+      self.assertAlmostEqual(path.path_angle, sign * .04, delta=.000251)
+      self.assertEqual((path.curvature, path.curvature_rate), (0., 0.))
+
+  def test_slow_turns_retain_large_absolute_demand_after_curvature_matches(self):
+    for speed in (2., 4., 6.):
+      for sign in (-1, 1):
+        controller = FordVirtualAngleController()
+        baseline = FordPathController()
+        model = circle(sign * .04)
+        for i in range(250):
+          path = step(controller, i * .01, sign * .04, model, speed, sign * .04 * speed)
+          recorded_base = baseline.update(model, sign * .04, current_curvature=sign * .04, v_ego=speed)
+        self.assertAlmostEqual(path.path_offset, recorded_base.path_offset, delta=.0051)
+        self.assertGreater(sign * path.path_offset, .9)
+        self.assertGreater(sign * path.path_angle, .2)
+
+  def test_model_heading_cannot_inject_commands_when_action_requests_zero(self):
+    for sign in (-1, 1):
+      controller = FordVirtualAngleController()
+      for i in range(250):
+        path = step(controller, i * .01, 0., circle(sign * .12), speed=5.)
+      self.assertAlmostEqual(path.path_offset, 0., delta=.0051)
+      self.assertAlmostEqual(path.path_angle, 0., delta=.000251)
+      self.assertGreater(sign * controller.diagnostics['model_heading_target'], .4)
+
+  def test_c1_reversal_cannot_delay_action_c0_release(self):
+    controller = FordVirtualAngleController()
+    # A shallow lateral displacement with a stronger heading request exercises
+    # independent release: the small C0 move must finish before the C1 slew.
+    model = circle(.06)
+    model.position.y *= .1
+    for i in range(200):
+      path = step(controller, i * .01, .04, model, speed=5.)
+    for i in range(200, 240):
+      path = step(controller, i * .01, .003125, model, speed=5.)
+    self.assertAlmostEqual(path.path_offset, .1)
+    self.assertGreater(path.path_angle, .2)
+    for i in range(240, 243):
+      path = step(controller, i * .01, 0., circle(-.12), speed=5.)
+    self.assertAlmostEqual(path.path_offset, 0., delta=.0051)
+    self.assertGreater(path.path_angle, .08)  # C1 is still in its own limited transition.
+
+  def test_both_commands_reverse_while_model_heading_requests_the_old_turn(self):
+    controller = FordVirtualAngleController()
+    model = circle(.04)
+    for i in range(200):
+      path = step(controller, i * .01, .01, model)
+    # Allow the bounded larger initial C1 request to cross zero at 0.5 rad/s.
+    for i in range(200, 320):
+      path = step(controller, i * .01, -.01, model)
+    self.assertLess(path.path_offset, -.3)
+    self.assertLess(path.path_angle, -.07)
+
+  def test_invalid_or_stale_action_clears_both_requests(self):
+    for desired, overrides in ((float('nan'), {}), (float('inf'), {}), (2., {}), (.01, {'reference_time': 0.}),
+                               (.01, {'reference_time': float('nan')}), (.01, {'reference_time': 1.2})):
+      controller = FordVirtualAngleController()
+      step(controller, .99, .01, circle(.04))
+      self.assertEqual(step(controller, 1., desired, circle(.04), **overrides), FordPath())
+      self.assertIsNone(controller.reference.path)
+
+  def test_fresh_action_source_can_change_without_an_inactive_cycle(self):
+    controller = FordVirtualAngleController()
+    self.assertTrue(step(controller, 1., .01, reference_time=.99).valid)
+    # A model/maneuver source switch can select an older but still fresh action.
+    self.assertTrue(step(controller, 1.01, .01, reference_time=.98).valid)
+
+
+if __name__ == '__main__':
+  unittest.main()
@@ -0,0 +1,54 @@
+"""Command-reference regressions, not predictions of vehicle response."""
+import unittest
+
+from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController
+from openpilot.selfdrive.controls.tests.test_ford_curvature_c0 import step
+from openpilot.selfdrive.controls.tests.test_ford_path_reference import circle
+
+
+class TestFordCurvatureHeading(unittest.TestCase):
+  def test_model_turn_cannot_hold_c1_after_action_releases(self):
+    controller = FordVirtualAngleController()
+    model = circle(.12)
+    for i in range(200):
+      path = step(controller, i * .01, .04, model, speed=5.)
+    self.assertAlmostEqual(path.path_angle, .5, delta=.000251)
+    for i in range(200, 330):
+      path = step(controller, i * .01, 0., model, speed=5.)
+    self.assertAlmostEqual(path.path_angle, 0., delta=.000251)
+    self.assertAlmostEqual(path.path_offset, 0., delta=.0051)
+
+  def test_full_heading_survives_flat_geometry_and_matching_actual_curvature(self):
+    for speed in (3., 8., 20.):
+      for sign in (-1, 1):
+        controller = FordVirtualAngleController()
+        for i in range(200):
+          path = step(controller, i * .01, sign * .02, circle(), speed=speed, yaw_rate=sign * .02 * speed)
+        self.assertAlmostEqual(path.path_angle, sign * .02 * max(7., speed), delta=.000251)
+        self.assertEqual((path.curvature, path.curvature_rate), (0., 0.))
+
+  def test_heading_reverses_with_action_while_model_keeps_old_turn(self):
+    controller = FordVirtualAngleController()
+    model = circle(.12)
+    for i in range(200):
+      path = step(controller, i * .01, .04, model, speed=5.)
+    for i in range(200, 360):
+      path = step(controller, i * .01, -.04, model, speed=5.)
+    self.assertAlmostEqual(path.path_angle, -.28, delta=.000251)
+    self.assertLess(path.path_offset, 0.)
+
+  def test_forward_geometry_supplies_large_turns_only_while_aligned(self):
+    straight, bent = FordVirtualAngleController(), FordVirtualAngleController()
+    for i in range(200):
+      plain = step(straight, i * .01, .04, circle(), speed=5.)
+      turn = step(bent, i * .01, .04, circle(.065), speed=5.)
+    self.assertGreater(turn.path_offset, plain.path_offset)
+    self.assertGreater(turn.path_angle, plain.path_angle)
+    for i in range(200, 400):
+      plain = step(straight, i * .01, -.04, circle(), speed=5.)
+      turn = step(bent, i * .01, -.04, circle(.065), speed=5.)
+    self.assertEqual(turn, plain)
+
+
+if __name__ == '__main__':
+  unittest.main()
@@ -0,0 +1,67 @@
+import hashlib
+import json
+from pathlib import Path
+from types import SimpleNamespace
+import unittest
+
+import numpy as np
+
+from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController
+
+
+class TestFordCurvatureHeadingRoutes(unittest.TestCase):
+  @classmethod
+  def setUpClass(cls):
+    fixture = Path(__file__).parent / 'fixtures/ford_curvature_heading_route80.npz'
+    metadata = json.loads(fixture.with_suffix('.json').read_text())
+    if hashlib.sha256(fixture.read_bytes()).hexdigest() != metadata['fixture_sha256']:
+      raise ValueError('Route80 command fixture hash mismatch')
+    cls.data = data = dict(np.load(fixture))
+    models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in data['models']]
+    previous_episode = None
+    commands, gates, statuses, biases = [], [], [], []
+    for i, now in enumerate(data['t']):
+      if data['episode'][i] != previous_episode:
+        controller = FordVirtualAngleController()
+        previous_episode = data['episode'][i]
+      command = controller.update(models[data['model_index'][i]], data['desired_curvature'][i],
+                                  yaw_rate=data['yaw_rate'][i], speed=data['speed'][i], now=now,
+                                  measurement_time=data['measurement_time'][i], model_time=data['model_time'][i],
+                                  reference_time=data['reference_time'][i], active=bool(data['active'][i]),
+                                  valid=bool(data['valid'][i]), steering_pressed=bool(data['pressed'][i]))
+      commands.append((command.path_offset, command.path_angle, command.curvature, command.curvature_rate))
+      gates.append(command.valid)
+      statuses.append(controller.diagnostics['status'])
+      biases.append(controller.diagnostics['heading_bias'])
+    cls.commands = np.array(commands)
+    cls.gates = np.array(gates)
+    cls.statuses = np.array(statuses)
+    cls.biases = np.array(biases)
+
+  def test_output_gates_match_frozen_v3(self):
+    np.testing.assert_array_equal(self.gates, self.data['v3_valid'])
+    np.testing.assert_array_equal(self.statuses, self.data['v3_status'])
+    np.testing.assert_array_equal(self.commands[:, 2:], 0.)
+
+  def test_missing_pscm_retains_bounded_base_without_integrating(self):
+    # These older inputs omit PSCM status. They must retain a usable base and
+    # normal output guards without inventing feedback eligibility. Large-turn
+    # authority and measured backoff have separate route83 evidence fixtures.
+    np.testing.assert_array_equal(self.biases, 0.)
+    self.assertTrue(np.isfinite(self.commands).all())
+    self.assertLessEqual(float(np.max(abs(self.commands[:, 0]))), 5.11 + 1e-9)
+    self.assertLessEqual(float(np.max(abs(self.commands[:, 1]))), .5 + 1e-9)
+    np.testing.assert_array_equal(self.commands[~self.gates], 0.)
+    for episode in range(3):
+      mask = (self.data['episode'] == episode) & self.data['evidence'] & self.data['benchmark_clean']
+      self.assertGreater(int(mask.sum()), 100)
+      self.assertGreater(float(np.median(abs(self.commands[mask, 1]))), .03)
+    continuing = self.gates[1:] & self.gates[:-1] & (np.diff(self.data['episode']) == 0)
+    elapsed = np.diff(self.data['t'])[continuing]
+    steps = abs(np.diff(self.commands[:, :2], axis=0))[continuing]
+    self.assertTrue(np.all(steps[:, 0] <= 4. * elapsed + .010001))
+    self.assertTrue(np.all(steps[:, 1] <= .5 * elapsed + .000501))
+
+
+if __name__ == '__main__':
+  unittest.main()
@@ -0,0 +1,327 @@
+import hashlib
+import json
+import math
+from pathlib import Path
+from types import SimpleNamespace
+import unittest
+
+import numpy as np
+
+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_virtual_angle import FordVirtualAngleController, HeadingFeedback, PathTuning, PscmStatus
+
+
+MODEL = SimpleNamespace(position=SimpleNamespace(x=np.linspace(0., 100., 33), y=np.zeros(33)),
+                        orientation=SimpleNamespace(z=np.zeros(33)))
+AUTO_STATUS = object()
+
+
+def step(controller, now, desired=.02, yaw_rate=.08, speed=8., pscm_status=AUTO_STATUS, **overrides):
+  if pscm_status is AUTO_STATUS:
+    pscm_status = PscmStatus(timestamp=now, lateral_state=2, limit=0, capability=2, denied=False)
+  inputs = {'yaw_rate': yaw_rate, 'speed': speed, 'now': now, 'measurement_time': now,
+            'model_time': math.floor((now + 1e-6) / .05) * .05, 'reference_time': now,
+            'active': True, 'pscm_status': pscm_status, 'steering_torque': 0.}
+  inputs.update(overrides)
+  return controller.update(MODEL, desired, **inputs)
+
+
+def warm(controller, desired=.02, yaw_rate=.08, speed=8., count=200):
+  command = None
+  for i in range(count):
+    command = step(controller, i * .01, desired, yaw_rate, speed)
+  return command
+
+
+class TestFordHeadingFeedback(unittest.TestCase):
+  def test_limited_backoff_cannot_grow_the_command_when_model_base_rises(self):
+    for sign in (-1, 1):
+      feedback = HeadingFeedback(.2, PathTuning())
+      previous = sign * .2
+      for i in range(40):
+        now = i * .01
+        previous = feedback.update(sign * .2, sign * .02, yaw_rate=sign * .1, speed=5., now=now,
+                                   measurement_time=now, dt=.01, previous_command=previous, heading_horizon=7.,
+                                   driver_override=False, pscm_status=PscmStatus(now, 2, 0, 2, False))
+      # A larger model base must not defeat the measured backoff by outweighing
+      # its subtractive integral increment while the PSCM is already limited.
+      target = feedback.update(sign * .4, sign * .02, yaw_rate=sign * .3, speed=5., now=.4,
+                               measurement_time=.4, dt=.01, previous_command=previous, heading_horizon=7.,
+                               driver_override=False, pscm_status=PscmStatus(.4, 2, 2, 2, False))
+      self.assertGreaterEqual(sign * target, 0.)
+      self.assertLessEqual(sign * target, sign * previous)
+      # A model-base change is not measured yaw error. Its temporary output
+      # ceiling must not become a persistent, artificially large integral.
+      self.assertAlmostEqual(sign * feedback.bias, -.002)
+      repeated = feedback.update(sign * .5, sign * .02, yaw_rate=sign * .3, speed=5., now=.41,
+                                 measurement_time=.4, dt=.01, previous_command=target, heading_horizon=7.,
+                                 driver_override=False, pscm_status=PscmStatus(.41, 2, 2, 2, False))
+      self.assertGreaterEqual(sign * repeated, 0.)
+      self.assertLessEqual(sign * repeated, sign * target)
+
+  def test_under_and_over_response_change_only_heading(self):
+    for sign in (-1, 1):
+      deficient = FordVirtualAngleController()
+      excessive = FordVirtualAngleController()
+      matched = FordVirtualAngleController()
+      low = warm(deficient, sign * .02, sign * .08)
+      high = warm(excessive, sign * .02, sign * .24)
+      steady = warm(matched, sign * .02, sign * .16)
+      self.assertGreater(sign * low.path_angle, .20)
+      self.assertLess(sign * high.path_angle, .12)
+      self.assertAlmostEqual(sign * steady.path_angle, .16, delta=.0005)
+      self.assertEqual(low.path_offset, high.path_offset)
+      self.assertEqual(low.path_offset, steady.path_offset)
+      self.assertEqual((low.curvature, low.curvature_rate), (0., 0.))
+
+  def test_missing_pscm_status_keeps_the_existing_base(self):
+    controller = FordVirtualAngleController()
+    for i in range(300):
+      command = step(controller, i * .01, pscm_status=None)
+    self.assertAlmostEqual(command.path_angle, .16, delta=.0005)
+    self.assertAlmostEqual(command.path_offset, .64, delta=.01)
+
+  def test_generic_eps_limit_blocks_growth_but_allows_same_direction_backoff(self):
+    for sign in (-1, 1):
+      for yaw_rate in (0., .4):
+        controller = FordVirtualAngleController()
+        before = warm(controller, desired=sign * .02, yaw_rate=sign * .08)
+        previous = sign * before.path_angle
+        for i in range(200, 500):
+          now = i * .01
+          command = step(controller, now, desired=sign * .02, yaw_rate=sign * yaw_rate,
+                         pscm_status=PscmStatus(now, 2, 2, 2, False))
+          self.assertGreaterEqual(sign * command.path_angle, -.000501)
+          self.assertLessEqual(sign * command.path_angle, previous + .000501)
+          previous = sign * command.path_angle
+          if yaw_rate == 0.:
+            self.assertAlmostEqual(command.path_angle, before.path_angle, delta=.0005)
+        if yaw_rate > 0.:
+          self.assertAlmostEqual(command.path_angle, 0., delta=.0005)
+
+  def test_release_allows_backoff_without_rebuilding_turn_demand(self):
+    for sign in (-1, 1):
+      controller = FordVirtualAngleController()
+      warm(controller, desired=sign * .04, yaw_rate=sign * .32)
+      previous = .32
+      for i in range(200, 219):
+        command = step(controller, i * .01, desired=sign * .035, yaw_rate=sign * .5)
+        self.assertGreaterEqual(sign * command.path_angle, -.000501)
+        self.assertLessEqual(sign * command.path_angle, previous + .000501)
+        previous = sign * command.path_angle
+      self.assertLess(sign * controller.diagnostics['heading_bias'], 0.)
+      self.assertEqual(controller.diagnostics['feedback_status'], 'release_backoff')
+
+  def test_ineligible_feedback_clears_bias_and_requires_fresh_history(self):
+    # These guards affect feedback eligibility, while the existing base path
+    # remains available. Limit 3 reports driver activity and clears, not freezes.
+    cases = [
+      {'pscm_status': None},
+      {'pscm_status': PscmStatus(1., 2, 0, 2, False)},
+      {'pscm_status': PscmStatus(2.01, 2, 0, 2, False)},
+      {'pscm_status': PscmStatus(2., 2, 0, 2, False, False)},
+      {'pscm_status': PscmStatus(2., 1, 0, 2, False)},
+      {'pscm_status': PscmStatus(2., 2, 0, 0, False)},
+      {'pscm_status': PscmStatus(2., 2, 0, 3, False)},
+      {'pscm_status': PscmStatus(2., 2, 0, 2, True)},
+      {'pscm_status': PscmStatus(2., 2, 3, 2, False)},
+      {'pscm_status': PscmStatus(float('nan'), 2, 0, 2, False)},
+      {'pscm_status': PscmStatus(2., 2, 4, 2, False)},
+      {'pscm_status': PscmStatus(1.98, 2, 0, 2, False)},  # Fresh but moves backward.
+      {'steering_pressed': True},
+      {'steering_torque': 1.01},
+      {'steering_torque': -1.01},
+      {'steering_torque': float('nan')},
+      {'speed': 1.9},
+    ]
+    for overrides in cases:
+      with self.subTest(overrides=overrides):
+        controller = FordVirtualAngleController()
+        warm(controller)
+        self.assertGreater(controller.diagnostics['heading_bias'], .04)
+        command = step(controller, 2., **overrides)
+        self.assertTrue(command.valid)
+        self.assertEqual(controller.diagnostics['heading_bias'], 0.)
+        for i in range(201, 220):
+          step(controller, i * .01)
+          self.assertEqual(controller.diagnostics['heading_bias'], 0.)
+        for i in range(220, 232):
+          step(controller, i * .01)
+        self.assertGreater(controller.diagnostics['heading_bias'], 0.)
+
+  def test_repeated_measurement_cannot_be_integrated_again(self):
+    controller = FordVirtualAngleController()
+    before = warm(controller)
+    bias = controller.diagnostics['heading_bias']
+    self.assertGreater(bias, .04)
+    for i in range(200, 213):
+      command = step(controller, i * .01, measurement_time=1.99)
+      self.assertEqual(controller.diagnostics['heading_bias'], bias)
+      self.assertAlmostEqual(command.path_angle, before.path_angle, delta=.0005)
+
+  def test_delayed_reference_is_held_without_future_interpolation(self):
+    controller = FordVirtualAngleController()
+    for i in range(41):
+      step(controller, i * .01, yaw_rate=.16)
+    # No control request existed at .405: historical values straddle that time
+    # at .400 and .415. The future .415 request must not enter the comparison.
+    step(controller, .415, desired=.021, yaw_rate=.16)
+    for now in np.arange(.425, .596, .01):
+      step(controller, float(now), desired=.021, yaw_rate=.16)
+    step(controller, .605, desired=.021, yaw_rate=.16)
+    self.assertAlmostEqual(controller.diagnostics['feedback_reference_time'], .4, places=9)
+    self.assertAlmostEqual(controller.diagnostics['heading_bias'], 0., places=10)
+    step(controller, .625, desired=.021, yaw_rate=.16)
+    self.assertGreater(controller.diagnostics['heading_bias'], 0.)
+
+  def test_zero_and_reversal_cannot_rebuild_previous_turn_bias(self):
+    for next_desired in (0., -.02):
+      controller = FordVirtualAngleController()
+      before = warm(controller)
+      self.assertGreater(controller.diagnostics['heading_bias'], .04)
+      previous = before.path_angle
+      for i in range(200, 220):
+        command = step(controller, i * .01, desired=next_desired)
+        self.assertEqual(controller.diagnostics['heading_bias'], 0.)
+        self.assertLessEqual(command.path_angle, previous + .0005)
+        self.assertLessEqual(abs(command.path_angle - previous), .005501)
+        previous = command.path_angle
+      if next_desired == 0.:
+        for i in range(220, 320):
+          command = step(controller, i * .01, desired=0.)
+        self.assertEqual(controller.diagnostics['heading_bias'], 0.)
+        self.assertAlmostEqual(command.path_angle, 0., delta=.0005)
+
+  def test_eps_limit_still_allows_base_relative_release(self):
+    controller = FordVirtualAngleController()
+    warm(controller)
+    before_bias = controller.diagnostics['heading_bias']
+    self.assertGreater(before_bias, .04)
+    for i in range(200, 280):
+      now = i * .01
+      command = step(controller, now, desired=.01, pscm_status=PscmStatus(now, 2, 2, 2, False))
+      self.assertAlmostEqual(controller.diagnostics['heading_bias'], before_bias * .5, places=10)
+    self.assertAlmostEqual(command.path_angle, .08 + before_bias * .5, delta=.0005)
+
+  def test_release_uses_clipped_base_not_raw_curvature(self):
+    controller = FordVirtualAngleController()
+    before = warm(controller, desired=.1, yaw_rate=1.)
+    before_bias = controller.diagnostics['heading_bias']
+    self.assertLess(before_bias, -.04)
+    # Both requests give clipped base C1=.5. Scaling a negative bias by the raw
+    # curvature reduction would increase total C1 during a release.
+    after = step(controller, 2., desired=.09, yaw_rate=1., pscm_status=PscmStatus(2., 2, 2, 2, False))
+    self.assertLessEqual(controller.diagnostics['heading_bias'], before_bias)
+    self.assertLessEqual(after.path_angle, before.path_angle + .0005)
+    self.assertGreaterEqual(after.path_angle, 0.)
+
+  def test_host_field_limit_prevents_hidden_integral_growth(self):
+    controller = FordVirtualAngleController()
+    for i in range(500):
+      command = step(controller, i * .01, desired=.1, yaw_rate=0.)
+      self.assertAlmostEqual(controller.diagnostics['heading_bias'], 0., places=10)
+      self.assertLessEqual(abs(command.path_angle), .5)
+    self.assertAlmostEqual(command.path_angle, .5, delta=.0005)
+
+  def test_host_slew_limit_does_not_store_undelivered_positive_bias(self):
+    controller = FordVirtualAngleController()
+    for i in range(30):
+      command = step(controller, i * .01, desired=.02, yaw_rate=0.)
+      self.assertAlmostEqual(controller.diagnostics['heading_bias'], 0., places=10)
+      self.assertLessEqual(command.path_angle, (i + 1) * .005 + .0005)
+    for i in range(30, 70):
+      step(controller, i * .01, desired=.02, yaw_rate=0.)
+    self.assertGreater(controller.diagnostics['heading_bias'], 0.)
+
+  def test_large_or_batched_error_uses_available_host_slew(self):
+    for measurement_period, yaw_rate in ((.01, -.4), (.02, -.1)):
+      with self.subTest(measurement_period=measurement_period, yaw_rate=yaw_rate):
+        controller = FordVirtualAngleController()
+        previous = 0.
+        for i in range(600):
+          now = i * .01
+          measurement_time = math.floor((now + 1e-6) / measurement_period) * measurement_period
+          command = step(controller, now, desired=.04, speed=5., yaw_rate=yaw_rate, measurement_time=measurement_time)
+          self.assertLessEqual(abs(command.path_angle - previous), .005501)
+          self.assertLessEqual(abs(command.path_angle), .5)
+          previous = command.path_angle
+        # An increment larger than a control tick's available slew must admit
+        # its deliverable portion, rather than permanently disabling feedback.
+        self.assertGreater(controller.diagnostics['heading_bias'], .05)
+        self.assertGreater(command.path_angle, .40)
+
+  def test_can_packing_preserves_the_combined_feedback_command(self):
+    controller = FordVirtualAngleController()
+    base = FordVirtualAngleController()
+    packer = CANPacker('ford_lincoln_base_pt')
+    parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], 0)
+    bus = CanBus(fingerprint={0: {}})
+    bias_seen = False
+    for i in range(600):
+      sign = 1 if i < 300 else -1
+      command = step(controller, i * .01, desired=sign * .02, yaw_rate=sign * .08)
+      base_command = step(base, i * .01, desired=sign * .02, yaw_rate=sign * .08, pscm_status=None)
+      self.assertEqual(command.path_offset, base_command.path_offset)
+      bias_seen |= abs(controller.diagnostics['heading_bias']) > .04
+      msg = custom.CarControlSP.new_message()
+      msg.fordLateralPath.pathOffset = command.path_offset
+      msg.fordLateralPath.pathAngle = command.path_angle
+      packet = create_lat_ctl2_msg(packer, bus, 2, -msg.fordLateralPath.pathOffset, -msg.fordLateralPath.pathAngle, 0., 0., i % 16)
+      parser.update([i * 10_000_000, [packet]])
+      decoded = parser.vl['LateralMotionControl2']
+      self.assertAlmostEqual(decoded['LatCtlPathOffst_L_Actl'], -command.path_offset)
+      self.assertAlmostEqual(decoded['LatCtlPath_An_Actl'], -command.path_angle)
+      self.assertEqual(decoded['LatCtlCurv_No_Actl'], 0.)
+      self.assertEqual(decoded['LatCtlCrv_NoRate2_Actl'], 0.)
+    self.assertTrue(bias_seen)
+
+  def test_recorded_requests_use_pscm_guards_without_changing_c0(self):
+    directory = Path(__file__).parent / 'fixtures'
+    status_fixture = directory / 'ford_heading_feedback_route80_status.npz'
+    metadata = json.loads(status_fixture.with_suffix('.json').read_text())
+    base_fixture = directory / metadata['base_fixture']
+    self.assertEqual(hashlib.sha256(status_fixture.read_bytes()).hexdigest(), metadata['fixture_sha256'])
+    self.assertEqual(hashlib.sha256(base_fixture.read_bytes()).hexdigest(), metadata['base_fixture_sha256'])
+    data, eps = dict(np.load(base_fixture)), dict(np.load(status_fixture))
+    np.testing.assert_array_equal(data['t'], eps['t'])
+    models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in data['models']]
+    previous_episode = None
+    commands, bases, biases, gates = [], [], [], []
+    limit_guards = 0
+    for i, now in enumerate(data['t']):
+      if data['episode'][i] != previous_episode:
+        controller = FordVirtualAngleController()
+        base_controller = FordVirtualAngleController(tuning=PathTuning(feedback_gain=0.))
+        previous_episode = data['episode'][i]
+      pscm = PscmStatus(float(eps['pscm_timestamp'][i]), int(eps['lateral_state'][i]), int(eps['limit'][i]),
+                        int(eps['capability'][i]), bool(eps['denied'][i]), bool(eps['valid'][i]))
+      inputs = {'yaw_rate': data['yaw_rate'][i], 'speed': data['speed'][i], 'now': now,
+                'measurement_time': data['measurement_time'][i], 'model_time': data['model_time'][i],
+                'reference_time': data['reference_time'][i], 'active': bool(data['active'][i]), 'valid': bool(data['valid'][i]),
+                'steering_pressed': bool(data['pressed'][i]), 'steering_torque': float(eps['steering_torque'][i]), 'pscm_status': pscm}
+      command = controller.update(models[data['model_index'][i]], data['desired_curvature'][i], **inputs)
+      base = base_controller.update(models[data['model_index'][i]], data['desired_curvature'][i], **inputs)
+      commands.append((command.path_offset, command.path_angle, command.curvature, command.curvature_rate))
+      bases.append((base.path_offset, base.path_angle))
+      gates.append(command.valid)
+      biases.append(controller.diagnostics['heading_bias'])
+      if pscm.limit == 2:
+        self.assertNotEqual(controller.diagnostics['feedback_status'], 'integrating')
+      limit_guards += controller.diagnostics['feedback_status'] in ('pscm_limit', 'pscm_backoff')
+    commands, bases, biases = np.array(commands), np.array(bases), np.array(biases)
+    np.testing.assert_array_equal(commands[:, 0], bases[:, 0])
+    np.testing.assert_array_equal(gates, data['v3_valid'])
+    np.testing.assert_array_equal(commands[:, 2:], 0.)
+    self.assertGreater(limit_guards, 0)
+    for episode in (0, 2):
+      mask = (data['episode'] == episode) & data['evidence'] & data['benchmark_clean']
+      # Recorded motion is frozen: this establishes correction direction only,
+      # not that a new vehicle drive will close the observed tracking deficit.
+      self.assertGreater(float(np.max(biases[mask])), .001)
+      self.assertGreater(float(np.max(commands[mask, 1] - bases[mask, 1])), .005)
+
+
+if __name__ == '__main__':
+  unittest.main()
@@ -0,0 +1,50 @@
+"""Large-turn command regressions, not a model of the PSCM's wheel response."""
+import unittest
+
+from openpilot.selfdrive.controls.lib.ford_path import FordPathController
+from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController, PscmStatus
+from openpilot.selfdrive.controls.tests.test_ford_curvature_c0 import step
+from openpilot.selfdrive.controls.tests.test_ford_path_reference import circle
+
+
+class TestFordLargeManeuverBase(unittest.TestCase):
+  def test_aligned_large_turn_keeps_baseline_pose_without_integral_authority(self):
+    # A stronger forward path than the instantaneous curvature is present in
+    # the recorded successful turns. A frozen integral cannot supply that base.
+    for sign in (-1, 1):
+      with self.subTest(sign=sign):
+        model = circle(sign * .065)
+        previous, controller = FordPathController(), FordVirtualAngleController()
+        for i in range(300):
+          now = i * .01
+          baseline = previous.update(model, sign * .04, current_curvature=sign * .04, v_ego=5.)
+          actual = step(controller, now, sign * .04, model, speed=5., yaw_rate=sign * .2,
+                        pscm_status=PscmStatus(now, 2, 2, 2, False))
+        self.assertEqual(controller.diagnostics['heading_bias'], 0.)
+        self.assertAlmostEqual(actual.path_offset, baseline.path_offset, delta=.010001)
+        self.assertAlmostEqual(actual.path_angle, baseline.path_angle, delta=.000501)
+        self.assertEqual((actual.curvature, actual.curvature_rate), (0., 0.))
+
+  def test_small_action_remains_a_centering_request_despite_a_distant_turn(self):
+    for sign in (-1, 1):
+      controller = FordVirtualAngleController()
+      for i in range(300):
+        actual = step(controller, i * .01, sign * .002, circle(sign * .065), speed=5.)
+      self.assertAlmostEqual(actual.path_offset, sign * .064, delta=.005001)
+      self.assertAlmostEqual(actual.path_angle, sign * .014, delta=.000501)
+
+  def test_zero_and_reversed_action_supersede_old_model_turn(self):
+    for next_action in (0., -.04):
+      controller = FordVirtualAngleController()
+      model = circle(.065)
+      for i in range(300):
+        step(controller, i * .01, .04, model, speed=5.)
+      for i in range(300, 510):
+        actual = step(controller, i * .01, next_action, model, speed=5.)
+      self.assertAlmostEqual(actual.path_offset, 32. * next_action, delta=.005001)
+      self.assertAlmostEqual(actual.path_angle, 7. * next_action, delta=.000501)
+      self.assertEqual(controller.diagnostics['heading_bias'], 0.)
+
+
+if __name__ == '__main__':
+  unittest.main()
@@ -0,0 +1,177 @@
+import hashlib
+import json
+from pathlib import Path
+from types import SimpleNamespace
+import unittest
+
+import numpy as np
+
+from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController, PscmStatus
+
+
+class TestFordLargeTurnRoutes(unittest.TestCase):
+  @classmethod
+  def setUpClass(cls):
+    cls.fixture = Path(__file__).parent / 'fixtures/ford_large_turn_requests_route83.npz'
+    cls.metadata = json.loads(cls.fixture.with_suffix('.json').read_text())
+    cls.data = dict(np.load(cls.fixture))
+    cls.models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in cls.data['models']]
+    data = cls.data
+    commands, gates, statuses, bases, targets, errors, before_backoff = [], [], [], [], [], [], []
+    backoff_active, previous_commands, repeated_measurements = [], [], []
+    previous_episode = None
+    for i, now in enumerate(data['t']):
+      if data['episode'][i] != previous_episode:
+        controller = FordVirtualAngleController(response_delay=cls.metadata['response_delay'])
+        previous_episode = data['episode'][i]
+      pscm = PscmStatus(float(data['pscm_timestamp'][i]), int(data['pscm_lateral_state'][i]), int(data['pscm_limit'][i]),
+                        int(data['pscm_capability'][i]), bool(data['pscm_denied'][i]), bool(data['pscm_valid'][i]))
+      previous_base, previous_bias = controller.feedback.previous_base, controller.feedback.bias
+      previous_commands.append(controller.heading_request)
+      repeated_measurements.append(data['measurement_time'][i] == controller.feedback.last_measurement_time)
+      path = controller.update(cls.models[data['model_index'][i]], data['desired_curvature'][i],
+                               yaw_rate=data['yaw_rate'][i], speed=data['speed'][i], now=now,
+                               measurement_time=data['measurement_time'][i], model_time=data['model_time'][i],
+                               reference_time=data['reference_time'][i], active=bool(data['active'][i]), valid=bool(data['valid'][i]),
+                               steering_pressed=bool(data['pressed'][i]), steering_torque=data['steering_torque'][i], pscm_status=pscm)
+      commands.append((path.path_offset, path.path_angle, path.curvature, path.curvature_rate))
+      gates.append(path.valid)
+      statuses.append(controller.diagnostics['feedback_status'])
+      base = controller.diagnostics.get('heading_base', 0.)
+      # Account for release of the base before testing the direction of the
+      # separate constrained feedback step on these changing recorded requests.
+      retained_bias = previous_bias
+      if previous_base is None or previous_base * base < 0.:
+        retained_bias = 0.
+      elif previous_base:
+        retained_bias *= min(1., abs(base / previous_base))
+      before_backoff.append(base + retained_bias)
+      bases.append(base)
+      targets.append(controller.diagnostics.get('heading_target', 0.))
+      errors.append(controller.diagnostics.get('feedback_yaw_error') or 0.)
+      backoff_active.append(controller.diagnostics.get('feedback_backoff_active', False))
+    cls.commands, cls.gates, cls.statuses = np.array(commands), np.array(gates), np.array(statuses)
+    cls.bases, cls.targets, cls.errors, cls.before_backoff = np.array(bases), np.array(targets), np.array(errors), np.array(before_backoff)
+    cls.backoff_active = np.array(backoff_active)
+    cls.previous_commands, cls.repeated_measurements = np.array(previous_commands), np.array(repeated_measurements)
+
+  def test_fixture_authority_targets_are_recorded_successes(self):
+    self.assertEqual(hashlib.sha256(self.fixture.read_bytes()).hexdigest(), self.metadata['fixture_sha256'])
+    authority_targets = 0
+    for i, window in enumerate(self.metadata['windows']):
+      if window['role'] != 'authority_target':
+        continue
+      authority_targets += 1
+      mask = self.data['window_masks'][:, i]
+      ratio = np.median(self.data['recorded_response_curvature_02s'][mask] / self.data['desired_curvature'][mask])
+      self.assertGreaterEqual(ratio, .90)
+      self.assertLessEqual(ratio, 1.10)
+      self.assertGreaterEqual(np.max(abs(self.data['wheel_deg'][mask])), 150.)
+    self.assertGreaterEqual(authority_targets, 2)
+    over = next(window for window in self.metadata['windows'] if window['name'] == 'large_over_response_290deg')
+    self.assertEqual(over['role'], 'over_response_challenge_not_target')
+
+  def test_successful_large_turns_retain_recorded_command_scale(self):
+    # The requirement is command construction, not a predicted wheel response.
+    # Retain at least 85% of the successful send-clamped C0/C1 medians during
+    # the complete eligible turn, held request, and eligible increasing request.
+    for i, window in enumerate(self.metadata['windows']):
+      if window['role'] != 'authority_target':
+        continue
+      for phase in (None, 'phase_held', 'phase_turn_in'):
+        with self.subTest(window=window['name'], phase=phase):
+          mask = self.data['window_masks'][:, i].copy()
+          if phase is not None:
+            mask &= self.data[phase]
+          self.assertGreaterEqual(int(mask.sum()), 10)
+          recorded = np.median(abs(self.data['recorded_send_clamped'][mask, :2]), axis=0)
+          candidate = np.median(abs(self.commands[mask, :2]), axis=0)
+          self.assertTrue(np.all(candidate >= .85 * recorded), (candidate, recorded))
+          self.assertGreater(np.median(self.commands[mask, 0] * self.data['desired_curvature'][mask]), 0.)
+          self.assertGreater(np.median(self.commands[mask, 1] * self.data['desired_curvature'][mask]), 0.)
+
+  def test_small_release_and_reversal_keep_curvature_centering(self):
+    for i, window in enumerate(self.metadata['windows']):
+      if window['role'] not in ('release', 'reversal'):
+        continue
+      with self.subTest(window=window['name']):
+        mask = self.data['window_masks'][:, i]
+        np.testing.assert_array_equal(self.commands[mask, 0], self.data['v5_full_replay'][mask, 0])
+        np.testing.assert_array_equal(self.bases[mask], self.data['v5_full_heading_base'][mask])
+
+  def test_all_windows_respect_gates_limits_and_pscm_guards(self):
+    data = self.data
+    np.testing.assert_array_equal(self.commands[:, 2:], 0.)
+    np.testing.assert_array_equal(self.gates[data['evidence']], data['v5_full_valid'][data['evidence']])
+    self.assertTrue(np.isfinite(self.commands).all())
+    self.assertTrue((abs(self.commands[:, :2]) <= np.array([5.110000001, .500000001])).all())
+    continuous = (data['episode'][1:] == data['episode'][:-1]) & self.gates[1:] & self.gates[:-1]
+    limits = np.diff(data['t'])[:, None] * np.array([4., .5]) + np.array([.01, .0005]) + 1e-8
+    self.assertTrue((abs(np.diff(self.commands[:, :2], axis=0))[continuous] <= limits[continuous]).all())
+    limited = data['pscm_limit'] >= 2
+    self.assertFalse(np.isin(self.statuses[limited], ('integrating', 'host_limit')).any())
+
+  def test_constrained_backoff_only_reduces_same_sign_heading(self):
+    backoff = np.isin(self.statuses, ('release_backoff', 'pscm_backoff'))
+    self.assertGreater(int(backoff.sum()), 100)
+    self.assertTrue((self.errors[backoff] * self.bases[backoff] < 0.).all())
+    current_error = self.data['speed'] * self.data['desired_curvature'] - self.data['yaw_rate']
+    self.assertTrue((current_error[backoff] * self.bases[backoff] < 0.).all())
+    self.assertTrue((self.before_backoff[backoff] * self.bases[backoff] > 0.).all())
+    self.assertTrue((abs(self.targets[backoff]) <= abs(self.before_backoff[backoff]) + 1e-10).all())
+    self.assertTrue((self.targets[backoff] * self.bases[backoff] >= -1e-12).all())
+
+  def test_recorded_over_response_gets_heading_backoff(self):
+    index = next(i for i, window in enumerate(self.metadata['windows']) if window['name'] == 'large_over_response_290deg')
+    mask = self.data['window_masks'][:, index]
+    # With this same v6 feedforward and the former freeze-only feedback policy,
+    # the recorded challenge's median C1 is .5 rad. Require a measurable command
+    # reduction, not a simulated improvement in the old vehicle trajectory.
+    self.assertLess(float(np.median(abs(self.commands[mask, 1]))), .5 - .02)
+    self.assertTrue(np.isin(self.statuses[mask], ('release_backoff', 'pscm_backoff')).any())
+    # The overshooting fallback C0 is a ceiling comparison, never an authority
+    # target that a test should force the candidate to reach or exceed.
+    self.assertLessEqual(float(np.median(abs(self.commands[mask, 0]))),
+                         float(np.median(abs(self.data['recorded_send_clamped'][mask, 0]))) + .01)
+
+  def test_backoff_ceiling_prevents_heading_growth_between_measurements(self):
+    # A rising model heading must not outweigh a measured backoff, including
+    # controller ticks that reuse the same CAN yaw observation. C0 is separate.
+    mask = self.backoff_active
+    self.assertGreater(int(mask.sum()), 100)
+    ceiling = np.maximum(0., np.sign(self.bases[mask]) * self.previous_commands[mask])
+    self.assertTrue((abs(self.targets[mask]) <= ceiling + 1e-10).all())
+    self.assertTrue((self.targets[mask] * self.bases[mask] >= -1e-12).all())
+    self.assertTrue((abs(self.commands[mask, 1]) <= abs(self.previous_commands[mask]) + .0005 + 1e-10).all())
+    repeated = mask & self.repeated_measurements
+    self.assertGreater(int(repeated.sum()), 0)
+    self.assertTrue((self.statuses[repeated] == 'no_new_measurement').all())
+
+  def test_feedback_error_sign_with_a_large_recorded_model(self):
+    window_index = next(i for i, window in enumerate(self.metadata['windows']) if window['name'] == 'successful_large_181deg')
+    indices = np.flatnonzero(self.data['window_masks'][:, window_index] & self.data['phase_held'])
+    index = int(indices[len(indices) // 2])
+    recorded_model = self.data['models'][self.data['model_index'][index]]
+    magnitude = abs(self.data['desired_curvature'][index])
+    speed = self.data['speed'][index]
+    original_sign = np.sign(self.data['desired_curvature'][index])
+    # Hold this recorded geometry and request while varying the yaw observation.
+    # This tests feedback direction with model-pose feedforward, not plant motion.
+    for sign in (-1., 1.):
+      model = SimpleNamespace(position=SimpleNamespace(x=recorded_model[0], y=recorded_model[1] * sign / original_sign),
+                              orientation=SimpleNamespace(z=recorded_model[2] * sign / original_sign))
+      for response_fraction in (.5, 1.5):
+        with self.subTest(sign=sign, response_fraction=response_fraction):
+          controller = FordVirtualAngleController()
+          for i in range(400):
+            now = i * .01
+            controller.update(model, sign * magnitude, yaw_rate=sign * magnitude * speed * response_fraction,
+                              speed=speed, now=now, measurement_time=now, model_time=now, reference_time=now, active=True,
+                              pscm_status=PscmStatus(now, 2, 0, 2, False))
+          self.assertEqual(controller.diagnostics['base_guard'], 'model_pose')
+          correction_along_error = controller.diagnostics['heading_bias'] * sign * np.sign(1. - response_fraction)
+          self.assertGreater(correction_along_error, .01)
+
+
+if __name__ == '__main__':
+  unittest.main()
@@ -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
@@ -0,0 +1,196 @@
+import math
+import hashlib
+import json
+from pathlib import Path
+from types import SimpleNamespace
+import unittest
+
+import numpy as np
+
+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_virtual_angle import FordVirtualAngleController
+
+
+def circle(curvature=0.0, offset=0.0):
+  arc = np.linspace(0, 60, 241)
+  heading = curvature * arc
+  x = np.sin(heading) / curvature if curvature else arc
+  y = (1 - np.cos(heading)) / curvature if curvature else np.zeros(len(arc))
+  return SimpleNamespace(position=SimpleNamespace(x=x, y=y + offset), orientation=SimpleNamespace(z=heading))
+
+
+def run_step(controller, model, t, curvature=0.0, speed=8.0, desired_curvature=0.0, **kwargs):
+  inputs = {'yaw_rate': curvature * speed, 'speed': speed, 'now': t, 'measurement_time': t,
+            'model_time': math.floor((t + 1e-6) / .05) * .05, 'reference_time': t, 'active': True}
+  inputs.update(kwargs)
+  return controller.update(model, desired_curvature, **inputs)
+
+
+class TestFordPathReference(unittest.TestCase):
+  def test_model_translation_cannot_add_c0_when_action_is_zero(self):
+    for offset in (-.8, .8):
+      controller = FordVirtualAngleController()
+      model = circle(offset=offset)
+      for i in range(500):
+        path = run_step(controller, model, i * .01)
+      self.assertAlmostEqual(path.path_offset, 0., delta=.01)
+      self.assertAlmostEqual(path.path_angle, 0., delta=.0005)
+      self.assertEqual((path.curvature, path.curvature_rate), (0, 0))
+
+  def test_large_path_demands_survive_even_when_measured_curvature_matches(self):
+    for sign in (-1, 1):
+      for curvature, speed, min_offset, min_heading in ((.02, 8., .4, .14), (.08, 4., 1.8, .45)):
+        controller = FordVirtualAngleController()
+        model = circle(sign * curvature)
+        for i in range(600):
+          path = run_step(controller, model, i * .01, curvature=sign * curvature, speed=speed, desired_curvature=sign * curvature)
+        self.assertGreater(sign * path.path_offset, min_offset)
+        self.assertGreater(sign * path.path_angle, min_heading)
+        self.assertEqual((path.curvature, path.curvature_rate), (0, 0))
+
+  def test_ego_motion_is_not_delayed_by_the_model_filter(self):
+    controller = FordVirtualAngleController()
+    model = circle(offset=.5)
+    run_step(controller, model, 0., curvature=.02, speed=10.)
+    initial = tuple(a.copy() for a in controller.reference.path)
+    for i in range(1, 11):
+      run_step(controller, model, i * .01, curvature=.02, speed=10., model_time=0.)
+    _, x, y, heading = controller.reference.path
+    yaw = .02  # 1 m traveled on 0.02/m curvature
+    dx, dy = math.sin(yaw) / .02, (1 - math.cos(yaw)) / .02
+    expected_x = math.cos(yaw) * (initial[1] - dx) + math.sin(yaw) * (initial[2] - dy)
+    expected_y = -math.sin(yaw) * (initial[1] - dx) + math.cos(yaw) * (initial[2] - dy)
+    np.testing.assert_allclose(x, expected_x, atol=1e-10)
+    np.testing.assert_allclose(y, expected_y, atol=1e-10)
+    np.testing.assert_allclose(heading, initial[3] - yaw, atol=1e-10)
+
+  def test_model_noise_is_filtered_for_diagnostics_without_steering_the_command(self):
+    controller = FordVirtualAngleController()
+    values = []
+    for i in range(1600):
+      t = i * .01
+      mt = math.floor((t + 1e-6) / .05) * .05
+      angle = .02 + .01 * math.sin(2 * math.pi * 1.78 * mt)
+      model = circle()
+      model.position.y = model.position.x * math.sin(angle)
+      model.position.x = model.position.x * math.cos(angle)
+      model.orientation.z[:] = angle
+      path = run_step(controller, model, t)
+      self.assertAlmostEqual(path.path_angle, 0.)
+      values.append(controller.diagnostics['model_heading_target'])
+    values = np.array(values[600:])
+    self.assertAlmostEqual(float(np.mean(values)), .02, delta=.001)
+    self.assertLess(float(np.ptp(values)), .009)  # raw heading varies by 0.02 rad
+
+  def test_invalid_or_stale_path_resets_and_reengages_from_zero(self):
+    for overrides in ({'valid': False}, {'active': False}, {'speed': .1}, {'model_time': 0.},
+                      {'measurement_time': 0.}, {'yaw_rate': float('nan')}):
+      controller = FordVirtualAngleController()
+      for i in range(100):
+        run_step(controller, circle(.03), i * .01, desired_curvature=.03)
+      self.assertEqual(run_step(controller, circle(.03), 1., **overrides), FordPath())
+      path = run_step(controller, circle(.03), 1.01, desired_curvature=.03)
+      self.assertLessEqual(abs(path.path_offset), .05)
+      self.assertLessEqual(abs(path.path_angle), .0055)
+
+  def test_clock_faults_clear_the_reference_and_slew_state(self):
+    for now, overrides in ((1.04, {}), (1.25, {}), (1.06, {'measurement_time': 1.049}), (1.06, {'model_time': 1.049})):
+      controller = FordVirtualAngleController()
+      run_step(controller, circle(.03), 1.)
+      run_step(controller, circle(.03), 1.05)
+      path = run_step(controller, circle(.03), now, **overrides)
+      self.assertEqual(path, FordPath())
+      self.assertEqual(controller.diagnostics['status'], 'timing_reset')
+      self.assertIsNone(controller.reference.path)
+      self.assertEqual((controller.offset_request, controller.heading_request), (0., 0.))
+
+  def test_malformed_new_geometry_cannot_keep_an_old_active_request(self):
+    malformed = [None, circle(), circle(), circle()]
+    malformed[1].position.y[5] = float('nan')
+    malformed[2].position.x = []
+    malformed[3].position.x[:] = 0.
+    for model in malformed:
+      for now, model_time in ((1.05, 1.05), (1.01, 1.)):
+        controller = FordVirtualAngleController()
+        run_step(controller, circle(.03), 1.)
+        self.assertEqual(run_step(controller, model, now, model_time=model_time), FordPath())
+        self.assertIsNone(controller.reference.path)
+
+  def test_independent_slew_and_dbc_bounds_during_large_reversal(self):
+    controller = FordVirtualAngleController()
+    previous = FordPath()
+    for i in range(900):
+      curvature = .2 if i < 400 else -.2
+      path = run_step(controller, circle(curvature), i * .01, speed=5., desired_curvature=2 * curvature)
+      self.assertLessEqual(abs(path.path_offset), 5.11)
+      self.assertLessEqual(abs(path.path_angle), .5)
+      self.assertLessEqual(abs(path.path_offset - previous.path_offset), .050001)
+      self.assertLessEqual(abs(path.path_angle - previous.path_angle), .005501)
+      self.assertEqual((path.curvature, path.curvature_rate), (0, 0))
+      previous = path
+      if i == 399:
+        self.assertAlmostEqual(path.path_offset, 5.11)
+    self.assertAlmostEqual(path.path_offset, -5.11)
+    self.assertLess(path.path_angle, -.3)
+
+  def test_float32_and_can_packing_preserve_the_path(self):
+    controller = FordVirtualAngleController()
+    packer = CANPacker('ford_lincoln_base_pt')
+    parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], 0)
+    bus = CanBus(fingerprint={0: {}})
+    for i in range(600):
+      curvature = .08 if i < 300 else -.08
+      path = run_step(controller, circle(curvature), i * .01, speed=5., desired_curvature=curvature)
+      msg = custom.CarControlSP.new_message()
+      msg.fordLateralPath.pathOffset = path.path_offset
+      msg.fordLateralPath.pathAngle = path.path_angle
+      packet = create_lat_ctl2_msg(packer, bus, 2, -msg.fordLateralPath.pathOffset, -msg.fordLateralPath.pathAngle, 0., 0., i % 16)
+      parser.update([i * 10_000_000, [packet]])
+      decoded = parser.vl['LateralMotionControl2']
+      self.assertAlmostEqual(decoded['LatCtlPathOffst_L_Actl'], -path.path_offset)
+      self.assertAlmostEqual(decoded['LatCtlPath_An_Actl'], -path.path_angle)
+      self.assertEqual(decoded['LatCtlCurv_No_Actl'], 0.)
+
+  def test_recorded_large_maneuvers_keep_substantial_path_demand(self):
+    fixture = Path(__file__).parent / 'fixtures/ford_c2_free_path_routes.npz'
+    metadata = json.loads(fixture.with_suffix('.json').read_text())
+    self.assertEqual(hashlib.sha256(fixture.read_bytes()).hexdigest(), metadata['fixture_sha256'])
+    z = np.load(fixture)
+    models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in z['models']]
+    previous_episode = None
+    commands = []
+    for i, t in enumerate(z['t']):
+      if z['episode'][i] != previous_episode:
+        controller = FordVirtualAngleController()
+        previous_episode = z['episode'][i]
+      path = controller.update(models[z['model_index'][i]], z['desired_curvature'][i], yaw_rate=z['yaw_rate'][i], speed=z['speed'][i], now=t,
+                               measurement_time=z['measurement_time'][i], model_time=z['model_time'][i],
+                               reference_time=z['reference_time'][i],
+                               active=bool(z['active'][i]), valid=bool(z['valid'][i]), steering_pressed=bool(z['pressed'][i]))
+      commands.append((path.path_offset, path.path_angle))
+      self.assertEqual((path.curvature, path.curvature_rate), (0, 0))
+    commands = np.array(commands)
+    for episode in range(4):
+      mask = (z['episode'] == episode) & z['evidence']
+      # Do not reward a quiet controller for throwing away large maneuver demand.
+      # This is a command-envelope check against the earlier path controller,
+      # not a claim that the measured motion was solely due to these fields.
+      reference = np.median(abs(z['recorded'][mask, :2]), axis=0)
+      actual = np.median(abs(commands[mask]), axis=0)
+      self.assertGreater(actual[0], .7 * reference[0])
+      self.assertGreater(actual[1], .7 * reference[1])
+      direction = np.sign(np.median(z['recorded'][mask, 1]))
+      self.assertGreater(direction * np.median(commands[mask, 1]), 0.)
+    for episode in (5, 6):
+      mask = (z['episode'] == episode) & z['evidence']
+      # Both newly supplied failed turns must receive heading as a path term,
+      # rather than the v1 controller's tiny acceleration-error correction.
+      self.assertGreater(np.median(abs(commands[mask, 1])), .03)
+
+
+if __name__ == '__main__':
+  unittest.main()
@@ -0,0 +1,61 @@
+from pathlib import Path
+import tempfile
+from types import SimpleNamespace
+import unittest
+
+from opendbc.car.ford.values import FordFlags
+from openpilot.selfdrive.controls.lib.ford_path import FordPathController, FordPscmObserverPathController
+from openpilot.selfdrive.controls.lib.ford_virtual_angle import (
+  FordVirtualAngleController, select_virtual_angle_controller,
+)
+
+
+def car_params(**kwargs):
+  values = {'brand': 'ford', 'flags': FordFlags.CANFD, 'carFingerprint': 'FORD_F_150_LIGHTNING_MK1',
+                'steerActuatorDelay': 0.2, 'carFw': [SimpleNamespace(ecu='eps', fwVersion=b'RL38-14D003-AA')]}
+  values.update(kwargs)
+  return SimpleNamespace(**values)
+
+
+class TestVirtualAngleSelection(unittest.TestCase):
+  def test_opt_in_and_exact_vehicle_scope(self):
+    for previous in (FordPathController(), FordPscmObserverPathController()):
+      self.assertIs(select_virtual_angle_controller(car_params(), False, previous), previous)
+      for overrides in ({'brand': 'tesla'}, {'flags': 0}, {'carFingerprint': 'FORD_F_150_MK14'}):
+        self.assertIs(select_virtual_angle_controller(car_params(**overrides), True, previous), previous)
+      self.assertIsInstance(select_virtual_angle_controller(car_params(), True, previous), FordVirtualAngleController)
+
+  def test_toggle_controls_selection_independently_of_firmware_query(self):
+    for firmware in ([], [SimpleNamespace(ecu='engine', fwVersion=b'engine')],
+                     [SimpleNamespace(ecu='eps', fwVersion=b'RL38-14D003-AA')],
+                     [SimpleNamespace(ecu='eps', fwVersion=b'other')]):
+      for previous in (FordPathController(), FordPscmObserverPathController()):
+        with self.subTest(firmware=firmware, previous=type(previous).__name__):
+          cp = car_params(carFw=firmware, steerActuatorDelay=.3)
+          self.assertIs(select_virtual_angle_controller(cp, False, previous), previous)
+          chosen = select_virtual_angle_controller(cp, True, previous)
+          self.assertIsInstance(chosen, FordVirtualAngleController)
+          self.assertEqual(chosen.delay, .3)
+
+  def test_old_setting_cannot_enable_new_controller(self):
+    from openpilot.common.params import Params
+    with tempfile.TemporaryDirectory(prefix='ford-virtual-params-') as directory:
+      params = Params(directory)
+      # Simulate a stored key left on an upgraded device; it is no longer registered.
+      Path(params.get_param_path('FordSharedPathController')).write_text('1')
+      self.assertNotIn(b'FordSharedPathController', params.all_keys())
+      self.assertIs(params.get_default_value('FordVirtualAngleController'), False)
+      self.assertFalse(params.get_bool('FordVirtualAngleController'))
+      previous = FordPathController()
+      # Route83 had the toggle on but no EPS firmware records in CarParams.
+      cp = car_params(carFw=[])
+      self.assertIs(select_virtual_angle_controller(cp, params.get_bool('FordVirtualAngleController'), previous), previous)
+      params.put_bool('FordVirtualAngleController', True, block=True)
+      chosen = select_virtual_angle_controller(cp, params.get_bool('FordVirtualAngleController'), previous)
+      params.put_bool('FordVirtualAngleController', False, block=True)
+      self.assertIsInstance(chosen, FordVirtualAngleController)  # only selected at startup
+      self.assertIs(select_virtual_angle_controller(cp, params.get_bool('FordVirtualAngleController'), previous), previous)
+
+
+if __name__ == '__main__':
+  unittest.main()
@@ -10,6 +10,11 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
 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()
@@ -19,22 +24,20 @@ 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):
-  # 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
+  return 1.2 * onnx_size  + 10 * 1024 * 1024  # 20% + 10MB is plenty

 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},
+    'chestnut': {'WARP_DEV': tg_backend, 'QUEUE_DEV': 'AMD'}
+  },
  'openpilot.selfdrive.modeld.dmonitoringmodeld': {
    'default': {'DEV': tg_backend}
  },
@@ -42,7 +45,7 @@ tg_devices = { # which device to put jit inputs to at runtime

 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'
+  chestnut_tg_flags = f'DEBUG=2 DEV=USB+AMD:LLVM WARP_DEV={tg_backend} FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2'
  # the USB+AMD GPU takes an exclusive flock; serialize all targets that touch it
  chestnut_lock = File("models/.chestnut.lock").abspath

@@ -73,9 +76,10 @@ frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
 if not os.getenv('SKIP_TINYGRAD_COMPILE'):
  for chestnut in [False, True] if CHESTNUT else [False]:
    target_pkl_path = File(modeld_pkl_path(chestnut)).abspath
-    file_prefix, cmd_flags = ('big_', chestnut_tg_flags) if chestnut else ('', tg_flags)
+    # BIG_INTO_SMALL=1 builds the default target from the big model, e.g. to test it without a chestnut
+    file_prefix, cmd_flags = ('big_', chestnut_tg_flags) if chestnut else ('big_' if os.getenv('BIG_INTO_SMALL') else '', tg_flags)
    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)
+    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 '
@@ -103,7 +107,7 @@ if not os.getenv('SKIP_TINYGRAD_COMPILE'):
    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],
+      tinygrad_files + compile_modeld_script + driving_onnx_deps + [Value(chunk_targets), chunker_file],
      actions,
    )
    if chestnut:
@@ -117,7 +121,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} '
@@ -139,4 +143,5 @@ def tg_compile(flags, model_name):
     Action(do_chunk, "  [CHUNK] $TARGET")],
  )

-tg_compile(tg_flags, 'dmonitoring_model')
+if not os.getenv('SKIP_TINYGRAD_COMPILE'):
+  tg_compile(tg_flags, 'dmonitoring_model')
@@ -37,12 +37,17 @@ from tinygrad.engine.jit import TinyJit


 NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size'])
-MODELD_INPUTS = ['img_q', 'big_img_q', 'feat_q', 'desire_q', 'packed_npy_inputs']
+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')


-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 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 warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad, border_fill_val=None):
@@ -94,7 +99,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=Device.DEFAULT)
+    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)
    # 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):
@@ -113,43 +118,49 @@ 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, T-1, ...) e.g. (1, 24, 32, 512) with spatial features
-  feat_dim = math.prod(fb[2:])
+  fb = input_shapes['features_buffer']  # (1, 24, 512)
  # 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], feat_dim)}
+  shapes = {'desire': (dp[2],), 'traffic_convention': tuple(tc), 'action_t': tuple(at), 'prev_feat': (fb[0], fb[2])}
  return shapes, [math.prod(s) for s in shapes.values()]


-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])
+def make_input_queues(input_shapes, frame_skip, device):
+  input_queues, npy = make_warp_input_queues(input_shapes, frame_skip, device)

-  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:]}
+  fb = input_shapes['features_buffer']  # (1, 24, 512), past features only; the model appends the current frame's feature
+  dp = input_shapes['desire_pulse']  # (1, 25, 8)
+
+  shapes, sizes = get_policy_npy_shapes(input_shapes)
+  packed_npy_inputs = np.zeros(sum(sizes), dtype=np.float32)
  # views into the packed inputs, to be refilled at runtime
-  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(),
+  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(),
    '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_input, device='NPY').realize(),
-  }
-  return input_queues, npy, frame_views
+    'packed_npy_inputs': Tensor(packed_npy_inputs, device='NPY').realize(),
+  })
+  return input_queues, npy


 def shift_and_sample(buf, new_val, sample_fn):
@@ -165,15 +176,13 @@ 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):
+def make_warp(nv12, model_w, model_h, frame_skip):
  frame_prepare = make_frame_prepare(nv12, model_w, model_h)

  def warp(tfm, big_tfm, frame, big_frame):
-    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)
+    tfm = tfm.to(WARP_DEV)
+    big_tfm = big_tfm.to(WARP_DEV)
+    Tensor.realize(tfm, big_tfm)

    warped_frame = frame_prepare(frame, tfm).unsqueeze(0)
    warped_big_frame = frame_prepare(big_frame, big_tfm).unsqueeze(0)
@@ -186,10 +195,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)
@@ -202,50 +211,33 @@ def make_run_policy(model_runner, model_metadata, frame_skip):
    inputs = {
      'img': img,
      'big_img': big_img,
-      'features_buffer': feat_buf.reshape(model_metadata['input_shapes']['features_buffer']),
+      'features_buffer': feat_buf,
      '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 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
-
-  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")
-
+def compile_jit(jit, make_random_inputs, input_keys, make_queues):
  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)
+  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)
+
+    testing = test_val is not None or test_buffers is not None
+    n_runs = 1 if testing else 3

    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})
+      outs = fn(**{k: input_queues[k] for k in input_keys}, **random_inputs)
      mt = time.perf_counter()
      Device.default.synchronize()
      et = time.perf_counter()
@@ -264,15 +256,14 @@ def compile_jit(jit, input_keys, make_queues, benchmark_runs):
    return val, buffers

  print('capture + replay')
-  test_val, test_buffers = random_inputs_run(jit, SEED, 3)
-  print(f'pickle round trip ({benchmark_runs} runs per seed)')
+  test_val, test_buffers = random_inputs_run(jit, SEED)
+  print('pickle round trip')
  with tempfile.TemporaryFile(dir=".") as f:
    dump_oob(jit, f)
    f.seek(0)
-    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.
+    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)
  return jit


@@ -301,31 +292,27 @@ 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),
-    'input_devices': {'model': Device.DEFAULT},
-    'run_model': {},
-  }
+  out = {'metadata': make_metadata_dict(model_path)}

-  run_policy = make_run_policy(model_runner, out['metadata'], args.frame_skip)
+  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)

  for cam_w, cam_h in args.camera_resolutions:
    nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-    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)
+    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)

  with open(args.output, "wb") as f:
    dump_oob(out, f)
@@ -64,7 +64,6 @@ 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
@@ -7,12 +7,10 @@ import tempfile
 from pathlib import Path

 from openpilot.common.file_chunker import get_manifest_path
-from openpilot.common.hardware.usb import CHESTNUT_USB_PRODUCT, USB_DEVICES_PATH, is_chestnut_usb_id
+from openpilot.common.hardware.usb import CHESTNUT_FW_VERSION, CHESTNUT_USB_IDS, USB_DEVICES_PATH

 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, chestnut: bool):
@@ -52,7 +50,7 @@ def chestnut_present() -> bool:
    try:
      usb_id = (int((d / "idVendor").read_text(), 16), int((d / "idProduct").read_text(), 16))
      product = (d / "product").read_text().strip()
-      if is_chestnut_usb_id(*usb_id) and product == CHESTNUT_USB_PRODUCT:
+      if usb_id in CHESTNUT_USB_IDS and product == f"custom {CHESTNUT_FW_VERSION}-CLEAN":
        return True
    except Exception:
      pass
@@ -60,7 +58,3 @@ def chestnut_present() -> bool:

 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
@@ -4,8 +4,8 @@ import ctypes
 from functools import cached_property
 import os
 os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom
+from tinygrad.tensor import Tensor
 from tinygrad.device import Device
-import usb1
 import struct
 import threading
 import time
@@ -28,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, nv12_copy_size, MODELD_INPUTS
+from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, WARP_INPUTS, POLICY_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 chestnut_present, chestnut_compiled, chestnut_ready, modeld_pkl_path, load_oob
+from openpilot.selfdrive.modeld.helpers import chestnut_present, chestnut_compiled, modeld_pkl_path, get_tg_input_devices, load_oob

 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 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
@@ -83,37 +83,6 @@ 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:
@@ -149,15 +118,13 @@ 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:
-        state.pcieLtssm = Device["AMD"].iface.pci_dev.usb.read(0xB450, 1)[0]
+        # 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
      except Exception:
        pass

@@ -180,9 +147,9 @@ class ModelState(ModelStateBase):

  def __init__(self, cam_w: int, cam_h: int, chestnut: bool):
    ModelStateBase.__init__(self)
+    input_devices = get_tg_input_devices(PROCESS_NAME, chestnut)
+    self.WARP_DEV, self.QUEUE_DEV = input_devices['WARP_DEV'], input_devices['QUEUE_DEV']
    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]
@@ -192,11 +159,13 @@ class ModelState(ModelStateBase):
    self.chestnut = chestnut

    self.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
-    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.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.parser = Parser()
-    self.run_model = jits['run_model'][(cam_w,cam_h)]
+    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)]

  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()}
@@ -204,8 +173,14 @@ class ModelState(ModelStateBase):

  def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
          inputs: dict[str, np.ndarray], after_enqueue: Callable[[], None] | None = None) -> dict[str, np.ndarray]:
-    for key, buf in bufs.items():
-      np.copyto(self.frame_views[key], np.frombuffer(buf.data, dtype=np.uint8, count=self.frame_copy_size))
+    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]

    # Model decides when action is completed, so desire input is just a pulse triggered on rising edge
    inputs['desire_pulse'][0] = 0
@@ -216,7 +191,11 @@ class ModelState(ModelStateBase):
    self.npy['tfm'][:,:] = transforms['img'][:,:]
    self.npy['big_tfm'][:,:] = transforms['big_img'][:,:]

-    outs, = self.run_model(**{k: self.input_queues[k] for k in MODELD_INPUTS})
+    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
+    )
    if after_enqueue is not None:
      after_enqueue()
    model_output = outs.numpy()[0]
@@ -230,37 +209,25 @@ class ModelState(ModelStateBase):
    return outputs_dict

  def warmup(self) -> None:
-    dummy_frames = {k: np.zeros(self.frame_copy_size, dtype=np.uint8) for k in self.vision_input_names}
+    dummy_frames = {k: np.zeros(self.frame_buf_params[k][3], 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, self.frame_views = make_input_queues(
-      self.input_shapes, self.frame_skip, device=self.model_device, frame_copy_size=self.frame_copy_size)
+    self.input_queues, self.npy = make_input_queues(self.input_shapes, self.frame_skip, device=self.QUEUE_DEV)
    self.prev_desire[:] = 0
+    self.full_frames.clear()
+    self._blob_cache.clear()


 def main(demo=False):
  cloudlog.warning("modeld init")

-  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)
+  CHESTNUT = chestnut_present() and chestnut_compiled()
  if CHESTNUT:
    os.environ['HCQDEV_WAIT_TIMEOUT_MS'] = '3000'
  params = Params()
  params.put_bool("ChestnutLoading", CHESTNUT)
-  if chestnut_available and not CHESTNUT:
-    params.put_bool("ChestnutActive", False)
-  else:
-    params.remove("ChestnutActive")
+  params.remove("ChestnutActive")

  config_realtime_process(7, 54)

@@ -304,11 +271,7 @@ def main(demo=False):
    loader.start()
    loader.join(BIG_MODEL_TIMEOUT)
    model = big_model
-    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 CHESTNUT else None
  if model is None:
@@ -442,7 +405,6 @@ def main(demo=False):
        raise
      # fallback to small model
      cloudlog.exception("big model failed, fall back to small")
-      params.put_bool("ChestnutModelError", True)
      params.put_bool("ChestnutActive", False)
      assert small_model is not None
      model = small_model
@@ -469,11 +431,12 @@ def main(demo=False):
      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
-      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)
+      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)
      mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

      fill_driving_model_data(drivingdata_send, modelv2_send)
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:1791d5940b2c048d0639813426dd2cf1d6f2a6727ed51e17c8bcea8bbe754123
-size 765950064
+oid sha256:a501760a9d1d5fef0eab2b8c5d122d06124fc26dc8e0782e0aa94b82a208f0ff
+size 1757355221
@@ -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.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.setIgnitionLine(health.ignition_line_pkt);
+  ps.setIgnitionCan(health.ignition_can_pkt);
+  ps.setControlsAllowed(health.controls_allowed_pkt);
  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((health.flags_pkt & HEALTH_FLAG_POWER_SAVE_ENABLED) != 0U);
-  ps.setHeartbeatLost((health.flags_pkt & HEALTH_FLAG_HEARTBEAT_LOST) != 0U);
+  ps.setPowerSaveEnabled((bool)(health.power_save_enabled_pkt));
+  ps.setHeartbeatLost((bool)(health.heartbeat_lost_pkt));
  ps.setAlternativeExperience(health.alternative_experience_pkt);
  ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt));
-  ps.setInterruptLoad(health.interrupt_load_pkt / 255.0f);
+  ps.setInterruptLoad(health.interrupt_load_pkt);
  ps.setFanPower(health.fan_power);
-  ps.setSafetyRxChecksInvalid((health.flags_pkt & HEALTH_FLAG_SAFETY_RX_CHECKS_INVALID) != 0U);
+  ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid_pkt));
  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.flags_pkt |= HEALTH_FLAG_IGNITION_LINE;
+    health.ignition_line_pkt = 1;
  }

-  bool ignition_local = ((health.flags_pkt & (HEALTH_FLAG_IGNITION_LINE | HEALTH_FLAG_IGNITION_CAN)) != 0U) && !always_offroad;
+  bool ignition_local = ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0)) && !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.flags_pkt & HEALTH_FLAG_POWER_SAVE_ENABLED) != 0U) != power_save_desired) {
+  if (health.power_save_enabled_pkt != power_save_desired) {
    panda->set_power_saving(power_save_desired);
  }

@@ -19,30 +19,6 @@
  },
  "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": {
@@ -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
@@ -528,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)

@@ -646,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)
@@ -655,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)

@@ -667,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)
@@ -680,6 +744,7 @@ class SelfdriveD(CruiseHelper):
        self.step()
        self.rk.monitor_time()
    finally:
+      self.assisted_driving_milestones.close()
      e.set()
      t.join()

@@ -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", "big", "action"]:
+    for field in ["frameId", "frameIdExtra", "frameDropPerc", "modelExecutionTime", "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))
@@ -33,9 +33,9 @@ MODEL_REPLAY_BUCKET="model_replay_master"
 GITHUB = GithubUtils(API_TOKEN, DATA_TOKEN)

 EXEC_TIMINGS = [
-  # model, instant max, average max, chestnut average max
-  ("modelV2", 0.05, 0.03, 0.05),
-  ("driverStateV2", 0.05, 0.018, 0.018),
+  # model, instant max, average max
+  ("modelV2", 0.05, 0.028),
+  ("driverStateV2", 0.05, 0.018),
 ]

 def get_log_fn(test_route, ref="master"):
@@ -169,13 +169,11 @@ 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, chestnut_avg_max) in EXEC_TIMINGS:
-    avg_max = chestnut_avg_max if chestnut else avg_max
+  for (s, instant_max, avg_max) in EXEC_TIMINGS:
    ts = [getattr(m, s).modelExecutionTime for m in msgs if m.which() == s]
    # TODO some init can happen in first iteration
    ts = ts[1:]
@@ -1,7 +1,7 @@
 import time
 import pyray as rl

-from openpilot.system.ui.lib.application import gui_app, FontWeight, TextAlignment, TextAlignmentVertical
+from openpilot.system.ui.lib.application import gui_app, FontWeight
 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=TextAlignment.CENTER,
-                                       alignment_vertical=TextAlignmentVertical.MIDDLE)
+                                       alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
+                                       alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_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)
@@ -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, TextAlignment
+from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
 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=TextAlignment.RIGHT)
+    gui_label(version_rect, self._version_text, 48, rl.WHITE, alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT)

  def _render_home_content(self):
    self._render_left_column()
@@ -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, TextAlignment, gui_app
+from openpilot.system.ui.lib.application import FontWeight, 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=TextAlignment.LEFT)
+    self._title = Label(tr("Welcome to sunnypilot"), font_size=90, font_weight=FontWeight.BOLD, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_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=TextAlignment.LEFT)
+                       font_size=90, font_weight=FontWeight.MEDIUM, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_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=TextAlignment.LEFT)
+                       font_size=90, font_weight=FontWeight.MEDIUM, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_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)
@@ -199,9 +199,6 @@ 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

@@ -1,5 +1,4 @@
 import datetime
-import math
 import time

 from openpilot.cereal import log
@@ -9,7 +8,7 @@ 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, TextAlignment, TextAlignmentVertical
+from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
 from openpilot.selfdrive.ui.ui_state import ui_state, ChestnutState
 from openpilot.common.version import RELEASE_BRANCHES

@@ -70,8 +69,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=TextAlignment.CENTER,
-                alignment_vertical=TextAlignmentVertical.MIDDLE)
+                alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
+                alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE)


 class NetworkIcon(Widget):
@@ -140,9 +139,7 @@ class MiciHomeLayout(Widget):
    self._version_text = self._get_version_text()

    self._experimental_icon = IconWidget("icons_mici/experimental_mode.png", (48, 48))
-    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))
@@ -153,15 +150,13 @@ class MiciHomeLayout(Widget):
      IconWidget("icons_mici/settings.png", (48, 48), opacity=0.9),
      NetworkIcon(),
      self._experimental_icon,
-      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("openpilot", font_size=96, font_weight=FontWeight.DISPLAY, max_width=480, wrap_text=False)
+    self._openpilot_label = UnifiedLabel("sunnypilot", 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)
@@ -252,20 +247,12 @@ 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)
    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._chestnut_icon.set_visible(ui_state.chestnut_state in (ChestnutState.READY, ChestnutState.LOADING, ChestnutState.ACTIVE))
+      self._chestnut_failed_icon.set_visible(ui_state.chestnut_state in (ChestnutState.UNCOMPILED, ChestnutState.FAILED))
    self._mic_icon.set_visible(ui_state.recording_audio)
    self._body_icon.set_visible(bool(ui_state.is_body))

@@ -1,5 +1,8 @@
+import os
+
 import pyray as rl
 import openpilot.cereal.messaging as messaging
+from openpilot.common.hardware import PC
 from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
 from openpilot.selfdrive.ui.mici.layouts.settings.settings import SettingsLayout
 from openpilot.selfdrive.ui.mici.layouts.offroad_alerts import MiciOffroadAlerts
@@ -61,7 +64,8 @@ class MiciMainLayout(Scroller):

    # Start onboarding if terms or training not completed, make sure to push after self
    self._onboarding_window = OnboardingWindow(lambda: gui_app.pop_widgets_to(self))
-    if not self._onboarding_window.completed:
+    skip_onboarding_for_milestone_preview = PC and os.getenv("SP_MILESTONE_PREVIEW") == "1"
+    if not self._onboarding_window.completed and not skip_onboarding_for_milestone_preview:
      gui_app.push_widget(self._onboarding_window)

    # initialize correct onroad layout
@@ -119,6 +123,8 @@ class MiciMainLayout(Scroller):
        self._onroad_time_delay = rl.get_time()
      else:
        self._scroll_to(self._home_layout)
+        if hasattr(self._home_layout, "request_drive_summary"):
+          self._home_layout.request_drive_summary()

    # FIXME: these two pops can interrupt user interacting in the settings
    if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY:
@@ -11,7 +11,7 @@ from openpilot.common.hardware import HARDWARE
 from openpilot.system.ui.widgets import Widget
 from openpilot.system.ui.widgets.label import UnifiedLabel
 from openpilot.system.ui.widgets.scroller import Scroller
-from openpilot.system.ui.lib.application import gui_app, FontWeight, TextAlignment, TextAlignmentVertical
+from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr

 REFRESH_INTERVAL = 5.0  # seconds
@@ -62,12 +62,12 @@ class AlertItem(Widget):
    self._icon_green = gui_app.texture("icons_mici/offroad_alerts/green_wheel.png", self.ICON_SIZE, self.ICON_SIZE)

    self._title_label = UnifiedLabel(text="", font_size=32, font_weight=FontWeight.SEMI_BOLD, text_color=self.TEXT_COLOR,
-                                     alignment=TextAlignment.LEFT,
-                                     alignment_vertical=TextAlignmentVertical.TOP, line_height=0.95)
+                                     alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT,
+                                     alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, line_height=0.95)

    self._body_label = UnifiedLabel(text="", font_size=28, font_weight=FontWeight.ROMAN, text_color=self.TEXT_COLOR,
-                                    alignment=TextAlignment.LEFT,
-                                    alignment_vertical=TextAlignmentVertical.BOTTOM, line_height=0.95)
+                                    alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT,
+                                    alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM, line_height=0.95)

    self._title_text = ""
    self._body_text = ""
@@ -200,8 +200,8 @@ class MiciOffroadAlerts(Scroller):

    # Create empty state label
    self._empty_label = UnifiedLabel(tr("no alerts"), 65, FontWeight.DISPLAY, rl.WHITE,
-                                     alignment=TextAlignment.CENTER,
-                                     alignment_vertical=TextAlignmentVertical.MIDDLE)
+                                     alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
+                                     alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE)

    # Build initial alert list
    self._build_alerts()
@@ -4,7 +4,7 @@ import pyray as rl
 from collections.abc import Callable
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.qrcode import make_texture
-from openpilot.system.ui.lib.application import FontWeight, gui_app, TextAlignment
+from openpilot.system.ui.lib.application import FontWeight, gui_app
 from openpilot.system.ui.widgets import Widget
 from openpilot.system.ui.widgets.button import SmallCircleIconButton
 from openpilot.system.ui.widgets.scroller import NavScroller, Scroller
@@ -35,7 +35,7 @@ class DriverCameraSetupDialog(BaseCabinCameraDialog):

    if not self._camera_view.frame:
      gui_label(rect, tr("camera starting"), font_size=64, font_weight=FontWeight.BOLD,
-                alignment=TextAlignment.CENTER)
+                alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
      rl.end_scissor_mode()
      return

@@ -74,10 +74,6 @@ class SoftwareInfoLayoutMici(Widget):


 class CheckUpdateButton(BigButton):
-  UPDATER_PROC = "openpilot.system.updated.updated"
-  CHECK_FOR_UPDATE = "SIGUSR1"
-  DOWNLOAD_UPDATE = "SIGHUP"
-
  def __init__(self):
    self._txt_update_icon = gui_app.texture("icons_mici/settings/device/update.png", 64, 75)
    self._txt_up_to_date_icon = gui_app.texture("icons_mici/settings/device/up_to_date.png", 64, 64)
@@ -101,20 +97,15 @@ class CheckUpdateButton(BigButton):
      gui_app.push_widget(dlg)
      return

-    self._signal_updater(self.DOWNLOAD_UPDATE if self.get_value() == "download update" else self.CHECK_FOR_UPDATE)
-
-  def check_for_update(self):
-    self._signal_updater(self.CHECK_FOR_UPDATE)
-
-  def _signal_updater(self, sig: str):
    self.set_enabled(False)
    self._state = UpdaterState.WAITING_FOR_UPDATER
-    self._hide_value_t = None
-    self.set_value("")
    self.set_icon(self._txt_update_icon)

    def run():
-      subprocess.run(f"pkill -{sig} -f {self.UPDATER_PROC}", shell=True)
+      if self.get_value() == "download update":
+        subprocess.run("pkill -SIGHUP -f openpilot.system.updated.updated", shell=True)
+      else:
+        subprocess.run("pkill -SIGUSR1 -f openpilot.system.updated.updated", shell=True)

    threading.Thread(target=run, daemon=True).start()

@@ -193,7 +184,7 @@ class CheckUpdateButton(BigButton):

 class InstallUpdateButton(BigButton):
  def __init__(self):
-    super().__init__("install now", "", gui_app.texture("icons_mici/settings/device/reboot.png", 64, 70))
+    super().__init__("install update", "", gui_app.texture("icons_mici/settings/device/reboot.png", 64, 70))
    self.set_visible(lambda: ui_state.is_offroad() and ui_state.params.get_bool("UpdateAvailable"))

  def _update_state(self):
@@ -241,9 +232,8 @@ class BranchSelectPage(NavScroller):


 class TargetBranchButton(BigButton):
-  def __init__(self, check_update_btn: CheckUpdateButton):
+  def __init__(self):
    super().__init__("target branch", ui_state.params.get("UpdaterTargetBranch") or "")
-    self._check_update_btn = check_update_btn
    self.set_click_callback(self._on_click)
    self.set_visible(not ui_state.params.get_bool("IsTestedBranch"))
    self.set_enabled(lambda: ui_state.is_offroad())
@@ -256,15 +246,12 @@ class TargetBranchButton(BigButton):
      self.set_value(target)

  def _on_click(self):
-    if not ui_state.params.get("UpdaterAvailableBranches"):
-      gui_app.push_widget(BigDialog("", tr("Failed to get available branches. Ensure you're connected to the internet and try again.")))
-      return
    gui_app.push_widget(BranchSelectPage(self._on_select))

  def _on_select(self, branch: str):
    ui_state.params.put("UpdaterTargetBranch", branch, block=True)
    self.set_value(branch)
-    self._check_update_btn.check_for_update()
+    subprocess.run("pkill -SIGUSR1 -f openpilot.system.updated.updated", shell=True)


 class SoftwareLayoutMici(NavScroller):
@@ -278,11 +265,10 @@ class SoftwareLayoutMici(NavScroller):
                                                        gui_app.texture("icons_mici/settings/device/uninstall.png", 64, 64),
                                                        uninstall_openpilot_callback, exit_on_confirm=False)

-    check_update_btn = CheckUpdateButton()
    self._scroller.add_widgets([
      SoftwareInfoLayoutMici(),
-      check_update_btn,
+      CheckUpdateButton(),
      InstallUpdateButton(),
-      TargetBranchButton(check_update_btn),
+      TargetBranchButton(),
      uninstall_openpilot_btn,
    ])
@@ -47,6 +47,7 @@ class TogglesLayoutMici(NavScroller):
    is_metric_toggle = BigParamControl("use metric units", "IsMetric")
    ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled")
    always_on_dm_toggle = BigParamControl("always-on driver monitor", "AlwaysOnDM")
+    milestone_celebrations_toggle = BigParamControl("assisted driving milestones", "AssistedDrivingMilestonesEnabled")
    record_front = BigParamControl("record & upload cabin camera", "RecordFront", toggle_callback=restart_needed_callback)
    record_mic = BigParamControl("record & upload mic audio", "RecordAudio", toggle_callback=restart_needed_callback)
    enable_openpilot = BigParamControl("enable sunnypilot", "OpenpilotEnabledToggle", toggle_callback=restart_needed_callback)
@@ -57,6 +58,7 @@ class TogglesLayoutMici(NavScroller):
      is_metric_toggle,
      ldw_toggle,
      always_on_dm_toggle,
+      milestone_celebrations_toggle,
      record_front,
      record_mic,
      enable_openpilot,
@@ -68,6 +70,7 @@ class TogglesLayoutMici(NavScroller):
      ("IsMetric", is_metric_toggle),
      ("IsLdwEnabled", ldw_toggle),
      ("AlwaysOnDM", always_on_dm_toggle),
+      ("AssistedDrivingMilestonesEnabled", milestone_celebrations_toggle),
      ("RecordFront", record_front),
      ("RecordAudio", record_mic),
      ("OpenpilotEnabledToggle", enable_openpilot),
@@ -10,7 +10,7 @@ from opendbc.car.structs import car
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.common.filter_simple import BounceFilter, FirstOrderFilter
 from openpilot.common.hardware import COMMA_HARDWARE
-from openpilot.system.ui.lib.application import gui_app, FontWeight, TextAlignment
+from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.widgets import Widget
 from openpilot.system.ui.widgets.label import UnifiedLabel

@@ -20,6 +20,7 @@ AlertSize = log.SelfdriveState.AlertSize
 AlertStatus = log.SelfdriveState.AlertStatus

 ALERT_MARGIN = 18
+ALERT_BACKGROUND_OPACITY = 0.90

 ALERT_FONT_SMALL = 66 - 50
 ALERT_FONT_BIG = 88 - 40
@@ -279,7 +280,7 @@ class AlertRenderer(Widget, SpeedLimitAlertRenderer):
  def _draw_background(self, alert: Alert) -> None:
    # draw top gradient for alert text at top
    color = ALERT_COLORS.get(alert.status, ALERT_COLORS[AlertStatus.normal])
-    color = rl.Color(color.r, color.g, color.b, int(255 * 0.90 * self._alpha_filter.x))
+    color = rl.Color(color.r, color.g, color.b, int(255 * ALERT_BACKGROUND_OPACITY * self._alpha_filter.x))
    translucent_color = rl.Color(color.r, color.g, color.b, int(0 * self._alpha_filter.x))

    small_alert_height = round(self._rect.height * 0.583) # 140px at mici height
@@ -333,7 +334,7 @@ class AlertRenderer(Widget, SpeedLimitAlertRenderer):
    self._alert_text1_label.set_text(alert_text1)
    self._alert_text1_label.set_text_color(color)
    self._alert_text1_label.set_font_size(font_size)
-    self._alert_text1_label.set_alignment(TextAlignment.LEFT if icon_side != 'left' else TextAlignment.RIGHT)
+    self._alert_text1_label.set_alignment(rl.GuiTextAlignment.TEXT_ALIGN_LEFT if icon_side != 'left' else rl.GuiTextAlignment.TEXT_ALIGN_RIGHT)
    self._alert_text1_label.render(text_rect1)

    alert_text2 = alert.text2.lower()
@@ -365,5 +366,5 @@ class AlertRenderer(Widget, SpeedLimitAlertRenderer):
      self._alert_text2_label.set_text(alert_text2)
      self._alert_text2_label.set_text_color(color)
      self._alert_text2_label.set_font_size(small_font_size)
-      self._alert_text2_label.set_alignment(TextAlignment.LEFT if icon_side != 'left' else TextAlignment.RIGHT)
+      self._alert_text2_label.set_alignment(rl.GuiTextAlignment.TEXT_ALIGN_LEFT if icon_side != 'left' else rl.GuiTextAlignment.TEXT_ALIGN_RIGHT)
      self._alert_text2_label.render(text_rect2)
@@ -11,7 +11,7 @@ from openpilot.selfdrive.ui.mici.onroad.hud_renderer import HudRenderer
 from openpilot.selfdrive.ui.mici.onroad.model_renderer import ModelRenderer
 from openpilot.selfdrive.ui.mici.onroad.confidence_ball import ConfidenceBall
 from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
-from openpilot.system.ui.lib.application import FontWeight, gui_app, MousePos, MouseEvent, TextAlignment, TextAlignmentVertical
+from openpilot.system.ui.lib.application import FontWeight, gui_app, MousePos, MouseEvent
 from openpilot.system.ui.widgets.label import UnifiedLabel
 from openpilot.system.ui.widgets import Widget
 from openpilot.common.filter_simple import BounceFilter
@@ -19,10 +19,15 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCamera
 from openpilot.common.transformations.orientation import rot_from_euler
 from enum import IntEnum

+MILESTONE_CELEBRATION_ENABLED = gui_app.sunnypilot_ui()
+
 if gui_app.sunnypilot_ui():
  from openpilot.selfdrive.ui.sunnypilot.mici.onroad.hud_renderer import HudRendererSP as HudRenderer
  from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus

+  if MILESTONE_CELEBRATION_ENABLED:
+    from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_celebration import MilestoneCelebration
+
 OpState = log.SelfdriveState.OpenpilotState
 CALIBRATED = log.ExtrinsicsCalibration.Status.calibrated
 NARROW_ROAD_CAM = VisionStreamType.VISION_STREAM_NARROW_ROAD
@@ -156,10 +161,11 @@ class AugmentedRoadView(CameraView):
    self._alert_renderer = AlertRenderer()
    self._driver_state_renderer = DriverStateRenderer()
    self._confidence_ball = ConfidenceBall()
+    self._milestone_celebration = self._child(MilestoneCelebration()) if MILESTONE_CELEBRATION_ENABLED else None
    self._offroad_label = UnifiedLabel("start the car to\nuse sunnypilot", 54, FontWeight.DISPLAY,
                                       text_color=rl.Color(255, 255, 255, int(255 * 0.9)),
-                                       alignment=TextAlignment.CENTER,
-                                       alignment_vertical=TextAlignmentVertical.MIDDLE)
+                                       alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
+                                       alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE)

    self._fade_texture = gui_app.texture("icons_mici/onroad/onroad_fade.png")

@@ -223,6 +229,12 @@ class AugmentedRoadView(CameraView):

    alert_to_render, not_animating_out = self._alert_renderer.will_render()

+    if self._milestone_celebration is not None:
+      if alert_to_render is not None:
+        self._milestone_celebration.cancel_for_alert()
+      else:
+        self._milestone_celebration.render(self._content_rect)
+
    # Hide DMoji when disengaged unless AlwaysOnDM is enabled
    should_draw_dmoji = (not self._hud_renderer.drawing_top_icons() and
                         (ui_state.status != UIStatus.DISENGAGED or ui_state.always_on_dm))
@@ -247,7 +259,6 @@ class AugmentedRoadView(CameraView):
    self._confidence_ball.render(self.rect)

    self._bookmark_icon.render(self.rect)
-
  def _switch_stream_if_needed(self, sm):
    if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams:
      v_ego = sm['carState'].vEgo
@@ -355,10 +366,12 @@ class AugmentedRoadView(CameraView):
    return self._cached_matrix

  def show_event(self):
+    super().show_event()
    if gui_app.sunnypilot_ui():
      ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.RESUME)

  def hide_event(self):
+    super().hide_event()
    if gui_app.sunnypilot_ui():
      ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.PAUSE)

@@ -4,7 +4,7 @@ from openpilot.cereal.visionipc import VisionStreamType
 from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
 from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer
 from openpilot.selfdrive.ui.ui_state import ui_state, device
-from openpilot.system.ui.lib.application import gui_app, FontWeight, TextAlignment, TextAlignmentVertical
+from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr
 from openpilot.system.ui.widgets import Widget
 from openpilot.system.ui.widgets.nav_widget import NavWidget
@@ -76,7 +76,7 @@ class BaseCabinCameraDialog(Widget):

    if not self._camera_view.frame:
      gui_label(rect, tr("camera starting"), font_size=54, font_weight=FontWeight.BOLD,
-                alignment=TextAlignment.CENTER)
+                alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
      rl.end_scissor_mode()
      self._publish_alert_sound(None)
      return
@@ -124,12 +124,12 @@ class BaseCabinCameraDialog(Widget):
    awareness_pct = dm_state.visionPolicyState.awarenessPercent if is_vision else dm_state.wheeltouchPolicyState.awarenessPercent
    gui_label(rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height),
              f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
-              alignment=TextAlignment.RIGHT,
-              alignment_vertical=TextAlignmentVertical.TOP,
+              alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
+              alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
              color=rl.Color(0, 0, 0, 180))
    gui_label(rect, f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
-              alignment=TextAlignment.RIGHT,
-              alignment_vertical=TextAlignmentVertical.TOP,
+              alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
+              alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
              color=rl.Color(255, 255, 255, int(255 * 0.9)))

    if dm_state.alertLevel == log.DriverMonitoringState.AlertLevel.none:
@@ -137,16 +137,16 @@ class BaseCabinCameraDialog(Widget):

    # Show alert level
    alert_level_str = f"{'Pay Attention' if is_vision else 'Touch Wheel'} - level {dm_state.alertLevel}"
-    alignment = TextAlignment.RIGHT if self.driver_state_renderer.is_rhd else TextAlignment.LEFT
+    alignment = rl.GuiTextAlignment.TEXT_ALIGN_RIGHT if self.driver_state_renderer.is_rhd else rl.GuiTextAlignment.TEXT_ALIGN_LEFT

    shadow_rect = rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height)
    gui_label(shadow_rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD,
              alignment=alignment,
-              alignment_vertical=TextAlignmentVertical.BOTTOM,
+              alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM,
              color=rl.Color(0, 0, 0, 180))
    gui_label(rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD,
              alignment=alignment,
-              alignment_vertical=TextAlignmentVertical.BOTTOM,
+              alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM,
              color=rl.Color(255, 255, 255, int(255 * 0.9)))

  def _load_eye_textures(self):
@@ -6,7 +6,7 @@ from collections.abc import Callable
 from openpilot.system.ui.widgets import Widget
 from openpilot.system.ui.widgets.label import UnifiedLabel
 from openpilot.system.ui.widgets.scroller import DO_ZOOM
-from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos, TextAlignmentVertical
+from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
 from openpilot.common.filter_simple import BounceFilter

 if TYPE_CHECKING:
@@ -125,10 +125,10 @@ class BigButton(Widget):
    self._rotate_icon_t: float | None = None

    self._label = UnifiedLabel(text, font_size=self._get_label_font_size(), font_weight=FontWeight.BOLD,
-                               text_color=LABEL_COLOR, alignment_vertical=TextAlignmentVertical.BOTTOM, scroll=scroll,
+                               text_color=LABEL_COLOR, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM, scroll=scroll,
                               line_height=0.9)
    self._sub_label = UnifiedLabel(value, font_size=COMPLICATION_SIZE, font_weight=FontWeight.ROMAN,
-                                   text_color=COMPLICATION_GREY, alignment_vertical=TextAlignmentVertical.BOTTOM)
+                                   text_color=COMPLICATION_GREY, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM)
    self._update_label_layout()

    self._load_images()
@@ -167,9 +167,9 @@ class BigButton(Widget):
  def _update_label_layout(self):
    self._label.set_font_size(self._get_label_font_size())
    if self.value:
-      self._label.set_alignment_vertical(TextAlignmentVertical.TOP)
+      self._label.set_alignment_vertical(rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP)
    else:
-      self._label.set_alignment_vertical(TextAlignmentVertical.BOTTOM)
+      self._label.set_alignment_vertical(rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM)

  def set_text(self, text: str):
    self.text = text
@@ -356,8 +356,8 @@ class GreyBigButton(BigButton):
    self._sub_label.set_font_size(36)
    self._sub_label.set_text_color(rl.Color(255, 255, 255, int(255 * 0.9)))
    self._sub_label.set_font_weight(FontWeight.DISPLAY_REGULAR)
-    self._sub_label.set_alignment_vertical(TextAlignmentVertical.MIDDLE if not self._label.text else
-                                           TextAlignmentVertical.BOTTOM)
+    self._sub_label.set_alignment_vertical(rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE if not self._label.text else
+                                           rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM)
    self._sub_label.set_line_height(0.95)

  @property
@@ -4,7 +4,7 @@ from dataclasses import dataclass
 from openpilot.cereal import messaging, log
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.common.hardware import COMMA_HARDWARE
-from openpilot.system.ui.lib.application import gui_app, FontWeight, TextAlignment, TextAlignmentVertical
+from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr
 from openpilot.system.ui.lib.text_measure import measure_text_cached
 from openpilot.system.ui.widgets import Widget
@@ -76,10 +76,10 @@ class AlertRenderer(Widget):
    self.font_bold: rl.Font = gui_app.font(FontWeight.BOLD)

    # font size is set dynamically
-    self._full_text1_label = Label("", font_size=0, font_weight=FontWeight.BOLD, text_alignment=TextAlignment.CENTER,
-                                   text_alignment_vertical=TextAlignmentVertical.TOP)
-    self._full_text2_label = Label("", font_size=ALERT_FONT_BIG, text_alignment=TextAlignment.CENTER,
-                                   text_alignment_vertical=TextAlignmentVertical.TOP)
+    self._full_text1_label = Label("", font_size=0, font_weight=FontWeight.BOLD, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
+                                   text_alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP)
+    self._full_text2_label = Label("", font_size=ALERT_FONT_BIG, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
+                                   text_alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP)

  def get_alert(self, sm: messaging.SubMaster) -> Alert | None:
    """Generate the current alert based on selfdrive state."""
@@ -4,7 +4,7 @@ from openpilot.cereal.visionipc import VisionStreamType
 from openpilot.selfdrive.ui.onroad.cameraview import CameraView
 from openpilot.selfdrive.ui.onroad.driver_state import DriverStateRenderer
 from openpilot.selfdrive.ui.ui_state import ui_state, device
-from openpilot.system.ui.lib.application import gui_app, FontWeight, TextAlignment
+from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr
 from openpilot.system.ui.widgets.label import gui_label

@@ -38,7 +38,7 @@ class CabinCameraDialog(CameraView):
        tr("camera starting"),
        font_size=100,
        font_weight=FontWeight.BOLD,
-        alignment=TextAlignment.CENTER,
+        alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
      )
      return -1

@@ -24,14 +24,8 @@ ALERT_RAMP_TIME = 4 # seconds to ramp to max volume for warningImmediate
 SELFDRIVE_STATE_TIMEOUT = 5 # 5 seconds
 FILTER_DT = 1. / (micd.SAMPLE_RATE / micd.FFT_SAMPLES)

-AMBIENT_DB = 26 # DB where MIN_VOLUME is applied
 DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied

-VOLUME_BASE = 20
-if HARDWARE.get_device_type() == "tizi":
-  AMBIENT_DB = 30
-  VOLUME_BASE = 10
-
 AudibleAlert = log.SelfdriveState.AudibleAlert
 AudibleAlertSP = custom.SelfdriveStateSP.AudibleAlert

@@ -53,6 +47,7 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
  AudibleAlert.promptDistracted: ("dm_warning.wav", None, MAX_VOLUME),

  AudibleAlert.preAlert: ("pre_alert.wav", 1, MAX_VOLUME),
+  AudibleAlert.complete: ("milestone.wav", 1, MAX_VOLUME),

  AudibleAlert.warningSoft: ("critical.wav", None, MAX_VOLUME),
  AudibleAlert.warningImmediate: ("dm_critical.wav", None, MAX_VOLUME),
@@ -60,6 +55,14 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
  **sound_list_sp,
 }

+
+def calculate_volume_for_device(weighted_db: float, device_type: str) -> float:
+  ambient_db = 30 if device_type in ("mici", "tizi") else 26
+  volume_base = 10 if device_type in ("mici", "tizi") else 20
+  volume_boost = 1.5 if device_type == "mici" else 1.0
+  volume = ((weighted_db - ambient_db) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME
+  return min(MAX_VOLUME, volume_boost * math.pow(volume_base, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1)))
+
 def check_selfdrive_timeout_alert(sm):
  ss_missing = time.monotonic() - sm.recv_time['selfdriveState']

@@ -74,6 +77,7 @@ class Soundd(QuietMode):
  def __init__(self):
    super().__init__()

+    self.device_type = HARDWARE.get_device_type()
    self.load_sounds()

    self.current_alert = AudibleAlert.none
@@ -85,6 +89,7 @@ class Soundd(QuietMode):

    self.selfdrive_timeout_alert = False
    self.pending_stop = False
+    self.last_milestone_event_id = 0

    self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)

@@ -164,9 +169,19 @@ class Soundd(QuietMode):
      self.update_alert(AudibleAlert.none)
      self.selfdrive_timeout_alert = False

+  def update_milestone_alert(self, sm):
+    if not sm.updated['assistedDrivingMilestoneState']:
+      return
+    milestone_state = sm['assistedDrivingMilestoneState']
+    event_id = milestone_state.event.id
+    if not milestone_state.enabled or event_id == 0 or event_id == self.last_milestone_event_id:
+      return
+    self.last_milestone_event_id = event_id
+    if self.current_alert == AudibleAlert.none and not self.enabled:
+      self.update_alert(AudibleAlert.complete)
+
  def calculate_volume(self, weighted_db):
-    volume = ((weighted_db - AMBIENT_DB) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME
-    return math.pow(VOLUME_BASE, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1))
+    return calculate_volume_for_device(weighted_db, self.device_type)

  @retry(attempts=10, delay=3)
  def get_stream(self, sd):
@@ -180,7 +195,7 @@ class Soundd(QuietMode):
    import sounddevice as sd
    micd.patch_sounddevice(sd)

-    sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure'])
+    sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure', 'assistedDrivingMilestoneState'])

    with self.get_stream(sd) as stream:
      rk = Ratekeeper(20)
@@ -198,6 +213,7 @@ class Soundd(QuietMode):
            self.current_volume = self.calculate_volume(float(self.spl_filter_weighted.x))

        self.get_audible_alert(sm)
+        self.update_milestone_alert(sm)

        # Ramp up immediate warning sound over 4s
        if self.current_alert == AudibleAlert.warningImmediate:
@@ -6,7 +6,7 @@ See the LICENSE.md file in the root directory for more details.
 """
 import pyray as rl
 from openpilot.selfdrive.ui.layouts.home import HomeLayout, HomeLayoutState, HEAD_BUTTON_FONT_SIZE, SPACING
-from openpilot.system.ui.lib.application import gui_app, FontWeight, TextAlignment
+from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.text_measure import measure_text_cached
 from openpilot.system.ui.lib.multilang import tr, trn
 from openpilot.system.ui.widgets.label import gui_label
@@ -59,7 +59,7 @@ class HomeLayoutSP(HomeLayout):
      desc_size = measure_text_cached(gui_app.font(FontWeight.NORMAL), description, BRAND_FONT_SIZE)
      desc_width = desc_size.x
      desc_rect = rl.Rectangle(version_right - desc_width, self.header_rect.y, desc_width, self.header_rect.height)
-      gui_label(desc_rect, description, BRAND_FONT_SIZE, rl.WHITE, alignment=TextAlignment.RIGHT)
+      gui_label(desc_rect, description, BRAND_FONT_SIZE, rl.WHITE, alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT)

    brand_size = measure_text_cached(gui_app.font(FontWeight.AUDIOWIDE), brand, BRAND_FONT_SIZE)
    spacing = BRAND_DESC_SPACING if description else 0
@@ -6,7 +6,7 @@ See the LICENSE.md file in the root directory for more details.
 """
 import pyray as rl
 from openpilot.selfdrive.ui.ui_state import ui_state
-from openpilot.system.ui.lib.application import FontWeight, TextAlignment
+from openpilot.system.ui.lib.application import FontWeight
 from openpilot.system.ui.lib.multilang import tr
 from openpilot.system.ui.widgets import Widget
 from openpilot.system.ui.widgets.button import Button, ButtonStyle
@@ -20,7 +20,7 @@ class SunnylinkConsentPage(Widget):
    self._done_callback = done_callback
    self._step = 0

-    self._title = self._child(Label(tr("sunnylink"), font_size=90, font_weight=FontWeight.AUDIOWIDE, text_alignment=TextAlignment.LEFT))
+    self._title = self._child(Label(tr("sunnylink"), font_size=90, font_weight=FontWeight.AUDIOWIDE, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT))

    self._content = [
      {
@@ -43,7 +43,7 @@ class SunnylinkConsentPage(Widget):
    self._primary_btn = self._child(Button("", button_style=ButtonStyle.PRIMARY, click_callback=lambda: self._handle_choice("enable")))
    self._secondary_btn = self._child(Button("", button_style=ButtonStyle.NORMAL, click_callback=lambda: self._handle_choice("secondary")))
    self._danger_btn = self._child(Button("", button_style=ButtonStyle.DANGER, click_callback=lambda: self._handle_choice("disable")))
-    self._desc = self._child(Label("", font_size=90, font_weight=FontWeight.MEDIUM, text_alignment=TextAlignment.LEFT))
+    self._desc = self._child(Label("", font_size=90, font_weight=FontWeight.MEDIUM, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT))

  def _handle_choice(self, choice):
    if choice == "enable":
@@ -9,7 +9,7 @@ from openpilot.cereal import custom
 from openpilot.selfdrive.ui.sunnypilot.layouts.onboarding import SunnylinkConsentPage
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.sunnypilot.sunnylink.api import UNREGISTERED_SUNNYLINK_DONGLE_ID
-from openpilot.system.ui.lib.application import gui_app, FontWeight, TextAlignment, TextAlignmentVertical
+from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr
 from openpilot.system.ui.sunnypilot.widgets.list_view import button_item_sp
 from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp
@@ -32,8 +32,8 @@ class SunnylinkHeader(Widget):
      font_size=90,
      font_weight=FontWeight.AUDIOWIDE,
      text_color=rl.WHITE,
-      alignment=TextAlignment.CENTER,
-      alignment_vertical=TextAlignmentVertical.TOP,
+      alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
+      alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
      wrap_text=False,
      elide=False
    )
@@ -43,8 +43,8 @@ class SunnylinkHeader(Widget):
      font_size=40,
      font_weight=FontWeight.NORMAL,
      text_color=rl.Color(0, 255, 0, 255),  # Green
-      alignment=TextAlignment.CENTER,
-      alignment_vertical=TextAlignmentVertical.TOP,
+      alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
+      alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
      wrap_text=True,
      elide=False
    )
@@ -55,8 +55,8 @@ class SunnylinkHeader(Widget):
      font_size=35,
      font_weight=FontWeight.NORMAL,
      text_color=rl.Color(255, 165, 0, 255),  # Orange
-      alignment=TextAlignment.CENTER,
-      alignment_vertical=TextAlignmentVertical.TOP,
+      alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
+      alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
      wrap_text=True,
      elide=False
    )
@@ -109,8 +109,8 @@ class SunnylinkDescriptionItem(Widget):
      font_size=40,
      font_weight=FontWeight.NORMAL,
      text_color=rl.WHITE,
-      alignment=TextAlignment.LEFT,
-      alignment_vertical=TextAlignmentVertical.TOP,
+      alignment=rl.GuiTextAlignment.TEXT_ALIGN_LEFT,
+      alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
      wrap_text=True,
      elide=False,
    )
@@ -5,29 +5,90 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 import math
+import time

 import pyray as rl

 from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
 from openpilot.selfdrive.ui.ui_state import ui_state, ChestnutState
 from openpilot.system.ui.lib.application import FontWeight
-from openpilot.system.ui.widgets.label import UnifiedLabel
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.widgets.icon_widget import IconWidget
+from openpilot.system.ui.widgets.label import UnifiedLabel, gui_label
+
+METERS_PER_MILE = 1609.344
+METERS_PER_KILOMETER = 1000.0
+SUMMARY_DURATION_SECONDS = 10.0
+SUMMARY_WAIT_SECONDS = 3.0
+
+
+def _nonnegative_float(value) -> float:
+  try:
+    return max(0.0, float(value))
+  except (TypeError, ValueError):
+    return 0.0


 class MiciHomeLayoutSP(MiciHomeLayout):
  def __init__(self):
    super().__init__()
    self._openpilot_label = UnifiedLabel("sunnypilot", font_size=88, font_weight=FontWeight.AUDIOWIDE, max_width=480, wrap_text=False)
+    self._chestnut_loading_icon = IconWidget("icons_mici/chestnut.png", (68, 40))
+    self._chestnut_loading_icon.set_visible(False)
+    failed_idx = self._status_bar_layout.widgets.index(self._chestnut_failed_icon)
+    self._status_bar_layout.widgets.insert(failed_idx + 1, self._chestnut_loading_icon)
+    initial_summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
+    self._last_summary_id = initial_summary.get("id", 0)
+    self._summary_wait_until = 0.0
+    self._summary_visible_until = 0.0
+    self._drive_summary = {}
+
+  def request_drive_summary(self) -> None:
+    self._summary_wait_until = time.monotonic() + SUMMARY_WAIT_SECONDS
+
+  def _render(self, _: rl.Rectangle) -> None:
+    super()._render(_)
+    now = time.monotonic()
+    if now < self._summary_wait_until:
+      summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
+      summary_id = summary.get("id", 0)
+      if summary_id and summary_id != self._last_summary_id:
+        self._last_summary_id = summary_id
+        distances = summary.get("distancesMeters", {})
+        enabled = ui_state.params.get_bool("AssistedDrivingMilestonesEnabled")
+        if enabled and any(_nonnegative_float(distances.get(category, 0.0)) > 0.0 for category in ("mads", "fullAssist")):
+          self._drive_summary = summary
+          self._summary_visible_until = now + SUMMARY_DURATION_SECONDS
+        self._summary_wait_until = 0.0
+
+    if now < self._summary_visible_until:
+      self._draw_drive_summary(_)
+
+  def _draw_drive_summary(self, rect: rl.Rectangle) -> None:
+    distances = self._drive_summary.get("distancesMeters", {})
+    metric = self._drive_summary.get("unit") == "metric"
+    meters_per_unit = METERS_PER_KILOMETER if metric else METERS_PER_MILE
+    unit = "KM" if metric else "MI"
+    mads = _nonnegative_float(distances.get("mads", 0.0)) / meters_per_unit
+    full_assist = _nonnegative_float(distances.get("fullAssist", 0.0)) / meters_per_unit
+
+    rl.draw_rectangle_rec(rect, rl.Color(0, 0, 0, 235))
+    gui_label(rl.Rectangle(rect.x, rect.y + 14, rect.width, 52), tr("DRIVE COMPLETE"), 42,
+              font_weight=FontWeight.SEMI_BOLD, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
+    gui_label(rl.Rectangle(rect.x + 20, rect.y + 78, rect.width / 2 - 30, 42), tr("MADS"), 28,
+              color=rl.Color(255, 255, 255, 184), alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
+    gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 78, rect.width / 2 - 30, 42), tr("FULL ASSIST"), 28,
+              color=rl.Color(255, 255, 255, 184), alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
+    gui_label(rl.Rectangle(rect.x + 20, rect.y + 116, rect.width / 2 - 30, 72), f"{mads:.1f} {unit}", 48,
+              font_weight=FontWeight.DISPLAY, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
+    gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 116, rect.width / 2 - 30, 72), f"{full_assist:.1f} {unit}", 48,
+              font_weight=FontWeight.DISPLAY, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)

  def _set_chestnut_visibility(self):
-    usb_connected = ui_state.usb_connected
-    usb_unknown = ui_state.usb_unknown
-    chestnut_state = ui_state.chestnut_state
-    loading = chestnut_state == ChestnutState.LOADING
-
-    self._usb_icon.set_visible(usb_connected and usb_unknown)
-    self._chestnut_loading_icon.set_opacity(0.35 + 0.65 * (0.5 - 0.5 * math.cos(rl.get_time() * 6.0)))
-    self._chestnut_loading_icon.set_visible(not usb_unknown and loading)
-    self._chestnut_icon.set_visible(not usb_unknown and not loading and
-                                    chestnut_state in (ChestnutState.READY, ChestnutState.ACTIVE))
-    self._chestnut_failed_icon.set_visible(not usb_unknown and chestnut_state in (ChestnutState.UNCOMPILED, ChestnutState.FAILED))
+    # stock has no loading tier: it shows green from the moment a big model is available. keep the
+    # pulse so the status bar and the onroad HUD agree on what loading looks like.
+    loading = ui_state.chestnut_state == ChestnutState.LOADING
+    self._chestnut_loading_icon._opacity = 0.35 + 0.65 * (0.5 - 0.5 * math.cos(rl.get_time() * 6.0))
+    self._chestnut_loading_icon.set_visible(loading)
+    self._chestnut_icon.set_visible(not loading and ui_state.chestnut_state in (ChestnutState.READY, ChestnutState.ACTIVE))
+    self._chestnut_failed_icon.set_visible(ui_state.chestnut_state in (ChestnutState.UNCOMPILED, ChestnutState.FAILED))
@@ -0,0 +1,228 @@
+"""Render assisted-driving milestone celebrations over the on-road view."""
+
+import math
+import random
+import time
+from collections import deque
+from dataclasses import dataclass
+
+import pyray as rl
+
+from openpilot.cereal import custom
+from openpilot.selfdrive.ui.mici.onroad.alert_renderer import ALERT_BACKGROUND_OPACITY
+from openpilot.selfdrive.ui.mici.onroad.hud_renderer import FONT_SIZES
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.lib.application import FontWeight, gui_app
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+from openpilot.system.ui.widgets import Widget
+
+
+CELEBRATION_DURATION = 4.5
+PARTICLE_COUNT = 150
+METERS_PER_MILE = 1609.344
+METERS_PER_KILOMETER = 1000.0
+
+CONFETTI_COLORS = (
+  rl.Color(255, 55, 95, 255),
+  rl.Color(255, 183, 3, 255),
+  rl.Color(48, 209, 88, 255),
+  rl.Color(36, 179, 255, 255),
+  rl.Color(112, 72, 232, 255),
+  rl.Color(255, 45, 196, 255),
+)
+
+
+@dataclass(frozen=True)
+class ConfettiParticle:
+  x: float
+  y: float
+  width: float
+  height: float
+  speed: float
+  drift: float
+  angle: float
+  spin: float
+  phase: float
+  color: rl.Color
+
+
+@dataclass(frozen=True)
+class CelebrationMilestone:
+  event_id: int
+  full_assist: bool
+  distance_meters: float
+  previous_distance_meters: float
+  metric: bool
+
+
+class MilestoneCelebration(Widget):
+  """Pure renderer for typed assisted-driving milestone events."""
+
+  def __init__(self):
+    super().__init__()
+    self._drive_started_time = -1.0
+    self._celebration_started_time: float | None = None
+    self._current_milestone: CelebrationMilestone | None = None
+    self._pending_milestones: deque[CelebrationMilestone] = deque()
+    self._last_event_id = 0
+    self._particles = self._make_particles()
+
+  @staticmethod
+  def _make_particles() -> list[ConfettiParticle]:
+    rng = random.Random(20260828)
+    return [
+      ConfettiParticle(
+        x=rng.random(),
+        y=rng.uniform(-0.25, 0.95),
+        width=rng.uniform(10, 24),
+        height=rng.uniform(24, 58),
+        speed=rng.uniform(0.12, 0.34),
+        drift=rng.uniform(-0.035, 0.035),
+        angle=rng.uniform(0, 360),
+        spin=rng.uniform(-150, 150),
+        phase=rng.uniform(0, math.tau),
+        color=CONFETTI_COLORS[rng.randrange(len(CONFETTI_COLORS))],
+      )
+      for _ in range(PARTICLE_COUNT)
+    ]
+
+  def _render(self, rect: rl.Rectangle, /) -> None:
+    now = time.monotonic()
+    if ui_state.started_time != self._drive_started_time:
+      self._drive_started_time = ui_state.started_time
+      self._celebration_started_time = None
+      self._current_milestone = None
+      self._pending_milestones.clear()
+
+    self._consume_event(suppress=False)
+
+    if self._current_milestone is None and self._pending_milestones:
+      self._current_milestone = self._pending_milestones.popleft()
+      self._celebration_started_time = now
+
+    if self._celebration_started_time is None or self._current_milestone is None:
+      return
+
+    elapsed = now - self._celebration_started_time
+    if elapsed >= CELEBRATION_DURATION:
+      self._celebration_started_time = None
+      self._current_milestone = None
+      return
+
+    alpha = min(1.0, elapsed / 0.2, (CELEBRATION_DURATION - elapsed) / 0.8)
+    self._draw_background_scrim(rect, alpha)
+    self._draw_confetti(rect, elapsed, alpha)
+    self._draw_milestone(rect, elapsed, alpha, self._current_milestone)
+
+  def cancel_for_alert(self) -> None:
+    self._consume_event(suppress=True)
+    self._celebration_started_time = None
+    self._current_milestone = None
+    self._pending_milestones.clear()
+
+  def _consume_event(self, suppress: bool) -> None:
+    if not ui_state.sm.updated["assistedDrivingMilestoneState"]:
+      return
+    state = ui_state.sm["assistedDrivingMilestoneState"]
+    event = state.event
+    if not state.enabled:
+      self._celebration_started_time = None
+      self._current_milestone = None
+      self._pending_milestones.clear()
+      return
+    if event.id == 0 or event.id == self._last_event_id:
+      return
+    self._last_event_id = event.id
+    if suppress:
+      return
+    self._pending_milestones.append(CelebrationMilestone(
+      event_id=event.id,
+      full_assist=event.category == custom.AssistedDrivingMilestoneState.Category.fullAssist,
+      distance_meters=event.distanceMeters,
+      previous_distance_meters=event.previousDistanceMeters,
+      metric=event.unit == custom.AssistedDrivingMilestoneState.Unit.metric,
+    ))
+
+  def _draw_confetti(self, rect: rl.Rectangle, elapsed: float, alpha: float) -> None:
+    travel_height = rect.height * 1.45
+    compact = rect.height <= 300
+    particle_scale = rect.height / 1080.0
+    particles = self._particles[:100] if compact else self._particles
+    for particle in particles:
+      x = rect.x + rect.width * (particle.x + particle.drift * elapsed + 0.012 * math.sin(elapsed * 3 + particle.phase))
+      y = rect.y - rect.height * 0.2 + (particle.y * travel_height + particle.speed * rect.height * elapsed) % travel_height
+      flip = 0.2 + 0.8 * abs(math.sin(elapsed * 5 + particle.phase))
+      particle_rect = rl.Rectangle(x, y, particle.width * particle_scale * flip, particle.height * particle_scale)
+      origin = rl.Vector2(particle_rect.width / 2, particle_rect.height / 2)
+      color = rl.Color(particle.color.r, particle.color.g, particle.color.b, int(255 * alpha))
+      rl.draw_rectangle_pro(particle_rect, origin, particle.angle + particle.spin * elapsed, color)
+
+  @staticmethod
+  def _draw_milestone(rect: rl.Rectangle, elapsed: float, alpha: float, milestone: CelebrationMilestone) -> None:
+    # Match the comma four set-speed hierarchy: DISPLAY number with a MAX-sized label.
+    scale = rect.height / 240.0
+    pulse = 1.0 + 0.025 * math.sin(min(elapsed, 0.6) / 0.6 * math.pi)
+    number_size = int(FONT_SIZES.set_speed * scale * pulse)
+    milestone_size = int(FONT_SIZES.max_speed * scale * pulse)
+    category_size = int(22 * scale * pulse)
+    unit_size = category_size
+
+    display_font = gui_app.font(FontWeight.DISPLAY)
+    semibold_font = gui_app.font(FontWeight.SEMI_BOLD)
+    tween_progress = min(elapsed / 0.85, 1.0)
+    tween_progress = 1.0 - (1.0 - tween_progress) ** 3
+    meters_per_unit = METERS_PER_KILOMETER if milestone.metric else METERS_PER_MILE
+    previous_distance = milestone.previous_distance_meters / meters_per_unit
+    milestone_distance = milestone.distance_meters / meters_per_unit
+    displayed_distance = previous_distance + (milestone_distance - previous_distance) * tween_progress
+    if tween_progress >= 1.0:
+      number = f"{round(milestone_distance):,}"
+    else:
+      number = f"{displayed_distance:,.1f}"
+    unit = tr("KM") if milestone.metric else tr("MI")
+    category = tr("FULL ASSIST") if milestone.full_assist else tr("MADS")
+    milestone_label = tr("MILESTONE")
+
+    unit_bounds = measure_text_cached(semibold_font, unit, unit_size)
+    number_bounds = measure_text_cached(display_font, number, number_size)
+    max_number_width = rect.width * 0.72 - unit_bounds.x - 8 * scale
+    if number_bounds.x > max_number_width:
+      number_size = max(1, int(number_size * max_number_width / number_bounds.x))
+      number_bounds = measure_text_cached(display_font, number, number_size)
+    category_bounds = measure_text_cached(semibold_font, category, category_size)
+    milestone_bounds = measure_text_cached(semibold_font, milestone_label, milestone_size)
+
+    center_x = rect.x + rect.width / 2
+    center_y = rect.y + rect.height / 2
+    text_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha))
+    secondary_color = rl.Color(255, 255, 255, int(255 * 0.72 * alpha))
+    number_line_width = number_bounds.x + 8 * scale + unit_bounds.x
+    number_x = center_x - number_line_width / 2
+    number_y = center_y - 76 * scale
+    unit_y = center_y + 14 * scale
+    category_y = center_y - 91 * scale
+    milestone_y = center_y + 50 * scale
+
+    rl.draw_text_ex(semibold_font, category, rl.Vector2(center_x - category_bounds.x / 2, category_y),
+                    category_size, 0, secondary_color)
+    rl.draw_text_ex(display_font, number, rl.Vector2(number_x, number_y), number_size, 0, text_color)
+    rl.draw_text_ex(semibold_font, unit, rl.Vector2(number_x + number_bounds.x + 8 * scale, unit_y),
+                    unit_size, 0, secondary_color)
+    rl.draw_text_ex(semibold_font, milestone_label, rl.Vector2(center_x - milestone_bounds.x / 2, milestone_y),
+                    milestone_size, 0, text_color)
+
+  @staticmethod
+  def _draw_background_scrim(rect: rl.Rectangle, alpha: float) -> None:
+    # Match the alert background: a mostly opaque black core fading to transparent.
+    fade_height = round(rect.height * 0.25)
+    solid_height = round(rect.height * 0.50)
+    solid_color = rl.Color(0, 0, 0, int(255 * ALERT_BACKGROUND_OPACITY * alpha))
+    transparent = rl.Color(0, 0, 0, 0)
+    x = int(rect.x)
+    y = int(rect.y)
+    width = int(rect.width)
+
+    rl.draw_rectangle_gradient_v(x, y, width, fade_height, transparent, solid_color)
+    rl.draw_rectangle(x, y + fade_height, width, solid_height, solid_color)
+    rl.draw_rectangle_gradient_v(x, y + fade_height + solid_height, width, fade_height, solid_color, transparent)
@@ -35,7 +35,8 @@ class UIStateSP:
    self.is_sp_release: bool = self.params.get_bool("IsReleaseSpBranch")
    self.sm_services_ext = [
      "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
-      "gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay"
+      "gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay",
+      "assistedDrivingMilestoneState",
    ]

    self.sunnylink_state = SunnylinkState()
@@ -0,0 +1,45 @@
+#!/usr/bin/env python3
+"""Generate the assisted-driving milestone celebration chime."""
+
+import math
+import wave
+from array import array
+from pathlib import Path
+
+
+SAMPLE_RATE = 48_000
+DURATION_SECONDS = 0.82
+NOTES = (
+  (0.00, 523.25),
+  (0.11, 659.25),
+  (0.22, 783.99),
+)
+
+
+def note_sample(age: float, frequency: float) -> float:
+  if not 0 <= age <= 0.58:
+    return 0.0
+  attack = min(age / 0.008, 1.0)
+  release = min((0.58 - age) / 0.15, 1.0)
+  envelope = attack * release * math.exp(-3.8 * age)
+  tone = math.sin(math.tau * frequency * age) + 0.16 * math.sin(math.tau * frequency * 2 * age)
+  return envelope * tone
+
+
+def main() -> None:
+  output = Path(__file__).parents[4] / "openpilot/selfdrive/assets/sounds/milestone.wav"
+  samples = array('h')
+  for frame in range(round(SAMPLE_RATE * DURATION_SECONDS)):
+    t = frame / SAMPLE_RATE
+    value = 0.38 * sum(note_sample(t - start, frequency) for start, frequency in NOTES)
+    samples.append(round(max(-1.0, min(1.0, value)) * 32767))
+
+  with wave.open(str(output), "wb") as wav:
+    wav.setnchannels(1)
+    wav.setsampwidth(2)
+    wav.setframerate(SAMPLE_RATE)
+    wav.writeframes(samples.tobytes())
+
+
+if __name__ == "__main__":
+  main()
@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+"""Publish deterministic milestone events for the local comma-four UI preview."""
+
+import itertools
+import time
+
+from openpilot.cereal import messaging
+
+
+def main() -> None:
+  pm = messaging.PubMaster(["assistedDrivingMilestoneState"])
+  milestones = itertools.cycle(((1, 0, "mads"), (2, 1, "fullAssist"), (5, 2, "mads"), (10, 5, "fullAssist")))
+  event_id = 0
+  milestone, previous_milestone, category = 0, 0, "mads"
+  next_event_time = time.monotonic() + 1.0
+
+  while True:
+    now = time.monotonic()
+    if now >= next_event_time:
+      event_id += 1
+      milestone, previous_milestone, category = next(milestones)
+      next_event_time = now + 6.0
+
+    msg = messaging.new_message("assistedDrivingMilestoneState")
+    state = msg.assistedDrivingMilestoneState
+    state.enabled = True
+    if event_id:
+      state.event.id = event_id
+      state.event.category = category
+      state.event.distanceMeters = milestone * 1609.344
+      state.event.previousDistanceMeters = previous_milestone * 1609.344
+      state.event.unit = "imperial"
+    pm.send("assistedDrivingMilestoneState", msg)
+    time.sleep(0.1)
+
+
+if __name__ == "__main__":
+  main()
@@ -0,0 +1,27 @@
+#!/usr/bin/env bash
+set -e
+
+repo_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../../.." && pwd)"
+replay_pid=""
+preview_pid=""
+
+cleanup() {
+  for pid in "$preview_pid" "$replay_pid"; do
+    if [[ -n "$pid" ]]; then
+      kill "$pid" 2>/dev/null || true
+      wait "$pid" 2>/dev/null || true
+    fi
+  done
+}
+trap cleanup EXIT INT TERM
+
+export PATH="$repo_root/.venv/bin:$PATH"
+export SP_MILESTONE_PREVIEW=1
+playback="${SP_MILESTONE_PLAYBACK:-1}"
+
+"$repo_root/openpilot/tools/replay/replay" --demo --playback "$playback" &
+replay_pid=$!
+"$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/tests/milestone_preview.py" &
+preview_pid=$!
+
+"$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py"
@@ -4,12 +4,63 @@ import time
 from openpilot.common.test import OpenpilotTestCase
 from openpilot.cereal import log, messaging
 from openpilot.cereal.messaging import SubMaster, PubMaster
-from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert
+from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, Soundd, calculate_volume_for_device, check_selfdrive_timeout_alert

 AudibleAlert = log.SelfdriveState.AudibleAlert


 class TestSoundd(OpenpilotTestCase):
+  @staticmethod
+  def milestone_submaster(event_id=42):
+    class SubMasterStub:
+      def __init__(self):
+        self.updated = {'assistedDrivingMilestoneState': True}
+        msg = messaging.new_message('assistedDrivingMilestoneState')
+        msg.assistedDrivingMilestoneState.enabled = True
+        msg.assistedDrivingMilestoneState.event.id = event_id
+        self.data = {'assistedDrivingMilestoneState': msg.assistedDrivingMilestoneState}
+
+      def __getitem__(self, service):
+        return self.data[service]
+
+    return SubMasterStub()
+
+  def test_comma_four_volume_is_50_percent_louder_than_comma_three_x(self):
+    for weighted_db in (20.0, 30.0, 40.0, 50.0):
+      with self.subTest(weighted_db=weighted_db):
+        comma_three_x_volume = calculate_volume_for_device(weighted_db, "tizi")
+        comma_four_volume = calculate_volume_for_device(weighted_db, "mici")
+        assert comma_four_volume == min(1.0, comma_three_x_volume * 1.5)
+
+  def test_milestone_chime_uses_typed_milestone_event_once(self):
+    soundd = Soundd()
+    sm = self.milestone_submaster()
+    soundd.update_milestone_alert(sm)
+
+    assert soundd.current_alert == AudibleAlert.complete
+    soundd.current_alert = AudibleAlert.none
+    soundd.update_milestone_alert(sm)
+    assert soundd.current_alert == AudibleAlert.none
+
+  def test_safety_alert_consumes_milestone_without_replaying_it(self):
+    soundd = Soundd()
+    sm = self.milestone_submaster()
+    soundd.current_alert = AudibleAlert.warningImmediate
+
+    soundd.update_milestone_alert(sm)
+    soundd.current_alert = AudibleAlert.none
+    soundd.update_milestone_alert(sm)
+
+    assert soundd.current_alert == AudibleAlert.none
+
+  def test_quiet_mode_consumes_milestone_without_playing_it(self):
+    soundd = Soundd()
+    soundd.enabled = True
+
+    soundd.update_milestone_alert(self.milestone_submaster())
+
+    assert soundd.current_alert == AudibleAlert.none
+
  def test_check_selfdrive_timeout_alert(self, mocker):
    sm = SubMaster(['selfdriveState', 'selfdriveStateSP'])
    pm = PubMaster(['selfdriveState', 'selfdriveStateSP'])
@@ -12,7 +12,6 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.ui.lib.prime_state import PrimeState
 from openpilot.system.ui.lib.application import gui_app
 from openpilot.common.hardware import HARDWARE, PC
-from openpilot.common.hardware.usb import TYPEC_CC_ORIENTATION_PATH, get_usb_state, is_chestnut_usb_id, read_int
 from openpilot.selfdrive.modeld.helpers import chestnut_compiled

 from openpilot.selfdrive.ui.sunnypilot.ui_state import UIStateSP, DeviceSP
@@ -96,10 +95,6 @@ class UIState(UIStateSP):
    self.chestnut_compiled: bool = chestnut_compiled()
    self.chestnut_active: bool | None = None
    self.chestnut_loading: bool = False
-    self.usb_connected: bool = False
-    self.usb_connected_ts: float | None = None
-    self.usb_disconnected_ts: float | None = None
-    self.usb_unknown: bool = False
    self.chestnut_state = ChestnutState.DISCONNECTED
    self.started: bool = False
    self.ignition: bool = False
@@ -259,23 +254,6 @@ class UIState(UIStateSP):
      self.chestnut_compiled = chestnut_compiled()
    self.chestnut_active = self.params.get("ChestnutActive")
    self.chestnut_loading = self.params.get_bool("ChestnutLoading")
-    now = time.monotonic()
-    if read_int(TYPEC_CC_ORIENTATION_PATH) != 0:
-      self.usb_disconnected_ts = None
-      if not self.usb_connected:
-        self.usb_connected = True
-        self.usb_connected_ts = now
-        self.usb_unknown = False
-      elif self.usb_connected_ts is not None and now - self.usb_connected_ts > 10.:
-        self.usb_unknown = not any(is_chestnut_usb_id(d["vendorId"], d["productId"], True) for d in get_usb_state())
-        self.usb_connected_ts = None
-    elif self.usb_connected:
-      if self.usb_disconnected_ts is None:
-        self.usb_disconnected_ts = now
-      elif now - self.usb_disconnected_ts > PARAM_UPDATE_TIME:
-        self.usb_connected = False
-        self.usb_connected_ts = None
-        self.usb_unknown = False

    UIStateSP.update_params(self)

@@ -32,7 +32,7 @@ def _patch_tinygrad_fetch_fw():
  helpers.fetch_fw = fetch_fw
 _patch_tinygrad_fetch_fw()

-import openpilot.selfdrive.modeld.compile_modeld as stock
+from openpilot.selfdrive.modeld.compile_modeld import NV12Frame, make_frame_prepare, sample_desire, sample_skip, shift_and_sample
 from tinygrad import dtypes
 from tinygrad.device import Device
 from tinygrad.engine.jit import TinyJit
@@ -41,7 +41,8 @@ from tinygrad.tensor import Tensor
 MODEL_TYPES = ('vision_policy', 'supercombo', 'vision_multi_policy')
 WARP_INPUTS = ['tfm', 'big_tfm']
 POLICY_INPUTS = ['img_q', 'big_img_q', 'feat_q', 'desire_q', 'packed_npy_inputs']
-nv12_copy_size = stock.nv12_copy_size
+WARP_DEV = os.getenv('WARP_DEV')
+

 def _detect_desire_key(shapes: dict) -> str | None:
  return next((key for key in shapes if key.startswith('desire')), None)
@@ -138,9 +139,7 @@ def make_supercombo_input_queues(input_shapes: dict, frame_skip: int,
  return generate_queues_and_npy(input_shapes, frame_skip, device, is_supercombo=True)


-def make_random_images(keys, shape, device, rng):
-  if device == 'NPY':
-    return {k: Tensor(rng.integers(0, 256, size=shape, dtype=np.uint8), device='NPY').realize() for k in keys}
+def make_random_images(keys, shape, device):
  return {k: Tensor.randint(shape, low=0, high=256, dtype=dtypes.uint8, device=device).realize() for k in keys}


@@ -153,16 +152,14 @@ def make_warp_queues(device=Device.DEFAULT):
  return queues, npy


-def make_warp(nv12: stock.NV12Frame, model_w: int, model_h: int):
-  frame_prepare = stock.make_frame_prepare(nv12, model_w, model_h)
+def make_warp(nv12: NV12Frame, model_w: int, model_h: int):
+  frame_prepare = make_frame_prepare(nv12, model_w, model_h)
+  WARP_DEV = os.getenv('WARP_DEV', Device.DEFAULT)

  def warp(tfm, big_tfm, frame, big_frame):
-    tfm = tfm.to(Device.DEFAULT)
-    big_tfm = big_tfm.to(Device.DEFAULT)
-    if Device.DEFAULT == 'AMD':
-      frame = frame.to(Device.DEFAULT)
-      big_frame = big_frame.to(Device.DEFAULT)
-    Tensor.realize(tfm, big_tfm, frame, big_frame)
+    tfm = tfm.to(WARP_DEV)
+    big_tfm = big_tfm.to(WARP_DEV)
+    Tensor.realize(tfm, big_tfm)

    warped_frame = frame_prepare(frame, tfm).unsqueeze(0)
    warped_big_frame = frame_prepare(big_frame, big_tfm).unsqueeze(0)
@@ -171,8 +168,8 @@ def make_warp(nv12: stock.NV12Frame, model_w: int, model_h: int):


 def make_run_policy(vision_runner, policy_runners: list, features_slice: slice, frame_skip: int, input_shapes: dict):
-  sample_skip_fn = partial(stock.sample_skip, frame_skip=frame_skip)
-  sample_desire_fn = partial(stock.sample_desire, frame_skip=frame_skip)
+  sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
+  sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)

  desire_key = _detect_desire_key(input_shapes)
  road_key, wide_key = _detect_vision_keys(input_shapes)
@@ -189,14 +186,14 @@ def make_run_policy(vision_runner, policy_runners: list, features_slice: slice,
    warped_dev = warped.to(Device.DEFAULT)
    Tensor.realize(packed_npy_inputs_dev, warped_dev)

-    img = stock.shift_and_sample(img_q, warped_dev[0:1], sample_skip_fn)
-    big_img = stock.shift_and_sample(big_img_q, warped_dev[1:2], sample_skip_fn)
+    img = shift_and_sample(img_q, warped_dev[0:1], sample_skip_fn)
+    big_img = shift_and_sample(big_img_q, warped_dev[1:2], sample_skip_fn)

    unpacked_tensors = [tensor.reshape(shape) for tensor, shape in zip(packed_npy_inputs_dev.split(npy_sizes), npy_shapes.values(), strict=True)]
    unpacked_dict = dict(zip(npy_shapes.keys(), unpacked_tensors, strict=True))

    desire_dev = unpacked_dict['desire']
-    desire_buf = stock.shift_and_sample(desire_q, desire_dev.reshape(1, 1, -1), sample_desire_fn)
+    desire_buf = shift_and_sample(desire_q, desire_dev.reshape(1, 1, -1), sample_desire_fn)

    inputs = {desire_key: desire_buf}
    for key, tensor_val in unpacked_dict.items():
@@ -205,13 +202,13 @@ def make_run_policy(vision_runner, policy_runners: list, features_slice: slice,

    if 'prev_feat' in unpacked_dict:
      prev_feat_dev = unpacked_dict['prev_feat']
-      inputs['features_buffer'] = stock.shift_and_sample(feat_q, prev_feat_dev.reshape(1, 1, -1), sample_skip_fn).reshape(input_shapes['features_buffer'])
+      inputs['features_buffer'] = shift_and_sample(feat_q, prev_feat_dev.reshape(1, 1, -1), sample_skip_fn).reshape(input_shapes['features_buffer'])

    if vision_runner:
      vision_out_cast = next(iter(vision_runner({road_key: img, wide_key: big_img}).values())).cast('float32').realize()
      if 'features_buffer' not in inputs:
        new_feat = vision_out_cast[:, features_slice].reshape(1, -1).unsqueeze(0)
-        inputs['features_buffer'] = stock.shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
+        inputs['features_buffer'] = shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
      policy_outs = [next(iter(pol_runner(inputs).values())).cast('float32').realize() for pol_runner in policy_runners]
      return (vision_out_cast, *policy_outs) if len(policy_outs) > 1 else (vision_out_cast, policy_outs[0])

@@ -222,28 +219,27 @@ def make_run_policy(vision_runner, policy_runners: list, features_slice: slice,
    policy_out = next(iter(policy_runners[0](inputs).values())).cast('float32').realize()
    if 'features_buffer' not in inputs and features_slice is not None:
      new_feat = policy_out[:, features_slice].reshape(1, -1).unsqueeze(0)
-      stock.shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
+      shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
    return policy_out

  return run_policy


-def compile_jit(jit, input_keys, make_queues, make_random_inputs=None, benchmark_runs: int = 1):
+def compile_jit(jit, make_random_inputs, input_keys, make_queues):
  SEED = 42
-  def random_inputs_run(fn, seed, n_runs, test_val=None, test_buffers=None, expect_match=True):
-    queues_res = make_queues(Device.DEFAULT)
-    input_queues, npy = queues_res[0], queues_res[1]
-    frame_views = queues_res[2] if len(queues_res) > 2 else {}
+  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)

+    testing = test_val is not None or test_buffers is not None
+    n_runs = 1 if testing else 3
+
    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(rng=rng) if make_random_inputs is not None else {}
+      random_inputs = make_random_inputs()
      st = time.perf_counter()
      outs = fn(**{k: input_queues[k] for k in input_keys if k in input_queues}, **random_inputs)
      mt = time.perf_counter()
@@ -264,15 +260,14 @@ def compile_jit(jit, input_keys, make_queues, make_random_inputs=None, benchmark
    return val, buffers

  print('capture + replay')
-  test_val, test_buffers = random_inputs_run(jit, SEED, 3)
-  print(f'pickle round trip ({benchmark_runs} runs per seed)')
+  test_val, test_buffers = random_inputs_run(jit, SEED)
+  print('pickle round trip')
  with tempfile.TemporaryFile(dir=".") as f:
    dump_oob(jit, f)
    f.seek(0)
-    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)
-  return jit
+    deserialized_jit = load_oob(f)
+  random_inputs_run(deserialized_jit, SEED, test_val=test_val, test_buffers=test_buffers)
+  return deserialized_jit


 def _parse_size(size_str: str) -> tuple[int, int]:
@@ -322,7 +317,6 @@ if __name__ == "__main__":
  parser.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
  parser.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True)
  parser.add_argument('--frame-skip', type=int, default=None, help='frame skip value (auto-derived if not provided)')
-  parser.add_argument('--benchmark-runs', type=int, default=1, help='benchmark runs')
  parser.add_argument('--output', required=True)

  parser.add_argument('--vision-onnx', help='vision ONNX (for split models)')
@@ -341,65 +335,48 @@ if __name__ == "__main__":
  args.on_policy_onnx = read_file_chunked_to_disk(args.on_policy_onnx)
  args.supercombo_onnx = read_file_chunked_to_disk(args.supercombo_onnx)

-  if args.model_type == 'supercombo':
+  vision_runner = OnnxRunner(args.vision_onnx) if args.vision_onnx else None
+
+  if args.model_type == 'vision_policy':
+    assert vision_runner and args.policy_onnx
+    policy_runners = [OnnxRunner(args.policy_onnx)]
+    output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx), 'policy': make_metadata_dict(args.policy_onnx)}
+  elif args.model_type == 'supercombo':
    assert args.supercombo_onnx
-    model_metadata = make_metadata_dict(args.supercombo_onnx)
-    output_data['metadata'] = {'model': model_metadata, **model_metadata}
-    output_data['input_devices'] = {'model': Device.DEFAULT}
-    output_data['run_model'] = {}
-    derived_frame_skip = args.frame_skip or derive_frame_skip({}, model_metadata['input_shapes'])
-    model_runner = OnnxRunner(args.supercombo_onnx)
-    run_policy = stock.make_run_policy(model_runner, model_metadata, derived_frame_skip)
-    for cam_w, cam_h in args.camera_resolutions:
-      print(f"Compiling unified run_model JIT for {cam_w}x{cam_h}...")
-      nv12 = stock.NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-      frame_copy_size = stock.nv12_copy_size(nv12.stride, nv12.y_height, nv12.uv_height)
-      make_model_queues = partial(stock.make_input_queues, model_metadata['input_shapes'], derived_frame_skip,
-                                  frame_copy_size=frame_copy_size)
-      warp = stock.make_warp(nv12, model_w, model_h)
-      run_model_jit = TinyJit(stock.make_run_model(warp, run_policy, model_metadata, frame_copy_size), prune=True)
-      output_data['run_model'][(cam_w, cam_h)] = compile_jit(run_model_jit, stock.MODELD_INPUTS, make_model_queues, benchmark_runs=args.benchmark_runs)
-  else:
-    vision_runner = OnnxRunner(args.vision_onnx) if args.vision_onnx else None
-    if args.model_type == 'vision_policy':
-      assert vision_runner and args.policy_onnx
-      policy_runners = [OnnxRunner(args.policy_onnx)]
-      output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx), 'policy': make_metadata_dict(args.policy_onnx)}
-    elif args.model_type == 'vision_multi_policy':
-      assert vision_runner
-      policy_runners, policy_names = _load_policy_runners(args)
-      output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx)}
-      for name in policy_names:
-        runner_arg = getattr(args, f"{name}_onnx")
-        output_data['metadata'][name] = make_metadata_dict(runner_arg)
+    policy_runners = [OnnxRunner(args.supercombo_onnx)]
+    output_data['metadata'] = {'model': make_metadata_dict(args.supercombo_onnx)}
+  elif args.model_type == 'vision_multi_policy':
+    assert vision_runner
+    policy_runners, policy_names = _load_policy_runners(args)
+    output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx)}
+    for name in policy_names:
+      runner_arg = getattr(args, f"{name}_onnx")
+      output_data['metadata'][name] = make_metadata_dict(runner_arg)

-    policy_keys = [key for key in output_data['metadata'].keys() if key != 'vision']
-    first_policy_meta = output_data['metadata'][policy_keys[0]] if policy_keys else {}
-    vision_meta = output_data['metadata'].get('vision', {})
+  policy_keys = [key for key in output_data['metadata'].keys() if key != 'vision']
+  first_policy_meta = output_data['metadata'][policy_keys[0]] if policy_keys else {}
+  vision_meta = output_data['metadata'].get('vision', {})

-    derived_frame_skip = args.frame_skip or derive_frame_skip(vision_meta.get('input_shapes', {}), first_policy_meta.get('input_shapes', {}))
-    all_shapes = {key: value for meta in output_data['metadata'].values() for key, value in meta['input_shapes'].items()}
-    feat_meta = output_data['metadata'].get('vision') or output_data['metadata'].get('policy')
-    assert feat_meta is not None
-    features_slice = feat_meta['output_slices']['hidden_state']
+  derived_frame_skip = args.frame_skip or derive_frame_skip(vision_meta.get('input_shapes', {}), first_policy_meta.get('input_shapes', {}))
+  all_shapes = {key: value for meta in output_data['metadata'].values() for key, value in meta['input_shapes'].items()}
+  feat_meta = output_data['metadata'].get('vision') or output_data['metadata'].get('model') or output_data['metadata'].get('policy')
+  assert feat_meta is not None
+  features_slice = feat_meta['output_slices']['hidden_state']
+  is_supercombo = vision_runner is None

-    print(f"Compiling run_policy JIT (model_size={model_w}x{model_h}, frame_skip={derived_frame_skip})...")
-    run_policy_func = make_run_policy(vision_runner, policy_runners, features_slice, derived_frame_skip, all_shapes)
-    run_policy_jit = TinyJit(run_policy_func, prune=True)
-    make_policy_queues = partial(generate_queues_and_npy, all_shapes, derived_frame_skip, is_supercombo=False)
-    make_random_model_inputs = partial(make_random_images, keys=['warped'], shape=(2, 6, model_h // 2, model_w // 2), device=Device.DEFAULT)
-    output_data['run_policy'] = compile_jit(run_policy_jit, POLICY_INPUTS, make_policy_queues, make_random_inputs=make_random_model_inputs)
+  print(f"Compiling run_policy JIT (model_size={model_w}x{model_h}, frame_skip={derived_frame_skip})...")
+  run_policy_func = make_run_policy(vision_runner, policy_runners, features_slice, derived_frame_skip, all_shapes)
+  run_policy_jit = TinyJit(run_policy_func, prune=True)
+  make_policy_queues = partial(generate_queues_and_npy, all_shapes, derived_frame_skip, is_supercombo=is_supercombo)
+  make_random_model_inputs = partial(make_random_images, keys=['warped'], shape=(2, 6, model_h // 2, model_w // 2), device=WARP_DEV)
+  output_data['run_policy'] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS, make_policy_queues)

-    for cam_w, cam_h in args.camera_resolutions:
-      print(f"Compiling warp JIT for {cam_w}x{cam_h}...")
-      nv12 = stock.NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-      frame_copy_size = stock.nv12_copy_size(nv12.stride, nv12.y_height, nv12.uv_height)
-      warp_input_dev = 'NPY' if Device.DEFAULT == 'AMD' else Device.DEFAULT
-      make_random_warp_inputs = partial(make_random_images, keys=['frame', 'big_frame'], shape=frame_copy_size, device=warp_input_dev)
-      warp = TinyJit(make_warp(nv12, model_w, model_h), prune=True)
-      output_data[(cam_w, cam_h)] = compile_jit(warp, WARP_INPUTS, make_warp_queues, make_random_inputs=make_random_warp_inputs)
-
-    output_data['metadata']['warp_dev'] = Device.DEFAULT
+  for cam_w, cam_h in args.camera_resolutions:
+    print(f"Compiling warp JIT for {cam_w}x{cam_h}...")
+    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), prune=True)
+    output_data[(cam_w, cam_h)] = compile_jit(warp, make_random_warp_inputs, WARP_INPUTS, make_warp_queues)

  with open(args.output, "wb") as file:
    dump_oob(output_data, file)
@@ -14,8 +14,6 @@ class ModelConstants:

  # model inputs constants
  MODEL_FREQ = 20
-  MODEL_RUN_FREQ = 20
-  MODEL_CONTEXT_FREQ = 5
  FEATURE_LEN = 512
  FULL_HISTORY_BUFFER_LEN = 99
  DESIRE_LEN = 8
@@ -6,7 +6,6 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """

-from collections.abc import Callable
 import os
 os.environ['GMMU'] = '0'
 import numpy as np
@@ -38,18 +37,11 @@ from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
 from openpilot.selfdrive.modeld.modeld import ChestnutState

-from openpilot.selfdrive.modeld.compile_modeld import (
-  MODELD_INPUTS,
-  make_input_queues as make_stock_input_queues,
-)
 from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
-from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser
-from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
+from openpilot.sunnypilot.modeld_v2.constants import Plan
 from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
 from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
-from openpilot.sunnypilot.modeld_v2.compile_modeld import (derive_frame_skip, make_split_input_queues,
-                                                           make_supercombo_input_queues, nv12_copy_size,
-                                                           WARP_INPUTS, POLICY_INPUTS)
+from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, make_split_input_queues, make_supercombo_input_queues, WARP_INPUTS, POLICY_INPUTS
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.models.helpers import get_active_bundle
@@ -118,41 +110,36 @@ class ModelState(ModelStateBase):
    cloudlog.warning(f"loading combined pkl: {pkl_path}")
    jits = load_oob(open_file_chunked(pkl_path))

-    metadata = jits['metadata']
-    self.use_frame_buffers = metadata.get('warp_dev') == 'AMD'
-    self.WARP_DEV = metadata.get('warp_dev', 'QCOM') if COMMA_HARDWARE else 'CPU'
-    self.DEV = ('AMD' if self.chestnut else 'QCOM') if COMMA_HARDWARE else 'CPU'
+    self.WARP_DEV = 'QCOM' if COMMA_HARDWARE else 'CPU'
+    self.DEV = 'AMD' if self.chestnut else self.WARP_DEV
    self.QUEUE_DEV = self.DEV
-    self.is_run_model = 'run_model' in jits
+    metadata = jits['metadata']

-    nv12_info = get_nv12_info(cam_w, cam_h)
-    self.frame_copy_size = nv12_copy_size(*nv12_info[:3])
-    self.full_frames: dict = {}
-    self._blob_cache: dict = {}
-    self.frame_buffers: dict = {}
+    self.is_legacy_model = 'run_policy' not in jits  # remove after next recompile
+    if self.is_legacy_model:
+      self.warp = jits[(cam_w, cam_h)]['warp_enqueue']
+      self.run_policy = jits[(cam_w, cam_h)]['run_policy']
+    else:
+      self.run_policy = jits['run_policy']
+      self.warp = jits[(cam_w, cam_h)]

-    if self.is_run_model or 'model' in metadata:
-      model_metadata = metadata.get('model', metadata)
-      self.input_shapes = model_metadata['input_shapes']
+    if 'model' in metadata:
+      model_metadata = metadata['model']
      self.vision_output_slices = model_metadata['output_slices']
      self.policy_output_slices = {}
      self._policy_slices_list = []
      self._combined_model_type = 'supercombo'
-      self._vision_input_names = [key for key in self.input_shapes if 'img' in key]
-      self.frame_skip = derive_frame_skip({}, self.input_shapes)
-      if self.is_run_model:
-        self.input_queues, self.numpy_inputs, self.frame_buffers = make_stock_input_queues(
-          self.input_shapes, self.frame_skip, device=self.DEV, frame_copy_size=self.frame_copy_size)
-        self.frame_views, self.npy = self.frame_buffers, self.numpy_inputs
-        self.run_model, self.run_policy, self.warp = jits['run_model'][(cam_w, cam_h)], None, None
-      else:
-        self.input_queues, self.numpy_inputs = make_supercombo_input_queues(self.input_shapes, self.frame_skip, device=self.QUEUE_DEV)
-        self.run_model, self.run_policy, self.warp = None, jits['run_policy'], jits[(cam_w, cam_h)]
+      self._vision_input_names = [key for key in model_metadata['input_shapes'] if 'img' in key]
+      frame_skip = derive_frame_skip({}, model_metadata['input_shapes'])
+      self.input_queues, self.numpy_inputs = make_supercombo_input_queues(model_metadata['input_shapes'],
+                                                                          frame_skip, device=self.QUEUE_DEV)
    else:
-      self.run_model, self.run_policy, self.warp = None, jits['run_policy'], jits[(cam_w, cam_h)]
      vision_metadata = metadata['vision']
-      policy_keys = [k for k in metadata if k not in ('vision', 'warp_dev')]
-      self._combined_model_type = 'split' if policy_keys == ['policy'] else 'multi_policy'
+      policy_keys = [k for k in metadata if k != 'vision']
+      if policy_keys == ['policy']:
+        self._combined_model_type = 'split'
+      else:
+        self._combined_model_type = 'multi_policy'
      self.vision_output_slices = vision_metadata['output_slices']
      self._policy_keys = policy_keys
      self._policy_slices_list = [metadata[k]['output_slices'] for k in policy_keys]
@@ -168,49 +155,54 @@ class ModelState(ModelStateBase):
    self._desire_key = next(key for key in self.numpy_inputs if key.startswith('desire'))
    self._road_key = next(key for key in self._vision_input_names if 'big' not in key)
    self._wide_key = next(key for key in self._vision_input_names if 'big' in key)
-    self.frame_buf_params = dict.fromkeys(self._vision_input_names, nv12_info)

    is_20hz = bundle.is20hz if bundle else self._combined_model_type in ('split', 'multi_policy')
    if is_20hz:
      from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants
      self.constants = SplitModelConstants()
    else:
+      from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
      self.constants = ModelConstants()

    if self._combined_model_type != 'supercombo':
      from openpilot.sunnypilot.modeld_v2.parse_model_outputs_split import Parser as SplitParser
      self.parser = SplitParser()
    else:
+      from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser
      self.parser = CombinedParser()

    self.prev_desire = np.zeros(self.constants.DESIRE_LEN, dtype=np.float32)
+    self.full_frames: dict = {}
+    self._blob_cache: dict = {}
+    nv12_info = get_nv12_info(cam_w, cam_h)
+    self.frame_buf_params = dict.fromkeys(self._vision_input_names, nv12_info)

-    if not self.is_run_model:
-      if self.use_frame_buffers:
-        self.frame_buffers = {k: np.zeros(self.frame_copy_size, dtype=np.uint8) for k in self._vision_input_names}
-        self.full_frames = {k: Tensor(self.frame_buffers[k], device='NPY').realize() for k in self._vision_input_names}
-      else:
-        self.full_frames = {k: Tensor(np.zeros(nv12_info[3], dtype=np.uint8), device=self.WARP_DEV).contiguous().realize() for k in self._vision_input_names}
-      self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=self.full_frames[self._road_key], big_frame=self.full_frames[self._wide_key])
+    yuv_size = self.frame_buf_params[self._road_key][3]
+    frame_tensor = Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.WARP_DEV).contiguous().realize()
+    big_frame_tensor = Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.WARP_DEV).contiguous().realize()
+
+    if self.is_legacy_model: # Remove this conditional hack after recompile
+      self.warp(**self.input_queues, frame=frame_tensor, big_frame=big_frame_tensor)
+    else:
+      self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=frame_tensor, big_frame=big_frame_tensor)

  def warmup(self) -> None:
-    dummy_size = self.frame_copy_size if (self.is_run_model or self.use_frame_buffers) else self.frame_buf_params[self._road_key][3]
-    dummy_frames = {k: np.zeros(dummy_size, dtype=np.uint8) for k in self._vision_input_names}
+    dummy_frames = {k: np.zeros(self.frame_buf_params[k][3], dtype=np.uint8) for k in self._vision_input_names}
    transforms = {k: np.eye(3, dtype=np.float32) for k in [self._road_key, self._wide_key] if k}
-    dummy_inputs = {k: np.zeros(v.shape, dtype=v.dtype) for k, v in self.numpy_inputs.items() if k not in ['tfm', 'big_tfm', 'prev_feat']}
-    self.run(dummy_frames, transforms, dummy_inputs)
-    if self.is_run_model:
-      self.input_queues, self.numpy_inputs, self.frame_buffers = make_stock_input_queues(
-        self.input_shapes, self.frame_skip, device=self.DEV, frame_copy_size=self.frame_copy_size)
-      self.frame_views = self.frame_buffers
-      self.npy = self.numpy_inputs
-    else:
-      for v in self.numpy_inputs.values():
-        v[:] = 0
-      if not self.use_frame_buffers:
-        self.full_frames.clear()
-        self._blob_cache.clear()
+
+    dummy_inputs = {}
+    for k, v in self.numpy_inputs.items():
+      if k not in ['tfm', 'big_tfm', 'prev_feat']:
+        dummy_inputs[k] = np.zeros(v.shape, dtype=v.dtype)
+
+    self.run(dummy_frames, transforms, dummy_inputs, prepare_only=False)
+
+    for v in self.numpy_inputs.values():
+      v[:] = 0
    self.prev_desire[:] = 0
+    self.full_frames.clear()
+    self._blob_cache.clear()
+

  @property
  def mlsim(self) -> bool:
@@ -225,49 +217,45 @@ class ModelState(ModelStateBase):
    return self._desire_key

  def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
-          inputs: dict[str, np.ndarray],
-          after_enqueue: Callable[[], None] | None = None) -> dict[str, np.ndarray] | None:
-    if self.is_run_model or self.use_frame_buffers:
-      for key, buf in bufs.items():
-        data = buf.data if hasattr(buf, 'data') else buf
-        np.copyto(self.frame_buffers[key], np.frombuffer(data, dtype=np.uint8, count=self.frame_copy_size))
-    else:
-      for key, buf in bufs.items():
-        ptr = np.frombuffer(buf.data, dtype=np.uint8).ctypes.data
-        cache_key = (key, ptr)
-        if cache_key not in self._blob_cache:
-          self._blob_cache[cache_key] = Tensor.from_blob(ptr, (self.frame_buf_params[key][3],), dtype='uint8', device=self.WARP_DEV)
-        self.full_frames[key] = self._blob_cache[cache_key]
+                inputs: dict[str, np.ndarray], prepare_only: bool) -> 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]
+      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]

    desire_key = self.desire_key
    inputs[desire_key][0] = 0
    self.numpy_inputs[desire_key][:] = np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0)
    self.prev_desire[:] = inputs[desire_key]
-
    for key in ('traffic_convention', 'lateral_control_params', 'action_t'):
      if key in self.numpy_inputs and key in inputs:
        self.numpy_inputs[key][:] = inputs[key]

-    self.numpy_inputs['tfm'][:, :] = transforms[self._road_key].reshape(3, 3)
-    self.numpy_inputs['big_tfm'][:, :] = transforms[self._wide_key].reshape(3, 3)
+    road_key = self._road_key
+    wide_key = self._wide_key
+    self.numpy_inputs['tfm'][:, :] = transforms[road_key].reshape(3, 3)
+    self.numpy_inputs['big_tfm'][:, :] = transforms[wide_key].reshape(3, 3)

-    if self.is_run_model:
-      outs, = self.run_model(**{k: self.input_queues[k] for k in MODELD_INPUTS})
-      raw_outputs = outs
+    if self.is_legacy_model:  # remove after next recompile
+      if prepare_only:
+        self.warp(**self.input_queues, frame=self.full_frames[road_key], big_frame=self.full_frames[wide_key])
+        return None
+      raw_outputs = self.run_policy(**self.input_queues, frame=self.full_frames[road_key], big_frame=self.full_frames[wide_key])
    else:
-      warped = self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=self.full_frames[self._road_key], big_frame=self.full_frames[self._wide_key])
+      if prepare_only:
+        self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=self.full_frames[road_key], big_frame=self.full_frames[wide_key])
+        return None
+      warped = self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=self.full_frames[road_key], big_frame=self.full_frames[wide_key])
      raw_outputs = self.run_policy(**{k: self.input_queues[k] for k in POLICY_INPUTS if k in self.input_queues}, warped=warped)

-    if after_enqueue is not None:
-      after_enqueue()
-
    if self._combined_model_type == 'supercombo':
      model_output = raw_outputs.numpy().flatten()
-      if self.chestnut and not np.all(np.isfinite(model_output)):
-        raise RuntimeError("model output not finite")
      sliced = {k: model_output[np.newaxis, v] for k, v in self.vision_output_slices.items()}
      outputs = self.parser.parse_outputs(sliced)
-      if 'prev_feat' in self.numpy_inputs and 'hidden_state' in self.vision_output_slices:
+      if 'prev_feat' in self.numpy_inputs:
        self.numpy_inputs['prev_feat'][:] = model_output[self.vision_output_slices['hidden_state']]
    else:
      vision_output = raw_outputs[0].numpy().flatten()
@@ -297,6 +285,9 @@ class ModelState(ModelStateBase):
      buf[0, :-1] = buf[0, 1:]
      buf[0, -1, :] = outputs['desired_curvature'][0, :] if not self.mlsim else 0

+    if self.chestnut and not np.all(np.isfinite(outputs.get('plan', np.array([0.])))):
+      raise RuntimeError("model output not finite")
+
    return outputs

  def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
@@ -382,11 +373,7 @@ def main(demo=False):
    loader.start()
    loader.join(BIG_MODEL_TIMEOUT)
    model = big_model
-    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(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, chestnut=False) if model is None or CHESTNUT else None
  if model is None:
@@ -500,6 +487,9 @@ def main(demo=False):
    run_count = run_count + 1

    frame_drop_ratio = frames_dropped / (1 + frames_dropped)
+    prepare_only = vipc_dropped_frames > 0
+    if prepare_only:
+      cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames")

    bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names}
    transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names}
@@ -522,14 +512,11 @@ def main(demo=False):

    mt1 = time.perf_counter()
    try:
-      send_chestnut = (chestnut_state is not None and
-                       run_count % round(model.constants.MODEL_FREQ / SERVICE_LIST['chestnutState'].frequency) == 0)
-      model_output = model.run(bufs, transforms, inputs, chestnut_state.send if send_chestnut else None)
+      model_output = model.run(bufs, transforms, inputs, prepare_only)
    except Exception:
      if not params.get_bool("ChestnutActive"):
        raise
      cloudlog.exception("chestnut failed, falling back to small")
-      params.put_bool("ChestnutModelError", True)
      params.put_bool("ChestnutActive", False)
      assert small_model is not None
      model = small_model
@@ -572,6 +559,9 @@ 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(model.constants.MODEL_FREQ / SERVICE_LIST['chestnutState'].frequency) == 0:
+      chestnut_state.send()
+
 if __name__ == "__main__":
  try:
    import argparse
@@ -75,11 +75,11 @@ class TestStockEquivalence(OpenpilotTestCase):

    frame_skip = derive_frame_skip(SPLIT_VISION_INPUT_SHAPES, SPLIT_POLICY_INPUT_SHAPES)
    stock_shapes = {**SPLIT_VISION_INPUT_SHAPES, **SPLIT_POLICY_INPUT_SHAPES, 'action_t': (1, 2)}
-    stock_queues, stock_npy, _frame_views = make_input_queues(stock_shapes, frame_skip, device='NPY', frame_copy_size=49152)
+    stock_queues, stock_npy = make_input_queues(stock_shapes, frame_skip, device='NPY')

-    # sunnypilot split pipeline has tfm/big_tfm as queues (stock has them in npy only)
-    assert set(stock_queues.keys()) <= set(state.input_queues.keys())
+    assert set(state.input_queues.keys()) == set(stock_queues.keys())
    assert {'desire', 'traffic_convention'} <= set(state.numpy_inputs.keys())
+    assert set(state.numpy_inputs.keys()) == set(stock_npy.keys()) - {'action_t', 'prev_feat'}

  def test_split_queue_keys_work_with_desire_key(self, model_state_factory):
    from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, make_split_input_queues
@@ -103,23 +103,6 @@ class TestStockEquivalence(OpenpilotTestCase):
    assert state.vision_output_slices == arch.metadata_structure['vision']['output_slices']
    assert state.policy_output_slices == arch.metadata_structure['policy']['output_slices']

-  def test_unified_run_model(self, tmp_path, monkeypatch, patch_modeld):
-    from openpilot.common.hardware import hw
-    from openpilot.selfdrive.modeld.helpers import dump_oob
-    shapes = {'img': (1, 12, 128, 256), 'big_img': (1, 12, 128, 256), 'features_buffer': (1, 24, 32, 512),
-              'desire_pulse': (1, 25, 8), 'traffic_convention': (1, 2), 'action_t': (1, 2)}
-    pkl_data = {'metadata': {'model': {'input_shapes': shapes, 'output_slices': {}}},
-                'run_model': {(CAM_W, CAM_H): tests_helpers._noop_jit}}
-    with open(tmp_path / 'driving_test_tinygrad.pkl', 'wb') as f:
-      dump_oob(pkl_data, f)
-    bundle = DummyBundle(models=[DummyModel('supercombo', 'driving_test_tinygrad.pkl')])
-    patch_modeld(bundle)
-    monkeypatch.setattr(hw.Paths, 'model_root', staticmethod(lambda: str(tmp_path)))
-    state = ModelState(cam_w=CAM_W, cam_h=CAM_H)
-    assert state.is_run_model and state.run_model is not None
-    assert state.run_policy is None and state.warp is None
-    assert 'img' in state.frame_views and 'big_img' in state.frame_views
-

 ARCHETYPE_NAMES = list(ARCHETYPES.keys())

@@ -7,7 +7,6 @@ See the LICENSE.md file in the root directory for more details.

 import os
 import tempfile
-import unittest
 from pathlib import Path

 import numpy as np
@@ -249,27 +248,23 @@ class TestStockCompileModeldEquivalence(OpenpilotTestCase):
    input_shapes = {
      'img': (1, 12, 128, 256),
      'desire_pulse': (1, 25, 8),
-      'features_buffer': (1, 24, 32, 512),
+      'features_buffer': (1, 24, 512),  # when https://github.com/commaai/openpilot/pull/38681 merges, update to 1,24,32,512
      'traffic_convention': (1, 2),
      'action_t': (1, 2),
    }
    frame_skip = 4

-    stock_queues, stock_npy, _frame_views = stock_make_input_queues(input_shapes, frame_skip, device='NPY', frame_copy_size=49152)
+    stock_queues, stock_npy = stock_make_input_queues(input_shapes, frame_skip, device='NPY')
    sunny_queues, sunny_npy = sunny_make_supercombo_input_queues(input_shapes, frame_skip, device='NPY')
-    # sunnypilot split pipeline has tfm/big_tfm as queues; packed_npy_inputs size differs (different frame packing)
-    assert set(stock_queues.keys()) <= set(sunny_queues.keys())
+    assert set(sunny_queues.keys()) == set(stock_queues.keys())
    for key in stock_queues:
-      if key == 'packed_npy_inputs':
-        continue
      assert sunny_queues[key].shape == stock_queues[key].shape, \
        f"Queue shape mismatch for {key}: sunny {sunny_queues[key].shape} != stock {stock_queues[key].shape}"
-    assert set(stock_npy.keys()) <= set(sunny_npy.keys())
+    assert set(sunny_npy.keys()) == set(stock_npy.keys())
    for key in stock_npy:
      assert sunny_npy[key].shape == stock_npy[key].shape, \
        f"Numpy array shape mismatch for {key}: sunny {sunny_npy[key].shape} != stock {stock_npy[key].shape}"

-  @unittest.skip("upstream removed make_warp_input_queues — warp merged into run_model")
  def test_make_warp_queues_stock_equivalence(self):
    from openpilot.selfdrive.modeld.compile_modeld import make_warp_input_queues as stock_make_warp_queues
    from openpilot.sunnypilot.modeld_v2.compile_modeld import make_warp_queues as sunny_make_warp_queues
@@ -20,46 +20,18 @@ def get_default_model() -> str:

 DEFAULT_MODEL_NAME_PATH = os.path.join(BASEDIR, "openpilot", "sunnypilot", "models", "model_name.py")
 MODEL_HASH_PATH = os.path.join(BASEDIR, "openpilot", "sunnypilot", "models", "tests", "model_hash")
-BIG_MODEL_HASH_PATH = os.path.join(BASEDIR, "openpilot", "sunnypilot", "models", "tests", "big_model_hash")
 SUPERCOMBO_ONNX_PATH = os.path.join(BASEDIR, "openpilot", "selfdrive", "modeld", "models", "driving_supercombo.onnx")
-BIG_SUPERCOMBO_ONNX_PATH = os.path.join(BASEDIR, "openpilot", "selfdrive", "modeld", "models", "big_driving_supercombo.onnx")
-
-
-def _read_model_name_fields():
-  with open(DEFAULT_MODEL_NAME_PATH) as f:
-    content = f.read()
-  fields = {}
-  for line in content.splitlines():
-    if "=" in line:
-      key, val = line.split("=", 1)
-      fields[key.strip()] = val.strip().strip('"')
-  return fields


 def update_model_hash():
-  fields = _read_model_name_fields()
  supercombo_hash = get_file_hash(SUPERCOMBO_ONNX_PATH)
-  fingerprint = f"{supercombo_hash}:{fields.get('DEFAULT_MODEL', '')}:{fields.get('DEFAULT_MODEL_REF', '')}"
-  combined_hash = hashlib.sha256(fingerprint.encode()).hexdigest()
+  combined_hash = hashlib.sha256(supercombo_hash.encode()).hexdigest()

  with open(MODEL_HASH_PATH, "w") as f:
    f.write(combined_hash)

  print(f"Generated and updated new combined model hash to {MODEL_HASH_PATH}")

-  if os.path.exists(BIG_SUPERCOMBO_ONNX_PATH):
-    import subprocess
-    rel = os.path.relpath(BIG_SUPERCOMBO_ONNX_PATH, os.getcwd())
-    pointer = subprocess.check_output(["git", "show", f"HEAD:{rel}"], text=True)
-    oid = next(l.split(":", 1)[1] for l in pointer.splitlines() if l.startswith("oid sha256:"))
-    big_fingerprint = f"{oid}:{fields.get('DEFAULT_BIG_MODEL', '')}:{fields.get('DEFAULT_BIG_MODEL_REF', '')}"
-    big_combined_hash = hashlib.sha256(big_fingerprint.encode()).hexdigest()
-
-    with open(BIG_MODEL_HASH_PATH, "w") as f:
-      f.write(big_combined_hash)
-
-    print(f"Generated and updated new big model hash to {BIG_MODEL_HASH_PATH}")
-

 def get_ref_for_name(url: str, name: str) -> str:
  response = requests.get(url, timeout=10)
@@ -138,8 +138,8 @@ class ModelCache:

 class ModelFetcher:
  """Handles fetching and caching of model data from remote source"""
-  MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v22.json"
-  MODEL_URL_CHESTNUT = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_chestnut_v25.json"
+  MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v21.json"
+  MODEL_URL_CHESTNUT = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_chestnut_v22.json"

  MODEL_SOURCES = {
    "qcom": (MODEL_URL, ""),
@@ -19,7 +19,7 @@ from openpilot.common.hardware.hw import Paths
 from openpilot.selfdrive.modeld.helpers import chestnut_present

 # SET ME TO THE EXACT JSON VERSION WE SET IN SUNNYPILOT_MODELS REPO
-REQUIRED_JSON_VERSION = 19
+REQUIRED_JSON_VERSION = 18

 CUSTOM_MODEL_PATH = Paths.model_root()
 METADATA_PATH = Path(__file__).parent / '../models/supercombo_metadata.pkl'
@@ -1,4 +1,4 @@
 DEFAULT_MODEL = "CD210"
 DEFAULT_MODEL_REF = "5b6436a90cf6902b8aaa71c2b6f3d7164d8ae391"
-DEFAULT_BIG_MODEL = "BMRLNAP Model v4"
-DEFAULT_BIG_MODEL_REF = "f877d7a0ccc3cce943c76e285214c020cd65c899"
+DEFAULT_BIG_MODEL = "Lebowski"
+DEFAULT_BIG_MODEL_REF = "fa0c6876d3cf070e91e25e5353ceadc68a5b3285"
@@ -1 +0,0 @@
-2c814f08a2c51323b87839fbf8d2c2a9853a2b5536271b3d67f7b7a2de7f9374
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Isaac Barham	61dac4977b	Ford: restore large-turn path demand with bounded heading backoff Reuse the existing model-pose allocator for aligned large maneuvers while encoding remaining selected curvature as C0/C1 and keeping C2/C3 zero. Permit measured heading backoff during release or PSCM limits without turning model-base changes into stored bias. Validate with 127 tests and 67 subtests, including recorded large-turn retention, release and reversal, repeated-measurement backoff, CAN packing, logging, and Sunnylink schema checks. Replay checks command behavior; enabled vehicle tracking remains unvalidated.	2026-09-05 13:28:56 -04:00
Isaac Barham	09acf8ec2f	Ford: honor C2-free toggle without EPS firmware gate	2026-09-05 12:08:21 -04:00
Isaac Barham	79a4caa1f6	Ford: add bounded yaw feedback to C2-free heading requests Retain the absolute desired-curvature base and unchanged C0, while adding measured yaw-error correction to C1 under fresh PSCM status. Preserve command limits, reset on override or unusable status, and release stored correction with the base request. Admit reachable partial increments at host slew limits. Publish PSCM enums with original CAN receipt timestamps through carStateSP. Add telemetry, status/driver guards, CAN roundtrip tests and recorded fixtures. Validation: focused suite 116 tests and 38 subtests; Ruff, settings compilation and diff checks pass. Production replay covers 52,273 route80 cycles; no-status fallback preserves v4 over 246,961 cycles / 43 segments. Physical stability and the reported 85-degree plateau remain unvalidated; EPS limits can inhibit the new correction.	2026-09-05 09:46:32 -04:00
Isaac Barham	0ace0b0510	Ford: align C2-free heading with desired curvature Derive full absolute C1 heading from the same selected curvature as C0, retaining existing bounds and independent slew. Keep the former filtered model heading as a diagnostic comparison and preserve input validity gates. Add real route80 command regressions and release/reversal checks. All 97 focused tests pass; 43-segment replay preserves C0 and gates exactly and matches the independent C1 candidate. Physical tracking and stability remain unvalidated for this revision.	2026-09-05 07:56:40 -04:00
Isaac Barham	98662df401	Ford: drive C2-free C0 from planned curvature Encode the selected bounded curvature as C0 with an 8 m minimum preview while retaining full model-heading C1. Limit each channel independently so C1 transitions cannot delay C0 release, and validate the selected action source timestamp. Add action, release, source-freshness, CAN and route regressions. Document the slow-turn reference disagreement and the limits of frozen-motion replay; physical centering remains unvalidated.	2026-09-04 21:15:17 -04:00
Isaac Barham	10e354d668	Ford: restore C0 centering and full C1 path demand Replace the weak nominal acceleration conversion with C2-free spatial path requests. Align retained model geometry using measured CAN yaw before filtering model innovations, and preserve large-turn demand and straight-path centering. Validate with seven recorded maneuver episodes, full-route command replay, real CAN packing and focused controller/settings tests. Physical closed-loop behavior remains unvalidated.	2026-09-04 16:59:24 -04:00
Isaac Barham	7d558c0650	Ford: replace shared path experiment with bounded virtual angle control Track the bounded planner reference through C0 and delay-aware PI/rate feedback through C1. Remove the failed Shared Path toggle and add a default-off, Lightning RL38-specific Virtual Angle setting. Reject stale inputs and disable outgoing lateral requests when the path is invalid. Validation: 85 focused tests plus 22 subtests, native Params, real CAN packing, settings generation and Ruff passed. Frozen route78 replay attenuates the observed command forcing; physical stability and turn authority remain unvalidated.	2026-09-04 16:11:33 -04:00
Isaac Barham	daeb966d05	Ford: add C2-free shared path experiment	2026-09-04 15:22:32 -04:00
Isaac Barham	727c26ce8c	Ford: allow earlier joint fast-path buildup experimentally Preserve geometric C0/C1 before nominal plateaus during same-direction buildup. Keep reversal guards, command limits, C2 policy, and the existing default-off Shared Path Controller selection. Require nonzero demand for joint buildup. Known limitation: nominal short-turn cancellation settles later with queued commands. Retain that regression as an explicit expected failure; this experiment does not establish physical response or resolve unwind. Add entry and zero-demand coverage. Assisted-by: OpenAI Codex	2026-09-04 11:54:41 -04:00
Isaac Barham	614022defb	test(ford): retain queued commands in turn-release regression Continue the same allocator through a short turn and cancellation so release checks retain command lead as well as nominal coefficient state. Reject earlier geometry buildup that keeps charging after cancellation. No production controller changes. Assisted-by: OpenAI Codex	2026-09-04 11:51:24 -04:00
Isaac Barham	5e67122e64	Ford: use opendbc LMC2 packing guard Update opendbc to 72a775d3 for C0/C1/C3 wire-range saturation and nonfinite input rejection. No fallback controller or tuning changes are included. Assisted-by: OpenAI Codex	2026-09-04 11:18:11 -04:00
Isaac Barham	25d095177e	Ford: preserve large shared-path geometry past nominal plateaus Retain larger model-derived fast fields when nominal allocation is equivalent, with per-field plateau qualification and inward-demand release priority. Keep corrected and geometric fast fields independently selectable without changing the existing contribution map, C2 policy, cadence, or command limits. Validated with 68 controller/fallback/logging tests, 5 adversarial release tests, and 38300 fixed-input replay updates. Physical turn authority and release remain unverified; the existing experiment stays default off. Assisted-by: OpenAI Codex	2026-09-04 11:09:13 -04:00
Isaac Barham	3eb7938aad	Ford: retain geometric demand in shared path diagnostics	2026-09-04 10:36:17 -04:00
Isaac Barham	a525905708	Ford: reuse coefficient calculations in shared allocator Cache per-field packet conversion and state projections within each allocation instead of recomputing them for every candidate combination. Preserve candidate ordering, scores, limits, and selected commands. Add a deterministic limiter-work regression budget. Local recorded-input mean controller CPU time falls 56%; 1292 recorded updates and 4000 randomized allocations match the previous outputs exactly. Device timing remains unverified. Assisted-by: Codex	2026-09-04 09:44:04 -04:00
Isaac Barham	e298864a50	Ford: fix controlsd structured logging crashes Use SwagLogger.event for controller selection and periodic diagnostics. Logger.info forwards arbitrary keywords to Logger._log and crashed all Ford startups, regardless of the experiment toggle. Exercise both actual call sites with INFO enabled and the real logger/formatter. Assisted-by: Codex	2026-09-04 09:33:28 -04:00
Isaac Barham	8639bdcca4	Ford: add opt-in shared path control experiment Separate holding demand, bounded pose feedback, and nominal coefficient allocation. Add a default-off Sunnylink selector with startup diagnostics and preserve the existing controller when disabled. Assisted-by: Codex	2026-09-04 09:19:22 -04:00
Isaac Barham	458a3015cd	Ford: add optional PSCM coefficient observer Assisted-by: Codex	2026-09-02 16:49:22 -04:00
Isaac Barham	336ce75f3d	Ford: keep gentle driving on C2 only Remove model-pose residuals and tracking trim from the gentle regime. Blend the model pose into C0/C1 only as maneuver demand rises, while retaining opposing-path C2 unload and the coordinated 100 Hz handoff. Assisted-by: Codex	2026-09-02 09:53:56 -04:00
Isaac Barham	517c15f9c2	Ford: restore upstream-strength normal C2 Use constrained desired curvature for ordinary C2 while keeping model geometry authoritative in the coordinated C0/C1 residual. This restores normal centering strength without changing large-maneuver or bounded-feedback behavior. Assisted-by: Codex	2026-09-02 08:32:03 -04:00
Isaac Barham	aa73207ab8	Ford: separate path feedforward from pose feedback Keep the model's remaining path as feedforward while using the delay-aligned measured pose only as a bounded trim. Allocate common gentle model curvature to C2 and carry changing geometry in C0/C1 without allowing the action head to invent a path. Assisted-by: Codex	2026-09-01 22:28:35 -04:00
Isaac Barham	184b73d8de	Revert "Ford: add optional native path polynomial" This reverts commit `6a1b697ed3`.	2026-09-01 21:33:58 -04:00
Isaac Barham	6a1b697ed3	Ford: add optional native path polynomial Assisted-by: Codex	2026-09-01 21:17:52 -04:00
Isaac Barham	7eb7e93deb	tools: evaluate native Ford path polynomial Assisted-by: Codex	2026-09-01 20:59:11 -04:00
Isaac Barham	693daf9866	Ford: align path to predicted vehicle pose Rebase model preview against a gainless 100 ms curvature-trend prediction. Remove direct local-curvature feedback while preserving coordinated C0/C1/C2 authority and geometric C2 unloads. Assisted-by: Codex	2026-09-01 17:02:28 -04:00
Isaac Barham	afcc2b9455	Ford: track local model curvature Use the first two meters of model heading for measured-curvature feedback while preserving the existing longer model-pose feedforward. This prevents future geometry from initiating premature correction without weakening turn anticipation. Assisted-by: Codex	2026-09-01 15:39:14 -04:00
Isaac Barham	3fdab7e8f0	Ford: close path loop on model curvature Use measured curvature error against the forward model path to add bounded bidirectional C0/C1 correction. Unload stale C2 when it would oppose an unwind or reversal. Assisted-by: Codex	2026-09-01 15:22:13 -04:00
Isaac Barham	f488bfc806	Ford: restore responsive path controller Return to the pre-predicted-pose C2-first controller from `a1dcec490` after road testing found both later variants weaker or unstable. Preserve the current sunnypilot master merge and 100 Hz LMC2 transport. Assisted-by: Codex	2026-09-01 13:26:57 -04:00
Isaac Barham	fd62fed669	Merge sunnypilot master into hiimisaac-dev Preserve the assisted-driving summary while adopting the current Chestnut status UI. Assisted-by: Codex	2026-09-01 12:55:47 -04:00
Isaac Barham	3a665737c2	Ford: encode path in current vehicle frame Remove delay-projected measured-curvature feedback that amplified curve hunting. Keep the model polynomial in the current vehicle frame while preserving the coordinated 100 Hz C0/C1/C2 handoff. Assisted-by: Codex	2026-09-01 12:54:14 -04:00
Isaac Barham	b6a87b8958	Ford: align path control to predicted pose Advance the rolling model path by the generic lateral delay, express its remaining seven-meter pose in the predicted vehicle frame, and derive C2 from the same steady geometry. Remove desiredCurvature as a competing Ford path target. Assisted-by: Codex	2026-09-01 07:50:03 -04:00
Isaac Barham	a1dcec490f	Ford: preserve pose authority when C1 clips Move heading authority lost at the DBC angle limit into available C0 endpoint authority while retaining the coordinated output limiter. Assisted-by: Codex	2026-09-01 01:09:47 -04:00
Isaac Barham	0729ce7c08	Ford: continuously blend model pose with C2 Use the model's forward offset and heading for fast path authority while C2 retains ordinary path following. Coordinate all transmitted coefficients through one bounded handoff and add measured-curvature catch-up without overshoot countersteer.\n\nAssisted-by: Codex	2026-09-01 00:15:07 -04:00
Isaac Barham	916fb1d522	Ford: separate centering and maneuver paths Use heading/action hysteresis to keep normal driving entirely on C2 and large maneuvers entirely on model C0/C1. Restore a one-second pose horizon with a 7 m floor. Assisted-by: Codex	2026-08-31 21:58:11 -04:00
Isaac Barham	af5e7f5327	Ford: use model pose for large maneuvers Keep desired curvature in C2 for ordinary driving, then continuously hand off to model offset and heading for large maneuvers. Use one fitted 0.5 second lookahead with a 7 meter floor for both pose fields and keep C3 zero. Assisted-by: Codex	2026-08-31 20:42:03 -04:00
Isaac Barham	bf2e9ca318	Ford: keep slow curvature out of turns Remove the one-frame C2 persistence and restore the continuous gentle-centering allocation. Real turn demand now clears C2 immediately and remains in the bounded fast path fields. Assisted-by: Codex	2026-08-31 20:34:42 -04:00
Isaac Barham	41b433c619	Ford: split one model frame into fast path fields Delay C2 by one 50 ms model frame and place the new-request difference in C0/C1 alongside measured tracking error. Clear the delay on inactive or invalid control. Assisted-by: Codex	2026-08-31 20:26:46 -04:00
Isaac Barham	ed56f3ff7c	Ford: restore curvature as primary path control Keep upstream-style desired curvature active in C2 for steady path following. Use C0/C1 only for demand beyond C2 and measured tracking error, preserving fast turn and unwind authority without replacing C2. Assisted-by: Codex	2026-08-31 17:31:40 -04:00
Isaac Barham	5865ad108c	Ford: align Panda safety with 100Hz path control Update the opendbc pointer for the tested CAN-FD Mode 2 safety cadence fix. Assisted-by: Codex	2026-08-31 14:50:32 -04:00
Isaac Barham	8ed82eae6f	Ford: use direct LMC2 mode transitions Remove the custom SafeRampOut sequence and follow the proven Mode 2 to Mode 0 behavior. Assisted-by: Codex	2026-08-31 14:11:02 -04:00
Isaac Barham	88f6f66032	Ford: restore proven LMC2 ramp sequence Keep active CAN-FD path control at 100 Hz while limiting SafeRampOut to the historically working 20-message sequence. Assisted-by: Codex	2026-08-31 13:22:02 -04:00
Isaac Barham	33e70080ad	Ford: update CAN-FD path control rate Assisted-by: Codex	2026-08-31 12:57:00 -04:00
Isaac Barham	b7f0e3fbdc	Ford: restore full C2 gentle path following Assisted-by: Codex	2026-08-31 11:41:29 -04:00
Isaac Barham	7f371b8acd	Ford: hold path authority through turns Assisted-by: Codex	2026-08-30 15:45:21 -04:00
Isaac Barham	24c858e618	Ford: strengthen bounded path tracking feedback Keep action curvature authoritative while increasing bounded C0/C1 feedback when measured curvature is behind. Keep C2 allocation tied to maneuver demand instead of tracking error. Assisted-by: Codex <codex@openai.com>	2026-08-30 11:29:09 -04:00
Isaac Barham	405407c252	Ford: drive fast path from desired curvature Make C0 and C1 a coherent virtual-curvature pair sourced from the constrained action target and measured tracking error. Keep model trend only for supplemental C2 unloading so model geometry cannot inflate fast steering authority across vehicles. Assisted-by: Codex	2026-08-30 09:37:52 -04:00
Isaac Barham	d49b56bff5	ford: drop under-actuating coherent path experiment Road testing showed the endpoint-constrained C0/C1 pair opposed the requested rotation and delivered less than half the needed authority. Restore the prior same-direction fast-path encoder. Assisted-by: Codex	2026-08-30 09:12:34 -04:00
Isaac Barham	f8d8b8ee56	ui: expose Ford path experiment on comma four Assisted-by: Codex	2026-08-30 08:54:37 -04:00
Isaac Barham	8774a462ac	ford: add coherent path pose experiment Assisted-by: Codex	2026-08-30 08:54:37 -04:00
Isaac Barham	1b41e9637f	ford: balance path pose and curvature unwind Assisted-by: Codex	2026-08-30 08:54:37 -04:00
Isaac Barham	27a220677a	Productionize assisted driving milestones Assisted-by: OpenAI Codex	2026-08-29 07:52:11 -04:00
Isaac Barham	26e4889fcb	Raise comma four alert volume	2026-08-28 19:38:48 -04:00
Isaac Barham	70fa5d0fca	Boost comma four alerts and reset milestones	2026-08-28 16:13:14 -04:00
Isaac Barham	2d700cc0d0	Add alert-style milestone scrim	2026-08-28 11:34:46 -04:00
Isaac Barham	cc9ae66b22	Persist assisted driving milestones	2026-08-28 09:41:13 -04:00
Isaac Barham	505270420f	Refine milestone celebration typography	2026-08-28 08:40:09 -04:00
Isaac Barham	bb1a17d2a0	Prototype assisted driving milestones	2026-08-28 07:34:03 -04:00
Isaac Barham	6db807b5a0	ford: narrow lateral path interface Assisted-by: Codex	2026-08-27 20:06:36 -04:00
Isaac Barham	42e1414bc4	ford: source C2 only from desired curvature Prevent model-fit curvature jitter from directly modulating the PSCM's slow C2 channel. Assisted-by: Codex	2026-08-27 19:53:59 -04:00
Isaac Barham	3e020e321f	ford: gate curvature rate with maneuver demand Assisted-by: Codex	2026-08-27 19:44:19 -04:00
Isaac Barham	7e2000e909	ford: make path allocation demand driven Assisted-by: Codex	2026-08-27 19:08:21 -04:00
Isaac Barham	e96055846c	ford: distill lateral path controller Assisted-by: Codex	2026-08-27 16:27:38 -04:00
Isaac Barham	d47646b28f	Ford: keep LMC2 available through path gaps Assisted-by: Codex	2026-08-27 15:36:50 -04:00
Isaac Barham	e75bc83424	Ford: retain centering through curve exits Keep a bounded geometric C2 band for lane centering, preserve established rolling arcs during same-direction unwind, and smoothly release old-direction C2 on reversals. Slew-limit the fast C1 command to prevent threshold chatter. Assisted-by: Codex	2026-08-27 15:13:56 -04:00
Isaac Barham	08e48958b6	Ford: close the loop on path curvature Use the rolling path for pose and slow geometry while allocating jerk-limited requested curvature and bounded tracking error to the fast heading field. Prevent filtered C2 from reinforcing an unwind or reversal. Assisted-by: Codex	2026-08-27 14:35:59 -04:00
Isaac Barham	25d0d0f1ff	Ford: embed model path in rolling reference Assisted-by: Codex	2026-08-27 13:08:15 -04:00
				`@@ -1 +0,0 @@`
				`2c814f08a2c51323b87839fbf8d2c2a9853a2b5536271b3d67f7b7a2de7f9374`