reboot

This reverts commit 15e5d2efb9.

.gitattributes

@@ -9,7 +9,6 @@
 *.ttf filter=lfs diff=lfs merge=lfs -text
 *.otf filter=lfs diff=lfs merge=lfs -text
 *.wav filter=lfs diff=lfs merge=lfs -text
-openpilot/selfdrive/assets/sounds/milestone.wav -filter -diff -merge -text
 
 openpilot/selfdrive/car/tests/test_models_segs.txt filter=lfs diff=lfs merge=lfs -text
 openpilot/common/hardware/comma/updater filter=lfs diff=lfs merge=lfs -text

.github/CODEOWNERS

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

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

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

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

@@ -341,7 +341,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 +367,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 +484,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

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

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

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

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

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

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

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

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

docs/ford_virtual_angle_experiment.md

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

openpilot/cereal/custom.capnp

@@ -383,7 +383,6 @@ struct CarControlSP @0xa5cd762cd951a455 {
   leadOne @2 :LeadData;
   leadTwo @3 :LeadData;
   intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
-  fordLateralPath @5 :FordLateralPath;
 
   struct Param {
     key @0 :Text;
@@ -404,14 +403,6 @@ 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;
@@ -456,16 +447,6 @@ 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 {
@@ -489,30 +470,7 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
   }
 }
 
-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 CustomReserved10 @0xcb9fd56c7057593a {
 }
 
 struct CustomReserved11 @0xc2243c65e0340384 {

openpilot/cereal/log.capnp

@@ -2642,7 +2642,7 @@ struct Event {
     carStateSP @114 :Custom.CarStateSP;
     liveMapDataSP @115 :Custom.LiveMapDataSP;
     modelDataV2SP @116 :Custom.ModelDataV2SP;
-    assistedDrivingMilestoneState @136 :Custom.AssistedDrivingMilestoneState;
+    customReserved10 @136 :Custom.CustomReserved10;
     customReserved11 @137 :Custom.CustomReserved11;
     customReserved12 @138 :Custom.CustomReserved12;
     customReserved13 @139 :Custom.CustomReserved13;

openpilot/cereal/services.py

@@ -90,7 +90,6 @@ _services: dict[str, tuple] = {
   "carParamsSP": (True, 0.02, 1),
   "carControlSP": (True, 100., 10),
   "carStateSP": (True, 100., 10),
-  "assistedDrivingMilestoneState": (True, 10., 1),
   "liveMapDataSP": (True, 1., 1),
   "modelDataV2SP": (True, 20., None, QueueSize.BIG),
   "liveLocationKalman": (True, 20.),

openpilot/common/params.cc

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

openpilot/common/params.h

@@ -75,7 +75,6 @@ public:
     return put(key.c_str(), val ? "1" : "0", 1);
   }
   void putNonBlocking(const std::string &key, const std::string &val);
-  void flushNonBlockingWrites();
   inline void putBoolNonBlocking(const std::string &key, bool val) {
     putNonBlocking(key, val ? "1" : "0");
   }

openpilot/common/params.py

@@ -73,7 +73,6 @@ 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)
@@ -179,10 +178,6 @@ class Params:
   def put_bool(self, key, val, block=False):
     params_put_bool(self.p, self.check_key(key), val, block)
 
-  def flush(self):
-    """Wait for all prior nonblocking writes from this Params instance."""
-    params_flush(self.p)
-
   def remove(self, key):
     params_remove(self.p, self.check_key(key))
 

openpilot/common/params_c.cc

@@ -133,12 +133,6 @@ 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);

openpilot/common/params_keys.h

@@ -143,8 +143,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
 
     // --- 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
@@ -165,7 +163,6 @@ 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"}},
@@ -175,9 +172,7 @@ 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}},
@@ -237,8 +232,6 @@ 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"}},

openpilot/common/tests/test_params.py

@@ -106,13 +106,6 @@ 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()
 

openpilot/selfdrive/car/card.py

@@ -21,7 +21,6 @@ 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
@@ -199,7 +198,6 @@ class Car:
     # Update carState from CAN
     CS, CS_SP = self.CI.update(can_list)
     CS_SP = convert_to_capnp(CS_SP)
-    populate_ford_pscm_status(self.CP, self.CI.can_parsers, CS_SP, CS.canValid)
 
     # Update radar tracks from CAN
     RD: structs.RadarDataT | None = self.RI.update(can_list)

openpilot/selfdrive/car/ford_pscm_status.py

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

openpilot/selfdrive/car/helpers.py

@@ -63,6 +63,5 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct
   struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement(
     **remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {}))
   )
-  struct_dataclass.fordLateralPath = structs.FordLateralPath(**remove_deprecated(struct_dict.get('fordLateralPath', {})))
 
   return struct_dataclass

openpilot/selfdrive/car/tests/test_ford_pscm_status.py

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

openpilot/selfdrive/controls/controlsd.py

@@ -1,6 +1,5 @@
 #!/usr/bin/env python3
 import math
-import time
 from numbers import Number
 
 from openpilot.cereal import log
@@ -12,11 +11,8 @@ 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
@@ -48,7 +44,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', 'carStateSP', 'carOutput',
+                                   'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput',
                                    'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + self.sm_services_ext,
                                   poll='selfdriveState')
     self.pm = messaging.PubMaster(['carControl', 'controlsState'] + self.pm_services_ext)
@@ -56,15 +52,6 @@ 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
@@ -168,39 +155,6 @@ 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)

openpilot/selfdrive/controls/lib/ford_path.py

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

openpilot/selfdrive/controls/lib/ford_virtual_angle.py

@@ -1,469 +0,0 @@
-"""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 ReleaseGuard:
-  """Keep pose growth from defeating a measured turn release.
-
-  Request history is independent of the integral: driver input resets
-  correction authority, but does not erase valid requests already sent.
-  Coefficient signs describe path geometry, not motor effort. Opposing path
-  terms stay available; only growth in the requested direction is limited.
-  """
-  def __init__(self, delay):
-    self.delay = delay
-    self.history = deque()
-    self.last_measurement_time = None
-    self.last_pscm_time = None
-    self.direction = 0.
-    self.active = False
-    self.reference_curvature = None
-
-  def update(self, desired, *, yaw_rate, speed, now, measurement_time, heading_horizon, driver_override, pscm_status):
-    self.history.append((now, desired))
-    while len(self.history) > 2 and self.history[1][0] < now - self.delay - .25:
-      self.history.popleft()
-    direction = float(np.sign(desired))
-    status_reason = pscm_status.invalid_reason(now) if pscm_status is not None else 'missing_pscm'
-    fresh_status = (status_reason in (None, 'unavailable_pscm') and
-                    (self.last_pscm_time is None or pscm_status.timestamp >= self.last_pscm_time))
-    available = (not driver_override and speed >= FEEDBACK_MIN_SPEED and direction != 0. and
-                 fresh_status and status_reason is None and pscm_status.limit < 3)
-    if fresh_status:
-      self.last_pscm_time = pscm_status.timestamp
-    if not available or direction != self.direction:
-      self.active = False
-    self.direction = direction
-    if measurement_time != self.last_measurement_time:
-      self.last_measurement_time = measurement_time
-      reference = next((sample for sample in reversed(self.history) if sample[0] <= measurement_time - self.delay), None)
-      self.reference_curvature = reference[1] if reference is not None else None
-      self.active = False
-      if available and reference is not None:
-        delayed = reference[1]
-        releasing = (abs(delayed) - abs(desired)) * heading_horizon > HEADING_RESOLUTION
-        self.active = (delayed * desired > 0. and releasing and
-                       (yaw_rate - speed * desired) * direction > 0. and
-                       (yaw_rate - speed * delayed) * direction > 0.)
-    return self.active
-
-  def limit(self, target, previous):
-    if self.active and target * self.direction > 0.:
-      return self.direction * min(target * self.direction, max(previous * self.direction, 0.))
-    return target
-
-
-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.response_history = deque()
-    self.bias = 0.
-    self.previous_base = None
-    self.last_measurement_time = self.last_pscm_time = None
-    self.backoff_active = False
-    self.release_command = self.release_reference = 0.
-    self.release_quiet_since = None
-    self.diagnostics = {'heading_bias': 0., 'feedback_status': status, 'feedback_reference_time': None,
-                        'feedback_reference_curvature': None, 'feedback_yaw_error': None,
-                        'feedback_backoff_active': False, 'feedback_recovery_active': False,
-                        'feedback_release_tracking_active': False, 'feedback_release_ceiling': None,
-                        'feedback_curvature_delta': None}
-
-  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'
-    recovery_active = release_tracking_active = False
-    release_ceiling = curvature_delta = None
-    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
-      self.response_history.append((measurement_time, yaw_rate / speed))
-      while len(self.response_history) > 2 and self.response_history[1][0] < target_time - .1:
-        self.response_history.popleft()
-      prior_response = next((sample for sample in reversed(self.response_history) if sample[0] <= target_time), None)
-      if prior_response is not None:
-        curvature_delta = yaw_rate / speed - prior_response[1]
-      # 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
-        # A brief quantization-level pause must not acquire a larger ceiling.
-        # A full response interval without release ends the previous episode.
-        if desired * base <= 0.:
-          self.release_command = self.release_reference = 0.
-          self.release_quiet_since = None
-        elif releasing:
-          self.release_quiet_since = None
-          if self.release_reference == 0. and reference_curvature * desired > 0. and desired * base > 0.:
-            self.release_reference = abs(reference_curvature)
-            self.release_command = max(0., math.copysign(1., base) * previous_command)
-        else:
-          if self.release_quiet_since is None:
-            self.release_quiet_since = measurement_time
-          if measurement_time - self.release_quiet_since >= self.delay:
-            self.release_command = self.release_reference = 0.
-        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.
-        recovering = (releasing and pscm_status.limit < 2 and self.bias * base < 0. and
-                      desired * base > 0. and reference_curvature * base > 0. and yaw_error * base > 0. and current_yaw_error * base > 0.)
-        tracking_release = (releasing and pscm_status.limit < 2 and self.bias * base >= 0. and
-                            desired * base > 0. and reference_curvature * base > 0. and yaw_error * base > 0. and current_yaw_error * base > 0. and
-                            curvature_delta is not None and math.copysign(1., base) * curvature_delta * heading_horizon <= HEADING_RESOLUTION and
-                            self.release_reference > 0.)
-        if tracking_release:
-          # Taper only extra correction; preserve the large-turn model base.
-          # The bound comes from earlier commands, not an EPS gain fit.
-          remaining = min(1., abs(desired) / self.release_reference)
-          headroom = max(0., self.release_command - abs(base)) * remaining
-          release_ceiling = abs(base) + headroom
-        if constrained and not (backoff or recovering or tracking_release):
-          status = 'release' if releasing else 'pscm_limit'
-        else:
-          bias_before = self.bias
-          increment = self.tuning.feedback_gain * yaw_error * measurement_dt
-          if recovering:
-            # Once both references show a shortfall, unwind a previous opposing
-            # correction during release. Use the current, smaller deficit and
-            # stop at zero bias; recovery cannot create demand beyond the base.
-            increment = float(np.clip(self.tuning.feedback_gain * current_yaw_error * measurement_dt,
-                                      min(0., -self.bias), max(0., -self.bias)))
-          elif tracking_release:
-            # Include the ceiling in integral admission, so no hidden bias
-            # accumulates behind an unreachable output request.
-            available = max(0., release_ceiling - math.copysign(1., base) * heading_before)
-            increment = math.copysign(1., base) * min(abs(self.tuning.feedback_gain * current_yaw_error * measurement_dt), available)
-          elif 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'
-          elif recovering and self.bias != bias_before:
-            recovery_active = True
-            status = 'release_recovery'
-          elif tracking_release:
-            release_tracking_active = self.bias != bias_before
-            status = 'release_tracking' if release_tracking_active else 'release'
-    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, 'feedback_recovery_active': recovery_active,
-                        'feedback_release_tracking_active': release_tracking_active, 'feedback_release_ceiling': release_ceiling,
-                        'feedback_curvature_delta': curvature_delta}
-    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.release_guard = ReleaseGuard(self.delay)
-    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-v8', 'command': (0., 0., 0., 0.),
-                        'release_guard_active': False, 'release_guard_reference_curvature': None,
-                        'offset_target_unguarded': 0., 'heading_target_unguarded': 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)
-    self.release_guard.update(desired_curvature, yaw_rate=yaw_rate, speed=speed, now=now, measurement_time=measurement_time,
-                              heading_horizon=heading_horizon, driver_override=driver_override, pscm_status=pscm_status)
-    unguarded_offset, unguarded_heading = target_offset, target_heading
-    target_offset = self.release_guard.limit(target_offset, self.offset_request)
-    target_heading = self.release_guard.limit(target_heading, self.heading_request)
-    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-v8',
-                        'desired_curvature': desired_curvature, 'offset_target': target_offset, 'heading_target': target_heading,
-                        'offset_target_unguarded': unguarded_offset, 'heading_target_unguarded': unguarded_heading,
-                        'release_guard_active': self.release_guard.active,
-                        'release_guard_reference_curvature': self.release_guard.reference_curvature,
-                        '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

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

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

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

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

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

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

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

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

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

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

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

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

openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py

@@ -1,319 +0,0 @@
-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-v8')
-      self.assertEqual(record['command'], list(controller.diagnostics['command']))
-      self.assertIs(record['feedback_backoff_active'], False)
-      self.assertIs(record['feedback_recovery_active'], False)
-      self.assertIs(record['feedback_release_tracking_active'], False)
-      self.assertIsNone(record['feedback_release_ceiling'])
-      self.assertIsNone(record['feedback_curvature_delta'])
-      if active and valid:
-        self.assertIs(record['release_guard_active'], False)
-        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',
-                                                    'offset_target_unguarded', 'heading_target_unguarded',
-                                                    'release_guard_reference_curvature', 'feedback_release_ceiling',
-                                                    'feedback_curvature_delta')))
-
-  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_periodic_diagnostics_log_recovery_only_on_accepted_fresh_updates(self):
-    for limit in (0, 2):
-      with self.subTest(pscm_limit=limit):
-        controller = FordVirtualAngleController()
-        for i in range(50):
-          now = 1. + i * .01
-          controller.update(circle(.02), .02, yaw_rate=.3, speed=10., now=now, measurement_time=now,
-                            model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, 0, 2, False))
-        self.assertLess(controller.diagnostics['heading_bias'], 0.)
-        first_status = 'release_recovery' if limit == 0 else 'release'
-        for now, expected_status in ((1.5, first_status), (1.51, 'no_new_measurement')):
-          controller.update(circle(.02), .01, yaw_rate=.03, speed=10., now=now, measurement_time=1.5,
-                            model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, limit, 2, False))
-          controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.01, curvature=.003,
-                                     sm=SimpleNamespace(logMonoTime={'modelV2': int(now * 1e9), 'carState': 1_500_000_000}))
-          record = self.emit_controls_event('Ford C2-free path tracking', controls)
-          self.assertEqual(record['feedback_status'], expected_status)
-          self.assertIs(record['feedback_recovery_active'], limit == 0 and now == 1.5)
-          self.assertIs(record['feedback_backoff_active'], False)
-          self.assertEqual(record['heading_bias'], controller.diagnostics['heading_bias'])
-
-  def test_periodic_diagnostics_expose_release_guard_during_feedback_history_reset(self):
-    for sign in (-1, 1):
-      with self.subTest(sign=sign):
-        controller = FordVirtualAngleController()
-        for i in range(60):
-          now = 1. + i * .01
-          controller.update(circle(sign * .02), sign * .02, yaw_rate=sign * .2, speed=10., now=now, measurement_time=now,
-                            model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, 0, 2, False))
-        for now, pressed, measurement, expected in ((1.6, True, 1.6, 'driver_override'),
-                                                  (1.61, False, 1.61, 'history'), (1.62, False, 1.61, 'no_new_measurement')):
-          controller.update(circle(sign * .03), sign * .018, yaw_rate=sign * .4, speed=10., now=now, measurement_time=measurement,
-                            model_time=now, reference_time=now, active=True, steering_pressed=pressed,
-                            pscm_status=PscmStatus(now, 2, 0, 2, False))
-          controls = SimpleNamespace(ford_path_controller=controller, curvature=sign * .04,
-                                     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)
-          self.assertIs(record['release_guard_active'], not pressed)
-          self.assertAlmostEqual(record['release_guard_reference_curvature'], sign * .02)
-          self.assertEqual(record['heading_bias'], 0.)
-          for field in ('offset_target', 'heading_target'):
-            self.assertEqual(record[field + '_unguarded'], controller.diagnostics[field + '_unguarded'])
-            if pressed:
-              self.assertEqual(record[field], record[field + '_unguarded'])
-            else:
-              self.assertLess(sign * record[field], sign * record[field + '_unguarded'])
-
-  def test_periodic_diagnostics_expose_accepted_release_tracking_and_its_ceiling(self):
-    for sign in (-1, 1):
-      with self.subTest(sign=sign):
-        controller = FordVirtualAngleController()
-        for i in range(60):
-          now = 1. + i * .01
-          controller.update(circle(sign * .04), sign * .04, yaw_rate=sign * .4, speed=10., now=now, measurement_time=now,
-                            model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, 0, 2, False))
-        for now in (1.6, 1.61):
-          controller.update(circle(sign * .02), sign * .03, yaw_rate=sign * .2, speed=10., now=now, measurement_time=1.6,
-                            model_time=now, reference_time=now, active=True, pscm_status=PscmStatus(now, 2, 0, 2, False))
-          controls = SimpleNamespace(ford_path_controller=controller, curvature=sign * .02,
-                                     sm=SimpleNamespace(logMonoTime={'modelV2': int(now * 1e9), 'carState': 1_600_000_000}))
-          record = self.emit_controls_event('Ford C2-free path tracking', controls)
-          fresh = now == 1.6
-          self.assertEqual(record['feedback_status'], 'release_tracking' if fresh else 'no_new_measurement')
-          self.assertIs(record['feedback_release_tracking_active'], fresh)
-          self.assertIs(record['feedback_recovery_active'], False)
-          self.assertIs(record['release_guard_active'], False)
-          if fresh:
-            self.assertLess(sign * record['feedback_curvature_delta'], 0.)
-            self.assertGreater(sign * record['heading_target'], sign * record['heading_base'])
-            self.assertLessEqual(sign * record['heading_target'], record['feedback_release_ceiling'])
-          else:
-            self.assertIsNone(record['feedback_release_ceiling'])
-            self.assertIsNone(record['feedback_curvature_delta'])
-
-  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()

openpilot/selfdrive/controls/tests/test_ford_curvature_c0.py

@@ -1,94 +0,0 @@
-"""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()

openpilot/selfdrive/controls/tests/test_ford_curvature_heading.py

