Merge remote-tracking branch 'sunnypilot/sunnypilot/master' into hkg-angle-steering-2025

* models: self healing test for tg

* rm container

* ew

* set env

* set var

.codespellignore

@@ -2,5 +2,6 @@ Wen
 REGIST
 PullRequest
 cancelled
+indeces
 FOF
 NoO

.gitattributes

@@ -3,7 +3,7 @@
 # to move existing files into LFS:
 # git add --renormalize .
 *.onnx filter=lfs diff=lfs merge=lfs -text
-openpilot/selfdrive/modeld/models/big_driving_tinygrad.pkl filter=lfs diff=lfs merge=lfs -text
+openpilot/selfdrive/modeld/models/big_*_tinygrad.pkl filter=lfs diff=lfs merge=lfs -text
 openpilot/sunnypilot/modeld_v2/models/*.pkl filter=lfs diff=lfs merge=lfs -text
 *.svg filter=lfs diff=lfs merge=lfs -text
 *.png filter=lfs diff=lfs merge=lfs -text

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

@@ -115,7 +115,7 @@ jobs:
           env ${TG_FLAGS} python3 ${{ github.workspace }}/${{ env.COMPILER }}/compile_onnx.py \
             ${{ github.workspace }}/${{ env.SMALL_ONNX }} \
             ${{ github.workspace }}/${{ env.SMALL_PKL }} \
-            --device-input "*" --out-of-band --benchmark-runs 1
+            --out-of-band --benchmark-runs 1
 
       - name: Prepare output
         env:
@@ -440,9 +440,10 @@ jobs:
           TG_FLAGS="DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
 
           taskset -c 7 env ${TG_FLAGS} python3 \
-            ${{ github.workspace }}/tinygrad_repo/examples/openpilot/compile3.py \
+            ${{ github.workspace }}/tinygrad_repo/examples/openpilot/compile_onnx.py \
             ${{ github.workspace }}/${{ env.DM_ONNX }} \
-            ${{ github.workspace }}/${{ env.DM_PKL }}
+            ${{ github.workspace }}/${{ env.DM_PKL }} \
+            --out-of-band --benchmark-runs 1
 
       - name: Compile DM warp
         run: |

.github/workflows/compile-warp-kernels.yaml

@@ -58,6 +58,11 @@ jobs:
 
       - name: Create Pull Request
         uses: peter-evans/create-pull-request@9153d834b60caba6d51c9b9510b087acf9f33f83
+        env:
+          HF_TOKEN: ${{ secrets.HF_TOKEN }}
+          GIT_CONFIG_COUNT: 1
+          GIT_CONFIG_KEY_0: credential.https://huggingface.co.helper
+          GIT_CONFIG_VALUE_0: '!f() { test "$1" = get && echo username=user && echo "password=$HF_TOKEN"; }; f'
         with:
           author: github-actions[bot] <github-actions[bot]@users.noreply.github.com>
           token: ${{ secrets.GITHUB_TOKEN }}
@@ -69,3 +74,7 @@ jobs:
           labels: bot
           add-paths: |
             openpilot/sunnypilot/modeld_v2/models/*.pkl
+
+      - name: Reset lfs access
+        if: always()
+        run: git config --local --unset-all "lfs.$(git config -f .lfsconfig lfs.url).access" || true

.github/workflows/lfs-maintenance.yaml

@@ -33,7 +33,7 @@ jobs:
 
       - name: LFS Fetch
         run: |
-          git lfs fetch
+          git lfs fetch || echo "::warning::some objects are missing on commaai/openpilot-lfs; mirroring the rest"
 
       - name: Set up Git
         run: |
@@ -68,6 +68,7 @@ jobs:
           git config lfs.activitytimeout 300
           git config lfs.transfer.maxretries 5
           git config lfs.concurrenttransfers 4
+          git config lfs.allowincompletepush true
 
       - name: Push LFS
         id: sync-and-commit
@@ -81,3 +82,80 @@ jobs:
           done
           echo "All push attempts failed"
           exit 1
+
+      - name: Checkout sunnypilot
+        uses: actions/checkout@v4
+        with:
+          repository: 'sunnypilot/sunnypilot'
+          path: sunnypilot
+          fetch-depth: 0
+          filter: blob:limit=1k
+          lfs: false
+
+      - name: Reference objects on sunnypilot-lfs
+        env:
+          HF_TOKEN: ${{ secrets.HF_TOKEN }}
+        run: |
+          set -euo pipefail
+          GIT_LFS_SKIP_SMUDGE=1 git clone --quiet https://huggingface.co/sunnypilot/sunnypilot-lfs hf-lfs
+          mkdir -p hf-lfs/all
+          cat > scan_pointers.py <<'EOF'
+          import re, subprocess, sys, threading
+          repo = sys.argv[1]
+          chk = subprocess.run(["git", "-C", repo, "cat-file", "--batch-all-objects",
+                                "--batch-check=%(objectname) %(objecttype) %(objectsize)"],
+                               capture_output=True, text=True, check=True).stdout
+          small = [f[0] for f in (l.split() for l in chk.splitlines()) if f[1] == "blob" and int(f[2]) < 1024]
+          p = subprocess.Popen(["git", "-C", repo, "cat-file", "--batch=%(objectsize)"],
+                               stdin=subprocess.PIPE, stdout=subprocess.PIPE)
+          def feed():
+            for sha in small:
+              p.stdin.write(sha.encode() + b"\n")
+            p.stdin.close()
+          threading.Thread(target=feed, daemon=True).start()
+          rx = re.compile(rb"^version https://git-lfs\.github\.com/spec/v1\noid sha256:([0-9a-f]{64})\nsize ([0-9]+)\n")
+          seen = {}
+          for _ in small:
+            n = int(p.stdout.readline()); body = p.stdout.read(n); p.stdout.read(1)
+            m = rx.match(body)
+            if m:
+              seen.setdefault(m.group(1).decode(), m.group(2).decode())
+          for oid, size in sorted(seen.items()):
+            print(oid, size)
+          EOF
+          for repo in . sunnypilot; do
+            python3 scan_pointers.py "$repo"
+          done | sort -u | while read -r oid size; do
+            [ -e "hf-lfs/all/$oid" ] && continue
+            printf 'version https://git-lfs.github.com/spec/v1\noid sha256:%s\nsize %s\n' "$oid" "$size" > "hf-lfs/all/$oid"
+            echo "$oid"
+          done > new_oids.txt
+          echo "new pointer files: $(wc -l < new_oids.txt)"
+
+          dropped=0
+          while mapfile -t -n 100 chunk && ((${#chunk[@]})); do
+            body=$(for oid in "${chunk[@]}"; do
+                     size=$(awk '/^size /{print $2}' "hf-lfs/all/$oid")
+                     printf '{"oid":"%s","size":%s}\n' "$oid" "$size"
+                   done | jq -s '{operation:"download",transfers:["basic"],objects:.}')
+            for attempt in 1 2 3 4 5; do
+              resp=$(curl -sS -u "user:${HF_TOKEN}" -w '\n%{http_code}' \
+                       -H 'Accept: application/vnd.git-lfs+json' -H 'Content-Type: application/vnd.git-lfs+json' \
+                       --data "$body" "${LFS_URL}/objects/batch") && [ "$(tail -n1 <<<"$resp")" = 200 ] && break
+              echo "batch check attempt $attempt failed ($(tail -n1 <<<"$resp")), retrying in 60s"; sleep 60
+            done
+            [ "$(tail -n1 <<<"$resp")" = 200 ] || { echo "batch check failed"; exit 1; }
+            for oid in $(sed '$d' <<<"$resp" | jq -r '.objects[] | select(.error) | .oid'); do
+              rm -f "hf-lfs/all/$oid"; dropped=$((dropped+1))
+            done
+          done < new_oids.txt
+          echo "dropped (not on HF): $dropped"
+
+          cd hf-lfs
+          git add -A all
+          if git diff --cached --quiet; then
+            echo "nothing new to reference"
+            exit 0
+          fi
+          git commit -q -m "Reference $(git diff --cached --name-only | wc -l | tr -d ' ') new LFS objects"
+          git push origin HEAD:main

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

@@ -594,8 +594,14 @@ jobs:
       - name: Publish chestnut branch
         env:
           GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+          HF_TOKEN: ${{ secrets.HF_TOKEN }}
         run: |
           CHESTNUT_BRANCH="${{ needs.prepare_strategy.outputs.new_branch }}-chestnut"
+          cp .lfsconfig "${{ env.OUTPUT_DIR }}/"
+          echo 'openpilot/selfdrive/modeld/models/big_*_tinygrad.pkl filter=lfs diff=lfs merge=lfs -text' > "${{ env.OUTPUT_DIR }}/.gitattributes"
+          git lfs install
+          git config --global credential.helper store
+          echo "https://user:${HF_TOKEN}@huggingface.co" > ~/.git-credentials
 
           ${{ env.CI_DIR }}/publish.sh \
             "${{ github.workspace }}" \

.github/workflows/test_models.yml

@@ -3,9 +3,62 @@ on:
   pull_request:
     paths:
       - 'tinygrad_repo'
+      - 'openpilot/sunnypilot/modeld_v2/**'
   workflow_dispatch:
 
+env:
+  CI: 1
+  GIT_CONFIG_COUNT: 1
+  GIT_CONFIG_KEY_0: lfs.fetchexclude
+  GIT_CONFIG_VALUE_0: openpilot/selfdrive/modeld/models/big_driving_tinygrad.pkl
+
 jobs:
+  plain-artifact:
+    name: Plain artifact round trip and pickle converter
+    runs-on: ubuntu-24.04
+    env:
+      PYTHONPATH: ${{ github.workspace }}/tinygrad_repo:${{ github.workspace }}
+      TINYGRAD_TESTS: 1
+    steps:
+      - uses: actions/checkout@v7
+      - run: ./tools/op.sh setup
+      - name: Build openpilot
+        run: scons -j$(nproc)
+      - name: Compile, load, cache and fuse a model with the pinned tinygrad
+        env:
+          DEV: CPU
+        run: tools/op.sh test -v openpilot/sunnypilot/modeld_v2/tests/test_plain_artifact.py
+      - name: Install LLVM 20 and ROCm comgr for the amd mock, as tinygrad_repo/.github/actions/setup-tinygrad does
+        run: |
+          . /etc/os-release
+          curl -fsSL https://apt.llvm.org/llvm-snapshot.gpg.key | sudo tee /etc/apt/trusted.gpg.d/apt.llvm.org.asc > /dev/null
+          echo "deb http://apt.llvm.org/$VERSION_CODENAME/ llvm-toolchain-$VERSION_CODENAME-20 main" | sudo tee /etc/apt/sources.list.d/llvm.list
+          sudo mkdir -p /etc/apt/keyrings
+          curl -fsSL https://repo.radeon.com/rocm/rocm.gpg.key | gpg --dearmor | sudo tee /etc/apt/keyrings/rocm.gpg > /dev/null
+          echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/rocm.gpg] https://repo.radeon.com/rocm/apt/7.1 $VERSION_CODENAME main" | sudo tee /etc/apt/sources.list.d/rocm.list
+          printf 'Package: *\nPin: release o=repo.radeon.com\nPin-Priority: 600\n' | sudo tee /etc/apt/preferences.d/rocm-pin-600
+          sudo apt-get -qq update && sudo apt-get -qq install -y --no-install-recommends libllvm20 clang-20 lld-20 comgr
+          printf '/opt/rocm/lib\n/opt/rocm/lib64\n' | sudo tee /etc/ld.so.conf.d/rocm.conf && sudo ldconfig
+          echo "/usr/lib/llvm-20/bin" >> "$GITHUB_PATH"
+      - name: Convert a pickled USB+AMD jit and replay it under tinygrad's amd mock
+        env:
+          DEV: MOCKUSB+AMD
+          CHECK_OOB: 0
+          GMMU: 0
+        run: tools/op.sh test -v openpilot/sunnypilot/modeld_v2/tests/test_convert_upstream_pkl.py
+      - name: Where tinygrad moved, for the helper or failure point named above
+        if: failure() && github.event_name == 'pull_request'
+        run: |
+          git fetch -q --depth=1 origin "${{ github.base_ref }}"
+          old=$(git rev-parse "origin/${{ github.base_ref }}:tinygrad_repo") new=$(git rev-parse HEAD:tinygrad_repo)
+          git -C tinygrad_repo fetch -q origin "$old" || true
+          echo "tinygrad_repo $old -> $new, in what helpers.py and plain_artifact_compiler.py read:"
+          git -C tinygrad_repo diff "$old" "$new" -- tinygrad/uop/ops.py tinygrad/engine/jit.py tinygrad/engine/realize.py tinygrad/device.py \
+            tinygrad/helpers.py tinygrad/dtype.py tinygrad/renderer/__init__.py
+          echo "and in what convert_upstream_pkl.py decodes:"
+          git -C tinygrad_repo diff --stat "$old" "$new" -- tinygrad/runtime/autogen/am/pm4_nv.py tinygrad/runtime/autogen/amd_gpu.py
+          git -C tinygrad_repo diff "$old" "$new" -- tinygrad/runtime/support/hcq2.py tinygrad/runtime/support/usb.py tinygrad/runtime/ops_amd.py
+
   generate-matrix:
     runs-on: ubuntu-latest
     outputs:

.github/workflows/tests.yaml

@@ -131,6 +131,7 @@ jobs:
 
   process_replay:
     name: process replay
+    if: false #process replay disabled for angle steering until we are ready to merge it
     runs-on: ${{
       (github.repository == 'commaai/openpilot') &&
       ((github.event_name != 'pull_request') ||

.gitmodules

@@ -4,7 +4,6 @@
 [submodule "opendbc"]
   path = opendbc_repo
   url = https://github.com/sunnypilot/opendbc.git
-  branch = tn
 [submodule "msgq"]
   path = msgq_repo
   url = https://github.com/sunnypilot/msgq.git

.idea/customTargets.xml

@@ -21,5 +21,12 @@
         </clean>
       </configuration>
     </target>
+    <target id="f2590b2b-9b93-49f9-8510-da3f3724a2ae" name="replay" defaultType="TOOL">
+      <configuration id="d475264f-6f4c-4092-9b4e-6773309f38b7" name="replay" toolchainName="Default">
+        <build type="TOOL">
+          <tool actionId="Tool_External Tools_uv build tools replay" />
+        </build>
+      </configuration>
+    </target>
   </component>
 </project>

.idea/tools/External Tools.xml

@@ -20,4 +20,11 @@
       <option name="WORKING_DIRECTORY" value="$ProjectFileDir$" />
     </exec>
   </tool>
+  <tool name="uv build tools replay" showInMainMenu="false" showInEditor="false" showInProject="false" showInSearchPopup="false" disabled="false" useConsole="true" showConsoleOnStdOut="false" showConsoleOnStdErr="false" synchronizeAfterRun="true">
+    <exec>
+      <option name="COMMAND" value="bash" />
+      <option name="PARAMETERS" value="-c &quot;source .venv/bin/activate &amp;&amp; scons -u -j$(nproc) tools/replay/&quot;" />
+      <option name="WORKING_DIRECTORY" value="$ProjectFileDir$" />
+    </exec>
+  </tool>
 </toolSet>

.run/Build Debug.run.xml

@@ -1,5 +1,5 @@
 <component name="ProjectRunConfigurationManager">
-  <configuration default="false" name="Build Debug" type="CLionExternalRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" WORKING_DIR="file://$ProjectFileDir$/selfdrive/ui" PASS_PARENT_ENVS_2="true" PROJECT_NAME="sunnypilot" TARGET_NAME="uv Scons Build Debug" CONFIG_NAME="uv Scons Build Debug" RUN_PATH="ui">
+  <configuration default="false" name="Build Debug" type="CLionExternalRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" WORKING_DIR="file://$ProjectFileDir$/selfdrive/ui" PASS_PARENT_ENVS_2="true" PROJECT_NAME="openpilot-special" TARGET_NAME="uv Scons Build Debug" CONFIG_NAME="uv Scons Build Debug" RUN_PATH="ui">
     <envs>
       <env name="QT_DBL_CLICK_DIST" value="150" />
     </envs>

.run/Debug Route Controls.run.xml

@@ -0,0 +1,27 @@
+<component name="ProjectRunConfigurationManager">
+  <configuration default="false" name="Debug Route Controls" type="PythonConfigurationType" factoryName="Python">
+    <module name="openpilot-special" />
+    <option name="ENV_FILES" value="" />
+    <option name="INTERPRETER_OPTIONS" value="" />
+    <option name="PARENT_ENVS" value="true" />
+    <envs>
+      <env name="PYTHONUNBUFFERED" value="1" />
+      <env name="FINGERPRINT" value="KIA_EV9" />
+      <env name="SKIP_FW_QUERY" value="1" />
+    </envs>
+    <option name="SDK_HOME" value="" />
+    <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/selfdrive/car" />
+    <option name="IS_MODULE_SDK" value="true" />
+    <option name="ADD_CONTENT_ROOTS" value="true" />
+    <option name="ADD_SOURCE_ROOTS" value="true" />
+    <EXTENSION ID="PythonCoverageRunConfigurationExtension" runner="coverage.py" />
+    <option name="SCRIPT_NAME" value="$PROJECT_DIR$/selfdrive/car/card.py" />
+    <option name="PARAMETERS" value="" />
+    <option name="SHOW_COMMAND_LINE" value="false" />
+    <option name="EMULATE_TERMINAL" value="true" />
+    <option name="MODULE_MODE" value="false" />
+    <option name="REDIRECT_INPUT" value="false" />
+    <option name="INPUT_FILE" value="" />
+    <method v="2" />
+  </configuration>
+</component>

.run/Replay for controls + ui.run.xml

@@ -0,0 +1,7 @@
+<component name="ProjectRunConfigurationManager">
+  <configuration default="false" name="Replay for controls + ui" type="Multirun" separateTabs="false" reuseTabsWithFailures="false" startOneByOne="true" markFailedProcess="true" hideSuccessProcess="false" delayTime="0.0">
+    <runConfiguration name="replay for controls" type="Native Application" />
+    <runConfiguration name="Build Debug" type="Custom Build Application" />
+    <method v="2" />
+  </configuration>
+</component>

.run/replay for controls.run.xml

@@ -0,0 +1,7 @@
+<component name="ProjectRunConfigurationManager">
+  <configuration default="false" name="replay for controls" type="CLionNativeAppRunConfigurationType" focusToolWindowBeforeRun="true" PROGRAM_PARAMS="&quot;$Prompt$&quot; --block &quot;sendcan,carState,carParams,carOutput,liveTracks,carParamsSP,carStateSP,bookmarkButton&quot;" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="true" WORKING_DIR="file://$ProjectFileDir$/tools/replay" PASS_PARENT_ENVS_2="true" PROJECT_NAME="openpilot-special" TARGET_NAME="replay" CONFIG_NAME="replay" version="1" RUN_PATH="replay">
+    <method v="2">
+      <option name="CLION.COMPOUND.BUILD" enabled="true" />
+    </method>
+  </configuration>
+</component>

Jenkinsfile

@@ -161,6 +161,14 @@ def step(String name, String cmd, Map args = [:]) {
   return [name, cmd, args]
 }
 
+def chestnutStage(String name = "chestnut") {
+  deviceStage(name, "mici-chestnut-ci", ["UNSAFE=1", "CHESTNUT=1"], [
+    step("build", "./openpilot/selfdrive/test/chestnut.sh"),
+    step("model replay", "openpilot/selfdrive/test/process_replay/model_replay.py --chestnut"),
+    step("onroad tests", "./openpilot/selfdrive/test/test_onroad.py TestChestnutOnroad", [timeout: 120]),
+  ])
+}
+
 node {
   env.CI = "1"
   env.PYTHONWARNINGS = "error"
@@ -171,6 +179,36 @@ node {
   env.GIT_BRANCH = checkout(scm).GIT_BRANCH
   env.GIT_COMMIT = checkout(scm).GIT_COMMIT
 
+  if (env.JOB_BASE_NAME == 'chestnut-stresstest') {
+    sh 'git fetch --no-tags origin nightly-chestnut'
+    env.GIT_COMMIT = sh(script: 'git show FETCH_HEAD:git_src_commit', returnStdout: true).trim()
+    if (!(env.GIT_COMMIT ==~ /[0-9a-f]{40}/)) {
+      error("invalid nightly-chestnut source commit")
+    }
+    def results = []
+    try {
+      timeout(time: 12, unit: 'HOURS') {
+        for (int run = 1; run <= 100; run++) {
+          results << "run ${run}: INCOMPLETE"
+          try {
+            chestnutStage("chestnut ${run}/100")
+            results[run - 1] = "run ${run}: PASS"
+          } catch (hudson.AbortException e) {
+            results[run - 1] = "run ${run}: FAIL"
+            echo e.toString()
+          }
+        }
+      }
+    } finally {
+      writeFile file: 'chestnut_stress_report.txt', text: "commit: ${env.GIT_COMMIT}\ncompleted: ${results.count { !it.endsWith('INCOMPLETE') }}/100\n" + results.join('\n') + '\n'
+      archiveArtifacts artifacts: 'chestnut_stress_report.txt'
+    }
+    if (results.any { !it.endsWith('PASS') }) {
+      error("chestnut stress test failed")
+    }
+    return
+  }
+
   def excludeBranches = ['__nightly', '__nightly-chestnut', 'devel', 'devel-staging',
                          'release-tizi', 'release-tizi-staging', 'release-mici', 'release-mici-staging', 'testing-closet*', 'hotfix-*']
   def excludeRegex = excludeBranches.join('|').replaceAll('\\*', '.*')
@@ -204,9 +242,18 @@ node {
     }
 
     if (env.BRANCH_NAME == '__nightly-chestnut') {
-      deviceStage("build nightly-chestnut", "mici-chestnut-ci", [], [
-        step("build nightly-chestnut", "SCONSFLAGS=-j4 INCLUDE_BIG_MODEL=1 PANDA_DEBUG_BUILD=1 RELEASE_BRANCH=nightly-chestnut $SOURCE_DIR/tools/release/build_release.sh TestChestnutOnroad"),
-      ])
+      parallel (
+        'nightly-chestnut': {
+          deviceStage("build nightly-chestnut", "mici-chestnut-ci", ["CHESTNUT=1"], [
+            step("build nightly-chestnut", "SCONSFLAGS=-j4 INCLUDE_BIG_MODEL=1 RELEASE_BRANCH=nightly-chestnut $SOURCE_DIR/tools/release/build_release.sh TestChestnutOnroad"),
+          ])
+        },
+        'nightly-chestnut-dev': {
+          deviceStage("build nightly-chestnut-dev", "mici-chestnut-ci", ["CHESTNUT=1"], [
+            step("build nightly-chestnut-dev", "SCONSFLAGS=-j4 INCLUDE_BIG_MODEL=1 PANDA_DEBUG_BUILD=1 RELEASE_BRANCH=nightly-chestnut-dev $SOURCE_DIR/tools/release/build_release.sh TestChestnutOnroad"),
+          ])
+        },
+      )
     }
 
     if (!env.BRANCH_NAME.matches(excludeRegex)) {
@@ -261,12 +308,7 @@ node {
         ])
       },
       'chestnut': {
-        deviceStage("chestnut", "mici-chestnut-ci", ["UNSAFE=1", "CHESTNUT=1"], [
-          step("build", "./openpilot/selfdrive/test/chestnut.sh"),
-          step("model replay", "openpilot/selfdrive/test/process_replay/model_replay.py --chestnut"),
-          step("onroad tests", "./openpilot/selfdrive/test/test_onroad.py TestChestnutOnroad", [timeout: 120]),
-          step("test power draw", "./openpilot/selfdrive/test/test_power_draw.py"),
-        ])
+        chestnutStage()
       },
 
     )

RELEASES.md

@@ -1,4 +1,14 @@
-Version 0.11.2 (2026-08-12)
+Version 0.11.3 (2026-XX-XX)
+========================
+* Autospeed
+  * On supported cars, openpilot will intelligently decide what speed to drive based on the scene, including:
+    * stopping at red lights, and going at green lights
+    * slowing down for curves
+    * adjusting speed based on surrounding traffic
+  * This behavior was previously available with openpilot's "Experimental mode", but is now out of beta and always enabled
+  * With openpilot deciding which speed to drive, the car's cruise speed is treated as a maximum speed openpilot will not exceed
+
+Version 0.11.2 (2026-10-14)
 =======================
 * New driving model
   * Big model with 880M parameters

SConstruct

@@ -20,7 +20,7 @@ SetOption('num_jobs', max(1, int(os.cpu_count()/(1 if "CI" in os.environ else 2)
 
 AddOption('--ccflags', action='store', type='string', default='', help='pass arbitrary flags over the command line')
 AddOption('--verbose', action='store_true', default=False, help='show full build commands')
-release = not os.path.exists(File('#.gitattributes').abspath) # file absent on release branch, see release_files.py
+release = not os.path.exists(File('#.gitmodules').abspath) # file absent on release branch, see release_files.py
 AddOption('--minimal',
           action='store_false',
           dest='extras',
@@ -316,8 +316,6 @@ def count_scons_nodes(nodes):
     if node in seen:
       continue
     seen.add(node)
-    if hasattr(node, 'has_builder') and node.has_builder():
-      build_product_nodes.add(node)
     executor = node.get_executor()
     if executor is not None:
       stack += executor.get_all_prerequisites() + executor.get_all_children()
@@ -326,7 +324,6 @@ def count_scons_nodes(nodes):
 
 progress_interval = 5
 progress_count = 0
-build_product_nodes = set()
 progress_total = max(1, count_scons_nodes(env.arg2nodes(BUILD_TARGETS or [Dir('.')], env.fs.Entry)))
 
 def progress_function(node):
@@ -342,13 +339,3 @@ def progress_function(node):
 
 Progress(progress_function, interval=progress_interval)
 AddPostAction(BUILD_TARGETS or [Dir('.')], prune_cache_dir)
-
-def check_build_product_size(target, source, env):
-  limit = 50 * 1024 * 1024  # GitHub max size
-  for t in target:
-    if str(t).endswith('.pkl'):  # chunked during release packaging
-      continue
-    if hasattr(t, 'isfile') and t.isfile() and (size := os.path.getsize(t.abspath)) > limit:
-      raise SCons.Errors.UserError(f"{t} is {size / (1024 * 1024):.1f} MiB, exceeding the {limit / (1024 * 1024):.1f} MiB limit")
-if not GetOption('extras'):
-  AddPostAction(list(build_product_nodes), Action(check_build_product_size, None))

launch_chffrplus.sh

@@ -56,6 +56,8 @@ function launch {
           mv "$DIR" /data/safe_staging/old_openpilot
           mv "${STAGING_ROOT}/finalized" "$DIR"
           cd "$DIR"
+          # releases before the openpilot/ move can't swap AGNOS on newer AGNOS, use ours
+          [ ! -L system/hardware/tici/agnos.json ] && /data/safe_staging/old_openpilot/openpilot/common/hardware/comma/agnos.py --verify system/hardware/tici/agnos.json && sudo reboot
 
           echo "Restarting launch script ${LAUNCHER_LOCATION}"
           unset AGNOS_VERSION

launch_env.sh

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

openpilot/cereal/custom.capnp

@@ -138,6 +138,9 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
     status @0 :DownloadStatus;
     progress @1 :Float32;
     eta @2 :UInt32;
+    speed @3 :Float32;
+    canceling @4 :Bool;
+    resuming @5 :Bool;
   }
 
   struct Chunk {
@@ -203,7 +206,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
-  accelController @8 :AccelController;
 
   deprecated :group {
     dec @0 :DynamicExperimentalControlDEPRECATED;
@@ -213,10 +215,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     state @0 :DynamicExperimentalControlState;
     enabled @1 :Bool;
     active @2 :Bool;
-    decelIntent @3 :Float32;
-    curveDetected @4 :Bool;
-    wantBlended @5 :Bool;
-    leadVeto @6 :Bool;
 
     enum DynamicExperimentalControlState {
       acc @0;
@@ -314,17 +312,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     greenLightAlert @0 :Bool;
     leadDepartAlert @1 :Bool;
   }
-
-  struct AccelController {
-    enabled @0 :Bool;
-    active @1 :Bool;
-    profile @2 :Profile;
-    enum Profile {
-      eco @0;
-      normal @1;
-      sport @2;
-    }
-  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {
@@ -361,7 +348,7 @@ struct OnroadEventSP @0xda96579883444c35 {
     silentParkBrake @11;
     controlsMismatchLateral @12;
     hyundaiRadarTracksConfirmed @13;
-    experimentalModeSwitched @14;
+    experimentalModeSwitchedDEPRECATED @14;
     wrongCarModeAlertOnly @15;
     pedalPressedAlertOnly @16;
     laneTurnLeft @17;
@@ -466,8 +453,6 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
 
 struct CarStateSP @0xb86e6369214c01c8 {
   speedLimit @0 :Float32;
-  engineOff @1 :Bool;
-  engineRpm @2 :Float32;
 }
 
 struct LiveMapDataSP @0xf416ec09499d9d19 {

openpilot/cereal/log.capnp

@@ -128,6 +128,7 @@ struct OnroadEvent @0xc4fa6047f024e718 {
     personalityChanged @91;
     aeb @92;
     userBookmark @95;
+    userBookmarkNotPaired @104;
     excessiveActuation @96;
     bigModelLoading @100;
     bigModelFailed @102;
@@ -166,12 +167,7 @@ struct InitData {
   gitSrcCommit @23 :Text;
   gitSrcCommitDate @24 :Text;
 
-  androidProperties @16 :Map(Text, Text);
-
-  pandaInfo @8 :PandaInfo;
-
   dirty @9 :Bool;
-  passive @12 :Bool;
   params @17 :Map(Text, Data);
 
   commands @19 :Map(Text, Data);
@@ -189,6 +185,17 @@ struct InitData {
     mici @7;  # comma four
   }
 
+  deprecated :group {
+    gctx @1 :Text;
+    androidBuildInfo @5 :Deprecated.AndroidBuildInfo;
+    androidSensors @6 :List(Deprecated.AndroidSensor);
+    chffrAndroidExtra @7 :ChffrAndroidExtra;
+    pandaInfo @8 :PandaInfo;
+    passive @12 :Bool;
+    iosBuildInfo @14 :Deprecated.IosBuildInfo;
+    androidProperties @16 :Map(Text, Text);
+  }
+
   struct PandaInfo {
     hasPanda @0 :Bool;
     dongleId @1 :Text;
@@ -199,14 +206,6 @@ struct InitData {
   struct ChffrAndroidExtra {
     allCameraCharacteristics @0 :Map(Text, Text);
   }
-
-  deprecated :group {
-    gctx @1 :Text;
-    androidBuildInfo @5 :Deprecated.AndroidBuildInfo;
-    androidSensors @6 :List(Deprecated.AndroidSensor);
-    chffrAndroidExtra @7 :ChffrAndroidExtra;
-    iosBuildInfo @14 :Deprecated.IosBuildInfo;
-  }
 }
 
 struct FrameData {
@@ -572,6 +571,9 @@ struct PandaState @0xa7649e2575e4591e {
   controlsAllowedLateral @38 :Bool;
   controlsAllowedLongitudinal @39 :Bool;
 
+  nmiReset @40 :Bool;
+  hardfaultReset @41 :Bool;
+
   enum FaultStatus {
     none @0;
     faultTemp @1;
@@ -694,6 +696,7 @@ struct PeripheralState {
 
 struct UsbState {
   devices @0 :List(Device);
+  connected @1 :Bool;
 
   struct Device {
     busnum @0 :UInt8;
@@ -819,7 +822,6 @@ struct SelfdriveState {
   alertHudVisual @12 :Car.CarControl.HUDControl.VisualAlert;
 
   # configurable driving settings
-  experimentalMode @10 :Bool;
   personality @11 :LongitudinalPersonality;
 
   enum AudibleAlert {
@@ -863,6 +865,7 @@ struct SelfdriveState {
 
   deprecated :group {
     alertSound @8 :Car.CarControl.HUDControl.AudibleAlert;
+    experimentalMode @10 :Bool;
   }
 }
 
@@ -1237,6 +1240,7 @@ struct LongitudinalPlan @0xe00b5b3eba12876c {
   shouldStop @37: Bool;
   allowThrottle @38: Bool;
   allowBrake @39: Bool;
+  accelBoost @40 :Float32;
 
 
   solverExecutionTime @35 :Float32;
@@ -2234,6 +2238,7 @@ struct DriverMonitoringState {
       pose @0: Bool;
       eye @1: Bool;
       phone @2: Bool;
+      sleep @3: Bool;
     }
 
     struct Pose {
@@ -2705,8 +2710,8 @@ struct Event {
     driverMonitoringStateDEPRECATED @71 :DriverMonitoringStateDEPRECATED;
     gpsNMEADEPRECATED @3 :GPSNMEAData;
     uploaderStateDEPRECATED @79 :UploaderState;
-    navInstructionDEPRECATED @82 :NavInstruction;
-    navRouteDEPRECATED @83 :NavRoute;
+    navInstruction @82 :NavInstruction;
+    navRoute @83 :NavRoute;
     navThumbnailDEPRECATED @84 :Thumbnail;
     gnssMeasurementsDEPRECATED @91 :GnssMeasurements;
     mapRenderStateDEPRECATED @105: MapRenderState;

openpilot/cereal/services.py

@@ -52,6 +52,8 @@ _services: dict[str, tuple] = {
   "procLog": (True, 0.5, 15, QueueSize.BIG),
   "gpsLocationExternal": (True, 10., 10),
   "gpsLocation": (True, 1., 1),
+  "navInstruction": (True, 1., 10),
+  "navRoute": (True, 0.),
   "ubloxGnss": (True, 10.),
   "qcomGnss": (True, 2.),
   "clocks": (True, 0.1, 1),

openpilot/common/api/__init__.py

@@ -1,4 +1,4 @@
-from openpilot.common.api.comma_connect import CommaConnectApi
+from openpilot.common.api.comma_connect import API_HOST as API_HOST, CommaConnectApi
 
 
 class Api:

openpilot/common/constants.py

@@ -16,6 +16,10 @@ class CV:
   DEG_TO_RAD = np.pi / 180.
   RAD_TO_DEG = 1. / DEG_TO_RAD
 
+  # Length
+  METER_TO_FOOT = 3.28084
+  METER_TO_MILE = 1. / 1609.344
+
   # Mass
   LB_TO_KG = 0.453592
 

openpilot/common/esim/base.py

@@ -1,6 +1,8 @@
 from abc import ABC, abstractmethod
 from dataclasses import dataclass
 
+WEBBING_ICCID_PREFIX = "8985235"
+
 
 class LPAError(RuntimeError):
   pass
@@ -19,7 +21,7 @@ class Profile:
 
   @property
   def is_comma(self) -> bool:
-    return self.provider == 'Webbing' and self.iccid.startswith('8985235')
+    return self.provider == 'Webbing' and self.iccid.startswith(WEBBING_ICCID_PREFIX)
 
   @property
   def display_name(self) -> str:

openpilot/common/file_chunker.py

@@ -1,75 +0,0 @@
-#!/usr/bin/env python3
-import io
-import sys
-import math
-import os
-from pathlib import Path
-
-CHUNK_SIZE = 45 * 1024 * 1024  # 45MB, under GitHub's 50MB limit
-
-def get_chunk_name(name, idx, num_chunks):
-  return f"{name}.chunk{idx+1:02d}of{num_chunks:02d}"
-
-def get_manifest_path(name):
-  return f"{name}.chunkmanifest"
-
-def _chunk_paths(path, num_chunks):
-  return [get_manifest_path(path)] + [get_chunk_name(path, i, num_chunks) for i in range(num_chunks)]
-
-def get_chunk_targets(path, file_size):
-  num_chunks = math.ceil(file_size / CHUNK_SIZE)
-  return _chunk_paths(path, num_chunks)
-
-def chunk_file(path, targets):
-  manifest_path, *chunk_paths = targets
-  actual_num_chunks = max(1, math.ceil(os.path.getsize(path) / CHUNK_SIZE))
-  assert len(chunk_paths) >= actual_num_chunks, f"Allowed {len(chunk_paths)} chunks but needs at least {actual_num_chunks}, for path {path}"
-  Path(manifest_path).unlink(missing_ok=True)
-  with open(path, 'rb') as f:
-    for chunk_path in chunk_paths:
-      with open(chunk_path, 'wb') as out:
-        out.write(f.read(CHUNK_SIZE))
-  Path(manifest_path).write_text(str(len(chunk_paths)))
-  os.remove(path)
-
-class ChunkStream(io.RawIOBase):
-  def __init__(self, paths):
-    self._paths = iter(paths)
-    self._f = None
-
-  def readable(self):
-    return True
-
-  def readinto(self, b):
-    n = 0
-    view = memoryview(b)
-    while n < len(b):
-      if self._f is None:
-        p = next(self._paths, None)
-        if p is None:
-          break
-        self._f = open(p, 'rb')
-      count = self._f.readinto(view[n:])
-      if not count:
-        self._f.close()
-        self._f = None
-        continue
-      n += count
-    return n
-
-def open_file_chunked(path):
-  manifest_path = get_manifest_path(path)
-  if os.path.isfile(path):
-    paths = [path]
-  elif os.path.isfile(manifest_path):
-    num_chunks = int(Path(manifest_path).read_text().strip())
-    paths = [get_chunk_name(path, i, num_chunks) for i in range(num_chunks)]
-  else:
-    raise FileNotFoundError(path)
-  return io.BufferedReader(ChunkStream(paths))
-
-
-if __name__ == "__main__":
-  path = sys.argv[1]
-  chunk_paths = get_chunk_targets(path, os.path.getsize(path))
-  chunk_file(path, chunk_paths)

openpilot/common/gpio.py

@@ -27,16 +27,6 @@ def gpio_read(pin: int) -> bool | None:
 
   return val
 
-def gpio_export(pin: int) -> None:
-  if os.path.isdir(f"/sys/class/gpio/gpio{pin}"):
-    return
-
-  try:
-    with open("/sys/class/gpio/export", 'w') as f:
-      f.write(str(pin))
-  except Exception:
-    print(f"Failed to export gpio {pin}")
-
 @cache
 def get_irq_action(irq: int) -> list[str]:
   try:

openpilot/common/hardware/comma/agnos.json

@@ -56,28 +56,28 @@
   },
   {
     "name": "boot",
-    "url": "https://commadist.azureedge.net/agnosupdate/boot-6ecf6f987cd11968104abcccabbe268485d329cdb73012dfd3c381a6b8deb27d.img.xz",
-    "hash": "6ecf6f987cd11968104abcccabbe268485d329cdb73012dfd3c381a6b8deb27d",
-    "hash_raw": "6ecf6f987cd11968104abcccabbe268485d329cdb73012dfd3c381a6b8deb27d",
-    "size": 46897152,
+    "url": "https://commadist.azureedge.net/agnosupdate/boot-b9c9b9261fca0cef9e6d9b198fa4b563767e6342afc7a61ab137b2b519260615.img.xz",
+    "hash": "b9c9b9261fca0cef9e6d9b198fa4b563767e6342afc7a61ab137b2b519260615",
+    "hash_raw": "b9c9b9261fca0cef9e6d9b198fa4b563767e6342afc7a61ab137b2b519260615",
+    "size": 46962688,
     "sparse": false,
     "full_check": true,
     "has_ab": true,
-    "ondevice_hash": "d12e1e5b9455b62a1464558716493b33e470d7a7e88da1c4105a3b21d0961808"
+    "ondevice_hash": "3ef4769bb1cc73bfc31f96f073b46af42dcc0bad4ab2dde32d11929745d0cd28"
   },
   {
     "name": "system",
-    "url": "https://commadist.azureedge.net/agnosupdate/system-3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f.img.xz",
-    "hash": "74ffc9c551e1f29cda897ace8a69080fe644f8039977c6885f2b48362e39b744",
-    "hash_raw": "3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f",
+    "url": "https://commadist.azureedge.net/agnosupdate/system-80d9373348ddff2a5d1f534fab3420383939c96f3eba60baab6b15339fc602c7.img.xz",
+    "hash": "9aebca09b9d142718e71efbfe44b723010eec6e70067c043c63186067291739d",
+    "hash_raw": "80d9373348ddff2a5d1f534fab3420383939c96f3eba60baab6b15339fc602c7",
     "size": 4718592000,
     "sparse": true,
     "full_check": false,
     "has_ab": true,
-    "ondevice_hash": "6a992680183685eea9db99d915219a37935f45989330d9b619e880450257f448",
+    "ondevice_hash": "270b76c5c5568828e3883390b83ea445425a104dd42119d68f06093ce6b6fcef",
     "alt": {
-      "hash": "3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f",
-      "url": "https://commadist.azureedge.net/agnosupdate/system-3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f.img",
+      "hash": "80d9373348ddff2a5d1f534fab3420383939c96f3eba60baab6b15339fc602c7",
+      "url": "https://commadist.azureedge.net/agnosupdate/system-80d9373348ddff2a5d1f534fab3420383939c96f3eba60baab6b15339fc602c7.img",
       "size": 4718592000
     }
   }

openpilot/common/hardware/comma/hardware.py

@@ -8,6 +8,7 @@ from functools import cached_property, lru_cache
 from pathlib import Path
 
 from openpilot.cereal import log
+from openpilot.common.esim.base import WEBBING_ICCID_PREFIX
 from openpilot.common.utils import sudo_read, sudo_write
 from openpilot.common.gpio import gpio_set, gpio_init, get_irqs_for_action
 from openpilot.common.esim.base import LPABase
@@ -207,6 +208,8 @@ class HardwareComma(HardwareBase):
 
   def get_network_metered(self, network_type) -> bool:
     if network_type in (NetworkType.cell2G, NetworkType.cell3G, NetworkType.cell4G, NetworkType.cell5G):
+      if self.get_modem_state().get("iccid", "").startswith(WEBBING_ICCID_PREFIX):
+        return True
       from openpilot.common.params import Params
       return Params().get_bool("GsmMetered")
     try:

openpilot/common/hardware/comma/modem.py

@@ -17,6 +17,8 @@ from ipaddress import IPv4Address, AddressValueError
 
 from enum import Enum
 
+from openpilot.common.esim.base import WEBBING_ICCID_PREFIX
+
 logging.basicConfig(
   level=logging.INFO,
   format="%(asctime)s.%(msecs)03d %(levelname)-7s modem: %(message)s",
@@ -44,7 +46,6 @@ NETWORK_TYPE = {0: "gsm", 1: "gsm", 3: "gsm", 8: "gsm",
                 11: "nr", 12: "nr", 13: "nr"}
 
 DIAL_CID = 1
-WEBBING_ICCID_PREFIX = "8985235"
 
 PPPD_CMD = [
   "sudo", "pppd", PPP_PORT, "460800", "noauth", "nodetach", "noipdefault", "usepeerdns",

openpilot/common/hardware/comma/power_monitor.py

@@ -9,25 +9,22 @@ from openpilot.common.realtime import Ratekeeper
 from openpilot.common.filter_simple import FirstOrderFilter
 
 
-def read_power(panda=None):
-  if panda is not None and panda.get_type() == panda.HW_TYPE_CUATRO:
-    health = panda.health()
-    return health['voltage'] * health['current'] / 1e6
+def read_power():
   with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/power1_input") as f:
     return int(f.read()) / 1e6
 
-def sample_power(seconds=5, panda=None) -> list[float]:
+def sample_power(seconds=5) -> list[float]:
   rate = 123
   rk = Ratekeeper(rate, print_delay_threshold=None)
 
   pwrs = []
   for _ in range(rate*seconds):
-    pwrs.append(read_power(panda))
+    pwrs.append(read_power())
     rk.keep_time()
   return pwrs
 
-def get_power(seconds=5, panda=None):
-  pwrs = sample_power(seconds, panda)
+def get_power(seconds=5):
+  pwrs = sample_power(seconds)
   return np.mean(pwrs)
 
 def wait_for_power(min_pwr, max_pwr, min_secs_in_range, timeout):

openpilot/common/hardware/comma/updater

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:6a7adb302d378dda7b1788a841b89e4f905c872550a70a76650dcde977b4ece0
-size 24709209
+oid sha256:f92c0a9b41613e063b6a1cd086729f5555d9df6482249e171ed1e276272cad7d
+size 24709520

openpilot/common/hardware/usb.py

@@ -1,7 +1,7 @@
 import os
 from pathlib import Path
 
-CHESTNUT_FW_VERSION = "ed4e39b7"
+CHESTNUT_FW_VERSION = "8516b768"
 CHESTNUT_USB_IDS = ((0xADD1, 0x0001), (0x3801, 0x0001))
 CHESTNUT_ROM_USB_IDS = ((0x174C, 0x2464), (0x174C, 0x2463))
 CHESTNUT_USB_PRODUCT = f"custom {CHESTNUT_FW_VERSION}-CLEAN"
@@ -36,6 +36,10 @@ def read_int(path: Path, base: int = 10) -> int:
     return 0
 
 
+def cable_connected() -> bool:
+  return read_int(TYPEC_CC_ORIENTATION_PATH) != 0
+
+
 def usb_devices() -> list[Path]:
   try:
     devices = (d for d in USB_DEVICES_PATH.glob("*") if (d / "idVendor").exists())
@@ -91,3 +95,4 @@ def set_usb_state(device_state, devices: list[dict]) -> None:
       chestnut_present = True
 
   device_state.chestnutPresent = chestnut_present
+  device_state.usbState.connected = cable_connected()

openpilot/common/params_keys.h

@@ -40,8 +40,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DriverTooDistracted", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, BOOL}},
     {"DriverLockoutCount", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, INT, "0"}},
     {"AlphaLongitudinalEnabled", {PERSISTENT | DEVELOPMENT_ONLY | BACKUP, BOOL}},
-    {"ExperimentalMode", {PERSISTENT | BACKUP, BOOL}},
-    {"ExperimentalModeConfirmed", {PERSISTENT | BACKUP, BOOL}},
     {"FirmwareQueryDone", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
     {"ForcePowerDown", {PERSISTENT, BOOL}},
     {"GitBranch", {PERSISTENT, STRING}},
@@ -69,7 +67,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"JoystickDebugMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
     {"LanguageSetting", {PERSISTENT | BACKUP, STRING, "en"}},
     {"LastAthenaPingTime", {CLEAR_ON_MANAGER_START, INT}},
-    {"LastGPSPosition", {PERSISTENT, STRING}},
+    {"LastGPSPosition", {PERSISTENT, BYTES}},
     {"LastManagerExitReason", {CLEAR_ON_MANAGER_START, STRING}},
     {"LastOffroadStatusPacket", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON}},
     {"LastAgnosPowerMonitorShutdown", {CLEAR_ON_MANAGER_START, STRING}},
@@ -139,6 +137,15 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"UptimeOnroad", {PERSISTENT, FLOAT, "0.0"}},
     {"ChestnutActive", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}},
     {"ChestnutLoading", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}},
+    {"ChestnutRetry", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, BOOL}},
+    {"InitialCruiseSpeedKph", {PERSISTENT, INT, "40"}},
+    {"NavDestination", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON}},
+    {"NavigationEnabled", {PERSISTENT, BOOL, "0"}},
+    {"Offroad_ChestnutModelError", {CLEAR_ON_MANAGER_START, JSON}},
+    {"Offroad_Pairing", {CLEAR_ON_MANAGER_START, JSON}},
+    {"Offroad_UploadsIgnored", {CLEAR_ON_MANAGER_START, JSON}},
+    {"PairingEmail", {PERSISTENT, STRING}},
+    {"PairingProvider", {PERSISTENT, STRING}},
     {"Version", {PERSISTENT, STRING}},
 
     // --- sunnypilot params --- //
@@ -192,16 +199,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"SunnypilotStoppingController", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
 
-    // toyota specific params
-    {"ToyotaAutoHold", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"ToyotaBrakeOnset", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"ToyotaEnhancedBsm", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"ToyotaTSS2Long", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"ToyotaDriveMode", {PERSISTENT | BACKUP, BOOL, "0"}},
-
     // MADS params
     {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"MadsMainCruiseAllowed", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -250,10 +249,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
 
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
-    // Accel Controller profiles (Eco / Normal / Sport)
-    {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
-
     // sunnypilot model params
     {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},

openpilot/common/realtime.py

@@ -91,8 +91,13 @@ class Ratekeeper:
     self.avg_dt.add_value(self._last_monitor_time - prev)
 
     lagged = False
-    remaining = self._next_frame_time - time.monotonic()
-    self._next_frame_time += self._interval
+    now = time.monotonic()
+    remaining = self._next_frame_time - now
+    if remaining < -self._interval:
+      # more than a frame behind (e.g. a stall): resync instead of running catch-up frames back-to-back
+      self._next_frame_time = now + self._interval
+    else:
+      self._next_frame_time += self._interval
     if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold:
       print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms")
       lagged = True

openpilot/common/tests/test_params.py

@@ -117,16 +117,12 @@ class TestParams(OpenpilotTestCase):
   def test_params_default_value(self):
     self.params.remove("LanguageSetting")
     self.params.remove("LongitudinalPersonality")
-    self.params.remove("AccelPersonalityEnabled")
-    self.params.remove("AccelPersonality")
     self.params.remove("LiveParametersV2")
 
     assert self.params.get("LanguageSetting") is None
     assert self.params.get("LanguageSetting", return_default=False) is None
     assert isinstance(self.params.get("LanguageSetting", return_default=True), str)
     assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int)
-    assert self.params.get("AccelPersonalityEnabled", return_default=True) is False
-    assert self.params.get("AccelPersonality", return_default=True) == 1
     assert self.params.get("LiveParametersV2") is None
     assert self.params.get("LiveParametersV2", return_default=True) is None
 

openpilot/common/utils.py

@@ -5,7 +5,6 @@ import contextlib
 import subprocess
 import time
 import functools
-from subprocess import Popen, PIPE, TimeoutExpired
 import zstandard as zstd
 
 LOG_COMPRESSION_LEVEL = 10  # little benefit up to level 15. level ~17 is a small step change
@@ -157,20 +156,6 @@ def run_cmd_default(cmd: list[str], default: str = "", cwd=None, env=None) -> st
     return default
 
 
-@contextlib.contextmanager
-def managed_proc(cmd: list[str], env: dict[str, str]):
-  proc = Popen(cmd, env=env, stdout=PIPE, stderr=PIPE)
-  try:
-    yield proc
-  finally:
-    if proc.poll() is None:
-      proc.terminate()
-    try:
-      proc.wait(timeout=5)
-    except TimeoutExpired:
-      proc.kill()
-
-
 def tabulate(tabular_data, headers=(), tablefmt="simple", floatfmt="g", stralign="left", numalign=None):
   rows = [list(row) for row in tabular_data]
 

openpilot/common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.11.2"
+#define COMMA_VERSION "0.11.3"

openpilot/common/version.py

@@ -37,6 +37,8 @@ SP_BRANCH_MIGRATIONS = {
   ("tizi", "release3-staging"): "release-tizi-staging",
   ("mici", "release3"): "release-mici",
   ("mici", "release3-staging"): "release-mici-staging",
+  ("tici", "hkg-angle-steering-2025"): "hkg-angle-steering-2025-tici",
+  ("tici", "hkg-angle-steering-2025-prebuilt"): "hkg-angle-steering-2025-tici-prebuilt"
 }
 
 BUILD_METADATA_FILENAME = "build.json"

openpilot/selfdrive/assets/icons/couch.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:911f59f248015600da7ecc689398103d47dfc57f6be17ac8c8e543a726a6c64b
-size 3311

openpilot/selfdrive/assets/icons/couch.svg

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

openpilot/selfdrive/assets/icons/experimental.png

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

openpilot/selfdrive/assets/icons/experimental.svg

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

openpilot/selfdrive/assets/icons/experimental_grey.png

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

openpilot/selfdrive/assets/icons/experimental_grey.svg

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:90d441310f8e1833c661d8a4f0546aab2eb50eb21cc21beb0aff0d27b5ee6066
-size 1571

openpilot/selfdrive/assets/icons/experimental_white.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:6b2dad33fead9f064c3a548651d6ef37daf82b6127c329683f538ec6e986ecbc
-size 11204

openpilot/selfdrive/assets/icons/experimental_white.svg

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:82648254da89edb0cf65ef63fc5e6e0741414051bfea8a40449132fd61b1ed0f
-size 1533

openpilot/selfdrive/assets/icons_mici/experimental_mode.png

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:01841b602632c66ab14a8e52b874a1623f09641dc2ef0620f4e2d00bb4a913f3
-size 16243

openpilot/selfdrive/assets/icons_mici/longitudinal/car.png

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

openpilot/selfdrive/assets/icons_mici/longitudinal/car_green.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:57c7ba72642b0350ee764c924586fa66ff94915be4aba1813797e67eb4b74376
+size 33215

openpilot/selfdrive/assets/icons_mici/longitudinal/distance_1.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:71dc069bd3dc5b11ba8e1b52eb3a1025ee0ad8c82834f94b4b2269ec0d401920
+size 237

openpilot/selfdrive/assets/icons_mici/longitudinal/distance_1_green.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:73a6fe02cf19105c0051a6664a424c1bf2ede364c6cf3791f235c86ffa203f39
+size 10348

openpilot/selfdrive/assets/icons_mici/longitudinal/distance_2.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4f500f118dca3eedbd86fc6cc943ad8be67fe5d4f7b474d5b452cd757ad805c3
+size 282

openpilot/selfdrive/assets/icons_mici/longitudinal/distance_2_green.png

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

openpilot/selfdrive/assets/icons_mici/longitudinal/distance_3.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8542374779f1908dc52c8a126bc041efea4c4d605afdd34e3bf8e520a7e78bf2
+size 320

openpilot/selfdrive/assets/icons_mici/longitudinal/distance_3_green.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:21e656b387c1a7e8de54d3e1b1c246f442cffe3c5e0924273d6d6d897c00b568
+size 13868

openpilot/selfdrive/assets/icons_mici/onroad/bookmark.png

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:fd91685bf656e828648acf035a4737acb2c4709e8514cf0aa0a10fa470a9bb60
-size 11580
+oid sha256:92431d37a844206570d73d48f39dd8d934539a89668fe8d5f4cccae38f7b301b
+size 11530

openpilot/selfdrive/assets/icons_mici/onroad/bookmark_fill.png

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

openpilot/selfdrive/assets/icons_mici/settings/device/apple.png

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

openpilot/selfdrive/assets/icons_mici/settings/device/commacare.png

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

openpilot/selfdrive/assets/icons_mici/settings/device/github.png

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

openpilot/selfdrive/assets/icons_mici/settings/device/google.png

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

openpilot/selfdrive/assets/icons_mici/settings/device/green_cell.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:710e74b526adb5bbac15cd8915a828dd673dd8ce15b2fd694c64c182eef6f56f
+size 13744

openpilot/selfdrive/assets/icons_mici/settings/device/green_settings.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:25db08a9038b5faa3b200ef5625cb216d7f4b31ea3d49fb0395f9b726bfaf8e6
+size 13135

openpilot/selfdrive/assets/icons_mici/settings/device/paired_apple.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:547f466cc59d29647f3bf131d8444d7f22b63b186bcea55ceb8109c68ca74fe8
+size 2345

openpilot/selfdrive/assets/icons_mici/settings/device/paired_github.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:55050096cd98e7f4ea3e843d927c18815e458dfbd5e7d100b503db92075d061b
+size 2373

openpilot/selfdrive/assets/icons_mici/settings/device/paired_google.png

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

openpilot/selfdrive/assets/icons_mici/settings/device/phone.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7aeccd3e0d8517f74eb34f7d73632c0b29b0ad5df5b9686c0f509bb6b136561e
+size 1728

openpilot/selfdrive/assets/sounds/critical_max.wav

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5d02369e21d3c78a3a5ece7e78deedb82343bc3228a28db73da48b5c27860004
+size 52374

openpilot/selfdrive/assets/sounds/dm_critical_max.wav

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

openpilot/selfdrive/assets/sounds/warning.wav

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:1bb440f424c989c06e203bfba0b32d39aede8f6c78a9b335e5e778460202b601
+oid sha256:884e828e5c51d980d41b6e2400fcb4f4a926275018a00674d8239184e1eb96db
 size 73026

openpilot/selfdrive/car/card.py

@@ -11,13 +11,13 @@ from opendbc.car.structs import car
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
 from openpilot.common.swaglog import cloudlog, ForwardingHandler
+
 from opendbc.car import DT_CTRL, structs
 from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
 from opendbc.car.carlog import carlog
 from opendbc.car.fw_versions import ObdCallback
 from opendbc.car.car_helpers import get_car, interfaces
 from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
-from opendbc.safety import ALTERNATIVE_EXPERIENCE
 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
@@ -123,9 +123,6 @@ class Car:
       self.RI = RI
 
     self.CP.alternativeExperience = 0
-    if self.params.get_bool("ToyotaAutoHold"):
-      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
-
     # mads
     set_alternative_experience(self.CP, self.CP_SP, self.params)
     set_car_specific_params(self.CP, self.CP_SP, self.params)
@@ -181,7 +178,7 @@ class Car:
     self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP)
 
     self.is_metric = self.params.get_bool("IsMetric")
-    self.experimental_mode = self.params.get_bool("ExperimentalMode")
+    self.initial_cruise_speed = self.params.get("InitialCruiseSpeedKph", return_default=True)
 
     # card is driven by can recv, expected at 100Hz
     self.rk = Ratekeeper(100, print_delay_threshold=None)
@@ -214,7 +211,7 @@ class Car:
     self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
     if self.sm['carControl'].enabled and not self.CC_prev.enabled:
       # Use CarState w/ buttons from the step selfdrived enables on
-      self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode)
+      self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.initial_cruise_speed)
 
     # TODO: mirror the carState.cruiseState struct?
     CS.vCruise = float(self.v_cruise_helper.v_cruise_kph)
@@ -299,7 +296,7 @@ class Car:
   def params_thread(self, evt):
     while not evt.is_set():
       self.is_metric = self.params.get_bool("IsMetric")
-      self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
+      self.initial_cruise_speed = self.params.get("InitialCruiseSpeedKph", return_default=True)
 
       # sunnypilot
       self.v_cruise_helper.read_custom_set_speed_params()

openpilot/selfdrive/car/cruise.py

@@ -13,13 +13,11 @@ V_CRUISE_MIN = 8
 V_CRUISE_MAX = 145
 V_CRUISE_UNSET = 255
 V_CRUISE_INITIAL = 40
-V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105
 IMPERIAL_INCREMENT = round(CV.MPH_TO_KPH, 1)  # round here to avoid rounding errors incrementing set speed
 
 ButtonEvent = car.CarState.ButtonEvent
 ButtonType = car.CarState.ButtonEvent.Type
 CRUISE_LONG_PRESS = 50
-TOYOTA_VIRTUAL_CRUISE_LONG_PRESS = 65
 CRUISE_NEAREST_FUNC = {
   ButtonType.accelCruise: math.ceil,
   ButtonType.decelCruise: math.floor,
@@ -44,30 +42,6 @@ class VCruiseHelper(VCruiseHelperSP):
   def v_cruise_initialized(self):
     return self.v_cruise_kph != V_CRUISE_UNSET
 
-  @property
-  def software_pcm_cruise_speed(self) -> bool:
-    return self.CP.brand == "toyota" and self.CP.pcmCruise and self.CP.openpilotLongitudinalControl and not self.CP_SP.pcmCruiseSpeed
-
-  @property
-  def cruise_long_press_frames(self) -> int:
-    return TOYOTA_VIRTUAL_CRUISE_LONG_PRESS if self.software_pcm_cruise_speed else CRUISE_LONG_PRESS
-
-  @property
-  def software_pcm_cruise_initialized(self) -> bool:
-    return 0 < self.v_cruise_kph < V_CRUISE_UNSET and 0 < self.v_cruise_cluster_kph < V_CRUISE_UNSET
-
-  def _apply_software_pcm_cruise_delta(self, delta_kph: float, is_metric: bool) -> None:
-    """Move Toyota's planner/display targets together while respecting both targets' bounds."""
-    cluster_min_kph = self.v_cruise_min if is_metric else self.v_cruise_min * CV.MPH_TO_KPH
-    min_delta = max(V_CRUISE_MIN - self.v_cruise_kph, cluster_min_kph - self.v_cruise_cluster_kph)
-    max_delta = min(V_CRUISE_MAX - self.v_cruise_kph, V_CRUISE_MAX - self.v_cruise_cluster_kph)
-    if delta_kph > 0:
-      applied_delta = min(delta_kph, max(0., max_delta))
-    else:
-      applied_delta = max(delta_kph, min(0., min_delta))
-    self.v_cruise_kph = round(self.v_cruise_kph + applied_delta, 1)
-    self.v_cruise_cluster_kph = round(self.v_cruise_cluster_kph + applied_delta, 1)
-
   def update_v_cruise(self, CS, enabled, is_metric):
     self.v_cruise_kph_last = self.v_cruise_kph
 
@@ -76,21 +50,11 @@ class VCruiseHelper(VCruiseHelperSP):
     _enabled = self.update_enabled_state(CS, enabled)
 
     if CS.cruiseState.available:
-      software_pcm_enabled = not self.CP_SP.pcmCruiseSpeed and _enabled
-      if self.software_pcm_cruise_speed:
-        software_pcm_enabled = software_pcm_enabled and self.software_pcm_cruise_initialized
-
-      if not self.CP.pcmCruise or software_pcm_enabled:
+      if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled):
         # if stock cruise is completely disabled, then we can use our own set speed logic
         self._update_v_cruise_non_pcm(CS, _enabled, is_metric)
-        v_cruise_kph_before_sla = self.v_cruise_kph
         self.update_speed_limit_assist_v_cruise_non_pcm()
-        if self.software_pcm_cruise_speed:
-          sla_delta_kph = self.v_cruise_kph - v_cruise_kph_before_sla
-          self.v_cruise_kph = v_cruise_kph_before_sla
-          self._apply_software_pcm_cruise_delta(sla_delta_kph, is_metric)
-        else:
-          self.v_cruise_cluster_kph = self.v_cruise_kph
+        self.v_cruise_cluster_kph = self.v_cruise_kph
       else:
         self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
         self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
@@ -120,13 +84,13 @@ class VCruiseHelper(VCruiseHelperSP):
 
     for b in CS.buttonEvents:
       if b.type.raw in self.button_timers and not b.pressed:
-        if self.button_timers[b.type.raw] > self.cruise_long_press_frames:
+        if self.button_timers[b.type.raw] > CRUISE_LONG_PRESS:
           return  # end long press
         button_type = b.type.raw
         break
     else:
       for k, timer in self.button_timers.items():
-        if timer and timer % self.cruise_long_press_frames == 0:
+        if timer and timer % CRUISE_LONG_PRESS == 0:
           button_type = k
           long_press = True
           break
@@ -150,26 +114,10 @@ class VCruiseHelper(VCruiseHelperSP):
       return
 
     long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta)
-    # Toyota's canonical PCM set speed and displayed cluster set speed can differ. In
-    # software-owned PCM mode, round the value the driver sees and apply the same delta
-    # to both targets so the planner/cluster calibration offset remains intact.
-    v_cruise_reference = self.v_cruise_cluster_kph if self.software_pcm_cruise_speed else self.v_cruise_kph
-    if long_press and v_cruise_reference % v_cruise_delta != 0:  # partial interval
-      v_cruise_reference_new = CRUISE_NEAREST_FUNC[button_type](v_cruise_reference / v_cruise_delta) * v_cruise_delta
+    if long_press and self.v_cruise_kph % v_cruise_delta != 0:  # partial interval
+      self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta
     else:
-      v_cruise_reference_new = v_cruise_reference + v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
-
-    if self.software_pcm_cruise_speed:
-      delta_kph = v_cruise_reference_new - v_cruise_reference
-
-      # If SET is pressed while overriding, do not lower the target below the current speed.
-      if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
-        delta_kph = max(delta_kph, CS.vEgo * CV.MS_TO_KPH - self.v_cruise_kph)
-
-      self._apply_software_pcm_cruise_delta(delta_kph, is_metric)
-      return
-
-    self.v_cruise_kph += v_cruise_reference_new - v_cruise_reference
+      self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
 
     # If set is pressed while overriding, clip cruise speed to minimum of vEgo
     if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
@@ -178,12 +126,6 @@ class VCruiseHelper(VCruiseHelperSP):
     self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), self.v_cruise_min, V_CRUISE_MAX)
 
   def update_button_timers(self, CS, enabled):
-    if self.software_pcm_cruise_speed and (not enabled or not CS.cruiseState.available or not self.software_pcm_cruise_initialized):
-      for k in self.button_timers:
-        self.button_timers[k] = 0
-        self.button_change_states[k] = {"standstill": False, "enabled": False}
-      return
-
     # increment timer for buttons still pressed
     for k in self.button_timers:
       if self.button_timers[k] > 0:
@@ -195,16 +137,14 @@ class VCruiseHelper(VCruiseHelperSP):
         self.button_timers[b.type.raw] = 1 if b.pressed else 0
         self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled}
 
-  def initialize_v_cruise(self, CS, experimental_mode: bool) -> None:
+  def initialize_v_cruise(self, CS, initial_speed: int) -> None:
     # initializing is handled by the PCM
     if self.CP.pcmCruise:
       return
 
-    initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL
-
     if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized:
       self.v_cruise_kph = self.v_cruise_kph_last
     else:
-      self.v_cruise_kph = int(round(np.clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
+      self.v_cruise_kph = int(round(np.clip(max(CS.vEgo * CV.MS_TO_KPH, initial_speed), V_CRUISE_INITIAL, V_CRUISE_MAX)))
 
     self.v_cruise_cluster_kph = self.v_cruise_kph

openpilot/selfdrive/car/tests/test_cruise_speed.py

@@ -14,7 +14,7 @@ ButtonEvent = car.CarState.ButtonEvent
 ButtonType = car.CarState.ButtonEvent.Type
 
 
-def run_cruise_simulation(cruise, e2e, personality, t_end=20.):
+def run_cruise_simulation(cruise, personality, t_end=20.):
   man = Maneuver(
     '',
     duration=t_end,
@@ -24,7 +24,6 @@ def run_cruise_simulation(cruise, e2e, personality, t_end=20.):
     cruise_values=[cruise],
     prob_lead_values=[0.0],
     breakpoints=[0.],
-    e2e=e2e,
     personality=personality,
   )
   valid, output = man.evaluate()
@@ -32,8 +31,7 @@ def run_cruise_simulation(cruise, e2e, personality, t_end=20.):
   return output[-1, 3]
 
 
-@parameterized_class(("e2e", "personality", "speed"), itertools.product(
-                      [True, False], # e2e
+@parameterized_class(("personality", "speed"), itertools.product(
                       log.LongitudinalPersonality.schema.enumerants, # personality
                       [5,35])) # speed
 class TestCruiseSpeed(OpenpilotTestCase):
@@ -41,7 +39,7 @@ class TestCruiseSpeed(OpenpilotTestCase):
     print(f'Testing {self.speed} m/s')
     cruise_speed = float(self.speed)
 
-    simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e, self.personality)
+    simulation_steady_state = run_cruise_simulation(cruise_speed, self.personality)
     self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {self.speed} m/s')
 
 
@@ -59,16 +57,16 @@ class TestVCruiseHelper(OpenpilotTestCase):
     for _ in range(2):
       self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False)
 
-  def enable(self, v_ego, experimental_mode):
+  def enable(self, v_ego):
     # Simulates user pressing set with a current speed
-    self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode)
+    self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), V_CRUISE_INITIAL)
 
   def test_adjust_speed(self):
     """
     Asserts speed changes on falling edges of buttons.
     """
 
-    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
+    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS)
 
     for btn in (ButtonType.accelCruise, ButtonType.decelCruise):
       for pressed in (True, False):
@@ -92,7 +90,7 @@ class TestVCruiseHelper(OpenpilotTestCase):
       CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)]
       self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False)
       if pressed:
-        self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
+        self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS)
 
       # Expected diff on enabling. Speed should not change on falling edge of pressed
       assert (not pressed) == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
@@ -102,7 +100,7 @@ class TestVCruiseHelper(OpenpilotTestCase):
     Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill.
     """
 
-    self.enable(0, False)
+    self.enable(0)
 
     for standstill in (True, False):
       for pressed in (True, False):
@@ -122,7 +120,7 @@ class TestVCruiseHelper(OpenpilotTestCase):
 
     for v_ego in np.linspace(0, 100, 101):
       self.reset_cruise_speed_state()
-      self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
+      self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS)
 
       # first decrement speed, then perform gas pressed logic
       expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT
@@ -143,11 +141,10 @@ class TestVCruiseHelper(OpenpilotTestCase):
     Asserts allowed cruise speeds on enabling with SET.
     """
 
-    for experimental_mode in (True, False):
-      for v_ego in np.linspace(0, 100, 101):
-        self.reset_cruise_speed_state()
-        assert not self.v_cruise_helper.v_cruise_initialized
+    for v_ego in np.linspace(0, 100, 101):
+      self.reset_cruise_speed_state()
+      assert not self.v_cruise_helper.v_cruise_initialized
 
-        self.enable(float(v_ego), experimental_mode)
-        assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX
-        assert self.v_cruise_helper.v_cruise_initialized
+      self.enable(float(v_ego))
+      assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX
+      assert self.v_cruise_helper.v_cruise_initialized

openpilot/selfdrive/controls/controlsd.py

@@ -136,7 +136,6 @@ class Controls(ControlsExt):
     # accel PID loop
     pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, self.CP_SP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
     actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits))
-    self.long_accel_sp(actuators, CS, long_plan, pid_accel_limits)  # sunnypilot hook
 
     # Steering PID loop and lateral MPC
     # Reset desired curvature to current to avoid violating the limits on engage

openpilot/selfdrive/controls/lib/accel_boost.py

@@ -0,0 +1,38 @@
+import numpy as np
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
+
+ACCEL_BOOST_MAX = 0.2
+ACCEL_BOOST_RATE = 0.025
+ACCEL_BOOST_PER_OVERRIDE = 0.05
+ACCEL_BOOST_MIN_SPEED = 10 * CV.MPH_TO_MS
+
+
+class AccelBoost:
+  def __init__(self):
+    self.total_boost = 0.0
+    self.boost_this_override = 0.0
+    self.boost_eligible = False
+
+  def update(self, sm, output_a_target_e2e, output_a_target_mpc, a_cruise):
+    enabled = sm['selfdriveState'].enabled
+    gas_pressed = sm['carState'].gasPressed
+    v_ego = sm['carState'].vEgo
+    speed_scale = np.interp(v_ego, [ACCEL_BOOST_MIN_SPEED, 2 * ACCEL_BOOST_MIN_SPEED], [0.0, 1.0])
+    boost_scale = speed_scale * np.interp(output_a_target_e2e, [-1.0, -0.5, 5.0], [0.0, 1.0, 1.0], right=0.0)
+    model_limited = output_a_target_e2e + self.total_boost * boost_scale < min(output_a_target_mpc, a_cruise) - 0.1
+
+    if not enabled or not gas_pressed:
+      self.boost_this_override = 0.0
+      # Latch eligibility before the override changes the lead plan.
+      self.boost_eligible = enabled and model_limited
+
+    if not enabled:
+      self.total_boost = 0.0
+    elif gas_pressed and self.boost_eligible:
+      increase = min(ACCEL_BOOST_RATE * DT_MDL * speed_scale,
+                     ACCEL_BOOST_PER_OVERRIDE - self.boost_this_override, ACCEL_BOOST_MAX - self.total_boost)
+      self.total_boost += increase
+      self.boost_this_override += increase
+
+    return output_a_target_e2e + self.total_boost * boost_scale

openpilot/selfdrive/controls/lib/desire_helper.py

@@ -64,19 +64,16 @@ class DesireHelper:
                          ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
                           (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
 
-        blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
-                              (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
+        blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
+                              ((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
 
-        edge_detected = ((left_edge_detected and self.lane_change_direction == LaneChangeDirection.left) or
-                         (right_edge_detected and self.lane_change_direction == LaneChangeDirection.right))
-
-        self.alc.update_lane_change(blindspot_detected, edge_detected, carstate.brakePressed, torque_applied)
+        self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
 
         if not one_blinker or below_lane_change_speed:
           self.lane_change_state = LaneChangeState.off
           self.lane_change_direction = LaneChangeDirection.none
           self.lane_change_timer = 0.0
-        elif (torque_applied or self.alc.auto_lane_change_allowed) and not self.alc.lane_change_blocked:
+        elif (torque_applied or self.alc.auto_lane_change_allowed) and not blindspot_detected:
           self.lane_change_state = LaneChangeState.laneChangeStarting
           self.lane_change_timer = 0.0
 

openpilot/selfdrive/controls/lib/drive_helpers.py

@@ -4,7 +4,6 @@ from openpilot.common.realtime import DT_CTRL, DT_MDL
 
 MIN_SPEED = 1.0
 CONTROL_N = 17
-CAR_ROTATION_RADIUS = 0.0
 # This is a turn radius smaller than most cars can achieve
 MAX_CURVATURE = 0.2
 MIN_STABLE_DELAY = 0.3

openpilot/selfdrive/controls/lib/latcontrol_curvature.py

@@ -1,11 +1,13 @@
 import math
+import numpy as np
+from collections import deque
 
 from openpilot.cereal import log
 from openpilot.common.pid import PIDController
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE
 
-CURVATURE_SATURATION_THRESHOLD = 1e-3  # 1/m
+LAT_ACCEL_SATURATION_THRESHOLD = 0.5  # m/s^2
 
 
 class LatControlCurvature(LatControl):
@@ -20,6 +22,8 @@ class LatControlCurvature(LatControl):
     else:
       self.pid = None
       self.kf = 1.
+    self.desired_curvature_buffer_len = int(1. / self.dt)
+    self.desired_curvature_buffer = deque([0.] * self.desired_curvature_buffer_len, maxlen=self.desired_curvature_buffer_len)
 
   def reset(self):
     super().reset()
@@ -30,6 +34,9 @@ class LatControlCurvature(LatControl):
     curvature_log = log.ControlsState.LateralCurvatureState.new_message()
     actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
     error = desired_curvature - actual_curvature
+    self.desired_curvature_buffer.append(desired_curvature)
+    delay_frames = int(np.clip(lat_delay / self.dt + 1, 1, self.desired_curvature_buffer_len))
+    expected_curvature = self.desired_curvature_buffer[-delay_frames]
 
     if not active:
       output_curvature = 0.0
@@ -53,6 +60,6 @@ class LatControlCurvature(LatControl):
     curvature_log.actualCurvature = float(actual_curvature)
     curvature_log.desiredCurvature = float(desired_curvature)
     curvature_log.output = float(output_curvature)
-    curvature_log.saturated = bool(self._check_saturation(abs(error) > CURVATURE_SATURATION_THRESHOLD, CS,
+    curvature_log.saturated = bool(self._check_saturation(abs(expected_curvature - actual_curvature) * CS.vEgo ** 2 > LAT_ACCEL_SATURATION_THRESHOLD, CS,
                                                           False, curvature_limited))
     return 0.0, float(output_curvature), curvature_log

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -1,5 +1,4 @@
 #!/usr/bin/env python3
-import math
 import numpy as np
 
 import openpilot.cereal.messaging as messaging
@@ -8,6 +7,7 @@ from openpilot.common.constants import CV
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.controls.lib.accel_boost import AccelBoost
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, LongitudinalPlanSource
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
@@ -17,42 +17,15 @@ from openpilot.common.swaglog import cloudlog
 
 from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
 
-A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
-A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
 J_CRUISE_VALS = [1.6, 1.2, 0.8, 0.6]
-A_CRUISE_MIN = -1.2
+J_CRUISE_BP = [0., 10.0, 25., 40.]
+A_CRUISE_MIN = -0.5
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
-ALLOW_THROTTLE_THRESHOLD = 0.4
-MIN_ALLOW_THROTTLE_SPEED = 2.5
 
-# Lookup table for turns
-_A_TOTAL_MAX_V = [1.7, 3.2]
-_A_TOTAL_MAX_BP = [20., 40.]
 
-def get_max_accel(v_ego):
-  return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
-
-def get_coast_accel(pitch):
-  return np.sin(pitch) * -5.65 - 0.3  # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
-
-def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle,
-                      max_accel_override=None):
-  if max_accel_override is not None:
-    max_accel = max_accel_override
-  else:
-    max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
-  if not e2e:
-    a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
-    a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
-    a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
-    max_accel = min(max_accel, a_x_allowed)
-    if not allow_throttle:
-      clipped_accel_coast = max(accel_coast, ACCEL_MIN)
-      coast_limit = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [max_accel, clipped_accel_coast])
-      max_accel = min(max_accel, coast_limit)
-
-  target_accel = np.clip(v_cruise - v_ego, A_CRUISE_MIN, max_accel)
-  j_cruise = np.interp(v_ego, A_CRUISE_MAX_BP, J_CRUISE_VALS)
+def get_cruise_accel(v_cruise, v_ego, a_cruise_prev, dt):
+  target_accel = np.clip(v_cruise - v_ego, A_CRUISE_MIN, ACCEL_MAX)
+  j_cruise = np.interp(v_ego, J_CRUISE_BP, J_CRUISE_VALS)
   target_accel = float(np.clip(target_accel, a_cruise_prev - j_cruise * dt, a_cruise_prev + j_cruise * dt))
 
   return target_accel
@@ -65,7 +38,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc)
     self.fcw = False
     self.dt = dt
-    self.allow_throttle = True
+    self.accel_boost = AccelBoost()
 
     self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
     self.a_cruise = init_a
@@ -79,16 +52,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
   def update(self, sm):
     LongitudinalPlannerSP.update(self, sm)
 
-    if len(sm['carControl'].orientationNED) == 3:
-      accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
-    else:
-      accel_coast = ACCEL_MAX
-
     v_ego = sm['carState'].vEgo
     v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
-    force_decel = sm['controlsState'].forceDecel
-    if force_decel:
+    if sm['controlsState'].forceDecel:
       v_cruise = 0.0
 
     long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
@@ -99,12 +66,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET
     reset_state = reset_state or not v_cruise_initialized
 
-    throttle_probs = sm['modelV2'].meta.disengagePredictions.gasPressProbs
-    throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0
-    self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
-
-    steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['vehicleParameters'].angleOffsetDeg
-
     if reset_state:
       self.v_desired_filter.x = v_ego
       self.output_a_target = np.clip(sm['carState'].aEgo, ACCEL_MIN, ACCEL_MAX)
@@ -120,7 +81,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     v_cruise, self.output_a_target = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.output_a_target, v_cruise)
 
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
-    self.mpc.set_cur_state(self.v_desired_filter.x, self.output_a_target)
+    if self.CP.openpilotLongitudinalControl:
+      self.mpc.set_cur_state(self.v_desired_filter.x, self.output_a_target)
+    else:
+      self.mpc.set_cur_state(v_ego, sm['carState'].aEgo)
     self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
@@ -142,29 +106,18 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
-    experimental_mode = sm['selfdriveState'].experimentalMode
-    max_accel_override = self.get_max_accel_override(v_ego, sm['carStateSP'].engineOff)
-    v_cruise = self.get_cruise_target_override(v_ego, v_cruise, force_decel, accel_coast if accel_coast < 0.0 else None)
-    a_cruise_prev = self.a_cruise
-    gated_cruise = get_cruise_accel(experimental_mode, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset,
-                                    self.CP, self.dt, accel_coast, self.allow_throttle, max_accel_override)
-    ungated_cruise = get_cruise_accel(experimental_mode, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset,
-                                      self.CP, self.dt, accel_coast, True, max_accel_override)
-    self.a_cruise = self.arbitrate_cruise_candidate(
-      sm, gated_cruise, ungated_cruise, output_a_target_mpc, self.mpc.source,
-      allow_throttle=self.allow_throttle, e2e=experimental_mode, force_decel=force_decel,
-    )
+    self.a_cruise = get_cruise_accel(v_cruise, v_ego, self.a_cruise, self.dt)
     cruise_should_stop = should_stop(v_ego, self.a_cruise)
 
+    output_a_target_e2e = self.accel_boost.update(sm, output_a_target_e2e, output_a_target_mpc, self.a_cruise)
+
     candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
-                  (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)]
-    if experimental_mode:
-      candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))
+                  (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop),
+                  (output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)]
 
     output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
     self.output_should_stop = any(should_stop for _, _, should_stop in candidates)
     self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)
-    self.accel_controller_active = self.is_accel_controller_active(force_decel)
 
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0
 
@@ -187,9 +140,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     longitudinalPlan.fcw = self.fcw
 
     longitudinalPlan.aTarget = float(self.output_a_target)
+    longitudinalPlan.accelBoost = float(self.accel_boost.total_boost)
     longitudinalPlan.shouldStop = bool(self.output_should_stop)
     longitudinalPlan.allowBrake = True
-    longitudinalPlan.allowThrottle = bool(self.allow_throttle)
 
     pm.send('longitudinalPlan', plan_send)
 

openpilot/selfdrive/controls/plannerd.py

@@ -28,7 +28,7 @@ def main():
   ldw = LaneDepartureWarning()
   longitudinal_planner = LongitudinalPlanner(CP, CP_SP)
   pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
-  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'vehicleParameters', 'radarState', 'modelV2', 'selfdriveState',
+  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState',
                             'liveMapDataSP', 'carStateSP', 'selfdriveStateSP', gps_location_service],
                            poll='modelV2', ignore_alive=ignore_services, ignore_avg_freq=ignore_services, ignore_valid=ignore_services)
 

openpilot/selfdrive/controls/tests/test_following_distance.py

@@ -13,7 +13,7 @@ def desired_follow_distance(v_ego, v_lead, t_follow=None):
     t_follow = get_T_FOLLOW()
   return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
 
-def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personality=0):
+def run_following_distance_simulation(v_lead, t_end=100.0, personality=0):
   man = Maneuver(
     '',
     duration=t_end,
@@ -22,7 +22,6 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personalit
     initial_distance_lead=100,
     speed_lead_values=[v_lead],
     breakpoints=[0.],
-    e2e=e2e,
     personality=personality,
   )
   valid, output = man.evaluate()
@@ -30,8 +29,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personalit
   return output[-1,2] - output[-1,1]
 
 
-@parameterized_class(("e2e", "personality", "speed"), itertools.product(
-                      [True, False], # e2e
+@parameterized_class(("personality", "speed"), itertools.product(
                       [log.LongitudinalPersonality.relaxed, # personality
                        log.LongitudinalPersonality.standard,
                        log.LongitudinalPersonality.aggressive],
@@ -39,9 +37,8 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personalit
 class TestFollowingDistance(OpenpilotTestCase):
   def test_following_distance(self):
     v_lead = float(self.speed)
-    simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e, personality=self.personality)
+    simulation_steady_state = run_following_distance_simulation(v_lead, personality=self.personality)
     correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
-    err_ratio = 0.2 if self.e2e else 0.1
     abs_err_margin = 0.5 if v_lead > 0.0 else 1.15
     self.assertAlmostEqual(simulation_steady_state, correct_steady_state,
-                           delta=err_ratio * correct_steady_state + abs_err_margin)
+                           delta=0.2 * correct_steady_state + abs_err_margin)

openpilot/selfdrive/controls/tests/test_latcontrol.py

@@ -8,12 +8,14 @@ from opendbc.car.honda.values import CAR as HONDA
 from opendbc.car.toyota.values import CAR as TOYOTA
 from opendbc.car.nissan.values import CAR as NISSAN
 from opendbc.car.gm.values import CAR as GM
+from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN
 from opendbc.car.vehicle_model import VehicleModel
 from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.car.helpers import convert_to_capnp
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature
 from openpilot.selfdrive.locationd.helpers import Pose
 from openpilot.common.mock.generators import generate_deviceMotion
 from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
@@ -22,7 +24,8 @@ from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfac
 class TestLatControl(OpenpilotTestCase):
 
   @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque),
-                         (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_EUV, LatControlTorque)])
+                         (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_EUV, LatControlTorque),
+                         (VOLKSWAGEN.VOLKSWAGEN_ID4_MK2, LatControlCurvature)])
   def test_saturation(self, car_name, controller):
     CarInterface = interfaces[car_name]
     CP = CarInterface.get_non_essential_params(car_name)

openpilot/selfdrive/locationd/models/car_kf.py

@@ -80,7 +80,6 @@ class CarKalman(KalmanFilter):
   }
 
   global_vars = [
-    'mass',
     'rotational_inertia',
     'center_to_front',
     'center_to_rear',
@@ -99,7 +98,10 @@ class CarKalman(KalmanFilter):
 
     # globals
     global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
-    m, j, aF, aR, cF_orig, cR_orig = global_vars
+    j, aF, aR, cF_orig, cR_orig = global_vars
+    # Tire stiffness and rotational inertia are already divided by vehicle mass, which cancels out of the model.
+    # Use unit mass to preserve the standard bicycle-model equations below.
+    m = 1
 
     # make functions and jacobians with sympy
     # state variables
@@ -166,8 +168,7 @@ class CarKalman(KalmanFilter):
     self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
                               dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
 
-  def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
-    self.filter.set_global("mass", mass)
+  def set_globals(self, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
     self.filter.set_global("rotational_inertia", rotational_inertia)
     self.filter.set_global("center_to_front", center_to_front)
     self.filter.set_global("center_to_rear", center_to_rear)

openpilot/selfdrive/locationd/models/pose_kf.py

@@ -40,7 +40,7 @@ class PoseKalman(KalmanFilter):
   initial_P = np.diag([0.01**2, 0.01**2, 0.01**2,
                        10**2, 10**2, 10**2,
                        1**2, 1**2, 1**2,
-                       1**2, 1**2, 1**2,
+                       0.1**2, 0.1**2, 0.1**2,
                        100**2, 100**2, 100**2,
                        0.01**2, 0.01**2, 0.01**2])
 

openpilot/selfdrive/locationd/paramsd.py

@@ -36,12 +36,11 @@ class VehicleParamsLearner:
     self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial
 
     self.kf.set_globals(
-      mass=CP.mass,
-      rotational_inertia=CP.rotationalInertia,
+      rotational_inertia=CP.rotationalInertiaPerMass,
       center_to_front=CP.centerToFront,
       center_to_rear=CP.wheelbase - CP.centerToFront,
-      stiffness_front=CP.tireStiffnessFront,
-      stiffness_rear=CP.tireStiffnessRear
+      stiffness_front=CP.tireStiffnessFrontPerMass,
+      stiffness_rear=CP.tireStiffnessRearPerMass
     )
 
     self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio

openpilot/selfdrive/modeld/SConscript

@@ -1,14 +1,20 @@
+import ctypes.util
 import glob
 import os
-import time
 from SCons.Script import Action, Value
 from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
 from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE, DM_INPUT_SIZE
-from openpilot.selfdrive.modeld.helpers import chestnut_present
 from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
 
 
 Import('env', 'arch')
+
+# TODO: add GCC support to tinygrad?
+# tinygrad's CPU:LLVM backend requires libLLVM, skip the build on PC if it's not available
+if arch not in ('comma_arm64', 'Darwin') and ctypes.util.find_library('LLVM') is None:
+  print("\033[93mmodeld: LLVM not found, skipping tinygrad model compilation. Install LLVM to compile models.\033[0m")
+  Return()
+
 lenv = env.Clone()
 lenv.PrependENVPath('PYTHONPATH', Dir('#tinygrad_repo').abspath)
 
@@ -24,33 +30,13 @@ else:
   # JIT=2 disables graph batching, which produces incorrect outputs after buffers change.
   tg_flags = 'DEV=METAL JIT=2' if arch == 'Darwin' else 'DEV=CPU:LLVM'
 
-CHESTNUT = chestnut_present()
-if CHESTNUT:
-  chestnut_tg_flags = 'DEBUG=1 DEV=USB+AMD:LLVM FRAME_DEV=CPU FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0 TC_OPT=2 TC_MIN_GLOBALS=32'
-  # the USB+AMD GPU takes an exclusive flock; serialize all targets that touch it
-  chestnut_lock = File("models/.chestnut.lock").abspath
-
 # tinygrad calls brew which needs a $HOME in the env
 mac_brew_string = f'HOME={os.path.expanduser("~")}' if arch == 'Darwin' else ''
 
 warp_deps = [File("#openpilot/system/camerad/cameras/nv12_info.py")]
 compiler = Dir('#tinygrad_repo/examples/openpilot').abspath
-# CPU 7 is isolated with isolcpus on AGNOS, so explicitly pin the compiler to it.
-taskset = 'taskset -c 7 ' if arch == 'comma_arm64' else ''
-
-def chestnut_action(command):
-  def do_compile(target, source, env):
-    from openpilot.system.hardware.chestnut.flash import link_up
-    # chestnut can enumerate before its PCIe link is up due to varying 12V power behavior across cars
-    for _ in range(10):
-      if link_up():
-        break
-      time.sleep(1)
-    else:
-      print("Chestnut not ready, skipping warp build")
-      return
-    return env.Execute(command)
-  return Action(do_compile, "  [CHESTNUT] $TARGET")
+# build on all cores
+taskset = 'taskset -c 0-7 ' if arch == 'comma_arm64' else ''
 
 def compile_model(onnx_path, pkl_path):
   onnx_path, target_pkl_path = File(onnx_path).abspath, File(pkl_path).abspath
@@ -62,26 +48,22 @@ def compile_model(onnx_path, pkl_path):
     Action(cmd, "  [ONNX] $TARGET"),
   )
 
-compile_model('models/dmonitoring_model.onnx', 'models/dmonitoring_model_tinygrad.pkl')
-compile_model('models/driving_supercombo.onnx', 'models/driving_tinygrad.pkl')
+if not os.getenv('SKIP_TINYGRAD_COMPILE'):
+  compile_model('models/dmonitoring_model.onnx', 'models/dmonitoring_model_tinygrad.pkl')
+  compile_model('models/driving_supercombo.onnx', 'models/driving_tinygrad.pkl')
 
 model_w, model_h = MEDMODEL_INPUT_SIZE
-for chestnut in [False, True] if CHESTNUT else [False]:
-  file_prefix, cmd_flags = ('big_', chestnut_tg_flags) if chestnut else ('', tg_flags)
-  for cam_w, cam_h in camera_configs:
-    warp_pkl_path = File(f"models/{file_prefix}driving_warp_{cam_w}x{cam_h}_tinygrad.pkl").abspath
-    stride, y_height, uv_height, _ = get_nv12_info(cam_w, cam_h)
-    cmd = (f'{cmd_flags} {mac_brew_string} {taskset}python3 "{compiler}/compile_warp.py" '
-           f'--frame {cam_w},{cam_h},{stride},{y_height},{uv_height},{stride * (y_height + uv_height)} '
-           f'--warp-to {model_w}x{model_h} --layout yuv420 --frames 2 '
-           f'--output {warp_pkl_path}')
-    action = chestnut_action(cmd) if chestnut else cmd
-    node = lenv.Command(warp_pkl_path, tinygrad_files + warp_deps + [Value(cmd)], action)
-    if chestnut:
-      lenv.SideEffect(chestnut_lock, node)
+for cam_w, cam_h in camera_configs:
+  warp_pkl_path = File(f"models/driving_warp_{cam_w}x{cam_h}_tinygrad.pkl").abspath
+  stride, y_height, uv_height, _ = get_nv12_info(cam_w, cam_h)
+  cmd = (f'{tg_flags} {mac_brew_string} {taskset}python3 "{compiler}/compile_warp.py" '
+         f'--frame {cam_w},{cam_h},{stride},{y_height},{uv_height},{stride * (y_height + uv_height)} '
+         f'--warp-to {model_w}x{model_h} --layout yuv420 --frames 2 '
+         f'--output {warp_pkl_path}')
+  lenv.Command(warp_pkl_path, tinygrad_files + warp_deps + [Value(cmd)], cmd)
 
 dm_w, dm_h = DM_INPUT_SIZE
-for cam_w, cam_h in camera_configs:
+for cam_w, cam_h in [] if os.getenv('SKIP_TINYGRAD_COMPILE') else camera_configs:
   dm_pkl_path = File(f"models/dm_warp_{cam_w}x{cam_h}_tinygrad.pkl").abspath
   stride, y_height, uv_height, frame_size = get_nv12_info(cam_w, cam_h)
   cmd = (f'{tg_flags} {mac_brew_string} python3 "{compiler}/compile_warp.py" '

openpilot/selfdrive/modeld/helpers.py

@@ -1,11 +1,7 @@
-import io
-import os
-import pickle
-import struct
 import sys
+import time
 from pathlib import Path
 
-from openpilot.common.file_chunker import get_chunk_name, get_manifest_path, open_file_chunked
 from openpilot.common.hardware import AGNOS
 from openpilot.common.hardware.usb import CHESTNUT_USB_PRODUCT, USB_DEVICES_PATH, is_chestnut_usb_id
 
@@ -17,18 +13,11 @@ def modeld_pkl_path(chestnut: bool):
   return MODELS_DIR / f'{prefix}driving_tinygrad.pkl'
 
 def load_oob(path, chestnut=False):
-  from tinygrad import Context, Device
-  from tinygrad.device import Buffer
+  from tinygrad import Context
   device = 'USB+AMD:LLVM' if chestnut else 'QCOM' if AGNOS else 'METAL' if sys.platform == 'darwin' else 'CPU:LLVM'
-  chunks = 0 if os.path.isfile(path) else int(Path(get_manifest_path(path)).read_text())
-  paths = [get_chunk_name(path, i, chunks) for i in range(chunks)] or [path]
-  with Context(DEV=device), open_file_chunked(path) as f:
-    opcodes = f.read(struct.unpack('<q', f.read(8))[0])
-    buffers, stage = Buffer(Device.DEFAULT, sum(map(os.path.getsize, paths)) - 8 - len(opcodes)).ensure_allocated(), bytearray(64 << 20)
-    for offset in range(0, buffers.nbytes, len(stage)):
-      n = f.readinto(piece := memoryview(stage)[:min(len(stage), buffers.nbytes - offset)])
-      buffers.view(n, offset).ensure_allocated().copy_from(Buffer("PYTHON", n, initial_value=piece[:n]))
-    return type("_Unpickler", (pickle.Unpickler,), {"persistent_load": lambda self, pid: buffers.view(*pid)})(io.BytesIO(opcodes)).load()
+  with Context(DEV=device):
+    from tinygrad_repo.examples.openpilot.helpers import load_pickle
+    return load_pickle(path, out_of_band=True)
 
 def chestnut_present() -> bool:
   for d in USB_DEVICES_PATH.glob("*"):
@@ -41,7 +30,15 @@ def chestnut_present() -> bool:
       pass
   return False
 
+def wait_for_chestnut(timeout: float = 10.) -> None:
+  # the cable is detected before chestnut enumerates
+  st = time.monotonic()
+  while not chestnut_present():
+    if time.monotonic() - st > timeout:
+      raise TimeoutError("chestnut did not enumerate")
+    time.sleep(0.1)
+
 def chestnut_compiled() -> bool:
   path = modeld_pkl_path(chestnut=True)
-  return (path.is_file() or Path(get_manifest_path(path)).is_file()) and all(
+  return path.is_file() and all(
     (MODELS_DIR / f'big_driving_warp_{size}_tinygrad.pkl').is_file() for size in ('1344x760', '1928x1208'))

openpilot/selfdrive/modeld/modeld.py

@@ -2,12 +2,15 @@
 from collections.abc import Callable
 import base64
 import ctypes
+import gc
 from functools import cached_property
 import os
 os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom
+os.environ.setdefault('AM_POWER_LIMIT', '100')
+os.environ.setdefault('AM_PERF_LEVEL', 'high')
 from tinygrad.device import Buffer, Device
-from tinygrad.engine.realize import lower_and_compile
 from tinygrad.dtype import DType, dtypes
+from tinygrad.engine.realize import lower_and_compile
 from tinygrad.tensor import Tensor
 from tinygrad.helpers import round_up
 from tinygrad.uop.ops import UOp
@@ -26,6 +29,7 @@ from msgq.visionipc import VisionIpcClient, VisionBuf
 from opendbc.car.car_helpers import get_demo_car_params
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.params import Params
+from openpilot.common.hardware.usb import cable_connected
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.transformations.camera import DEVICE_CAMERAS
@@ -36,7 +40,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan,
 from openpilot.selfdrive.modeld.parse_model_outputs import Parser
 from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_driving_model_data, fill_pose_msg, PublishState
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
-from openpilot.selfdrive.modeld.helpers import MODELS_DIR, chestnut_present, chestnut_compiled, modeld_pkl_path, load_oob
+from openpilot.selfdrive.modeld.helpers import MODELS_DIR, chestnut_present, chestnut_compiled, modeld_pkl_path, load_oob, wait_for_chestnut
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
@@ -205,6 +209,8 @@ class ModelState(ModelStateBase):
     self.input_device.copy_from(self.input_host)
     self.input_queues['new_img'] = self.run_warp(**self.warp_inputs)
     self.run_model(output_buffers=self.outputs, **self.input_queues)
+    if self.chestnut and (err := Device[self.outputs['outputs'].device].error_state.host.view(fmt='q')[0]):
+      raise RuntimeError(f"chestnut failed with {err}")
     if after_enqueue is not None:
       after_enqueue()
     model_output = self.outputs['outputs'].numpy()[0]
@@ -230,7 +236,7 @@ class ModelState(ModelStateBase):
 def main(demo=False):
   cloudlog.warning("modeld init")
 
-  CHESTNUT = chestnut_present() and chestnut_compiled()
+  CHESTNUT = chestnut_compiled() and (chestnut_present() or cable_connected())
   if CHESTNUT:
     from tinygrad.runtime.ops_amd import AMDDevice
     AMDDevice.wait_timeout_ms = 3000
@@ -238,7 +244,7 @@ def main(demo=False):
   params.put_bool("ChestnutLoading", CHESTNUT)
   params.remove("ChestnutActive")
 
-  config_realtime_process(7, 54)
+  gc.disable()
 
   # visionipc clients
   while True:
@@ -271,6 +277,7 @@ def main(demo=False):
     def load_big():
       nonlocal big_model
       try:
+        wait_for_chestnut()
         m = ModelState(vipc_client_main.width, vipc_client_main.height, True)
         m.warmup()
         big_model = m
@@ -280,7 +287,13 @@ def main(demo=False):
     loader.start()
     loader.join(BIG_MODEL_TIMEOUT)
     model = big_model
-    params.put_bool("ChestnutActive", model is not None)
+    if model is None and not params.get_bool("ChestnutRetry"):
+      # a crank can cut the chestnut power during the load, retry once with an onroad cycle
+      cloudlog.error("big model load failed, cycling onroad to retry")
+      params.put_bool("ChestnutRetry", True)
+      params.put_bool("OnroadCycleRequested", True)
+    else:
+      params.put_bool("ChestnutActive", model is not None)
 
   small_model = ModelState(vipc_client_main.width, vipc_client_main.height, False) if model is None or CHESTNUT else None
   if model is None:
@@ -289,10 +302,13 @@ def main(demo=False):
   params.put_bool("ChestnutLoading", False)
   cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
 
+  config_realtime_process(7, 54)
+
   # messaging
   pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutGpuState"] if CHESTNUT else [])
   pm = PubMaster(pub_socks)
-  sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"])
+  sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState",
+                  "carControl", "controlsState", "lateralDelay"])
 
   publish_state = PublishState()
   params = Params()
@@ -410,7 +426,7 @@ def main(demo=False):
                        run_count % round(ModelConstants.MODEL_RUN_FREQ / SERVICE_LIST['chestnutGpuState'].frequency) == 0)
       model_output = model.run(bufs, transforms, inputs, chestnut_state.send if send_chestnut else None)
     except Exception:
-      if not params.get_bool("ChestnutActive"):
+      if not model.chestnut:
         raise
       # fallback to small model
       cloudlog.exception("big model failed, fall back to small")
@@ -430,6 +446,8 @@ def main(demo=False):
       posenet_send = messaging.new_message('cameraOdometry')
       mdv2sp_send = messaging.new_message('modelDataV2SP')
 
+      if not sm['carControl'].latActive:
+        prev_action.desiredCurvature = sm['controlsState'].curvature
       action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
       prev_action = action
       fill_model_msg(modelv2_send, model_output, action,

openpilot/selfdrive/modeld/models/big_driving_tinygrad.pkl

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:76cc0a9bc3af7a318889483dcbe126337f8d338f5abcbe664a8988c9b18b6639
-size 776634338
+oid sha256:d4c7a3fcfc95813826ca871a80bea18df833aacb2eb6f5333e25a01e65b1a422
+size 799941965

openpilot/selfdrive/modeld/models/big_driving_warp_1344x760_tinygrad.pkl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:18c994e4500fe9f34daa8f33202a8606d3817e09e2c79ae8f9795e37159b5143
+size 850515

openpilot/selfdrive/modeld/models/big_driving_warp_1928x1208_tinygrad.pkl

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

openpilot/selfdrive/modeld/models/dmonitoring_model.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:dd299afabe7a3e0d04cbe2bd97fdb0c93bba8ad6d3cc3663a0e0ededaf243ac2
-size 7497335
+oid sha256:dee5a294e8afaacc9295ac5d100e00733ecac278e79264b96331e40a3ede1b04
+size 7844499

openpilot/selfdrive/monitoring/policy.py

@@ -51,6 +51,7 @@ class DRIVER_MONITOR_SETTINGS:
     self._SG_THRESHOLD = 0.9
     self._BLINK_THRESHOLD = 0.865
     self._PHONE_THRESH = 0.5
+    self._SLEEP_THRESH = 0.75
     self._POSE_PITCH_THRESHOLD = 0.3133
     self._POSE_PITCH_THRESHOLD_SLACK = 0.3237
     self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD
@@ -138,6 +139,7 @@ class DriverMonitoring:
     self.pose = DriverPose(settings=self.settings)
     self.blink = DriverBlink()
     self.phone_prob = 0.
+    self.sleep_prob = 0.
 
     self.alert_level = AlertLevel.none
     self.always_on = always_on
@@ -240,6 +242,7 @@ class DriverMonitoring:
     self.distracted_types['pose'] = bool((pitch_error > pitch_threshold) or (yaw_error > yaw_threshold))
     self.distracted_types['eye'] = bool((self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD)
     self.distracted_types['phone'] = bool(self.phone_prob > self.settings._PHONE_THRESH)
+    self.distracted_types['sleep'] = bool(self.sleep_prob > self.settings._SLEEP_THRESH)
 
   def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, lowspeed, demo_mode=False, steering_angle_deg=0.):
     rhd_pred = driver_state.wheelOnRightProb
@@ -277,6 +280,7 @@ class DriverMonitoring:
     self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \
                       * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
     self.phone_prob = driver_data.phoneProb
+    self.sleep_prob = driver_data.sleepProb
 
     self._get_distracted_types()
     self.driver_distracted = any(self.distracted_types.values()) and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
@@ -406,6 +410,7 @@ class DriverMonitoring:
     dm.visionPolicyState.distractedTypes.pose = self.distracted_types['pose']
     dm.visionPolicyState.distractedTypes.eye = self.distracted_types['eye']
     dm.visionPolicyState.distractedTypes.phone = self.distracted_types['phone']
+    dm.visionPolicyState.distractedTypes.sleep = self.distracted_types['sleep']
     dm.visionPolicyState.faceDetected = self.face_detected
     dm.visionPolicyState.pose.pitch = self.pose.pitch
     dm.visionPolicyState.pose.yaw = self.pose.yaw

openpilot/selfdrive/monitoring/test_monitoring.py

@@ -28,6 +28,7 @@ def make_msg(face_detected, distracted=False, model_uncertain=False):
   ds.leftDriverData.facePositionStd = [1.*model_uncertain, 1.*model_uncertain]
   # TODO: test both separately when e2e is used
   ds.leftDriverData.phoneProb = 0.
+  ds.leftDriverData.sleepProb = 0.
   return ds
 
 

openpilot/selfdrive/navigation/helpers.py

@@ -0,0 +1,9 @@
+import time
+
+
+def latest_gps_source(sm) -> str | None:
+  """The service with the freshest usable GPS fix, or None."""
+  now = time.monotonic()
+  sources = [s for s in ('gpsLocation', 'gpsLocationExternal')
+             if sm.valid[s] and sm[s].hasFix and 0 <= now - sm.recv_time[s] < 2]
+  return max(sources, key=lambda s: sm.logMonoTime[s]) if sources else None

openpilot/selfdrive/navigation/mapsd.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+import time
+
+import numpy as np
+
+import openpilot.cereal.messaging as messaging
+from msgq.visionipc import VisionIpcServer
+from openpilot.cereal.visionipc import VisionStreamType
+from openpilot.common.realtime import Ratekeeper
+from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.navigation.helpers import latest_gps_source
+from openpilot.selfdrive.navigation.pmtiles import PMTiles
+from openpilot.selfdrive.navigation.renderer import SIZE, MapRenderer
+from openpilot.selfdrive.navigation.route import RouteTracker
+from openpilot.selfdrive.navigation.tiles import RemoteTiles
+
+RETRY_SECONDS = 10
+
+
+class MapsD:
+  def __init__(self):
+    self.renderer = None
+    self.tracker = None
+    self.route = np.empty((0, 2))
+    self.prefetched = False
+    self.retry_at = 0.0
+
+  def update(self, sm) -> tuple[np.ndarray, int]:
+    """The map frame for the latest messages: black with a zero timestamp without a usable map, never stale."""
+    if sm.updated['navRoute']:
+      route = np.array([(c.longitude, c.latitude) for c in sm['navRoute'].coordinates], dtype=np.float64).reshape((-1, 2))
+      if not np.array_equal(route, self.route):
+        self.route = route
+        self.tracker = RouteTracker(route) if len(route) >= 2 else None  # progress along the route starts over
+        self.prefetched = False
+    now = time.monotonic()
+    source = latest_gps_source(sm)
+    try:
+      if self.tracker is not None and now >= self.retry_at:
+        if self.renderer is None:
+          self.renderer = MapRenderer(PMTiles(RemoteTiles()))
+        if not self.prefetched:
+          # TODO: fetch the tiles more intelligently on demand
+          # Every tile the route can show is fetched up front, so each render below is a cache hit.
+          self.renderer.prefetch(self.route)
+          self.prefetched = True
+        if source is not None:
+          gps = sm[source]
+          # As in training, the map is centered on the route and headed along it, not at the GPS fix.
+          image, valid = self.renderer.render(*self.tracker.snap(gps.longitude, gps.latitude), self.route)
+          if valid:
+            return image, sm.logMonoTime[source]
+    except Exception:  # a failed fetch, or a corrupt cached tile
+      cloudlog.exception('mapsd.render_failed')
+      self.close()  # a changed archive needs a fresh reader
+      self.retry_at = now + RETRY_SECONDS
+    return np.zeros((SIZE, SIZE), dtype=np.uint8), 0
+
+  def close(self):
+    if self.renderer is not None:
+      self.renderer.archive.close()
+    self.renderer = None
+    self.prefetched = False
+
+
+def main():
+  rk = Ratekeeper(20.0)
+  sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal', 'navRoute'])
+  server = VisionIpcServer('mapsd')
+  server.create_buffers_with_sizes(VisionStreamType.VISION_STREAM_MAP, 4, SIZE, SIZE, SIZE * SIZE, SIZE, SIZE * SIZE)
+  server.start_listener()
+  mapsd = MapsD()
+  try:
+    while True:
+      sm.update(0)
+      image, timestamp = mapsd.update(sm)
+      server.send(VisionStreamType.VISION_STREAM_MAP, image.tobytes(), rk.frame, timestamp, time.monotonic_ns())
+      rk.keep_time()
+  finally:
+    mapsd.close()
+
+
+if __name__ == '__main__':
+  main()