@@ -1,54 +0,0 @@
-"""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()

openpilot/selfdrive/controls/tests/test_ford_curvature_heading_routes.py

@@ -1,67 +0,0 @@
-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()

openpilot/selfdrive/controls/tests/test_ford_heading_feedback.py

@@ -1,327 +0,0 @@
-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()

openpilot/selfdrive/controls/tests/test_ford_heading_recovery.py

@@ -1,138 +0,0 @@
-import unittest
-
-from openpilot.selfdrive.controls.lib.ford_virtual_angle import HeadingFeedback, PathTuning, PscmStatus
-
-
-def update(feedback, sign, now, *, base=.2, desired=.02, yaw=.1, previous=.2, measurement=None, limit=0, **overrides):
-  inputs = {'yaw_rate': sign * yaw, 'speed': 10., 'now': now, 'measurement_time': now if measurement is None else measurement,
-            'dt': .01, 'previous_command': sign * previous, 'heading_horizon': 10., 'driver_override': False,
-            'pscm_status': PscmStatus(now, 2, limit, 2, False)}
-  inputs.update(overrides)
-  return feedback.update(sign * base, sign * desired, **inputs)
-
-
-def acquired_correction(sign, yaw=.4, desired=.03):
-  feedback = HeadingFeedback(.2, PathTuning())
-  previous = .3
-  for i in range(60):
-    target = update(feedback, sign, i * .01, base=.3, desired=desired, yaw=yaw, previous=previous)
-    previous = sign * target
-  return feedback, previous
-
-
-class TestFordHeadingRecovery(unittest.TestCase):
-  def test_release_recovers_opposing_bias_using_current_error(self):
-    for sign in (-1, 1):
-      with self.subTest(sign=sign):
-        feedback, previous = acquired_correction(sign)
-        retained = feedback.bias * .2 / .3
-        self.assertLess(sign * retained, -.01)
-        target = update(feedback, sign, .6, previous=previous)
-        # Current request needs 0.2 rad/s, delayed request 0.3 rad/s, measured
-        # yaw is 0.1 rad/s. Use the smaller current deficit, not the old turn.
-        self.assertAlmostEqual(sign * (feedback.bias - retained), .1 * .01)
-        self.assertLessEqual(sign * feedback.bias, 0.)
-        self.assertLessEqual(sign * target, .2)
-        self.assertEqual(feedback.diagnostics['feedback_status'], 'release_recovery')
-        self.assertTrue(feedback.diagnostics['feedback_recovery_active'])
-
-  def test_recovery_stops_at_zero_bias_with_batched_measurement(self):
-    for sign in (-1, 1):
-      with self.subTest(sign=sign):
-        feedback, previous = acquired_correction(sign, yaw=.301)
-        self.assertLess(sign * feedback.bias, 0.)
-        target = update(feedback, sign, .65, previous=previous, yaw=0.)
-        self.assertAlmostEqual(feedback.bias, 0.)
-        self.assertAlmostEqual(sign * target, .2)
-        self.assertEqual(feedback.diagnostics['feedback_status'], 'release_recovery')
-        # The recovery update stops exactly at zero. A later new observation
-        # can enter the separately bounded release-tracking policy.
-        target = update(feedback, sign, .66, yaw=0.)
-        self.assertGreater(sign * feedback.bias, 0.)
-        self.assertLessEqual(sign * target, feedback.diagnostics['feedback_release_ceiling'])
-        self.assertEqual(feedback.diagnostics['feedback_status'], 'release_tracking')
-        self.assertFalse(feedback.diagnostics['feedback_recovery_active'])
-        self.assertTrue(feedback.diagnostics['feedback_release_tracking_active'])
-
-  def test_opposing_delayed_request_blocks_recovery_even_with_both_positive_errors(self):
-    for sign in (-1, 1):
-      feedback, previous = acquired_correction(sign, yaw=-.2, desired=-.03)
-      retained = feedback.bias * .2 / .3
-      self.assertLess(sign * retained, 0.)
-      update(feedback, sign, .6, previous=previous, yaw=-.5)
-      self.assertGreater(sign * feedback.diagnostics['feedback_yaw_error'], 0.)
-      self.assertAlmostEqual(feedback.bias, retained)
-      self.assertEqual(feedback.diagnostics['feedback_status'], 'release')
-      self.assertFalse(feedback.diagnostics['feedback_recovery_active'])
-
-  def test_new_base_cannot_fabricate_recovery_beyond_available_slew(self):
-    for sign in (-1, 1):
-      for partial in (False, True):
-        with self.subTest(sign=sign, partial=partial):
-          feedback, previous = acquired_correction(sign)
-          bias = feedback.bias
-          before = .4 + sign * bias
-          if partial:
-            previous = before - .0045  # Only .0005 rad of the .001 recovery is deliverable.
-          target = update(feedback, sign, .6, base=.4, previous=previous)
-          self.assertAlmostEqual(sign * (feedback.bias - bias), .0005 if partial else 0.)
-          self.assertLessEqual(sign * target, .4)
-          self.assertEqual(feedback.diagnostics['feedback_status'], 'release_recovery' if partial else 'host_limit')
-          self.assertEqual(feedback.diagnostics['feedback_recovery_active'], partial)
-
-  def test_recovery_requires_both_undertracking_errors_and_no_eps_limit(self):
-    for sign in (-1, 1):
-      for overrides in ({'yaw': .25}, {'yaw': .2}, {'limit': 2}, {'desired': 0.}, {'desired': -.02}):
-        with self.subTest(sign=sign, overrides=overrides):
-          feedback, previous = acquired_correction(sign)
-          retained = feedback.bias * .2 / .3
-          update(feedback, sign, .6, previous=previous, **overrides)
-          self.assertAlmostEqual(feedback.bias, retained)
-          self.assertNotEqual(feedback.diagnostics['feedback_status'], 'release_recovery')
-
-  def test_same_direction_bias_uses_release_tracking_instead_of_opposing_bias_recovery(self):
-    for sign in (-1, 1):
-      feedback, previous = acquired_correction(sign, yaw=.2)
-      retained = feedback.bias * .2 / .3
-      self.assertGreater(sign * retained, 0.)
-      update(feedback, sign, .6, previous=previous)
-      self.assertGreater(sign * feedback.bias, sign * retained)
-      self.assertEqual(feedback.diagnostics['feedback_status'], 'release_tracking')
-      self.assertFalse(feedback.diagnostics['feedback_recovery_active'])
-
-  def test_fresh_recovery_clears_backoff_without_reusing_measurements(self):
-    for sign in (-1, 1):
-      with self.subTest(sign=sign):
-        feedback, previous = acquired_correction(sign)
-        target = update(feedback, sign, .6, previous=previous, yaw=.4)
-        self.assertTrue(feedback.backoff_active)
-        bias = feedback.bias
-        # The new current request alone cannot recover from an old observation.
-        repeated = update(feedback, sign, .61, measurement=.6, previous=sign * target, yaw=.4)
-        self.assertEqual(feedback.bias, bias)
-        self.assertTrue(feedback.backoff_active)
-        self.assertLessEqual(sign * repeated, sign * target)
-        update(feedback, sign, .62, previous=sign * repeated)
-        self.assertGreater(sign * feedback.bias, sign * bias)
-        self.assertFalse(feedback.backoff_active)
-        self.assertEqual(feedback.diagnostics['feedback_status'], 'release_recovery')
-        bias = feedback.bias
-        update(feedback, sign, .63, measurement=.62)
-        self.assertEqual(feedback.bias, bias)
-        self.assertEqual(feedback.diagnostics['feedback_status'], 'no_new_measurement')
-        self.assertFalse(feedback.diagnostics['feedback_recovery_active'])
-
-  def test_driver_or_missing_status_clears_the_correction(self):
-    for sign in (-1, 1):
-      for overrides in ({'driver_override': True}, {'pscm_status': None},
-                        {'pscm_status': PscmStatus(.3, 2, 0, 2, False)}):
-        with self.subTest(sign=sign, overrides=overrides):
-          feedback, previous = acquired_correction(sign)
-          target = update(feedback, sign, .6, previous=previous, **overrides)
-          self.assertEqual(feedback.bias, 0.)
-          self.assertAlmostEqual(sign * target, .2)
-          self.assertNotEqual(feedback.diagnostics['feedback_status'], 'release_recovery')
-
-
-if __name__ == '__main__':
-  unittest.main()

openpilot/selfdrive/controls/tests/test_ford_heading_recovery_routes.py

@@ -1,122 +0,0 @@
-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 TestFordHeadingRecoveryRoutes(unittest.TestCase):
-  @classmethod
-  def setUpClass(cls):
-    cls.fixture = Path(__file__).parent / 'fixtures/ford_heading_recovery_requests.npz'
-    cls.metadata = json.loads(cls.fixture.with_suffix('.json').read_text())
-    cls.data = dict(np.load(cls.fixture))
-    d = cls.data
-    models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in d['models']]
-    commands, gates, rows = [], [], []
-    previous_episode = None
-    for i, now in enumerate(d['t']):
-      if d['episode'][i] != previous_episode:
-        controller = FordVirtualAngleController(response_delay=cls.metadata['response_delay'])
-        previous_episode = d['episode'][i]
-      eps = PscmStatus(float(d['pscm_timestamp'][i]), int(d['pscm_lateral_state'][i]), int(d['pscm_limit'][i]),
-                        int(d['pscm_capability'][i]), bool(d['pscm_denied'][i]), bool(d['pscm_valid'][i]))
-      prior_bias, prior_base = controller.feedback.bias, controller.feedback.previous_base
-      path = controller.update(models[d['model_index'][i]], d['desired_curvature'][i], yaw_rate=d['yaw_rate'][i], speed=d['speed'][i],
-                               now=now, measurement_time=d['measurement_time'][i], model_time=d['model_time'][i],
-                               reference_time=d['reference_time'][i], active=bool(d['active'][i]), valid=bool(d['valid'][i]),
-                               steering_pressed=bool(d['pressed'][i]), steering_torque=d['steering_torque'][i], pscm_status=eps)
-      row = dict(controller.diagnostics)
-      base = row.get('heading_base', 0.)
-      retained_bias = prior_bias if prior_base is not None and prior_base * base >= 0. else 0.
-      if prior_base and prior_base * base >= 0.:
-        retained_bias *= min(1., abs(base / prior_base))
-      row['bias_before_update'] = retained_bias
-      commands.append((path.path_offset, path.path_angle, path.curvature, path.curvature_rate))
-      gates.append(path.valid)
-      rows.append(row)
-    cls.commands, cls.gates = np.array(commands), np.array(gates)
-    cls.status = np.array([row['feedback_status'] for row in rows])
-    cls.recovering = np.array([row.get('feedback_recovery_active', False) for row in rows])
-    cls.backoff = np.array([row.get('feedback_backoff_active', False) for row in rows])
-    for key in ('heading_base', 'heading_target', 'heading_bias', 'bias_before_update', 'feedback_reference_curvature', 'feedback_yaw_error'):
-      setattr(cls, key, np.array([row.get(key, np.nan) for row in rows], dtype=float))
-
-  def test_fixture_hash_and_original_controller_provenance(self):
-    self.assertEqual(hashlib.sha256(self.fixture.read_bytes()).hexdigest(), self.metadata['fixture_sha256'])
-    self.assertEqual(self.metadata['baseline_revision'], '61dac4977bf9c36504398e8a4959dfed79cf6f05')
-    self.assertEqual(len(self.metadata['windows']), 3)
-    self.assertGreater(int(self.data['evidence'].sum()), 1000)
-
-  def test_recorded_turn_exit_releases_opposing_correction(self):
-    d = self.data
-    # Select the captured command problem from the old policy, not from the
-    # candidate result: release was freezing an opposing bias while both
-    # current and delayed requests still exceeded measured turning.
-    base, bias = d['baseline_heading_base'], d['baseline_heading_bias']
-    current_error = d['desired_curvature'] * d['speed'] - d['yaw_rate']
-    delayed = d['baseline_feedback_reference_curvature']
-    mask = (d['window_masks'][:, 0] & (d['baseline_status'] == 'release') & (bias * base < 0.) &
-            (current_error * base > 0.) & (d['baseline_feedback_yaw_error'] * base > 0.) &
-            (d['desired_curvature'] * base > 0.) & (delayed * base > 0.) & (d['pscm_limit'] < 2))
-    self.assertGreater(int(mask.sum()), 100)
-    along_turn = np.sign(d['desired_curvature'][mask])
-    increase = (self.commands[mask, 1] - d['baseline_commands'][mask, 1]) * along_turn
-    self.assertGreater(float(np.median(increase)), .005)
-    self.assertGreater(int((self.recovering & mask).sum()), 25)
-    self.assertLess(float(np.median(abs(self.heading_bias[mask]))), float(np.median(abs(bias[mask]))) - .005)
-
-  def test_recovery_only_cancels_bias_with_both_requests_undertracked(self):
-    d, mask = self.data, self.recovering
-    self.assertGreater(int(mask.sum()), 25)
-    self.assertTrue((self.status[mask] == 'release_recovery').all())
-    self.assertTrue((d['pscm_limit'][mask] < 2).all())
-    self.assertTrue((d['pscm_valid'][mask] & self.gates[mask] & ~d['pressed'][mask]).all())
-    self.assertTrue((abs(d['steering_torque'][mask]) <= 1.).all())
-    self.assertFalse(self.backoff[mask].any())
-    self.assertTrue((self.bias_before_update[mask] * self.heading_base[mask] < 0.).all())
-    self.assertTrue((self.feedback_yaw_error[mask] * self.heading_base[mask] > 0.).all())
-    current_error = d['speed'] * d['desired_curvature'] - d['yaw_rate']
-    self.assertTrue((current_error[mask] * self.heading_base[mask] > 0.).all())
-    self.assertTrue((d['desired_curvature'][mask] * self.heading_base[mask] > 0.).all())
-    self.assertTrue((self.feedback_reference_curvature[mask] * self.heading_base[mask] > 0.).all())
-    self.assertTrue((abs(self.heading_bias[mask]) < abs(self.bias_before_update[mask])).all())
-    self.assertTrue((self.heading_bias[mask] * self.bias_before_update[mask] >= -1e-12).all())
-    self.assertTrue((abs(self.heading_target[mask]) <= abs(self.heading_base[mask]) + 1e-12).all())
-
-  def test_guard_does_not_add_c0_and_preserves_heading_base_and_validity(self):
-    evidence = self.data['evidence']
-    # The release guard now intentionally prevents same-direction C0 growth.
-    # Keep the original fixture's no-extra-demand requirement and its separate
-    # good-curve retention checks, rather than insisting on old excess demand.
-    direction = np.sign(self.data['desired_curvature'][evidence])
-    self.assertTrue(((self.commands[evidence, 0] - self.data['baseline_commands'][evidence, 0]) * direction <= 1e-12).all())
-    np.testing.assert_array_equal(self.heading_base[evidence], self.data['baseline_heading_base'][evidence])
-    np.testing.assert_array_equal(self.gates[evidence], self.data['baseline_valid'][evidence])
-
-  def test_well_tracked_curves_keep_command_scale(self):
-    for index in (1, 2):
-      with self.subTest(window=self.metadata['windows'][index]['name']):
-        mask = self.data['window_masks'][:, index]
-        old, new = self.data['baseline_commands'][mask, 1], self.commands[mask, 1]
-        # This bounds collateral command change; it cannot guarantee the same
-        # future vehicle response on a drive with the candidate installed.
-        self.assertGreaterEqual(float(np.median(abs(new))), .95 * float(np.median(abs(old))))
-        self.assertLess(float(np.quantile(abs(new - old), .9)), .02)
-
-  def test_all_fixture_commands_respect_field_and_rate_limits(self):
-    d = self.data
-    np.testing.assert_array_equal(self.commands[:, 2:], 0.)
-    self.assertTrue(np.isfinite(self.commands).all())
-    self.assertTrue((abs(self.commands[:, :2]) <= [5.110000001, .500000001]).all())
-    continuous = (d['episode'][1:] == d['episode'][:-1]) & self.gates[1:] & self.gates[:-1]
-    allowed = np.diff(d['t'])[:, None] * [4., .5] + [.01, .0005] + np.array([1e-8, 1e-8])
-    self.assertTrue((abs(np.diff(self.commands[:, :2], axis=0))[continuous] <= allowed[continuous]).all())
-
-
-if __name__ == '__main__':
-  unittest.main()

openpilot/selfdrive/controls/tests/test_ford_large_maneuver_base.py

@@ -1,50 +0,0 @@
-"""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()

openpilot/selfdrive/controls/tests/test_ford_large_turn_routes.py

@@ -1,177 +0,0 @@
-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()

openpilot/selfdrive/controls/tests/test_ford_path.py

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

openpilot/selfdrive/controls/tests/test_ford_path_reference.py

@@ -1,196 +0,0 @@
-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()

openpilot/selfdrive/controls/tests/test_ford_turn_exit.py

@@ -1,46 +0,0 @@
-"""Request-level turn-exit regressions; these do not simulate EPS 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_heading_recovery import acquired_correction, update
-from openpilot.selfdrive.controls.tests.test_ford_path_reference import circle
-from openpilot.selfdrive.controls.lib.ford_virtual_angle import PscmStatus
-
-
-class TestFordTurnExit(unittest.TestCase):
-  def test_release_can_correct_a_deficit_after_opposing_bias_is_gone(self):
-    for sign in (-1, 1):
-      with self.subTest(sign=sign):
-        feedback, previous = acquired_correction(sign, yaw=.3)
-        self.assertAlmostEqual(feedback.bias, 0.)
-        target = update(feedback, sign, .6, base=.1, desired=.02, yaw=.1, previous=previous)
-        self.assertGreater(sign * target, .1)
-        first_bias = sign * feedback.bias
-        target = update(feedback, sign, .61, base=.0995, desired=.0199, yaw=.1, previous=sign * target)
-        self.assertGreater(sign * feedback.bias, first_bias)
-        self.assertLessEqual(sign * target, previous)
-
-  def test_model_growth_cannot_defeat_release_after_driver_bias_reset(self):
-    for sign in (-1, 1):
-      with self.subTest(sign=sign):
-        controller = FordVirtualAngleController()
-        for i in range(300):
-          now = i * .01
-          step(controller, now, sign * .03, circle(sign * .035), speed=10., yaw_rate=sign * .3,
-               steering_pressed=(i == 299), pscm_status=PscmStatus(now, 2, 0, 2, False))
-        prior_offset, prior_heading = controller.offset_request, controller.heading_request
-        self.assertEqual(controller.feedback.bias, 0.)
-        step(controller, 3., sign * .025, circle(sign * .06), speed=10., yaw_rate=sign * .4,
-             pscm_status=PscmStatus(3., 2, 0, 2, False))
-        self.assertLessEqual(sign * controller.offset_request, sign * prior_offset + 1e-12)
-        self.assertLessEqual(sign * controller.heading_request, sign * prior_heading + 1e-12)
-        # The same strong model pose stays available once new measured motion
-        # shows a deficit. The guard must not impose a fixed geometry cap.
-        step(controller, 3.01, sign * .024, circle(sign * .06), speed=10., yaw_rate=sign * .1,
-             pscm_status=PscmStatus(3.01, 2, 0, 2, False))
-        self.assertGreater(sign * controller.offset_request, sign * prior_offset)
-
-
-if __name__ == '__main__':
-  unittest.main()

openpilot/selfdrive/controls/tests/test_ford_turn_exit_guards.py

@@ -1,199 +0,0 @@
-"""Independent turn-exit guard properties, not a PSCM response simulation."""
-import unittest
-
-from openpilot.selfdrive.controls.lib.ford_virtual_angle import FordVirtualAngleController, HeadingFeedback, PathTuning, PscmStatus, ReleaseGuard
-
-
-AUTO_STATUS = object()
-
-
-def guard_step(guard, sign, now, *, desired=.02, yaw=.4, speed=10., measurement=None, pscm=AUTO_STATUS, **overrides):
-  inputs = {'yaw_rate': sign * yaw, 'speed': speed, 'now': now, 'measurement_time': now if measurement is None else measurement,
-            'heading_horizon': 10., 'driver_override': False,
-            'pscm_status': PscmStatus(now, 2, 0, 2, False) if pscm is AUTO_STATUS else pscm}
-  inputs.update(overrides)
-  return guard.update(sign * desired, **inputs)
-
-
-def warm_guard(guard, sign, last_override=False):
-  for i in range(40):
-    guard_step(guard, sign, i * .01, desired=.03, yaw=.3, driver_override=last_override and i == 39)
-
-
-def feedback_step(feedback, sign, now, *, base=.1, desired=.02, yaw=.1, speed=10., previous=.3, measurement=None, **overrides):
-  inputs = {'yaw_rate': sign * yaw, 'speed': speed, 'now': now, 'measurement_time': now if measurement is None else measurement,
-            'dt': .01, 'previous_command': sign * previous, 'heading_horizon': 10., 'driver_override': False,
-            'pscm_status': PscmStatus(now, 2, 0, 2, False)}
-  inputs.update(overrides)
-  return feedback.update(sign * base, sign * desired, **inputs)
-
-
-def warm_feedback(sign, speed=10., yaw=.3):
-  feedback = HeadingFeedback(.2, PathTuning())
-  previous = .3
-  for i in range(60):
-    target = feedback_step(feedback, sign, i * .01, base=.3, desired=.03, yaw=yaw, speed=speed, previous=previous)
-    previous = sign * target
-  return feedback, previous
-
-
-class TestFordReleaseGuard(unittest.TestCase):
-  def test_only_growth_in_the_requested_direction_is_capped(self):
-    for sign in (-1, 1):
-      with self.subTest(sign=sign):
-        guard = ReleaseGuard(.2)
-        warm_guard(guard, sign)
-        self.assertTrue(guard_step(guard, sign, .4))
-        self.assertAlmostEqual(guard.limit(sign * .5, sign * .2), sign * .2)
-        self.assertAlmostEqual(guard.limit(sign * .1, sign * .2), sign * .1)
-        self.assertAlmostEqual(guard.limit(-sign * .5, sign * .2), -sign * .5)
-        self.assertAlmostEqual(guard.limit(sign * .5, -sign * .2), 0.)
-
-  def test_driver_reset_does_not_erase_valid_request_history(self):
-    for sign in (-1, 1):
-      guard = ReleaseGuard(.2)
-      warm_guard(guard, sign, last_override=True)
-      self.assertFalse(guard.active)
-      self.assertTrue(guard_step(guard, sign, .4))
-      self.assertAlmostEqual(guard.reference_curvature, sign * .03)
-
-  def test_repeated_measurement_keeps_guard_but_current_override_disables_it(self):
-    for sign in (-1, 1):
-      guard = ReleaseGuard(.2)
-      warm_guard(guard, sign)
-      self.assertTrue(guard_step(guard, sign, .4))
-      self.assertTrue(guard_step(guard, sign, .41, measurement=.4))
-      self.assertFalse(guard_step(guard, sign, .42, measurement=.4, driver_override=True))
-      self.assertAlmostEqual(guard.limit(sign * .5, sign * .2), sign * .5)
-      self.assertTrue(guard_step(guard, sign, .43))
-
-  def test_status_speed_zero_and_reversal_disable_action(self):
-    cases = [
-      {'pscm': None}, {'pscm': PscmStatus(.1, 2, 0, 2, False)},
-      {'pscm': PscmStatus(.4, 2, 0, 2, False, False)}, {'pscm': PscmStatus(.4, 2, 0, 2, True)},
-      {'pscm': PscmStatus(.4, 1, 0, 2, False)}, {'pscm': PscmStatus(.4, 2, 0, 0, False)},
-      {'pscm': PscmStatus(.4, 2, 3, 2, False)}, {'pscm': PscmStatus(float('nan'), 2, 0, 2, False)},
-      {'speed': 1.99}, {'desired': 0.}, {'desired': -.02},
-    ]
-    for sign in (-1, 1):
-      for overrides in cases:
-        with self.subTest(sign=sign, overrides=overrides):
-          guard = ReleaseGuard(.2)
-          warm_guard(guard, sign)
-          self.assertFalse(guard_step(guard, sign, .4, **overrides))
-          self.assertAlmostEqual(guard.limit(sign * .5, sign * .2), sign * .5)
-
-  def test_repeated_backward_status_cannot_restore_guard_authority(self):
-    guard = ReleaseGuard(.2)
-    warm_guard(guard, 1)
-    self.assertTrue(guard_step(guard, 1, .4))
-    self.assertFalse(guard_step(guard, 1, .41, pscm=PscmStatus(.39, 2, 0, 2, False)))
-    self.assertFalse(guard_step(guard, 1, .42, pscm=PscmStatus(.39, 2, 0, 2, False)))
-    self.assertTrue(guard_step(guard, 1, .43, pscm=PscmStatus(.43, 2, 0, 2, False)))
-
-  def test_invalid_future_status_does_not_poison_later_fresh_status(self):
-    guard = ReleaseGuard(.2)
-    warm_guard(guard, 1)
-    self.assertFalse(guard_step(guard, 1, .4, pscm=PscmStatus(10., 2, 0, 2, False)))
-    self.assertTrue(guard_step(guard, 1, .41, pscm=PscmStatus(.41, 2, 0, 2, False)))
-
-  def test_fresh_unavailable_status_prevents_older_in_progress_reactivation(self):
-    for unavailable in (PscmStatus(.41, 1, 0, 2, False), PscmStatus(.41, 2, 0, 2, True), PscmStatus(.41, 2, 0, 0, False)):
-      with self.subTest(unavailable=unavailable):
-        guard = ReleaseGuard(.2)
-        warm_guard(guard, 1)
-        self.assertTrue(guard_step(guard, 1, .4))
-        self.assertFalse(guard_step(guard, 1, .41, pscm=unavailable))
-        self.assertFalse(guard_step(guard, 1, .42, pscm=PscmStatus(.4, 2, 0, 2, False)))
-        self.assertFalse(guard_step(guard, 1, .43, pscm=PscmStatus(.4, 2, 0, 2, False)))
-        self.assertAlmostEqual(guard.limit(.5, .2), .5)
-        self.assertTrue(guard_step(guard, 1, .44, pscm=PscmStatus(.44, 2, 0, 2, False)))
-
-  def test_parent_reset_clears_the_independent_history(self):
-    controller = FordVirtualAngleController()
-    warm_guard(controller.release_guard, 1)
-    self.assertTrue(guard_step(controller.release_guard, 1, .4))
-    controller.reset()
-    self.assertFalse(guard_step(controller.release_guard, 1, .41))
-    self.assertAlmostEqual(controller.release_guard.limit(.5, .2), .5)
-
-
-class TestFordReleaseTrackingGuards(unittest.TestCase):
-  def test_repeated_measurement_does_not_add_another_release_correction(self):
-    for sign in (-1, 1):
-      feedback, previous = warm_feedback(sign)
-      target = feedback_step(feedback, sign, .6, previous=previous)
-      bias = feedback.bias
-      self.assertGreater(sign * bias, 0.)
-      feedback_step(feedback, sign, .61, previous=sign * target, measurement=.6)
-      self.assertEqual(feedback.bias, bias)
-      self.assertFalse(feedback.diagnostics['feedback_release_tracking_active'])
-      feedback_step(feedback, sign, .62, previous=sign * target)
-      self.assertGreater(sign * feedback.bias, sign * bias)
-
-  def test_response_trend_uses_curvature_despite_opposite_yaw_rate_trend(self):
-    # First case: yaw rises .025->.04, but curvature falls .005->.004.
-    # Second case: yaw falls .05->.04, but curvature rises .005->.008.
-    for sign in (-1, 1):
-      for old_speed, old_yaw, new_speed, allowed in ((5., .025, 10., True), (10., .05, 5., False)):
-        with self.subTest(sign=sign, allowed=allowed):
-          feedback, previous = warm_feedback(sign, speed=old_speed, yaw=old_yaw)
-          retained = feedback.bias * (.1 / .3)
-          feedback_step(feedback, sign, .6, speed=new_speed, yaw=.04, previous=previous)
-          self.assertEqual(feedback.diagnostics['feedback_release_tracking_active'], allowed)
-          if allowed:
-            self.assertGreater(sign * feedback.bias, sign * retained)
-          else:
-            self.assertAlmostEqual(feedback.bias, retained)
-
-  def test_zero_release_headroom_does_not_reduce_base_or_store_boost(self):
-    for sign in (-1, 1):
-      feedback, previous = warm_feedback(sign)
-      target = feedback_step(feedback, sign, .6, base=.4, previous=previous)
-      self.assertAlmostEqual(feedback.bias, 0.)
-      self.assertAlmostEqual(sign * target, .4)
-      self.assertFalse(feedback.diagnostics['feedback_release_tracking_active'])
-
-  def test_release_tracking_needs_current_deficit_and_no_eps_limit(self):
-    for sign in (-1, 1):
-      for overrides in ({'yaw': .25}, {'yaw': .2}, {'pscm_status': PscmStatus(.6, 2, 2, 2, False)}):
-        with self.subTest(sign=sign, overrides=overrides):
-          feedback, previous = warm_feedback(sign)
-          target = feedback_step(feedback, sign, .6, previous=previous, **overrides)
-          self.assertAlmostEqual(feedback.bias, 0.)
-          self.assertAlmostEqual(sign * target, .1)
-          self.assertFalse(feedback.diagnostics['feedback_release_tracking_active'])
-
-  def test_release_tracking_respects_blocked_and_partial_slew_admission(self):
-    for sign in (-1, 1):
-      for partial in (False, True):
-        with self.subTest(sign=sign, partial=partial):
-          feedback, previous = warm_feedback(sign)
-          feedback_step(feedback, sign, .6, previous=previous)
-          retained = feedback.bias * (.09 / .1)
-          heading_before = .09 + sign * retained
-          previous = heading_before - (.0045 if partial else .005)
-          feedback_step(feedback, sign, .61, base=.09, desired=.0199, previous=previous)
-          self.assertAlmostEqual(sign * (feedback.bias - retained), .0005 if partial else 0.)
-          self.assertEqual(feedback.diagnostics['feedback_release_tracking_active'], partial)
-
-  def test_brief_release_pause_cannot_capture_a_larger_entry_command(self):
-    for sign in (-1, 1):
-      with self.subTest(sign=sign):
-        feedback, previous = warm_feedback(sign)
-        feedback_step(feedback, sign, .6, previous=previous)
-        # Hold the smaller request until the delayed reference catches up,
-        # but not for a full response interval after release becomes false.
-        for i in range(61, 84):
-          feedback_step(feedback, sign, i * .01, desired=.02, yaw=.2)
-        feedback_step(feedback, sign, .84, desired=.019, previous=.45)
-        self.assertAlmostEqual(feedback.diagnostics['feedback_release_ceiling'], .1 + (.3 - .1) * (.019 / .03))
-        # A full quiet response interval starts a new independent episode.
-        for i in range(85, 129):
-          feedback_step(feedback, sign, i * .01, desired=.019, yaw=.19)
-        feedback_step(feedback, sign, 1.29, desired=.018, previous=.45)
-        self.assertAlmostEqual(feedback.diagnostics['feedback_release_ceiling'], .1 + (.45 - .1) * (.018 / .019))
-
-
-if __name__ == '__main__':
-  unittest.main()

openpilot/selfdrive/controls/tests/test_ford_turn_exit_routes.py

@@ -1,181 +0,0 @@
-"""Recorded-input regression checks; changed commands do not predict motion."""
-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 TestFordTurnExitRoutes(unittest.TestCase):
-  @classmethod
-  def setUpClass(cls):
-    cls.fixture = Path(__file__).parent / 'fixtures/ford_turn_exit_requests.npz'
-    cls.metadata = json.loads(cls.fixture.with_suffix('.json').read_text())
-    cls.data = dict(np.load(cls.fixture, allow_pickle=False))
-    d = cls.data
-    models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in d['models']]
-    commands, gates, rows, previous_commands, prior_biases = [], [], [], [], []
-    episode = None
-    for i, now in enumerate(d['t']):
-      if d['episode'][i] != episode:
-        controller = FordVirtualAngleController(response_delay=cls.metadata['response_delay'])
-        episode = d['episode'][i]
-      previous_commands.append([controller.offset_request, controller.heading_request])
-      old_base, old_bias = controller.feedback.previous_base, controller.feedback.bias
-      eps = PscmStatus(float(d['pscm_timestamp'][i]), int(d['pscm_lateral_state'][i]), int(d['pscm_limit'][i]),
-                        int(d['pscm_capability'][i]), bool(d['pscm_denied'][i]), bool(d['pscm_valid'][i]))
-      path = controller.update(models[d['model_index'][i]], d['desired_curvature'][i], yaw_rate=d['yaw_rate'][i], speed=d['speed'][i],
-                               now=now, measurement_time=d['measurement_time'][i], model_time=d['model_time'][i],
-                               reference_time=d['reference_time'][i], active=bool(d['active'][i]), valid=bool(d['valid'][i]),
-                               steering_pressed=bool(d['pressed'][i]), steering_torque=d['steering_torque'][i], pscm_status=eps)
-      row = dict(controller.diagnostics)
-      base = row.get('heading_base', 0.)
-      retained = old_bias if old_base is not None and old_base * base >= 0. else 0.
-      if old_base and old_base * base >= 0.:
-        retained *= min(1., abs(base / old_base))
-      prior_biases.append(retained)
-      commands.append([path.path_offset, path.path_angle, path.curvature, path.curvature_rate])
-      gates.append(path.valid)
-      rows.append(row)
-    cls.commands, cls.gates = np.array(commands), np.array(gates)
-    cls.previous_commands, cls.prior_biases = np.array(previous_commands), np.array(prior_biases)
-    cls.status = np.array([row['feedback_status'] for row in rows])
-    for key in ('heading_base', 'heading_bias', 'offset_target', 'heading_target', 'offset_target_unguarded', 'heading_target_unguarded',
-                'feedback_yaw_error', 'feedback_reference_curvature', 'release_guard_reference_curvature',
-                'feedback_release_ceiling', 'feedback_curvature_delta', 'heading_horizon'):
-      setattr(cls, key, np.array([row.get(key, np.nan) for row in rows], dtype=float))
-    cls.guarded = np.array([row.get('release_guard_active', False) for row in rows])
-    cls.tracking = np.array([row.get('feedback_release_tracking_active', False) for row in rows])
-
-  def window(self, name):
-    index = next(i for i, window in enumerate(self.metadata['windows']) if window['name'] == name)
-    return self.data['window_masks'][:, index]
-
-  def test_fixture_provenance_and_minimal_signal_schema(self):
-    self.assertEqual(hashlib.sha256(self.fixture.read_bytes()).hexdigest(), self.metadata['fixture_sha256'])
-    self.assertEqual(self.metadata['baseline_revision'], 'dfcfddb91ce2409511f5b2dbce25d06d5056b3d6')
-    self.assertEqual(self.metadata['baseline_hypothesis'], 'model-pose-c0-c1-feedback-v7')
-    self.assertEqual(set(self.data), set(self.metadata['retained_fields']))
-    self.assertGreater(self.metadata['samples'], 15000)
-    self.assertEqual(len(self.data['t']), self.metadata['samples'])
-    self.assertGreater(int(self.data['evidence'].sum()), 4000)
-    self.assertTrue(np.isfinite(self.data['models']).all())
-    self.assertEqual(self.data['t'][0], 0.)
-    for key in ('wheel_deg', 'wheel_rate', 'eps_torque', 'recorded_wheel_curvature', 'origin_ns', 'publication_time'):
-      self.assertNotIn(key, self.data)
-    for key in ('commands', 'valid', 'heading_bias'):
-      self.assertTrue(self.metadata['compact_full_baseline_evidence_parity'][key]['exact'])
-    for digest in self.metadata['baseline_source_hashes'].values():
-      self.assertRegex(digest, r'^[0-9a-f]{64}$')
-
-  def test_recorded_model_growth_is_guarded_while_feedback_rebuilds_history(self):
-    d = self.data
-    # Select the problem using pinned baseline status and recorded inputs,
-    # not candidate success. Nearby driver input is deliberately retained.
-    selected = self.window('first_reversal') | self.window('second_reversal') | self.window('over_growth')
-    index = np.searchsorted(d['t'], d['measurement_time'] - self.metadata['response_delay'], side='right') - 1
-    safe_index = np.maximum(index, 0)
-    delayed = d['desired_curvature'][safe_index]
-    direction = np.sign(d['desired_curvature'])
-    horizon = np.maximum(7., d['speed'])
-    mask = (selected & (d['baseline_status'] == 'history') & d['baseline_valid'] & (index >= 0) &
-            (d['episode'][safe_index] == d['episode']) & ~d['pressed'] & (abs(d['steering_torque']) <= 1.) &
-            (delayed * d['desired_curvature'] > 0.) & ((abs(delayed) - abs(d['desired_curvature'])) * horizon > .0005) &
-            ((d['yaw_rate'] - d['speed'] * delayed) * direction > 0.) &
-            ((d['yaw_rate'] - d['speed'] * d['desired_curvature']) * direction > 0.))
-    self.assertGreater(int(mask.sum()), 25)
-    self.assertGreater(int((mask & self.window('over_growth')).sum()), 10)
-    self.assertTrue(self.guarded[mask].all())
-    self.assertTrue((self.status[mask] == 'history').all())
-    np.testing.assert_array_equal(self.heading_bias[mask], 0.)
-    targets = np.column_stack((self.offset_target, self.heading_target))
-    unguarded = np.column_stack((self.offset_target_unguarded, self.heading_target_unguarded))
-    for sign in (-1, 1):
-      case = mask & (direction == sign)
-      self.assertGreater(int(case.sum()), 0)
-      growth_removed = (unguarded[case] - targets[case]) * sign
-      self.assertTrue((growth_removed >= -1e-12).all())
-      self.assertTrue((growth_removed.max(axis=0) > [.01, .0005]).all())
-
-  def test_every_release_guard_ceiling_preserves_opposing_path_terms(self):
-    mask = self.guarded
-    d = self.data
-    self.assertGreater(int(mask.sum()), 100)
-    direction = np.sign(d['desired_curvature'][mask])[:, None]
-    targets = np.column_stack((self.offset_target, self.heading_target))[mask]
-    unguarded = np.column_stack((self.offset_target_unguarded, self.heading_target_unguarded))[mask]
-    previous = self.previous_commands[mask]
-    same_direction = unguarded * direction > 0.
-    self.assertTrue((targets * direction <= np.maximum(previous * direction, 0.) + 1e-12)[same_direction].all())
-    np.testing.assert_array_equal(targets[~same_direction], unguarded[~same_direction])
-    self.assertTrue((~d['pressed'][mask] & (abs(d['steering_torque'][mask]) <= 1.) & (d['pscm_limit'][mask] < 3)).all())
-
-  def test_recorded_zero_bias_release_can_track_with_bounded_new_correction(self):
-    d = self.data
-    base = d['baseline_heading_base']
-    current_error = d['speed'] * d['desired_curvature'] - d['yaw_rate']
-    mask = (self.window('zero_bias_release') & d['clean_rawtorque'] & (d['demand'] >= .5) &
-            (d['baseline_status'] == 'release') & (abs(d['baseline_heading_bias']) <= 1e-9) & (d['pscm_limit'] < 2) &
-            (current_error * base > 0.) & (d['baseline_feedback_yaw_error'] * base > 0.) &
-            (d['desired_curvature'] * base > 0.) & (d['baseline_feedback_reference_curvature'] * base > 0.))
-    self.assertGreater(int(mask.sum()), 100)
-    direction = np.sign(d['desired_curvature'][mask])
-    increase = (self.commands[mask, 1] - d['baseline_commands'][mask, 1]) * direction
-    self.assertGreater(float(np.median(increase)), .005)
-    self.assertGreater(int((mask & self.tracking).sum()), 25)
-    np.testing.assert_array_equal(self.commands[mask, 0], d['baseline_commands'][mask, 0])
-
-  def test_release_tracking_admits_only_eligible_reachable_headroom(self):
-    d, mask = self.data, self.tracking
-    self.assertGreater(int(mask.sum()), 25)
-    base, bias = self.heading_base[mask], self.heading_bias[mask]
-    sign = np.sign(base)
-    self.assertTrue((self.status[mask] == 'release_tracking').all())
-    self.assertTrue((d['pscm_limit'][mask] < 2).all())
-    self.assertTrue((d['pscm_valid'][mask] & self.gates[mask] & ~d['pressed'][mask]).all())
-    self.assertTrue((abs(d['steering_torque'][mask]) <= 1.).all())
-    self.assertTrue((self.prior_biases[mask] * base >= 0.).all())
-    self.assertTrue((self.feedback_yaw_error[mask] * base > 0.).all())
-    current_error = d['speed'][mask] * d['desired_curvature'][mask] - d['yaw_rate'][mask]
-    self.assertTrue((current_error * base > 0.).all())
-    self.assertTrue((d['desired_curvature'][mask] * base > 0.).all())
-    self.assertTrue((self.feedback_reference_curvature[mask] * base > 0.).all())
-    self.assertTrue((sign * self.feedback_curvature_delta[mask] * self.heading_horizon[mask] <= .0005 + 1e-12).all())
-    self.assertTrue(((bias - self.prior_biases[mask]) * sign > 0.).all())
-    self.assertTrue(((base + bias) * sign <= self.feedback_release_ceiling[mask] + 1e-12).all())
-
-  def test_raw_model_bases_and_validity_remain_unchanged_on_evidence(self):
-    d, mask = self.data, self.data['evidence']
-    np.testing.assert_array_equal(self.gates[mask], d['baseline_valid'][mask])
-    valid = mask & self.gates
-    np.testing.assert_array_equal(self.heading_base[valid], d['baseline_heading_base'][valid])
-    np.testing.assert_array_equal(self.offset_target_unguarded[valid], d['baseline_offset_target'][valid])
-
-  def test_preselected_good_curves_retain_command_scale(self):
-    d = self.data
-    for name in ('good_curve_a', 'good_curve_b'):
-      with self.subTest(window=name):
-        mask = self.window(name) & d['clean_rawtorque'] & (d['demand'] >= .5)
-        self.assertGreater(int(mask.sum()), 200)
-        old, new = d['baseline_commands'][mask, :2], self.commands[mask, :2]
-        # These are collateral command bounds, not a new-motion prediction.
-        self.assertTrue((np.median(abs(new), axis=0) >= .95 * np.median(abs(old), axis=0)).all())
-        self.assertTrue((np.median(abs(new), axis=0) <= 1.05 * np.median(abs(old), axis=0)).all())
-        self.assertTrue((np.quantile(abs(new - old), .9, axis=0) <= [.02, .005]).all())
-
-  def test_all_commands_keep_zero_c2_c3_and_existing_field_and_rate_limits(self):
-    d = self.data
-    np.testing.assert_array_equal(self.commands[:, 2:], 0.)
-    self.assertTrue(np.isfinite(self.commands).all())
-    self.assertTrue((abs(self.commands[:, :2]) <= [5.110000001, .500000001]).all())
-    continuous = (d['episode'][1:] == d['episode'][:-1]) & self.gates[1:] & self.gates[:-1]
-    allowed = np.diff(d['t'])[:, None] * [4., .5] + [.01, .0005] + np.array([1e-8, 1e-8])
-    self.assertTrue((abs(np.diff(self.commands[:, :2], axis=0))[continuous] <= allowed[continuous]).all())
-
-
-if __name__ == '__main__':
-  unittest.main()

openpilot/selfdrive/controls/tests/test_ford_virtual_angle.py

@@ -1,61 +0,0 @@
-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()

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -32,14 +32,7 @@ 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
@@ -95,8 +88,7 @@ class SelfdriveD(CruiseHelper):
     self.big_model_ready_t = 0.
 
     # Setup sockets
-    self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] +
-                                  ['selfdriveStateSP', 'onroadEventsSP', 'assistedDrivingMilestoneState'])
+    self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] + ['selfdriveStateSP', 'onroadEventsSP'])
 
     self.gps_location_service = get_gps_location_service(self.params)
     self.gps_packets = [self.gps_location_service]
@@ -135,7 +127,6 @@ 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()
 
@@ -146,11 +137,6 @@ 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
@@ -542,8 +528,6 @@ 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)
 
@@ -662,31 +646,6 @@ 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)
@@ -696,28 +655,6 @@ 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)
 
@@ -730,7 +667,6 @@ 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)
@@ -744,7 +680,6 @@ class SelfdriveD(CruiseHelper):
         self.step()
         self.rk.monitor_time()
     finally:
-      self.assisted_driving_milestones.close()
       e.set()
       t.join()
 

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

@@ -1,8 +1,5 @@
-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
@@ -64,8 +61,7 @@ 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))
-    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:
+    if not self._onboarding_window.completed:
       gui_app.push_widget(self._onboarding_window)
 
     # initialize correct onroad layout
@@ -123,8 +119,6 @@ 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:

openpilot/selfdrive/ui/mici/layouts/settings/device.py

@@ -62,7 +62,25 @@ class MiciFccModal(NavRawScrollPanel):
     rl.draw_texture_ex(self._fcc_logo, fcc_pos, 0.0, 1.0, rl.WHITE)
 
 
-def _engaged_confirmation_click(callback: Callable, action_text: str, icon: rl.Texture, exit_on_confirm: bool = True, red: bool = False):
+class DisengageDialog(BigDialog):
+  def __init__(self, title: str, description: str, disengaged_callback: Callable[[], None]):
+    super().__init__(title, description)
+    self._disengaged_callback = disengaged_callback
+
+  def _update_state(self):
+    super()._update_state()
+    if not ui_state.engaged and not self.is_dismissing:
+      self.dismiss(self._disengaged_callback)
+
+
+class EngagedConfirmationDialog(BigConfirmationDialog):
+  def _update_state(self):
+    super()._update_state()
+    if ui_state.engaged and not self.is_dismissing:
+      self.dismiss()
+
+
+def engaged_confirmation_click(callback: Callable, action_text: str, icon: rl.Texture, exit_on_confirm: bool = True, red: bool = False):
   if not ui_state.engaged:
     def confirm_callback():
       # Check engaged again in case it changed while the dialog was open
@@ -70,23 +88,26 @@ def _engaged_confirmation_click(callback: Callable, action_text: str, icon: rl.T
       if not ui_state.engaged:
         callback()
 
-    gui_app.push_widget(BigConfirmationDialog(f"slide to\n{action_text.lower()}", icon, confirm_callback, exit_on_confirm=exit_on_confirm, red=red))
+    gui_app.push_widget(EngagedConfirmationDialog(f"slide to\n{action_text.lower()}", icon, confirm_callback,
+                                                  exit_on_confirm=exit_on_confirm, red=red))
   else:
-    gui_app.push_widget(BigDialog("", f"Disengage to {action_text}"))
+    gui_app.push_widget(DisengageDialog("", f"Disengage to {action_text}",
+                                           lambda: engaged_confirmation_click(callback, action_text, icon,
+                                                                               exit_on_confirm=exit_on_confirm, red=red)))
 
 
 class EngagedConfirmationCircleButton(BigCircleButton):
   def __init__(self, title: str, icon: rl.Texture, callback: Callable[[], None], exit_on_confirm: bool = True,
                red: bool = False, icon_offset: tuple[int, int] = (0, 0)):
     super().__init__(icon, red, icon_offset)
-    self.set_click_callback(lambda: _engaged_confirmation_click(callback, title, icon, exit_on_confirm=exit_on_confirm, red=red))
+    self.set_click_callback(lambda: engaged_confirmation_click(callback, title, icon, exit_on_confirm=exit_on_confirm, red=red))
 
 
 class EngagedConfirmationButton(BigButton):
   def __init__(self, text: str, action_text: str, icon: rl.Texture, callback: Callable[[], None],
                exit_on_confirm: bool = True, red: bool = False):
     super().__init__(text, "", icon)
-    self.set_click_callback(lambda: _engaged_confirmation_click(callback, action_text, icon, exit_on_confirm=exit_on_confirm, red=red))
+    self.set_click_callback(lambda: engaged_confirmation_click(callback, action_text, icon, exit_on_confirm=exit_on_confirm, red=red))
 
 
 class DeviceInfoLayoutMici(Widget):

openpilot/selfdrive/ui/mici/layouts/settings/toggles.py

@@ -47,7 +47,6 @@ 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)
@@ -58,7 +57,6 @@ class TogglesLayoutMici(NavScroller):
       is_metric_toggle,
       ldw_toggle,
       always_on_dm_toggle,
-      milestone_celebrations_toggle,
       record_front,
       record_mic,
       enable_openpilot,
@@ -70,7 +68,6 @@ 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),

openpilot/selfdrive/ui/mici/onroad/alert_renderer.py

@@ -20,7 +20,6 @@ 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
@@ -280,7 +279,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 * ALERT_BACKGROUND_OPACITY * self._alpha_filter.x))
+    color = rl.Color(color.r, color.g, color.b, int(255 * 0.90 * 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

openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py

@@ -19,15 +19,10 @@ 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
@@ -161,7 +156,6 @@ 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,
@@ -229,12 +223,6 @@ 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))
@@ -259,6 +247,7 @@ 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
@@ -366,12 +355,10 @@ 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)
 

openpilot/selfdrive/ui/soundd.py

@@ -24,8 +24,14 @@ 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
 
@@ -47,7 +53,6 @@ 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),
@@ -55,14 +60,6 @@ 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']
 
@@ -77,7 +74,6 @@ class Soundd(QuietMode):
   def __init__(self):
     super().__init__()
 
-    self.device_type = HARDWARE.get_device_type()
     self.load_sounds()
 
     self.current_alert = AudibleAlert.none
@@ -89,7 +85,6 @@ 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)
 
@@ -169,19 +164,9 @@ 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):
-    return calculate_volume_for_device(weighted_db, self.device_type)
+    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))
 
   @retry(attempts=10, delay=3)
   def get_stream(self, sd):
@@ -195,7 +180,7 @@ class Soundd(QuietMode):
     import sounddevice as sd
     micd.patch_sounddevice(sd)
 
-    sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure', 'assistedDrivingMilestoneState'])
+    sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure'])
 
     with self.get_stream(sd) as stream:
       rk = Ratekeeper(20)
@@ -213,7 +198,6 @@ 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:

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

@@ -5,79 +5,19 @@ 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, TextAlignment
-from openpilot.system.ui.lib.multilang import tr
-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
+from openpilot.system.ui.lib.application import FontWeight
+from openpilot.system.ui.widgets.label import UnifiedLabel
 
 
 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)
-    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=TextAlignment.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=TextAlignment.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=TextAlignment.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=TextAlignment.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=TextAlignment.CENTER)
 
   def _set_chestnut_visibility(self):
     usb_connected = ui_state.usb_connected

openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py

@@ -6,9 +6,9 @@ See the LICENSE.md file in the root directory for more details.
 """
 from openpilot.selfdrive.ui.mici.layouts.settings import settings as OP
 from openpilot.selfdrive.ui.mici.layouts.settings.settings import SettingsBigButton
-from openpilot.selfdrive.ui.mici.layouts.settings.device import DeviceLayoutMici
+from openpilot.selfdrive.ui.mici.layouts.settings.device import DeviceLayoutMici, engaged_confirmation_click
 from openpilot.selfdrive.ui.mici.widgets.button import BigCircleButton
-from openpilot.selfdrive.ui.mici.widgets.dialog import BigConfirmationDialog, BigDialog
+from openpilot.selfdrive.ui.mici.widgets.dialog import BigConfirmationDialog
 from openpilot.selfdrive.ui.sunnypilot.mici.layouts.sunnylink import SunnylinkLayoutMici
 from openpilot.selfdrive.ui.sunnypilot.mici.layouts.models import ModelsLayoutMici
 from openpilot.selfdrive.ui.ui_state import ui_state
@@ -93,10 +93,6 @@ class SettingsLayoutSP(OP.SettingsLayout):
       dlg = BigConfirmationDialog(tr("slide to exit always offroad"), self.icon_offroad_slider, red=False,
                                   confirm_callback=lambda: _set_offroad_status(False))
     else:
-      if ui_state.engaged:
-        gui_app.push_widget(BigDialog(tr("disengage to enable always offroad"), "", ))
-        return
-
-      dlg = BigConfirmationDialog(tr("slide to force offroad"), self.icon_offroad_slider, red=True,
-                                  confirm_callback=lambda: _set_offroad_status(True))
+      engaged_confirmation_click(lambda: _set_offroad_status(True), "force offroad", self.icon_offroad_slider, red=True)
+      return
     gui_app.push_widget(dlg)

openpilot/selfdrive/ui/sunnypilot/onroad/milestone_celebration.py

@@ -1,228 +0,0 @@
-"""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)

openpilot/selfdrive/ui/sunnypilot/ui_state.py

@@ -35,8 +35,7 @@ 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",
-      "assistedDrivingMilestoneState",
+      "gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay"
     ]
 
     self.sunnylink_state = SunnylinkState()

openpilot/selfdrive/ui/tests/generate_milestone_sound.py

@@ -1,45 +0,0 @@
-#!/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()

openpilot/selfdrive/ui/tests/milestone_preview.py

@@ -1,38 +0,0 @@
-#!/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()

openpilot/selfdrive/ui/tests/run_milestone_celebration.sh

@@ -1,27 +0,0 @@
-#!/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"

openpilot/selfdrive/ui/tests/test_soundd.py

@@ -4,63 +4,12 @@ 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, Soundd, calculate_volume_for_device, check_selfdrive_timeout_alert
+from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, 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'])

openpilot/sunnypilot/modeld_v2/__init__.py

@@ -0,0 +1,5 @@
+from pathlib import Path
+
+MODEL_PATH = Path(__file__).parent / 'models/supercombo.onnx'
+MODEL_PKL_PATH = Path(__file__).parent / 'models/supercombo_tinygrad.pkl'
+METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'

openpilot/sunnypilot/modeld_v2/compile_modeld.py

@@ -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,7 +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=None):
+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}
 
 
@@ -151,9 +152,24 @@ def make_warp_queues(device=Device.DEFAULT):
   return queues, npy
 
 
+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(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)
+    return Tensor.cat(warped_frame, warped_big_frame)
+  return warp
+
+
 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)
@@ -170,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():
@@ -186,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])
 
@@ -203,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()
@@ -245,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]:
@@ -303,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)')
@@ -322,64 +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)
-      make_random_warp_inputs = partial(make_random_images, keys=['frame', 'big_frame'], shape=frame_copy_size, device=Device.DEFAULT)
-      warp = TinyJit(stock.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)

openpilot/sunnypilot/modeld_v2/constants.py

@@ -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
@@ -37,7 +35,6 @@ class ModelConstants:
   LANE_LINES_WIDTH = 2
   ROAD_EDGES_WIDTH = 2
   PLAN_WIDTH = 15
-  ACTION_WIDTH = 2
   DESIRE_PRED_WIDTH = 8
   LAT_PLANNER_SOLUTION_WIDTH = 4
   DESIRED_CURV_WIDTH = 1

openpilot/sunnypilot/modeld_v2/meta_helper.py

@@ -1,9 +1,26 @@
 from openpilot.sunnypilot.modeld_v2.constants import Meta
+from openpilot.cereal import custom
 from openpilot.sunnypilot.modeld_v2.meta_20hz import Meta20hz
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 
+ModelBundle = custom.ModelManagerSP.ModelBundle
+
 
 def load_meta_constants():
+  """
+  Determines and loads the appropriate meta model class based on the metadata provided. The function checks
+  specific keys and conditions within the provided metadata dictionary to identify the corresponding meta
+  model class to return.
+
+  :param model_metadata: Dictionary containing metadata about the model. It includes
+      details such as input shapes, output slices, and other configurations for identifying
+      metadata-dependent meta model classes.
+  :type model_metadata: dict
+  :return: The appropriate meta model class (Meta, MetaSimPose, or MetaTombRaider)
+      based on the conditions and metadata provided.
+  :rtype: type
+  """
   if (bundle := get_active_bundle()) and bundle.is20hz:
     return Meta20hz
-  return Meta
+
+  return Meta  # Default

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -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
-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,40 +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.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]
@@ -167,39 +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()
 
-    self.parser = Parser()
-    self.prev_desire = np.zeros(self.constants.DESIRE_LEN, dtype=np.float32)
+    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()
 
-    if self.warp is not None:
-      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])
+    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)
+
+    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 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
-      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:
@@ -214,50 +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:
-      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.run_model is not None:
-      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:
-      assert self.warp is not None and self.run_policy is not None
-      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()
@@ -287,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,
@@ -372,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:
@@ -490,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}
@@ -512,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
@@ -562,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

openpilot/sunnypilot/modeld_v2/parse_model_outputs.py

@@ -115,41 +115,22 @@ class Parser:
     outs[name + '_stds'] = pred_std_final.reshape(final_shape)
 
   def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
-    if 'plan' in outs:
-      self.parse_mdn('plan', outs, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
-    if 'planplus' in outs:
-      self.parse_mdn('planplus', outs, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
-    if 'lane_lines' in outs:
-      self.parse_mdn('lane_lines', outs, out_shape=(ModelConstants.NUM_LANE_LINES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
-    if 'road_edges' in outs:
-      self.parse_mdn('road_edges', outs, out_shape=(ModelConstants.NUM_ROAD_EDGES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
-    if 'pose' in outs:
-      self.parse_mdn('pose', outs, out_shape=(ModelConstants.POSE_WIDTH,))
-    if 'road_transform' in outs:
-      self.parse_mdn('road_transform', outs, out_shape=(ModelConstants.POSE_WIDTH,))
+    # supercombo (4955 / 102) and newer variants (e.g. 990 / 144).
+    self.parse_mdn('plan',   outs, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
+    self.parse_mdn('lane_lines', outs, out_shape=(ModelConstants.NUM_LANE_LINES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
+    self.parse_mdn('road_edges', outs, out_shape=(ModelConstants.NUM_ROAD_EDGES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
+    self.parse_mdn('pose',         outs, out_shape=(ModelConstants.POSE_WIDTH,))
+    self.parse_mdn('road_transform', outs, out_shape=(ModelConstants.POSE_WIDTH,))
     if 'sim_pose' in outs:
       self.parse_mdn('sim_pose', outs, out_shape=(ModelConstants.POSE_WIDTH,))
-    if 'wide_from_device_euler' in outs:
-      self.parse_mdn('wide_from_device_euler', outs, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
-    if 'lead' in outs:
-      self.parse_mdn('lead', outs, out_shape=(ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH))
+    self.parse_mdn('wide_from_device_euler', outs, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
+    self.parse_mdn('lead', outs, out_shape=(ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH))
     if 'lat_planner_solution' in outs:
       self.parse_mdn('lat_planner_solution', outs, out_shape=(ModelConstants.IDX_N, ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
     if 'desired_curvature' in outs:
       self.parse_mdn('desired_curvature', outs, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
-    if 'action' in outs:
-      self.parse_mdn('action', outs, out_shape=(ModelConstants.ACTION_WIDTH,))
     for k in ['lead_prob', 'lane_lines_prob', 'meta']:
-      if k in outs:
-        self.parse_binary_crossentropy(k, outs)
-    if 'desire_state' in outs:
-      self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
-    if 'desire_pred' in outs:
-      self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN, ModelConstants.DESIRE_PRED_WIDTH))
+      self.parse_binary_crossentropy(k, outs)
+    self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
+    self.parse_categorical_crossentropy('desire_pred',  outs, out_shape=(ModelConstants.DESIRE_PRED_LEN, ModelConstants.DESIRE_PRED_WIDTH))
     return outs
-
-  def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
-    return self.parse_outputs(outs)
-
-  def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
-    return self.parse_outputs(outs)

openpilot/sunnypilot/modeld_v2/parse_model_outputs_split.py

@@ -0,0 +1,159 @@
+import numpy as np
+from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants
+
+
+def safe_exp(x, out=None):
+  # -11 is around 10**14, more causes float16 overflow
+  return np.exp(np.clip(x, -np.inf, 11), out=out)
+
+
+def sigmoid(x):
+  return 1. / (1. + safe_exp(-x))
+
+
+def softmax(x, axis=-1):
+  x -= np.max(x, axis=axis, keepdims=True)
+  if x.dtype == np.float32 or x.dtype == np.float64:
+    safe_exp(x, out=x)
+  else:
+    x = safe_exp(x)
+  x /= np.sum(x, axis=axis, keepdims=True)
+  return x
+
+
+class Parser:
+  def __init__(self, ignore_missing=False):
+    self.ignore_missing = ignore_missing
+
+  def check_missing(self, outs, name):
+    if name not in outs and not self.ignore_missing:
+      raise ValueError(f"Missing output {name}")
+    return name not in outs
+
+  def parse_categorical_crossentropy(self, name, outs, out_shape=None):
+    if self.check_missing(outs, name):
+      return
+    raw = outs[name]
+    if out_shape is not None:
+      raw = raw.reshape((raw.shape[0],) + out_shape)
+    outs[name] = softmax(raw, axis=-1)
+
+  def parse_binary_crossentropy(self, name, outs):
+    if self.check_missing(outs, name):
+      return
+    raw = outs[name]
+    outs[name] = sigmoid(raw)
+
+  def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None):
+    if self.check_missing(outs, name):
+      return
+    raw = outs[name]
+    raw = raw.reshape((raw.shape[0], max(in_N, 1), -1))
+
+    n_values = (raw.shape[2] - out_N)//2
+    pred_mu = raw[:,:,:n_values]
+    pred_std = safe_exp(raw[:,:,n_values: 2*n_values])
+
+    if in_N > 1:
+      weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
+      for i in range(out_N):
+        weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1)
+
+      if out_N == 1:
+        for fidx in range(weights.shape[0]):
+          idxs = np.argsort(weights[fidx][:,0])[::-1]
+          weights[fidx] = weights[fidx][idxs]
+          pred_mu[fidx] = pred_mu[fidx][idxs]
+          pred_std[fidx] = pred_std[fidx][idxs]
+      assert out_shape is not None
+      full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
+      outs[name + '_weights'] = weights
+      outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
+      outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape)
+
+      pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
+      pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
+      for fidx in range(weights.shape[0]):
+        for hidx in range(out_N):
+          idxs = np.argsort(weights[fidx,:,hidx])[::-1]
+          pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]]
+          pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]]
+    else:
+      pred_mu_final = pred_mu
+      pred_std_final = pred_std
+
+    if out_N > 1:
+      assert out_shape is not None
+      final_shape = tuple([raw.shape[0], out_N] + list(out_shape))
+    else:
+      assert out_shape is not None
+      final_shape = tuple([raw.shape[0],] + list(out_shape))
+    outs[name] = pred_mu_final.reshape(final_shape)
+    outs[name + '_stds'] = pred_std_final.reshape(final_shape)
+
+  def is_mhp(self, outs, name, shape):
+    if self.check_missing(outs, name):
+      return False
+    if outs[name].shape[1] == 2 * shape:
+      return False
+    return True
+
+  def parse_dynamic_outputs(self, outs: dict[str, np.ndarray]) -> None:
+    if 'lead' in outs:
+      lead_mhp = self.is_mhp(outs, 'lead',
+                             SplitModelConstants.LEAD_MHP_SELECTION * SplitModelConstants.LEAD_TRAJ_LEN * SplitModelConstants.LEAD_WIDTH)
+      lead_in_N, lead_out_N = (SplitModelConstants.LEAD_MHP_N, SplitModelConstants.LEAD_MHP_SELECTION) if lead_mhp else (0, 0)
+      lead_out_shape = (SplitModelConstants.LEAD_TRAJ_LEN, SplitModelConstants.LEAD_WIDTH) if lead_mhp else \
+        (SplitModelConstants.LEAD_MHP_SELECTION, SplitModelConstants.LEAD_TRAJ_LEN, SplitModelConstants.LEAD_WIDTH)
+      self.parse_mdn('lead', outs, in_N=lead_in_N, out_N=lead_out_N, out_shape=lead_out_shape)
+    if 'plan' in outs:
+      plan_mhp = self.is_mhp(outs, 'plan', SplitModelConstants.IDX_N * SplitModelConstants.PLAN_WIDTH)
+      plan_in_N, plan_out_N = (SplitModelConstants.PLAN_MHP_N, SplitModelConstants.PLAN_MHP_SELECTION) if plan_mhp else (0, 0)
+      self.parse_mdn('plan', outs, in_N=plan_in_N, out_N=plan_out_N,
+                     out_shape=(SplitModelConstants.IDX_N, SplitModelConstants.PLAN_WIDTH))
+    if 'planplus' in outs:
+      self.parse_mdn('planplus', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.IDX_N, SplitModelConstants.PLAN_WIDTH))
+
+  def split_outputs(self, outs: dict[str, np.ndarray]) -> None:
+    if 'desired_curvature' in outs:
+      self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.DESIRED_CURV_WIDTH,))
+    if 'desire_pred' in outs:
+      self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(SplitModelConstants.DESIRE_PRED_LEN,SplitModelConstants.DESIRE_PRED_WIDTH))
+    if 'desire_state' in outs:
+      self.parse_categorical_crossentropy('desire_state', outs, out_shape=(SplitModelConstants.DESIRE_PRED_WIDTH,))
+    if 'lane_lines' in outs:
+      self.parse_mdn('lane_lines', outs, in_N=0, out_N=0,
+                     out_shape=(SplitModelConstants.NUM_LANE_LINES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
+    if 'lane_lines_prob' in outs:
+      self.parse_binary_crossentropy('lane_lines_prob', outs)
+    if 'lead_prob' in outs:
+      self.parse_binary_crossentropy('lead_prob', outs)
+    if 'lat_planner_solution' in outs:
+      self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
+    if 'meta' in outs:
+      self.parse_binary_crossentropy('meta', outs)
+    if 'road_edges' in outs:
+      self.parse_mdn('road_edges', outs, in_N=0, out_N=0,
+                     out_shape=(SplitModelConstants.NUM_ROAD_EDGES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
+    if 'sim_pose' in outs:
+      self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
+    if 'action' in outs:
+      self.parse_mdn('action', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.ACTION_WIDTH,))
+
+  def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+    self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
+    self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.WIDE_FROM_DEVICE_WIDTH,))
+    self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
+    self.parse_dynamic_outputs(outs)
+    self.split_outputs(outs)
+    return outs
+
+  def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+    self.parse_dynamic_outputs(outs)
+    self.split_outputs(outs)
+    return outs
+
+  def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+    outs = self.parse_vision_outputs(outs)
+    outs = self.parse_policy_outputs(outs)
+    return outs

openpilot/sunnypilot/modeld_v2/tests/helpers.py

@@ -117,7 +117,7 @@ ARCHETYPES = {
     is_20hz=True,
     expected_model_type='split',
     expected_constants_class=SplitModelConstants,
-    expected_parser_module='parse_model_outputs',
+    expected_parser_module='parse_model_outputs_split',
     expected_desire_key='desire',
   ),
   'vision_multi_policy': Archetype(
@@ -130,7 +130,7 @@ ARCHETYPES = {
     is_20hz=True,
     expected_model_type='multi_policy',
     expected_constants_class=SplitModelConstants,
-    expected_parser_module='parse_model_outputs',
+    expected_parser_module='parse_model_outputs_split',
     expected_desire_key='desire',
   ),
   'tri_policy': Archetype(
@@ -144,7 +144,7 @@ ARCHETYPES = {
     is_20hz=True,
     expected_model_type='multi_policy',
     expected_constants_class=SplitModelConstants,
-    expected_parser_module='parse_model_outputs',
+    expected_parser_module='parse_model_outputs_split',
     expected_desire_key='desire',
   ),
   'supercombo_non20hz': Archetype(

openpilot/sunnypilot/modeld_v2/tests/test_combined_pkl_loader.py

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

openpilot/sunnypilot/modeld_v2/tests/test_parse_model_outputs.py

@@ -1,81 +0,0 @@
-import numpy as np
-from openpilot.common.test import OpenpilotTestCase
-from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
-from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser, _infer_mhp, sigmoid, softmax
-
-
-class TestParseModelOutputs(OpenpilotTestCase):
-  def test_infer_mhp_lead(self):
-    in_hypotheses, out_selections = _infer_mhp(102, 24)
-    assert in_hypotheses == 2
-    assert out_selections == 3
-
-  def test_infer_mhp_plan(self):
-    in_hypotheses, out_selections = _infer_mhp(4955, 495)
-    assert in_hypotheses == 5
-    assert out_selections == 1
-
-  def test_infer_mhp_non_mdn(self):
-    in_hypotheses, out_selections = _infer_mhp(48, 24)
-    assert in_hypotheses == 1
-    assert out_selections == 0
-
-  def test_check_missing_raises(self):
-    parser = Parser(ignore_missing=False)
-    with self.assertRaises(ValueError):
-      parser.check_missing({}, "missing_key")
-
-  def test_check_missing_ignored(self):
-    parser = Parser(ignore_missing=True)
-    assert parser.check_missing({}, "missing_key") is True
-
-  def test_binary_crossentropy(self):
-    parser = Parser()
-    raw_logits = np.array([[-10.0, 0.0, 10.0]], dtype=np.float32)
-    outs = {"meta": raw_logits.copy()}
-    parser.parse_binary_crossentropy("meta", outs)
-    expected_probabilities = sigmoid(raw_logits)
-    np.testing.assert_allclose(outs["meta"], expected_probabilities, rtol=1e-5, atol=1e-6)
-
-  def test_categorical_crossentropy(self):
-    parser = Parser()
-    raw_logits = np.array([[1.0, 2.0, 3.0]], dtype=np.float32)
-    outs = {"desire_state": raw_logits.copy()}
-    parser.parse_categorical_crossentropy("desire_state", outs)
-    expected_probabilities = softmax(raw_logits)
-    np.testing.assert_allclose(outs["desire_state"], expected_probabilities, rtol=1e-5, atol=1e-6)
-
-  def test_parse_vision_outputs(self):
-    parser = Parser()
-    pose_raw = np.zeros((1, ModelConstants.POSE_WIDTH * 2), dtype=np.float32)
-    road_transform_raw = np.zeros((1, ModelConstants.POSE_WIDTH * 2), dtype=np.float32)
-    lead_raw = np.zeros((1, 102), dtype=np.float32)
-    meta_raw = np.zeros((1, 55), dtype=np.float32)
-    vision_outputs = {"pose": pose_raw, "road_transform": road_transform_raw, "lead": lead_raw, "meta": meta_raw}
-    parsed = parser.parse_vision_outputs(vision_outputs)
-    assert "pose" in parsed
-    assert "road_transform" in parsed
-    assert "lead" in parsed
-    assert "meta" in parsed
-    assert parsed["pose"].shape == (1, ModelConstants.POSE_WIDTH)
-    assert parsed["lead"].shape == (1, ModelConstants.LEAD_MHP_SELECTION, ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH)
-
-  def test_parse_policy_outputs(self):
-    parser = Parser()
-    plan_raw = np.zeros((1, 4955), dtype=np.float32)
-    desire_state_raw = np.zeros((1, ModelConstants.DESIRE_PRED_WIDTH), dtype=np.float32)
-    action_raw = np.zeros((1, ModelConstants.ACTION_WIDTH * 2), dtype=np.float32)
-    policy_outputs = {"plan": plan_raw, "desire_state": desire_state_raw, "action": action_raw}
-    parsed = parser.parse_policy_outputs(policy_outputs)
-    assert parsed["plan"].shape == (1, ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH)
-    assert parsed["action"].shape == (1, ModelConstants.ACTION_WIDTH)
-    assert parsed["desire_state"].shape == (1, ModelConstants.DESIRE_PRED_WIDTH)
-
-  def test_parse_outputs_combined(self):
-    parser = Parser()
-    outputs = {"plan": np.zeros((1, 4955), dtype=np.float32), "pose": np.zeros((1, ModelConstants.POSE_WIDTH * 2),
-               dtype=np.float32), "meta": np.zeros((1, 55), dtype=np.float32)}
-    parsed = parser.parse_outputs(outputs)
-    assert parsed["plan"].shape == (1, ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH)
-    assert parsed["pose"].shape == (1, ModelConstants.POSE_WIDTH)
-    assert parsed["meta"].shape == (1, 55)

openpilot/sunnypilot/models/constants.py

@@ -0,0 +1,121 @@
+import numpy as np
+
+def index_function(idx, max_val=192, max_idx=32):
+  return max_val * ((idx/max_idx)**2)
+
+
+class ModelConstants:
+  # time and distance indices
+  IDX_N = 33
+  T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
+  X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)]
+  LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.]
+  LEAD_T_OFFSETS = [0., 2., 4.]
+  META_T_IDXS = [2., 4., 6., 8., 10.]
+
+  # model inputs constants
+  MODEL_FREQ = 20
+  FEATURE_LEN = 512
+  HISTORY_BUFFER_LEN = 99
+  DESIRE_LEN = 8
+  TRAFFIC_CONVENTION_LEN = 2
+  NAV_FEATURE_LEN = 256
+  NAV_INSTRUCTION_LEN = 150
+  LAT_PLANNER_STATE_LEN = 4
+  LATERAL_CONTROL_PARAMS_LEN = 2
+  PREV_DESIRED_CURV_LEN = 1
+
+  # model outputs constants
+  FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
+  FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32)
+  FCW_5MS2_PROBS_WIDTH = 5
+  FCW_3MS2_PROBS_WIDTH = 2
+
+  DISENGAGE_WIDTH = 5
+  POSE_WIDTH = 6
+  WIDE_FROM_DEVICE_WIDTH = 3
+  SIM_POSE_WIDTH = 6
+  LEAD_WIDTH = 4
+  LANE_LINES_WIDTH = 2
+  ROAD_EDGES_WIDTH = 2
+  PLAN_WIDTH = 15
+  DESIRE_PRED_WIDTH = 8
+  LAT_PLANNER_SOLUTION_WIDTH = 4
+  DESIRED_CURV_WIDTH = 1
+
+  NUM_LANE_LINES = 4
+  NUM_ROAD_EDGES = 2
+
+  LEAD_TRAJ_LEN = 6
+  DESIRE_PRED_LEN = 4
+
+  PLAN_MHP_N = 5
+  LEAD_MHP_N = 2
+  PLAN_MHP_SELECTION = 1
+  LEAD_MHP_SELECTION = 3
+
+  FCW_THRESHOLD_5MS2_HIGH = 0.15
+  FCW_THRESHOLD_5MS2_LOW = 0.05
+  FCW_THRESHOLD_3MS2 = 0.7
+
+  CONFIDENCE_BUFFER_LEN = 5
+  RYG_GREEN = 0.01165
+  RYG_YELLOW = 0.06157
+
+  POLY_PATH_DEGREE = 4
+
+
+# model outputs slices
+class Plan:
+  POSITION = slice(0, 3)
+  VELOCITY = slice(3, 6)
+  ACCELERATION = slice(6, 9)
+  T_FROM_CURRENT_EULER = slice(9, 12)
+  ORIENTATION_RATE = slice(12, 15)
+
+
+class Meta:
+  ENGAGED = slice(0, 1)
+  # next 2, 4, 6, 8, 10 seconds
+  GAS_DISENGAGE = slice(1, 31, 6)
+  BRAKE_DISENGAGE = slice(2, 31, 6)
+  STEER_OVERRIDE = slice(3, 31, 6)
+  HARD_BRAKE_3 = slice(4, 31, 6)
+  HARD_BRAKE_4 = slice(5, 31, 6)
+  HARD_BRAKE_5 = slice(6, 31, 6)
+  # next 0, 2, 4, 6, 8, 10 seconds
+  GAS_PRESS = slice(31, 55, 4)
+  BRAKE_PRESS = slice(32, 55, 4)
+  LEFT_BLINKER = slice(33, 55, 4)
+  RIGHT_BLINKER = slice(34, 55, 4)
+
+
+class MetaTombRaider:
+  ENGAGED = slice(0, 1)
+  # next 2, 4, 6, 8, 10 seconds
+  GAS_DISENGAGE = slice(1, 41, 8)
+  BRAKE_DISENGAGE = slice(2, 41, 8)
+  STEER_OVERRIDE = slice(3, 41, 8)
+  HARD_BRAKE_3 = slice(4, 41, 8)
+  HARD_BRAKE_4 = slice(5, 41, 8)
+  HARD_BRAKE_5 = slice(6, 41, 8)
+  GAS_PRESS = slice(7, 41, 8)
+  BRAKE_PRESS = slice(8, 41, 8)
+  # next 0, 2, 4, 6, 8, 10 seconds
+  LEFT_BLINKER = slice(41, 53, 2)
+  RIGHT_BLINKER = slice(42, 53, 2)
+
+
+class MetaSimPose:
+  ENGAGED = slice(0, 1)
+  # next 2, 4, 6, 8, 10 seconds
+  GAS_DISENGAGE = slice(1, 36, 7)
+  BRAKE_DISENGAGE = slice(2, 36, 7)
+  STEER_OVERRIDE = slice(3, 36, 7)
+  HARD_BRAKE_3 = slice(4, 36, 7)
+  HARD_BRAKE_4 = slice(5, 36, 7)
+  HARD_BRAKE_5 = slice(6, 36, 7)
+  GAS_PRESS = slice(7, 36, 7)
+  # next 0, 2, 4, 6, 8, 10 seconds
+  LEFT_BLINKER = slice(36, 48, 2)
+  RIGHT_BLINKER = slice(37, 48, 2)

openpilot/sunnypilot/models/fetcher.py

@@ -139,7 +139,7 @@ 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_CHESTNUT = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_chestnut_v23.json"
 
   MODEL_SOURCES = {
     "qcom": (MODEL_URL, ""),

openpilot/sunnypilot/models/helpers.py

@@ -7,11 +7,14 @@ See the LICENSE.md file in the root directory for more details.
 
 import hashlib
 import os
+import pickle
+from pathlib import Path
 import numpy as np
 
 from openpilot.cereal import custom
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
+from openpilot.sunnypilot.models.constants import Meta, MetaSimPose, MetaTombRaider
 from openpilot.common.hardware.hw import Paths
 from openpilot.selfdrive.modeld.helpers import chestnut_present
 
@@ -19,6 +22,7 @@ from openpilot.selfdrive.modeld.helpers import chestnut_present
 REQUIRED_JSON_VERSION = 19
 
 CUSTOM_MODEL_PATH = Paths.model_root()
+METADATA_PATH = Path(__file__).parent / '../models/supercombo_metadata.pkl'
 ModelManager = custom.ModelManagerSP
 
 ACTIVE_BUNDLE_KEYS = {
@@ -197,6 +201,33 @@ def _get_model():
   return None
 
 
+def load_metadata():
+  metadata_path = METADATA_PATH
+
+  with open(metadata_path, 'rb') as f:
+    return pickle.load(f)
+
+
+def prepare_inputs(model_metadata: dict) -> dict[str, np.ndarray]:
+  return {
+    key: np.zeros(shape, dtype=np.float32).flatten()
+    for key, shape in model_metadata['input_shapes'].items()
+    if 'img' not in key
+  }
+
+
+def load_meta_constants(model_metadata: dict):
+  """ Loads the appropriate meta model class based on key shapes"""
+  if 'sim_pose' in model_metadata['input_shapes']:
+    return MetaSimPose
+
+  meta_slice = model_metadata['output_slices']['meta']
+  if (meta_slice.start, meta_slice.stop, meta_slice.step) == (5868, 5921, None):
+    return MetaTombRaider
+
+  return Meta
+
+
 # The following method(s) are modeld helper methods
 def plan_x_idxs_helper(constants, plan, model_output) -> list[float]:
   # times at X_IDXS according to plan.

openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -104,14 +104,6 @@ class ControlsExt(ModelStateBase):
     CC_SP.intelligentCruiseButtonManagement.sendButton = icbm_src.sendButton
     CC_SP.intelligentCruiseButtonManagement.vTarget = icbm_src.vTarget
 
-    ford_path = getattr(self, 'ford_path', None)
-    if ford_path is not None:
-      CC_SP.fordLateralPath.valid = ford_path.valid
-      CC_SP.fordLateralPath.pathOffset = ford_path.path_offset
-      CC_SP.fordLateralPath.pathAngle = ford_path.path_angle
-      CC_SP.fordLateralPath.curvature = ford_path.curvature
-      CC_SP.fordLateralPath.curvatureRate = ford_path.curvature_rate
-
     return CC_SP
 
   @staticmethod

openpilot/sunnypilot/selfdrive/selfdrived/assisted_driving_milestones.py

@@ -1,260 +0,0 @@
-"""Authoritative assisted-driving distance and milestone tracking."""
-
-import math
-from collections.abc import Mapping
-from dataclasses import dataclass
-from enum import StrEnum
-
-from openpilot.common.params import Params
-
-
-METERS_PER_MILE = 1609.344
-METERS_PER_KILOMETER = 1000.0
-MAX_SAMPLE_INTERVAL_SECONDS = 0.5
-PERSIST_INTERVAL_NS = 10_000_000_000
-STATE_VERSION = 1
-STATE_PARAM = "AssistedDrivingMilestoneState"
-LAST_DRIVE_SUMMARY_PARAM = "LastDriveAssistedDrivingSummary"
-
-
-class AssistCategory(StrEnum):
-  MADS = "mads"
-  FULL_ASSIST = "fullAssist"
-
-
-class MilestoneUnit(StrEnum):
-  IMPERIAL = "imperial"
-  METRIC = "metric"
-
-
-@dataclass(frozen=True)
-class MilestoneEvent:
-  event_id: int
-  category: AssistCategory
-  distance_meters: float
-  previous_distance_meters: float
-  unit: MilestoneUnit
-
-
-@dataclass(frozen=True)
-class MilestoneSnapshot:
-  distances_meters: dict[AssistCategory, float]
-  drive_start_distances_meters: dict[AssistCategory, float]
-  next_event_id: int
-  next_summary_id: int
-  unit: MilestoneUnit
-  active_drive_id: str
-
-
-def assist_category(lat_active: bool, long_active: bool) -> AssistCategory | None:
-  if not lat_active:
-    return None
-  return AssistCategory.FULL_ASSIST if long_active else AssistCategory.MADS
-
-
-def _meters_per_unit(unit: MilestoneUnit) -> float:
-  return METERS_PER_KILOMETER if unit == MilestoneUnit.METRIC else METERS_PER_MILE
-
-
-def _next_ladder_value(value: float) -> float:
-  value = max(0.0, value)
-  magnitude = 10.0 ** math.floor(math.log10(max(1.0, value)))
-  for multiplier in (1.0, 2.0, 5.0):
-    candidate = multiplier * magnitude
-    if candidate > value + 1e-9:
-      return candidate
-  return 10.0 * magnitude
-
-
-def _previous_ladder_value(value: float) -> float:
-  if value <= 1.0:
-    return 0.0
-  magnitude = 10.0 ** math.floor(math.log10(value))
-  normalized = value / magnitude
-  if normalized <= 1.0 + 1e-9:
-    return 5.0 * magnitude / 10.0
-  if normalized <= 2.0 + 1e-9:
-    return magnitude
-  return 2.0 * magnitude
-
-
-def next_milestone_meters(distance_meters: float, unit: MilestoneUnit) -> float:
-  meters_per_unit = _meters_per_unit(unit)
-  return _next_ladder_value(distance_meters / meters_per_unit) * meters_per_unit
-
-
-class MilestoneStore:
-  def __init__(self, params: Params | None = None):
-    self._params = params or Params()
-
-  def load(self) -> MilestoneSnapshot:
-    raw = self._params.get(STATE_PARAM, return_default=True)
-    raw = raw if isinstance(raw, dict) else {}
-    raw_distances = raw.get("distancesMeters", {})
-    raw_distances = raw_distances if isinstance(raw_distances, dict) else {}
-    try:
-      unit = MilestoneUnit(raw.get("unit", MilestoneUnit.IMPERIAL))
-    except ValueError:
-      unit = MilestoneUnit.IMPERIAL
-
-    def distance(category: AssistCategory) -> float:
-      try:
-        return max(0.0, float(raw_distances.get(category.value, 0.0)))
-      except (TypeError, ValueError):
-        return 0.0
-
-    distances = {category: distance(category) for category in AssistCategory}
-    raw_drive_start = raw.get("driveStartDistancesMeters", {})
-    raw_drive_start = raw_drive_start if isinstance(raw_drive_start, dict) else {}
-
-    def drive_start_distance(category: AssistCategory) -> float:
-      try:
-        return max(0.0, min(float(raw_drive_start.get(category.value, distances[category])), distances[category]))
-      except (TypeError, ValueError):
-        return distances[category]
-
-    try:
-      next_event_id = max(1, int(raw.get("nextEventId", 1)))
-    except (TypeError, ValueError):
-      next_event_id = 1
-    try:
-      next_summary_id = max(1, int(raw.get("nextSummaryId", 1)))
-    except (TypeError, ValueError):
-      next_summary_id = 1
-
-    return MilestoneSnapshot(
-      distances_meters=distances,
-      drive_start_distances_meters={category: drive_start_distance(category) for category in AssistCategory},
-      next_event_id=next_event_id,
-      next_summary_id=next_summary_id,
-      unit=unit,
-      active_drive_id=str(raw.get("activeDriveId", "")),
-    )
-
-  def save(self, snapshot: MilestoneSnapshot, block: bool = False) -> None:
-    if block:
-      self._params.flush()
-    self._params.put(STATE_PARAM, {
-      "version": STATE_VERSION,
-      "distancesMeters": {category.value: max(0.0, snapshot.distances_meters.get(category, 0.0)) for category in AssistCategory},
-      "driveStartDistancesMeters": {
-        category.value: max(0.0, snapshot.drive_start_distances_meters.get(category, 0.0)) for category in AssistCategory
-      },
-      "nextEventId": max(1, snapshot.next_event_id),
-      "nextSummaryId": max(1, snapshot.next_summary_id),
-      "unit": snapshot.unit.value,
-      "activeDriveId": snapshot.active_drive_id,
-    }, block=block)
-
-  def save_drive_summary(self, summary_id: int, distances_meters: Mapping[AssistCategory, float], unit: MilestoneUnit) -> None:
-    self._params.put(LAST_DRIVE_SUMMARY_PARAM, {
-      "version": STATE_VERSION,
-      "id": summary_id,
-      "distancesMeters": {category.value: max(0.0, distances_meters.get(category, 0.0)) for category in AssistCategory},
-      "unit": unit.value,
-    }, block=True)
-
-
-class AssistedDrivingMilestones:
-  """Tracks, persists, and emits milestones through one small interface."""
-
-  def __init__(self, store: MilestoneStore | None = None):
-    self._store = store or MilestoneStore()
-    snapshot = self._store.load()
-    self._distances_meters = snapshot.distances_meters
-    self._drive_start_distances_meters = snapshot.drive_start_distances_meters
-    self._next_event_id = snapshot.next_event_id
-    self._next_summary_id = snapshot.next_summary_id
-    self._unit = snapshot.unit
-    self._active_drive_id = snapshot.active_drive_id
-    self._next_milestone_meters = {
-      category: next_milestone_meters(distance, self._unit)
-      for category, distance in self._distances_meters.items()
-    }
-    self._last_timestamp_ns: int | None = None
-    self._last_persist_timestamp_ns: int | None = None
-    self._last_speed_mps = 0.0
-    self._last_category: AssistCategory | None = None
-    self._enabled = False
-    self._closed = False
-
-  def snapshot(self) -> MilestoneSnapshot:
-    return MilestoneSnapshot(
-      self._distances_meters.copy(),
-      self._drive_start_distances_meters.copy(),
-      self._next_event_id,
-      self._next_summary_id,
-      self._unit,
-      self._active_drive_id,
-    )
-
-  def set_drive_id(self, drive_id: str) -> None:
-    if not drive_id or drive_id == self._active_drive_id:
-      return
-    self._active_drive_id = drive_id
-    self._drive_start_distances_meters = self._distances_meters.copy()
-    self._persist()
-
-  def update(self, timestamp_ns: int, speed_mps: float, *, lat_active: bool, long_active: bool,
-             is_metric: bool, enabled: bool) -> MilestoneEvent | None:
-    self._enabled = enabled
-    unit = MilestoneUnit.METRIC if is_metric else MilestoneUnit.IMPERIAL
-    if unit != self._unit:
-      self._unit = unit
-      self._next_milestone_meters = {
-        category: next_milestone_meters(distance, unit)
-        for category, distance in self._distances_meters.items()
-      }
-
-    speed_mps = max(0.0, speed_mps)
-    category = assist_category(lat_active, long_active) if enabled else None
-    event = None
-
-    if self._last_timestamp_ns is not None and timestamp_ns != self._last_timestamp_ns:
-      dt = (timestamp_ns - self._last_timestamp_ns) / 1e9
-      if 0 < dt <= MAX_SAMPLE_INTERVAL_SECONDS and self._last_category is not None:
-        active_category = self._last_category
-        self._distances_meters[active_category] += (self._last_speed_mps + speed_mps) / 2.0 * dt
-        threshold_meters = self._next_milestone_meters[active_category]
-        if self._distances_meters[active_category] >= threshold_meters:
-          meters_per_unit = _meters_per_unit(self._unit)
-          threshold_units = threshold_meters / meters_per_unit
-          event = MilestoneEvent(
-            event_id=self._next_event_id,
-            category=active_category,
-            distance_meters=threshold_meters,
-            previous_distance_meters=_previous_ladder_value(threshold_units) * meters_per_unit,
-            unit=self._unit,
-          )
-          self._next_event_id += 1
-          self._next_milestone_meters[active_category] = next_milestone_meters(threshold_meters, self._unit)
-          self._persist(timestamp_ns=timestamp_ns)
-
-    self._last_timestamp_ns = timestamp_ns
-    self._last_speed_mps = speed_mps
-    self._last_category = category
-
-    if self._last_persist_timestamp_ns is None:
-      self._last_persist_timestamp_ns = timestamp_ns
-    elif timestamp_ns - self._last_persist_timestamp_ns >= PERSIST_INTERVAL_NS:
-      self._persist(timestamp_ns=timestamp_ns)
-
-    return event
-
-  def close(self) -> None:
-    if self._closed:
-      return
-    self._closed = True
-    drive_distances = {
-      category: self._distances_meters[category] - self._drive_start_distances_meters[category]
-      for category in AssistCategory
-    }
-    summary_id = self._next_summary_id
-    self._next_summary_id += 1
-    self._persist(block=True)
-    if self._enabled:
-      self._store.save_drive_summary(summary_id, drive_distances, self._unit)
-
-  def _persist(self, block: bool = False, timestamp_ns: int | None = None) -> None:
-    self._store.save(self.snapshot(), block=block)
-    self._last_persist_timestamp_ns = self._last_timestamp_ns if timestamp_ns is None else timestamp_ns

openpilot/sunnypilot/selfdrive/selfdrived/tests/test_assisted_driving_milestones.py

@@ -1,123 +0,0 @@
-import unittest
-
-from openpilot.sunnypilot.selfdrive.selfdrived.assisted_driving_milestones import (
-  METERS_PER_MILE,
-  AssistCategory,
-  AssistedDrivingMilestones,
-  MilestoneStore,
-  MilestoneUnit,
-)
-
-
-class ParamsStub:
-  def __init__(self, state=None):
-    self.values = {"AssistedDrivingMilestoneState": state or {}}
-    self.writes = []
-
-  def get(self, key, return_default=False):
-    return self.values.get(key, {} if return_default else None)
-
-  def put(self, key, value, block=False):
-    self.values[key] = value
-    self.writes.append((key, value, block))
-
-  def flush(self):
-    pass
-
-
-class TestAssistedDrivingMilestones(unittest.TestCase):
-  def test_emits_and_asynchronously_persists_first_imperial_milestone(self):
-    params = ParamsStub({
-      "version": 1,
-      "distancesMeters": {"mads": METERS_PER_MILE - 5.0, "fullAssist": 0.0},
-      "nextEventId": 7,
-      "unit": "imperial",
-    })
-    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
-
-    self.assertIsNone(milestones.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True))
-    event = milestones.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
-
-    self.assertIsNotNone(event)
-    assert event is not None
-    self.assertEqual(event.event_id, 7)
-    self.assertEqual(event.category, AssistCategory.MADS)
-    self.assertEqual(event.unit, MilestoneUnit.IMPERIAL)
-    self.assertAlmostEqual(event.distance_meters, METERS_PER_MILE)
-    self.assertFalse(params.writes[-1][2])
-
-  def test_switching_units_schedules_only_a_future_milestone(self):
-    params = ParamsStub({
-      "version": 1,
-      "distancesMeters": {"mads": 9_500.0, "fullAssist": 0.0},
-      "nextEventId": 2,
-      "unit": "imperial",
-    })
-    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
-
-    self.assertIsNone(milestones.update(0, 1_000.0, lat_active=True, long_active=False, is_metric=True, enabled=True))
-    event = milestones.update(500_000_000, 1_000.0, lat_active=True, long_active=False, is_metric=True, enabled=True)
-
-    self.assertIsNotNone(event)
-    assert event is not None
-    self.assertEqual(event.unit, MilestoneUnit.METRIC)
-    self.assertAlmostEqual(event.distance_meters, 10_000.0)
-
-  def test_ignores_disabled_reverse_and_timestamp_gaps(self):
-    params = ParamsStub()
-    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
-
-    milestones.update(0, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
-    milestones.update(500_000_000, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
-    milestones.update(1_000_000_000, -20.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
-    milestones.update(2_000_000_000, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
-
-    self.assertEqual(milestones.snapshot().distances_meters[AssistCategory.MADS], 0.0)
-
-  def test_close_persists_totals_and_last_drive_summary(self):
-    params = ParamsStub()
-    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
-    milestones.update(0, 10.0, lat_active=True, long_active=True, is_metric=False, enabled=True)
-    milestones.update(500_000_000, 10.0, lat_active=True, long_active=True, is_metric=False, enabled=True)
-
-    milestones.close()
-
-    summary = params.values["LastDriveAssistedDrivingSummary"]
-    self.assertAlmostEqual(summary["distancesMeters"]["fullAssist"], 5.0)
-    self.assertTrue(params.writes[-1][2])
-
-    write_count = len(params.writes)
-    milestones.close()
-    self.assertEqual(len(params.writes), write_count)
-
-  def test_process_restart_preserves_the_current_drive_start(self):
-    params = ParamsStub()
-    first_process = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
-    first_process.set_drive_id("route-1")
-    first_process.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
-    first_process.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
-    first_process.close()
-
-    second_process = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
-    second_process.set_drive_id("route-1")
-    second_process.update(1_000_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
-    second_process.update(1_500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
-    second_process.close()
-
-    summary = params.values["LastDriveAssistedDrivingSummary"]
-    self.assertAlmostEqual(summary["distancesMeters"]["mads"], 10.0)
-
-  def test_disabled_feature_does_not_publish_drive_summary(self):
-    params = ParamsStub()
-    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
-    milestones.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
-    milestones.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
-    milestones.update(1_000_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
-
-    milestones.close()
-
-    self.assertNotIn("LastDriveAssistedDrivingSummary", params.values)
-
-
-if __name__ == "__main__":
-  unittest.main()

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -1383,12 +1383,6 @@
               "title": "Steering Arc",
               "description": "Display steering arc on the driving screen when lateral control is enabled."
             },
-            {
-              "key": "AssistedDrivingMilestonesEnabled",
-              "widget": "toggle",
-              "title": "Assisted Driving Milestones",
-              "description": "Celebrate cumulative MADS and full-assist distance milestones while driving."
-            },
             {
               "key": "ShowTurnSignals",
               "widget": "toggle",
@@ -2174,43 +2168,6 @@
     }
   ],
   "vehicle_settings": {
-    "ford": {
-      "title": "Ford Settings",
-      "description": "",
-      "items": [
-        {
-          "key": "FordVirtualAngleController",
-          "widget": "toggle",
-          "needs_onroad_cycle": true,
-          "title": "C2-Free Path Tracking (Experimental)",
-          "description": "Follow large turns from the model path while retaining planned-curvature centering on the F-150 Lightning with C2 off.",
-          "details": "Uses the existing controller's model-path geometry for large turns when the model and planned curvature agree. Smaller or opposing requests use planned curvature for centering. A bounded measured-turning correction requires fresh, valid steering-controller status and clears during driver override. During turn release, planned-curvature tracking can recover within bounded earlier-command headroom when measured turning falls below both recent and current requests and is no longer increasing. An opposing correction still unwinds only to zero. When turning exceeds both requests, a separate guard prevents model-driven growth of the offset and heading requests, including after driver input resets feedback. The guard preserves opposing centering terms. A reported steering-controller limit still prevents request-increasing correction. Default off and this version is not road-validated. When enabled, this controller is always selected on the Ford CAN FD F-150 Lightning regardless of steering-firmware identification; other vehicles retain their existing controller. Enable only for controlled testing. Takes priority over PSCM Coefficient Observer while enabled. Turning it off restores the previous controller selection. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.",
-          "enablement": [
-            {
-              "type": "offroad_only"
-            }
-          ]
-        },
-        {
-          "key": "FordPscmObserver",
-          "widget": "toggle",
-          "needs_onroad_cycle": true,
-          "title": "PSCM Coefficient Observer (Experimental)",
-          "description": "Track the Ford steering controller's internal polynomial states and use fast path terms only for the response that slow curvature cannot provide.",
-          "details": "This changes live steering behavior on Ford CAN FD vehicles. Use only for supervised testing and be ready to take over immediately. This strategy is bypassed when C2-Free Path Tracking is selected on a supported vehicle; its selection is retained when that experiment is turned off.",
-          "enablement": [
-            {
-              "type": "offroad_only"
-            },
-            {
-              "type": "param",
-              "key": "FordVirtualAngleController",
-              "equals": false
-            }
-          ]
-        }
-      ]
-    },
     "hyundai": {
       "title": "Hyundai / Kia / Genesis Settings",
       "description": "",

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

@@ -6,29 +6,6 @@ icon: vehicle
 order: 99
 kind: vehicle
 sections:
-- id: ford
-  title: Ford Settings
-  description: ''
-  items:
-  - key: FordVirtualAngleController
-    widget: toggle
-    needs_onroad_cycle: true
-    title: C2-Free Path Tracking (Experimental)
-    description: Follow large turns from the model path while retaining planned-curvature centering on the F-150 Lightning with C2 off.
-    details: Uses the existing controller's model-path geometry for large turns when the model and planned curvature agree. Smaller or opposing requests use planned curvature for centering. A bounded measured-turning correction requires fresh, valid steering-controller status and clears during driver override. During turn release, planned-curvature tracking can recover within bounded earlier-command headroom when measured turning falls below both recent and current requests and is no longer increasing. An opposing correction still unwinds only to zero. When turning exceeds both requests, a separate guard prevents model-driven growth of the offset and heading requests, including after driver input resets feedback. The guard preserves opposing centering terms. A reported steering-controller limit still prevents request-increasing correction. Default off and this version is not road-validated. When enabled, this controller is always selected on the Ford CAN FD F-150 Lightning regardless of steering-firmware identification; other vehicles retain their existing controller. Enable only for controlled testing. Takes priority over PSCM Coefficient Observer while enabled. Turning it off restores the previous controller selection. Changes apply after a real offroad-to-onroad cycle, not immediately or on disengagement alone.
-    enablement:
-    - $ref: '#/macros/offroad'
-  - key: FordPscmObserver
-    widget: toggle
-    needs_onroad_cycle: true
-    title: PSCM Coefficient Observer (Experimental)
-    description: Track the Ford steering controller's internal polynomial states and use fast path terms only for the response that slow curvature cannot provide.
-    details: This changes live steering behavior on Ford CAN FD vehicles. Use only for supervised testing and be ready to take over immediately. This strategy is bypassed when C2-Free Path Tracking is selected on a supported vehicle; its selection is retained when that experiment is turned off.
-    enablement:
-    - $ref: '#/macros/offroad'
-    - type: param
-      key: FordVirtualAngleController
-      equals: false
 - id: hyundai
   title: Hyundai / Kia / Genesis Settings
   description: ''

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

@@ -20,10 +20,6 @@ sections:
     widget: toggle
     title: Steering Arc
     description: Display steering arc on the driving screen when lateral control is enabled.
-  - key: AssistedDrivingMilestonesEnabled
-    widget: toggle
-    title: Assisted Driving Milestones
-    description: Celebrate cumulative MADS and full-assist distance milestones while driving.
   - key: ShowTurnSignals
     widget: toggle
     title: Display Turn Signals

openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -5,7 +5,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.
 """
 import json
-import tempfile
 
 from openpilot.common.params import Params
 from openpilot.sunnypilot.sunnylink.tools.generate_settings_schema import (
@@ -279,39 +278,6 @@ class TestKnownPanels(OpenpilotTestCase):
 
 
 class TestKnownVehicleSettings(OpenpilotTestCase):
-  def test_ford_virtual_angle_replaces_shared_path_and_is_cycle_only(self, schema):
-    items = _brand_items(schema["vehicle_settings"].get("ford"))
-    assert "FordSharedPathController" not in {item["key"] for item in items}
-    servo = next(item for item in items if item["key"] == "FordVirtualAngleController")
-    assert servo["title"] == "C2-Free Path Tracking (Experimental)"
-    assert servo["widget"] == "toggle"
-    assert servo["needs_onroad_cycle"] is True
-    # No other toggle can prevent disabling this experiment while offroad.
-    assert servo["enablement"] == [{"type": "offroad_only"}]
-    assert "F-150 Lightning" in servo["description"]
-    assert "model path" in servo["description"]
-    assert "planned-curvature centering" in servo["description"]
-    assert "always selected on the Ford CAN FD F-150 Lightning regardless of steering-firmware identification" in servo["details"]
-    assert "Turning it off restores the previous controller selection" in servo["details"]
-    assert "RL38-14D003-AA" not in servo["details"]
-    assert "not road-validated" in servo["details"]
-    assert "offroad" in servo["details"] and "onroad" in servo["details"]
-
-  def test_ford_virtual_angle_defaults_off(self):
-    with tempfile.TemporaryDirectory() as path:
-      params = Params(path)
-      assert params.get_default_value("FordVirtualAngleController") is False
-      assert b"FordSharedPathController" not in params.all_keys()
-
-  def test_ford_has_pscm_observer(self, schema):
-    items = _brand_items(schema["vehicle_settings"].get("ford"))
-    observer = next(item for item in items if item["key"] == "FordPscmObserver")
-    assert observer["needs_onroad_cycle"] is True
-    assert observer["enablement"] == [
-      {"type": "offroad_only"},
-      {"type": "param", "key": "FordVirtualAngleController", "equals": False},
-    ]
-
   def test_hyundai_has_longitudinal_tuning(self, schema):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("hyundai"))}
     assert "HyundaiLongitudinalTuning" in keys

openpilot/sunnypilot/system/params_migration.py

@@ -103,32 +103,6 @@ def _migrate_model_bundle_slots(_params):
     cloudlog.exception(f"Error migrating model bundle slots: {e}")
 
 
-def _migrate_assisted_driving_milestones(_params):
-  try:
-    state = _params.get("AssistedDrivingMilestoneState", return_default=True)
-    if isinstance(state, dict) and state.get("version") == 1:
-      return
-
-    _params.put("AssistedDrivingMilestoneState", {
-      "version": 1,
-      "distancesMeters": {
-        "mads": max(0.0, _params.get("MadsDrivenDistanceMeters", return_default=True) or 0.0),
-        "fullAssist": max(0.0, _params.get("FullAssistDrivenDistanceMeters", return_default=True) or 0.0),
-      },
-      "driveStartDistancesMeters": {
-        "mads": max(0.0, _params.get("MadsDrivenDistanceMeters", return_default=True) or 0.0),
-        "fullAssist": max(0.0, _params.get("FullAssistDrivenDistanceMeters", return_default=True) or 0.0),
-      },
-      "nextEventId": 1,
-      "nextSummaryId": 1,
-      "unit": "metric" if _params.get_bool("IsMetric") else "imperial",
-      "activeDriveId": "",
-    }, block=True)
-    cloudlog.info("params_migration: migrated assisted-driving milestone state")
-  except Exception as e:
-    cloudlog.exception(f"Error migrating assisted-driving milestone state: {e}")
-
-
 def run_migration(_params):
   # migrate OnroadScreenOffBrightness
   if _params.get("OnroadScreenOffBrightnessMigrated") != ONROAD_BRIGHTNESS_MIGRATION_VERSION:
@@ -168,5 +142,3 @@ def run_migration(_params):
 
   # seed the chestnut model slot from the pre-split single slot
   _migrate_model_bundle_slots(_params)
-
-  _migrate_assisted_driving_milestones(_params)

openpilot/sunnypilot/system/tests/test_params_migration.py

@@ -7,44 +7,7 @@ See the LICENSE.md file in the root directory for more details.
 
 from openpilot.common.params import Params
 from openpilot.common.test import OpenpilotTestCase
-from openpilot.sunnypilot.system.params_migration import _migrate_model_bundle_slots, run_migration
-
-
-class TestAssistedDrivingMilestoneMigration(OpenpilotTestCase):
-  def test_preserves_prototype_distances_once(self):
-    class ParamsStub:
-      def __init__(self):
-        self.values = {
-          "MadsDrivenDistanceMeters": 123.0,
-          "FullAssistDrivenDistanceMeters": 456.0,
-          "OnroadScreenOffBrightness": 0,
-          "OnroadScreenOffTimer": 15,
-          "AssistedDrivingMilestoneState": {},
-          "IsMetric": False,
-        }
-
-      def get(self, key, return_default=False):
-        return self.values.get(key)
-
-      def put(self, key, value, block=False):
-        self.values[key] = value
-
-      def get_bool(self, key):
-        return bool(self.values.get(key, False))
-
-    params = ParamsStub()
-
-    run_migration(params)
-
-    state = params.get("AssistedDrivingMilestoneState")
-    assert state["distancesMeters"] == {"mads": 123.0, "fullAssist": 456.0}
-
-    params.put("MadsDrivenDistanceMeters", 12.0, block=True)
-    params.put("FullAssistDrivenDistanceMeters", 34.0, block=True)
-    run_migration(params)
-
-    state = params.get("AssistedDrivingMilestoneState")
-    assert state["distancesMeters"] == {"mads": 123.0, "fullAssist": 456.0}
+from openpilot.sunnypilot.system.params_migration import _migrate_model_bundle_slots
 
 
 class TestModelBundleSlotMigration(OpenpilotTestCase):

tools/ford_native_path.py

@@ -1,498 +0,0 @@
-#!/usr/bin/env python3
-"""Offline evaluation of Ford's native four-field path polynomial.
-
-The experiment deliberately does not alter the live controller. It rebases the
-model path into the vehicle pose expected at actuation time, fits one cubic over
-the remaining short path, and converts the cubic into the LMC2 C0/C1/C2/C3
-signals. A first-order C2 response envelope is included to expose commands that
-would look good only if the PSCM curvature channel were instantaneous.
-"""
-
-import argparse
-from collections import defaultdict
-from dataclasses import dataclass
-import glob
-import math
-from pathlib import Path
-
-import numpy as np
-
-from openpilot.tools.lib.logreader import LogReader
-
-
-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)
-MAX_LATERAL_ACCEL = 3.0 + 9.81 * 0.06
-MAX_LATERAL_JERK = 3.0 + 9.81 * 0.06
-@dataclass(frozen=True)
-class ModelPath:
-  x: np.ndarray
-  y: np.ndarray
-  heading: np.ndarray
-  distance: np.ndarray
-
-
-@dataclass(frozen=True)
-class Sample:
-  route: str
-  time: float
-  speed: float
-  curvature: float
-  steering_pressed: bool
-  path: ModelPath
-  sent_c0: float
-  sent_c1: float
-  sent_c2: float
-  sent_c3: float
-@dataclass(frozen=True)
-class NativePath:
-  c0: float
-  c1: float
-  c2: float
-  c3: float
-  fit_rmse: float
-  path_rms: float
-
-
-def _model_path(model) -> ModelPath | None:
-  try:
-    x = np.asarray(model.position.x, dtype=float)
-    y = np.asarray(model.position.y, dtype=float)
-    heading = np.unwrap(np.asarray(model.orientation.z, dtype=float))
-  except (AttributeError, TypeError, ValueError):
-    return None
-  if len(x) < 4 or len(x) != len(y) or len(x) != len(heading):
-    return None
-  if not np.isfinite(np.concatenate((x, y, heading))).all():
-    return None
-  distance = np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(x), np.diff(y)))))
-  unique_distance, unique = np.unique(distance, return_index=True)
-  if len(unique_distance) < 4 or unique_distance[-1] <= 0.0:
-    return None
-  return ModelPath(x[unique], y[unique], heading[unique], unique_distance)
-
-
-def _arc_pose(distance: float, curvature: float) -> tuple[float, float, float]:
-  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_points(path: ModelPath, vehicle_pose: tuple[float, float, float], start: float,
-                     horizon: float, count: int = 25) -> tuple[np.ndarray, np.ndarray]:
-  sample_distance = np.linspace(start, min(start + horizon, path.distance[-1]), count)
-  desired_x = np.interp(sample_distance, path.distance, path.x)
-  desired_y = np.interp(sample_distance, path.distance, path.y)
-  vehicle_x, vehicle_y, vehicle_heading = vehicle_pose
-  dx = desired_x - vehicle_x
-  dy = desired_y - vehicle_y
-  cosine = math.cos(vehicle_heading)
-  sine = math.sin(vehicle_heading)
-  return cosine * dx + sine * dy, -sine * dx + cosine * dy
-
-
-def _fit_points(path: ModelPath, speed: float, current_curvature: float, delay: float,
-                horizon: float) -> tuple[np.ndarray, np.ndarray] | None:
-  advance = min(max(speed, 0.0) * delay, path.distance[-1])
-  available = min(horizon, path.distance[-1] - advance)
-  if available <= 0.25:
-    return None
-  x, y = _relative_points(path, _arc_pose(advance, current_curvature), advance, available)
-  forward = (x >= -0.25) & (x <= horizon)
-  x = x[forward]
-  y = y[forward]
-  if len(x) < 4 or np.ptp(x) <= 0.25:
-    return None
-  return x, y
-
-
-def _wire_coefficients(c0: float, c1: float, c2: float, c3: float) -> tuple[float, float, float, float]:
-  slope = math.tan(c1)
-  slope_norm = 1.0 + slope ** 2
-  a2 = 0.5 * c2 * slope_norm ** 1.5
-  a3 = (c3 + 12.0 * slope * a2 ** 2 / slope_norm ** 3) * slope_norm ** 2 / 6.0
-  return c0, slope, a2, a3
-
-
-def _wire_rmse(command: tuple[float, float, float, float], x: np.ndarray, y: np.ndarray) -> float:
-  a0, a1, a2, a3 = _wire_coefficients(*command)
-  reconstructed = a0 + a1 * x + a2 * x ** 2 + a3 * x ** 3
-  return float(np.sqrt(np.mean((reconstructed - y) ** 2)))
-
-
-def fit_native_path(path: ModelPath, speed: float, current_curvature: float, *, delay: float,
-                    horizon: float) -> NativePath:
-  """Fit the delay-aligned path and return physical LMC2 fields.
-
-  C2 and C3 are curvature and curvature rate at the vehicle-frame origin, not
-  the raw quadratic and cubic polynomial coefficients.
-  """
-  points = _fit_points(path, speed, current_curvature, delay, horizon)
-  if points is None:
-    return NativePath(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
-  x, y = points
-
-  # Scaling x before the least-squares solve keeps tight-turn fits well
-  # conditioned while preserving an ordinary cubic in vehicle coordinates.
-  scale = max(float(np.max(np.abs(x))), 1.0)
-  normalized_x = x / scale
-  design = np.column_stack((np.ones(len(x)), normalized_x, normalized_x ** 2, normalized_x ** 3))
-  scaled, *_ = np.linalg.lstsq(design, y, rcond=None)
-  a0, a1, a2, a3 = (float(scaled[index] / scale ** index) for index in range(4))
-  slope = a1
-  slope_norm = 1.0 + slope ** 2
-  curvature = 2.0 * a2 / slope_norm ** 1.5
-  curvature_rate = 6.0 * a3 / slope_norm ** 2 - 12.0 * slope * a2 ** 2 / slope_norm ** 3
-  command = (float(np.clip(a0, *DBC_OFFSET)),
-             float(np.clip(math.atan(slope), *DBC_ANGLE)),
-             float(np.clip(curvature, *DBC_CURVATURE)),
-             float(np.clip(curvature_rate, *DBC_CURVATURE_RATE)))
-  return NativePath(
-    *command,
-    _wire_rmse(command, x, y),
-    float(np.sqrt(np.mean(y ** 2))),
-  )
-
-
-def fit_c2_aware_path(path: ModelPath, speed: float, current_curvature: float, *, delay: float,
-                      horizon: float, target_c2: float, effective_c2: float,
-                      use_c3: bool = True) -> NativePath:
-  """Fit fast fields around the C2 curvature the PSCM is expected to realize."""
-  points = _fit_points(path, speed, current_curvature, delay, horizon)
-  if points is None:
-    return NativePath(0.0, 0.0, target_c2, 0.0, 0.0, 0.0)
-  x, y = points
-
-  slope = 0.0
-  a0 = a1 = a3 = 0.0
-  for _ in range(3):
-    a2 = 0.5 * effective_c2 * (1.0 + slope ** 2) ** 1.5
-    design = np.column_stack((np.ones(len(x)), x, x ** 3))
-    (a0, a1, a3), *_ = np.linalg.lstsq(design, y - a2 * x ** 2, rcond=None)
-    slope = float(a1)
-
-  slope_norm = 1.0 + slope ** 2
-  c3 = 6.0 * float(a3) / slope_norm ** 2 - 12.0 * slope * a2 ** 2 / slope_norm ** 3
-  c3 = float(np.clip(c3, *DBC_CURVATURE_RATE)) if use_c3 else 0.0
-  # Once C2 and C3 are fixed to what the hardware can realize, refit C0/C1 so
-  # their fast feedback preserves as much of the same path as possible.
-  _, _, fixed_a2, fixed_a3 = _wire_coefficients(0.0, math.atan(slope), effective_c2, c3)
-  (a0, a1), *_ = np.linalg.lstsq(np.column_stack((np.ones(len(x)), x)),
-                                  y - fixed_a2 * x ** 2 - fixed_a3 * x ** 3, rcond=None)
-  c0 = float(np.clip(a0, *DBC_OFFSET))
-  c1 = float(np.clip(math.atan(float(a1)), *DBC_ANGLE))
-  effective_command = (c0, c1, effective_c2, c3)
-  return NativePath(c0, c1, target_c2, c3, _wire_rmse(effective_command, x, y),
-                    float(np.sqrt(np.mean(y ** 2))))
-
-
-def _route(path: str) -> str:
-  return Path(path).name.split("--", 1)[0]
-
-
-def load_samples(paths: list[str], stride: int = 2) -> list[Sample]:
-  grouped: dict[str, list[str]] = defaultdict(list)
-  for path in paths:
-    grouped[_route(path)].append(path)
-
-  samples = []
-  for route, route_paths in sorted(grouped.items()):
-    events = []
-    for path in sorted(route_paths):
-      events.extend(LogReader(path))
-    events.sort(key=lambda event: event.logMonoTime)
-    if not events:
-      continue
-    start_time = events[0].logMonoTime
-    model_path = None
-    curvature = 0.0
-    lat_active = path_valid = False
-    sent = (0.0, 0.0, 0.0, 0.0)
-    car_state_count = 0
-    for event in events:
-      which = event.which()
-      if which == "modelV2":
-        model_path = _model_path(event.modelV2)
-      elif which == "controlsState":
-        curvature = float(event.controlsState.curvature)
-      elif which == "carControl":
-        lat_active = bool(event.carControl.latActive)
-      elif which == "carControlSP":
-        command = event.carControlSP.fordLateralPath
-        path_valid = bool(command.valid)
-        sent = (float(command.pathOffset), float(command.pathAngle),
-                float(command.curvature), float(command.curvatureRate))
-      elif which == "carState" and lat_active and path_valid and model_path is not None:
-        car_state_count += 1
-        if car_state_count % stride:
-          continue
-        samples.append(Sample(
-          route, (event.logMonoTime - start_time) * 1e-9, float(event.carState.vEgo), curvature,
-          bool(event.carState.steeringPressed), model_path, *sent,
-        ))
-  return samples
-
-
-def _percentile(values: np.ndarray, percentile: float, mask: np.ndarray | None = None) -> float:
-  selected = values if mask is None else values[mask]
-  return float(np.percentile(np.abs(selected), percentile)) if len(selected) else math.nan
-
-
-def _route_rate(samples: list[Sample], values: np.ndarray) -> np.ndarray:
-  rate = np.zeros(len(values))
-  for index in range(1, len(values)):
-    dt = samples[index].time - samples[index - 1].time
-    if samples[index].route == samples[index - 1].route and 0.005 <= dt <= 0.2:
-      rate[index] = (values[index] - values[index - 1]) / dt
-  return rate
-
-
-def _c2_response(samples: list[Sample], target: np.ndarray, tau_load: float,
-                 tau_unload: float) -> np.ndarray:
-  effective = np.zeros(len(target))
-  previous_route = None
-  previous_time = 0.0
-  state = 0.0
-  for index, sample in enumerate(samples):
-    if sample.route != previous_route:
-      state = 0.0
-      previous_time = sample.time
-    dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
-    loading = target[index] * state >= 0.0 and abs(target[index]) > abs(state)
-    tau = tau_load if loading else tau_unload
-    state += (1.0 - math.exp(-dt / tau)) * (target[index] - state)
-    effective[index] = state
-    previous_route, previous_time = sample.route, sample.time
-  return effective
-
-
-def _limit_c2_command(samples: list[Sample], target: np.ndarray) -> np.ndarray:
-  """Mirror the CAN-FD Ford curvature acceleration/jerk limiter."""
-  limited = np.zeros(len(target))
-  previous_route = None
-  previous_time = 0.0
-  previous = 0.0
-  for index, sample in enumerate(samples):
-    if sample.route != previous_route:
-      previous = 0.0
-      previous_time = sample.time
-    dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
-    speed = max(sample.speed, 1.0)
-    value = float(np.clip(target[index], -MAX_LATERAL_ACCEL / speed ** 2,
-                          MAX_LATERAL_ACCEL / speed ** 2))
-    step = MAX_LATERAL_JERK / speed ** 2 * dt
-    value = float(np.clip(value, previous - step, previous + step))
-    limited[index] = float(np.clip(value, *DBC_CURVATURE))
-    previous = limited[index]
-    previous_route, previous_time = sample.route, sample.time
-  return limited
-
-
-def _limit_fast_fields(samples: list[Sample], c0_target: np.ndarray,
-                       c1_target: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
-  c0 = np.zeros(len(samples))
-  c1 = np.zeros(len(samples))
-  previous_route = None
-  previous_time = 0.0
-  previous_c0 = previous_c1 = 0.0
-  for index, sample in enumerate(samples):
-    if sample.route != previous_route:
-      previous_c0 = previous_c1 = 0.0
-      previous_time = sample.time
-    dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
-    c0[index] = np.clip(c0_target[index], previous_c0 - 4.0 * dt, previous_c0 + 4.0 * dt)
-    c1[index] = np.clip(c1_target[index], previous_c1 - 1.0 * dt, previous_c1 + 1.0 * dt)
-    previous_c0, previous_c1 = c0[index], c1[index]
-    previous_route, previous_time = sample.route, sample.time
-  return c0, c1
-
-
-def evaluate(samples: list[Sample], *, delay: float, horizon: float,
-             tau_load: float, tau_unload: float, horizon_time: float = 0.0,
-             assumed_tau_load: float | None = None, assumed_tau_unload: float | None = None,
-             use_c3: bool = True, c2_limit: float = DBC_CURVATURE[1]) -> dict[str, float]:
-  horizons = np.asarray([float(np.clip(sample.speed * horizon_time, 1.0, horizon))
-                         if horizon_time > 0.0 else horizon for sample in samples])
-  commands = [fit_native_path(sample.path, sample.speed, sample.curvature,
-                              delay=delay, horizon=sample_horizon)
-              for sample, sample_horizon in zip(samples, horizons, strict=True)]
-  c0 = np.asarray([command.c0 for command in commands])
-  c1 = np.asarray([command.c1 for command in commands])
-  raw_c2 = np.asarray([command.c2 for command in commands])
-  c2 = np.clip(raw_c2, -c2_limit, c2_limit)
-  c3 = np.asarray([command.c3 for command in commands])
-  fit_rmse = np.asarray([command.fit_rmse for command in commands])
-  path_rms = np.asarray([command.path_rms for command in commands])
-  transmitted_c2 = _limit_c2_command(samples, c2)
-  effective_c2 = _c2_response(samples, transmitted_c2, tau_load, tau_unload)
-  estimated_c2 = _c2_response(samples, transmitted_c2,
-                              tau_load if assumed_tau_load is None else assumed_tau_load,
-                              tau_unload if assumed_tau_unload is None else assumed_tau_unload)
-  compensated = [fit_c2_aware_path(sample.path, sample.speed, sample.curvature,
-                                   delay=delay, horizon=sample_horizon, target_c2=target,
-                                   effective_c2=estimated, use_c3=use_c3)
-                 for sample, sample_horizon, target, estimated in
-                 zip(samples, horizons, c2, estimated_c2, strict=True)]
-  compensated_c0 = np.asarray([command.c0 for command in compensated])
-  compensated_c1 = np.asarray([command.c1 for command in compensated])
-  compensated_c3 = np.asarray([command.c3 for command in compensated])
-  limited_c0, limited_c1 = _limit_fast_fields(samples, compensated_c0, compensated_c1)
-  estimated_compensated_rmse = np.asarray([command.fit_rmse for command in compensated])
-  compensated_rmse = []
-  for sample, sample_horizon, command, effective in zip(samples, horizons, compensated, effective_c2, strict=True):
-    points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
-    compensated_rmse.append(0.0 if points is None else _wire_rmse(
-      (command.c0, command.c1, effective, command.c3), *points))
-  compensated_rmse = np.asarray(compensated_rmse)
-  limited_compensated_rmse = []
-  for sample, sample_horizon, c0_value, c1_value, c3_value, effective in \
-      zip(samples, horizons, limited_c0, limited_c1, compensated_c3, effective_c2, strict=True):
-    points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
-    limited_compensated_rmse.append(0.0 if points is None else _wire_rmse(
-      (c0_value, c1_value, effective, c3_value), *points))
-  limited_compensated_rmse = np.asarray(limited_compensated_rmse)
-  missing_c2 = transmitted_c2 - effective_c2
-  # Compare channels by their lateral contribution at the fit horizon. This
-  # includes C3: treating it as zero would incorrectly blame C0/C1 for a
-  # curvature transition the native polynomial assigns to curvature rate.
-  fast = (2.0 * compensated_c0 / horizons ** 2 +
-          2.0 * np.tan(compensated_c1) / horizons +
-          compensated_c3 * horizons / 3.0)
-  lagging = np.abs(missing_c2) > 0.0005
-  unloading = lagging & (np.abs(c2) < 0.75 * np.abs(effective_c2))
-  pressed = np.asarray([sample.steering_pressed for sample in samples])
-  speed = np.asarray([sample.speed for sample in samples])
-  sent_c2 = np.asarray([sample.sent_c2 for sample in samples])
-  sent_transmitted_c2 = _limit_c2_command(samples, sent_c2)
-  sent_effective_c2 = _c2_response(samples, sent_transmitted_c2, tau_load, tau_unload)
-  sent_lpf_rmse = []
-  for sample, sample_horizon, effective in zip(samples, horizons, sent_effective_c2, strict=True):
-    points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
-    sent_lpf_rmse.append(0.0 if points is None else _wire_rmse(
-      (sample.sent_c0, sample.sent_c1, effective, sample.sent_c3), *points))
-  sent_lpf_rmse = np.asarray(sent_lpf_rmse)
-  raw_c2_rate = _route_rate(samples, c2)
-  c2_rate = _route_rate(samples, transmitted_c2)
-  sent_c2_rate = _route_rate(samples, sent_c2)
-  compensated_c0_rate = _route_rate(samples, compensated_c0)
-  compensated_c1_rate = _route_rate(samples, compensated_c1)
-  compensated_c3_rate = _route_rate(samples, compensated_c3)
-  normalized_fit = np.divide(fit_rmse, path_rms, out=np.zeros_like(fit_rmse), where=path_rms > 1e-4)
-  return {
-    "samples": float(len(samples)),
-    "delay": delay,
-    "horizon": horizon,
-    "horizon_time": horizon_time,
-    "assumed_tau_load": tau_load if assumed_tau_load is None else assumed_tau_load,
-    "assumed_tau_unload": tau_unload if assumed_tau_unload is None else assumed_tau_unload,
-    "use_c3": float(use_c3),
-    "c2_limit": c2_limit,
-    "actual_horizon_p50": _percentile(horizons, 50),
-    "actual_horizon_p95": _percentile(horizons, 95),
-    "fit_rmse_p50": _percentile(fit_rmse, 50),
-    "fit_rmse_p95": _percentile(fit_rmse, 95),
-    "normalized_fit_p95": _percentile(normalized_fit, 95),
-    "c2_aware_rmse_p50": _percentile(compensated_rmse, 50),
-    "c2_aware_rmse_p95": _percentile(compensated_rmse, 95),
-    "c2_aware_estimated_rmse_p95": _percentile(estimated_compensated_rmse, 95),
-    "c2_aware_limited_rmse_p95": _percentile(limited_compensated_rmse, 95),
-    "sent_lpf_rmse_p50": _percentile(sent_lpf_rmse, 50),
-    "sent_lpf_rmse_p95": _percentile(sent_lpf_rmse, 95),
-    "c0_p95": _percentile(c0, 95),
-    "c1_p95": _percentile(c1, 95),
-    "c2_p95": _percentile(c2, 95),
-    "c3_p95": _percentile(c3, 95),
-    "c0_clip_rate": float(np.mean((c0 <= DBC_OFFSET[0]) | (c0 >= DBC_OFFSET[1]))),
-    "c1_clip_rate": float(np.mean((c1 <= DBC_ANGLE[0]) | (c1 >= DBC_ANGLE[1]))),
-    "c2_clip_rate": float(np.mean((c2 <= DBC_CURVATURE[0]) | (c2 >= DBC_CURVATURE[1]))),
-    "c3_clip_rate": float(np.mean((c3 <= DBC_CURVATURE_RATE[0]) | (c3 >= DBC_CURVATURE_RATE[1]))),
-    "c2_aware_c0_p95": _percentile(compensated_c0, 95),
-    "c2_aware_c1_p95": _percentile(compensated_c1, 95),
-    "c2_aware_c3_p95": _percentile(compensated_c3, 95),
-    "c2_aware_c0_rate_p95": _percentile(compensated_c0_rate, 95),
-    "c2_aware_c1_rate_p95": _percentile(compensated_c1_rate, 95),
-    "c2_aware_c0_rate_limit_rate": float(np.mean(np.abs(compensated_c0_rate) > 4.0)),
-    "c2_aware_c1_rate_limit_rate": float(np.mean(np.abs(compensated_c1_rate) > 1.0)),
-    "c2_aware_c3_rate_p95": _percentile(compensated_c3_rate, 95),
-    "raw_c2_rate_p95": _percentile(raw_c2_rate, 95),
-    "c2_rate_p95": _percentile(c2_rate, 95),
-    "sent_c2_rate_p95": _percentile(sent_c2_rate, 95),
-    "c2_lag_p95": _percentile(missing_c2, 95),
-    "lag_samples": float(np.count_nonzero(lagging)),
-    "lag_fast_support_rate": float(np.mean(fast[lagging] * missing_c2[lagging] > 0.0)) if np.any(lagging) else math.nan,
-    "lag_fast_coverage_p50": _percentile(np.divide(fast, missing_c2, out=np.zeros_like(fast),
-                                                    where=np.abs(missing_c2) > 1e-6), 50, lagging),
-    "unload_samples": float(np.count_nonzero(unloading)),
-    "unload_fast_counter_rate": float(np.mean(fast[unloading] * effective_c2[unloading] < 0.0)) if np.any(unloading) else math.nan,
-    "unload_residual_c2_p95": _percentile(effective_c2 - c2, 95, unloading),
-    "pressed_c0_p95": _percentile(c0, 95, pressed),
-    "pressed_c1_p95": _percentile(c1, 95, pressed),
-    "low_speed_fit_p95": _percentile(fit_rmse, 95, speed < 5.0),
-    "road_speed_fit_p95": _percentile(fit_rmse, 95, speed >= 15.0),
-  }
-
-
-def _expand(patterns: list[str]) -> list[str]:
-  return sorted({path for pattern in patterns for path in glob.glob(pattern)})
-
-
-def _self_test() -> None:
-  distance = np.linspace(0.0, 20.0, 81)
-  coefficients = (0.2, 0.03, 0.004, -0.00005)
-  y = sum(coefficient * distance ** power for power, coefficient in enumerate(coefficients))
-  slope = coefficients[1] + 2.0 * coefficients[2] * distance + 3.0 * coefficients[3] * distance ** 2
-  heading = np.arctan(slope)
-  path = ModelPath(distance, y, heading, np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(distance), np.diff(y))))))
-  command = fit_native_path(path, 0.0, 0.0, delay=0.1, horizon=7.0)
-  assert abs(command.c0 - coefficients[0]) < 2e-3
-  assert abs(command.c1 - math.atan(coefficients[1])) < 2e-3
-  expected_c2 = 2.0 * coefficients[2] / (1.0 + coefficients[1] ** 2) ** 1.5
-  assert abs(command.c2 - expected_c2) < 2e-4
-  assert command.fit_rmse < 1e-4
-
-
-def main() -> int:
-  parser = argparse.ArgumentParser(description=__doc__)
-  parser.add_argument("--logs", action="append", help="rlog glob", default=[])
-  parser.add_argument("--delay", type=float, default=0.1)
-  parser.add_argument("--horizon", type=float, action="append")
-  parser.add_argument("--time-horizon", type=float, default=0.0,
-                      help="if nonzero, use clamp(speed * seconds, 1 m, --horizon)")
-  parser.add_argument("--tau-load", type=float, default=0.75)
-  parser.add_argument("--tau-unload", type=float, default=1.3)
-  parser.add_argument("--assumed-tau-load", type=float)
-  parser.add_argument("--assumed-tau-unload", type=float)
-  parser.add_argument("--zero-c3", action="store_true")
-  parser.add_argument("--c2-limit", type=float, action="append",
-                      help="C2 cap to test; defaults to gentle 0.006 and full 0.02")
-  parser.add_argument("--self-test", action="store_true")
-  args = parser.parse_args()
-  if args.self_test:
-    _self_test()
-  paths = _expand(args.logs)
-  if not paths:
-    if args.self_test:
-      return 0
-    parser.error("at least one usable --logs glob is required")
-  samples = load_samples(paths)
-  if not samples:
-    parser.error("logs contain no active Ford path samples")
-  print(f"loaded_logs={len(paths)} samples={len(samples)} tau_load={args.tau_load} tau_unload={args.tau_unload}")
-  for horizon in args.horizon or [3.5, 5.0, 7.0, 10.0]:
-    for c2_limit in args.c2_limit or [0.006, DBC_CURVATURE[1]]:
-      result = evaluate(samples, delay=args.delay, horizon=horizon,
-                        tau_load=args.tau_load, tau_unload=args.tau_unload,
-                        horizon_time=args.time_horizon,
-                        assumed_tau_load=args.assumed_tau_load,
-                        assumed_tau_unload=args.assumed_tau_unload,
-                        use_c3=not args.zero_c3,
-                        c2_limit=c2_limit)
-      print(" ".join(f"{key}={value:.8g}" for key, value in result.items()))
-  return 0
-
-
-if __name__ == "__main__":
-  raise SystemExit(main())