Merge remote-tracking branch 'origin/comma-pkl-converter' into tn

these models will fail, look at workflow, likely need env setup now that we use recursion

.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_*_tinygrad.pkl filter=lfs diff=lfs merge=lfs -text
+openpilot/selfdrive/modeld/models/big_driving_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

.gitmodules

@@ -4,6 +4,7 @@
 [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

Jenkinsfile

@@ -161,14 +161,6 @@ 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"
@@ -179,36 +171,6 @@ 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('\\*', '.*')
@@ -242,18 +204,9 @@ node {
     }
 
     if (env.BRANCH_NAME == '__nightly-chestnut') {
-      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"),
-          ])
-        },
-      )
+      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"),
+      ])
     }
 
     if (!env.BRANCH_NAME.matches(excludeRegex)) {
@@ -308,7 +261,12 @@ node {
         ])
       },
       'chestnut': {
-        chestnutStage()
+        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"),
+        ])
       },
 
     )

RELEASES.md

@@ -1,14 +1,4 @@
-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)
+Version 0.11.2 (2026-08-12)
 =======================
 * 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('#.gitmodules').abspath) # file absent on release branch, see release_files.py
+release = not os.path.exists(File('#.gitattributes').abspath) # file absent on release branch, see release_files.py
 AddOption('--minimal',
           action='store_false',
           dest='extras',
@@ -316,6 +316,8 @@ 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()
@@ -324,6 +326,7 @@ 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):
@@ -339,3 +342,13 @@ 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,8 +56,6 @@ 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.9"
+  export AGNOS_VERSION="19.7"
 fi
 
 export STAGING_ROOT="/data/safe_staging"

openpilot/cereal/custom.capnp

@@ -203,6 +203,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
+  accelController @8 :AccelController;
 
   deprecated :group {
     dec @0 :DynamicExperimentalControlDEPRECATED;
@@ -212,6 +213,10 @@ 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;
@@ -309,6 +314,17 @@ 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 {
@@ -345,7 +361,7 @@ struct OnroadEventSP @0xda96579883444c35 {
     silentParkBrake @11;
     controlsMismatchLateral @12;
     hyundaiRadarTracksConfirmed @13;
-    experimentalModeSwitchedDEPRECATED @14;
+    experimentalModeSwitched @14;
     wrongCarModeAlertOnly @15;
     pedalPressedAlertOnly @16;
     laneTurnLeft @17;
@@ -450,6 +466,8 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
 
 struct CarStateSP @0xb86e6369214c01c8 {
   speedLimit @0 :Float32;
+  engineOff @1 :Bool;
+  engineRpm @2 :Float32;
 }
 
 struct LiveMapDataSP @0xf416ec09499d9d19 {

openpilot/cereal/log.capnp

@@ -128,7 +128,6 @@ struct OnroadEvent @0xc4fa6047f024e718 {
     personalityChanged @91;
     aeb @92;
     userBookmark @95;
-    userBookmarkNotPaired @104;
     excessiveActuation @96;
     bigModelLoading @100;
     bigModelFailed @102;
@@ -167,7 +166,12 @@ 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);
@@ -185,17 +189,6 @@ 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;
@@ -206,6 +199,14 @@ 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 {
@@ -571,9 +572,6 @@ struct PandaState @0xa7649e2575e4591e {
   controlsAllowedLateral @38 :Bool;
   controlsAllowedLongitudinal @39 :Bool;
 
-  nmiReset @40 :Bool;
-  hardfaultReset @41 :Bool;
-
   enum FaultStatus {
     none @0;
     faultTemp @1;
@@ -696,7 +694,6 @@ struct PeripheralState {
 
 struct UsbState {
   devices @0 :List(Device);
-  connected @1 :Bool;
 
   struct Device {
     busnum @0 :UInt8;
@@ -822,6 +819,7 @@ struct SelfdriveState {
   alertHudVisual @12 :Car.CarControl.HUDControl.VisualAlert;
 
   # configurable driving settings
+  experimentalMode @10 :Bool;
   personality @11 :LongitudinalPersonality;
 
   enum AudibleAlert {
@@ -865,7 +863,6 @@ struct SelfdriveState {
 
   deprecated :group {
     alertSound @8 :Car.CarControl.HUDControl.AudibleAlert;
-    experimentalMode @10 :Bool;
   }
 }
 
@@ -1240,7 +1237,6 @@ struct LongitudinalPlan @0xe00b5b3eba12876c {
   shouldStop @37: Bool;
   allowThrottle @38: Bool;
   allowBrake @39: Bool;
-  accelBoost @40 :Float32;
 
 
   solverExecutionTime @35 :Float32;
@@ -2238,7 +2234,6 @@ struct DriverMonitoringState {
       pose @0: Bool;
       eye @1: Bool;
       phone @2: Bool;
-      sleep @3: Bool;
     }
 
     struct Pose {
@@ -2710,8 +2705,8 @@ struct Event {
     driverMonitoringStateDEPRECATED @71 :DriverMonitoringStateDEPRECATED;
     gpsNMEADEPRECATED @3 :GPSNMEAData;
     uploaderStateDEPRECATED @79 :UploaderState;
-    navInstruction @82 :NavInstruction;
-    navRoute @83 :NavRoute;
+    navInstructionDEPRECATED @82 :NavInstruction;
+    navRouteDEPRECATED @83 :NavRoute;
     navThumbnailDEPRECATED @84 :Thumbnail;
     gnssMeasurementsDEPRECATED @91 :GnssMeasurements;
     mapRenderStateDEPRECATED @105: MapRenderState;

openpilot/cereal/services.py

@@ -52,8 +52,6 @@ _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 API_HOST as API_HOST, CommaConnectApi
+from openpilot.common.api.comma_connect import CommaConnectApi
 
 
 class Api:

openpilot/common/constants.py

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

openpilot/common/file_chunker.py

@@ -0,0 +1,75 @@
+#!/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,6 +27,16 @@ 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-b9c9b9261fca0cef9e6d9b198fa4b563767e6342afc7a61ab137b2b519260615.img.xz",
-    "hash": "b9c9b9261fca0cef9e6d9b198fa4b563767e6342afc7a61ab137b2b519260615",
-    "hash_raw": "b9c9b9261fca0cef9e6d9b198fa4b563767e6342afc7a61ab137b2b519260615",
-    "size": 46962688,
+    "url": "https://commadist.azureedge.net/agnosupdate/boot-6ecf6f987cd11968104abcccabbe268485d329cdb73012dfd3c381a6b8deb27d.img.xz",
+    "hash": "6ecf6f987cd11968104abcccabbe268485d329cdb73012dfd3c381a6b8deb27d",
+    "hash_raw": "6ecf6f987cd11968104abcccabbe268485d329cdb73012dfd3c381a6b8deb27d",
+    "size": 46897152,
     "sparse": false,
     "full_check": true,
     "has_ab": true,
-    "ondevice_hash": "3ef4769bb1cc73bfc31f96f073b46af42dcc0bad4ab2dde32d11929745d0cd28"
+    "ondevice_hash": "d12e1e5b9455b62a1464558716493b33e470d7a7e88da1c4105a3b21d0961808"
   },
   {
     "name": "system",
-    "url": "https://commadist.azureedge.net/agnosupdate/system-80d9373348ddff2a5d1f534fab3420383939c96f3eba60baab6b15339fc602c7.img.xz",
-    "hash": "9aebca09b9d142718e71efbfe44b723010eec6e70067c043c63186067291739d",
-    "hash_raw": "80d9373348ddff2a5d1f534fab3420383939c96f3eba60baab6b15339fc602c7",
+    "url": "https://commadist.azureedge.net/agnosupdate/system-3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f.img.xz",
+    "hash": "74ffc9c551e1f29cda897ace8a69080fe644f8039977c6885f2b48362e39b744",
+    "hash_raw": "3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f",
     "size": 4718592000,
     "sparse": true,
     "full_check": false,
     "has_ab": true,
-    "ondevice_hash": "270b76c5c5568828e3883390b83ea445425a104dd42119d68f06093ce6b6fcef",
+    "ondevice_hash": "6a992680183685eea9db99d915219a37935f45989330d9b619e880450257f448",
     "alt": {
-      "hash": "80d9373348ddff2a5d1f534fab3420383939c96f3eba60baab6b15339fc602c7",
-      "url": "https://commadist.azureedge.net/agnosupdate/system-80d9373348ddff2a5d1f534fab3420383939c96f3eba60baab6b15339fc602c7.img",
+      "hash": "3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f",
+      "url": "https://commadist.azureedge.net/agnosupdate/system-3c271e2b3d20d2f0a8bf6555a1319f3efb12845490967d6151195174a01e912f.img",
       "size": 4718592000
     }
   }

openpilot/common/hardware/comma/hardware.py

@@ -8,7 +8,6 @@ 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
@@ -208,8 +207,6 @@ 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,8 +17,6 @@ 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",
@@ -46,6 +44,7 @@ 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,22 +9,25 @@ from openpilot.common.realtime import Ratekeeper
 from openpilot.common.filter_simple import FirstOrderFilter
 
 
-def read_power():
+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
   with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/power1_input") as f:
     return int(f.read()) / 1e6
 
-def sample_power(seconds=5) -> list[float]:
+def sample_power(seconds=5, panda=None) -> list[float]:
   rate = 123
   rk = Ratekeeper(rate, print_delay_threshold=None)
 
   pwrs = []
   for _ in range(rate*seconds):
-    pwrs.append(read_power())
+    pwrs.append(read_power(panda))
     rk.keep_time()
   return pwrs
 
-def get_power(seconds=5):
-  pwrs = sample_power(seconds)
+def get_power(seconds=5, panda=None):
+  pwrs = sample_power(seconds, panda)
   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:f92c0a9b41613e063b6a1cd086729f5555d9df6482249e171ed1e276272cad7d
-size 24709520
+oid sha256:6a7adb302d378dda7b1788a841b89e4f905c872550a70a76650dcde977b4ece0
+size 24709209

openpilot/common/hardware/usb.py

@@ -1,7 +1,7 @@
 import os
 from pathlib import Path
 
-CHESTNUT_FW_VERSION = "8516b768"
+CHESTNUT_FW_VERSION = "ed4e39b7"
 CHESTNUT_USB_IDS = ((0xADD1, 0x0001), (0x3801, 0x0001))
 CHESTNUT_ROM_USB_IDS = ((0x174C, 0x2464), (0x174C, 0x2463))
 CHESTNUT_USB_PRODUCT = f"custom {CHESTNUT_FW_VERSION}-CLEAN"
@@ -36,10 +36,6 @@ 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())
@@ -95,4 +91,3 @@ 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,6 +40,8 @@ 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}},
@@ -67,7 +69,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, BYTES}},
+    {"LastGPSPosition", {PERSISTENT, STRING}},
     {"LastManagerExitReason", {CLEAR_ON_MANAGER_START, STRING}},
     {"LastOffroadStatusPacket", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON}},
     {"LastAgnosPowerMonitorShutdown", {CLEAR_ON_MANAGER_START, STRING}},
@@ -137,15 +139,6 @@ 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 --- //
@@ -199,8 +192,16 @@ 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"}},
@@ -249,6 +250,10 @@ 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"}},
@@ -256,7 +261,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}},
     {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
-    {"LanefulStrength", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
 
     // mapd

openpilot/common/realtime.py

@@ -91,13 +91,8 @@ class Ratekeeper:
     self.avg_dt.add_value(self._last_monitor_time - prev)
 
     lagged = False
-    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
+    remaining = self._next_frame_time - time.monotonic()
+    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,12 +117,16 @@ 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,6 +5,7 @@ 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
@@ -156,6 +157,20 @@ 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.3"
+#define COMMA_VERSION "0.11.2"

openpilot/selfdrive/assets/icons/couch.png

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

openpilot/selfdrive/assets/icons/couch.svg

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

openpilot/selfdrive/assets/icons/experimental.png

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

openpilot/selfdrive/assets/icons/experimental.svg

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

openpilot/selfdrive/assets/icons/experimental_grey.png

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

openpilot/selfdrive/assets/icons/experimental_grey.svg

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

openpilot/selfdrive/assets/icons/experimental_white.png

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

openpilot/selfdrive/assets/icons/experimental_white.svg

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

openpilot/selfdrive/assets/icons_mici/experimental_mode.png

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -1,3 +0,0 @@
-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:92431d37a844206570d73d48f39dd8d934539a89668fe8d5f4cccae38f7b301b
-size 11530
+oid sha256:fd91685bf656e828648acf035a4737acb2c4709e8514cf0aa0a10fa470a9bb60
+size 11580

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

openpilot/selfdrive/assets/sounds/critical_max.wav

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

openpilot/selfdrive/assets/sounds/dm_critical_max.wav

@@ -1,3 +0,0 @@
-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:884e828e5c51d980d41b6e2400fcb4f4a926275018a00674d8239184e1eb96db
+oid sha256:1bb440f424c989c06e203bfba0b32d39aede8f6c78a9b335e5e778460202b601
 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,6 +123,9 @@ 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)
@@ -178,7 +181,7 @@ class Car:
     self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP)
 
     self.is_metric = self.params.get_bool("IsMetric")
-    self.initial_cruise_speed = self.params.get("InitialCruiseSpeedKph", return_default=True)
+    self.experimental_mode = self.params.get_bool("ExperimentalMode")
 
     # card is driven by can recv, expected at 100Hz
     self.rk = Ratekeeper(100, print_delay_threshold=None)
@@ -211,7 +214,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.initial_cruise_speed)
+      self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode)
 
     # TODO: mirror the carState.cruiseState struct?
     CS.vCruise = float(self.v_cruise_helper.v_cruise_kph)
@@ -296,7 +299,7 @@ class Car:
   def params_thread(self, evt):
     while not evt.is_set():
       self.is_metric = self.params.get_bool("IsMetric")
-      self.initial_cruise_speed = self.params.get("InitialCruiseSpeedKph", return_default=True)
+      self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
 
       # sunnypilot
       self.v_cruise_helper.read_custom_set_speed_params()

openpilot/selfdrive/car/cruise.py

@@ -13,11 +13,13 @@ 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,
@@ -42,6 +44,30 @@ 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
 
@@ -50,11 +76,21 @@ class VCruiseHelper(VCruiseHelperSP):
     _enabled = self.update_enabled_state(CS, enabled)
 
     if CS.cruiseState.available:
-      if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled):
+      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 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()
-        self.v_cruise_cluster_kph = self.v_cruise_kph
+        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
       else:
         self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
         self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
@@ -84,13 +120,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] > CRUISE_LONG_PRESS:
+        if self.button_timers[b.type.raw] > self.cruise_long_press_frames:
           return  # end long press
         button_type = b.type.raw
         break
     else:
       for k, timer in self.button_timers.items():
-        if timer and timer % CRUISE_LONG_PRESS == 0:
+        if timer and timer % self.cruise_long_press_frames == 0:
           button_type = k
           long_press = True
           break
@@ -114,10 +150,26 @@ class VCruiseHelper(VCruiseHelperSP):
       return
 
     long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, 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
+    # 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
     else:
-      self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
+      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
 
     # If set is pressed while overriding, clip cruise speed to minimum of vEgo
     if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
@@ -126,6 +178,12 @@ 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:
@@ -137,14 +195,16 @@ 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, initial_speed: int) -> None:
+  def initialize_v_cruise(self, CS, experimental_mode: bool) -> 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(max(CS.vEgo * CV.MS_TO_KPH, initial_speed), V_CRUISE_INITIAL, V_CRUISE_MAX)))
+      self.v_cruise_kph = int(round(np.clip(CS.vEgo * CV.MS_TO_KPH, 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, personality, t_end=20.):
+def run_cruise_simulation(cruise, e2e, personality, t_end=20.):
   man = Maneuver(
     '',
     duration=t_end,
@@ -24,6 +24,7 @@ def run_cruise_simulation(cruise, personality, t_end=20.):
     cruise_values=[cruise],
     prob_lead_values=[0.0],
     breakpoints=[0.],
+    e2e=e2e,
     personality=personality,
   )
   valid, output = man.evaluate()
@@ -31,7 +32,8 @@ def run_cruise_simulation(cruise, personality, t_end=20.):
   return output[-1, 3]
 
 
-@parameterized_class(("personality", "speed"), itertools.product(
+@parameterized_class(("e2e", "personality", "speed"), itertools.product(
+                      [True, False], # e2e
                       log.LongitudinalPersonality.schema.enumerants, # personality
                       [5,35])) # speed
 class TestCruiseSpeed(OpenpilotTestCase):
@@ -39,7 +41,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.personality)
+    simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e, self.personality)
     self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {self.speed} m/s')
 
 
@@ -57,16 +59,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):
+  def enable(self, v_ego, experimental_mode):
     # Simulates user pressing set with a current speed
-    self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), V_CRUISE_INITIAL)
+    self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode)
 
   def test_adjust_speed(self):
     """
     Asserts speed changes on falling edges of buttons.
     """
 
-    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS)
+    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
 
     for btn in (ButtonType.accelCruise, ButtonType.decelCruise):
       for pressed in (True, False):
@@ -90,7 +92,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)
+        self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
 
       # 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)
@@ -100,7 +102,7 @@ class TestVCruiseHelper(OpenpilotTestCase):
     Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill.
     """
 
-    self.enable(0)
+    self.enable(0, False)
 
     for standstill in (True, False):
       for pressed in (True, False):
@@ -120,7 +122,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)
+      self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
 
       # first decrement speed, then perform gas pressed logic
       expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT
@@ -141,10 +143,11 @@ class TestVCruiseHelper(OpenpilotTestCase):
     Asserts allowed cruise speeds on enabling with SET.
     """
 
-    for v_ego in np.linspace(0, 100, 101):
-      self.reset_cruise_speed_state()
-      assert not self.v_cruise_helper.v_cruise_initialized
+    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
 
-      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
+        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

openpilot/selfdrive/controls/controlsd.py

@@ -136,6 +136,7 @@ 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

@@ -1,38 +0,0 @@
-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,16 +64,19 @@ 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 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))
+        blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
+                              (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
 
-        self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
+        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)
 
         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 blindspot_detected:
+        elif (torque_applied or self.alc.auto_lane_change_allowed) and not self.alc.lane_change_blocked:
           self.lane_change_state = LaneChangeState.laneChangeStarting
           self.lane_change_timer = 0.0
 

openpilot/selfdrive/controls/lib/drive_helpers.py

@@ -4,6 +4,7 @@ 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,13 +1,11 @@
 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
 
-LAT_ACCEL_SATURATION_THRESHOLD = 0.5  # m/s^2
+CURVATURE_SATURATION_THRESHOLD = 1e-3  # 1/m
 
 
 class LatControlCurvature(LatControl):
@@ -22,8 +20,6 @@ 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()
@@ -34,9 +30,6 @@ 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
@@ -60,6 +53,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(expected_curvature - actual_curvature) * CS.vEgo ** 2 > LAT_ACCEL_SATURATION_THRESHOLD, CS,
+    curvature_log.saturated = bool(self._check_saturation(abs(error) > CURVATURE_SATURATION_THRESHOLD, CS,
                                                           False, curvature_limited))
     return 0.0, float(output_curvature), curvature_log

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+import math
 import numpy as np
 
 import openpilot.cereal.messaging as messaging
@@ -7,7 +8,6 @@ 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,15 +17,42 @@ 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]
-J_CRUISE_BP = [0., 10.0, 25., 40.]
-A_CRUISE_MIN = -0.5
+A_CRUISE_MIN = -1.2
 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_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)
+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)
   target_accel = float(np.clip(target_accel, a_cruise_prev - j_cruise * dt, a_cruise_prev + j_cruise * dt))
 
   return target_accel
@@ -38,7 +65,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc)
     self.fcw = False
     self.dt = dt
-    self.accel_boost = AccelBoost()
+    self.allow_throttle = True
 
     self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
     self.a_cruise = init_a
@@ -52,10 +79,16 @@ 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
-    if sm['controlsState'].forceDecel:
+    force_decel = sm['controlsState'].forceDecel
+    if force_decel:
       v_cruise = 0.0
 
     long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
@@ -66,6 +99,12 @@ 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)
@@ -81,10 +120,7 @@ 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)
-    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.set_cur_state(self.v_desired_filter.x, self.output_a_target)
     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)
@@ -106,18 +142,29 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
-    self.a_cruise = get_cruise_accel(v_cruise, v_ego, self.a_cruise, self.dt)
+    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,
+    )
     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),
-                  (output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)]
+                  (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)]
+    if experimental_mode:
+      candidates.append((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
 
@@ -140,9 +187,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', 'radarState', 'modelV2', 'selfdriveState',
+  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'vehicleParameters', '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, personality=0):
+def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personality=0):
   man = Maneuver(
     '',
     duration=t_end,
@@ -22,6 +22,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0, personality=0):
     initial_distance_lead=100,
     speed_lead_values=[v_lead],
     breakpoints=[0.],
+    e2e=e2e,
     personality=personality,
   )
   valid, output = man.evaluate()
@@ -29,7 +30,8 @@ def run_following_distance_simulation(v_lead, t_end=100.0, personality=0):
   return output[-1,2] - output[-1,1]
 
 
-@parameterized_class(("personality", "speed"), itertools.product(
+@parameterized_class(("e2e", "personality", "speed"), itertools.product(
+                      [True, False], # e2e
                       [log.LongitudinalPersonality.relaxed, # personality
                        log.LongitudinalPersonality.standard,
                        log.LongitudinalPersonality.aggressive],
@@ -37,8 +39,9 @@ def run_following_distance_simulation(v_lead, t_end=100.0, personality=0):
 class TestFollowingDistance(OpenpilotTestCase):
   def test_following_distance(self):
     v_lead = float(self.speed)
-    simulation_steady_state = run_following_distance_simulation(v_lead, personality=self.personality)
+    simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e, 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=0.2 * correct_steady_state + abs_err_margin)
+                           delta=err_ratio * correct_steady_state + abs_err_margin)

openpilot/selfdrive/controls/tests/test_latcontrol.py

@@ -8,14 +8,12 @@ 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
@@ -24,8 +22,7 @@ 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),
-                         (VOLKSWAGEN.VOLKSWAGEN_ID4_MK2, LatControlCurvature)])
+                         (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_EUV, LatControlTorque)])
   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,6 +80,7 @@ class CarKalman(KalmanFilter):
   }
 
   global_vars = [
+    'mass',
     'rotational_inertia',
     'center_to_front',
     'center_to_rear',
@@ -98,10 +99,7 @@ class CarKalman(KalmanFilter):
 
     # globals
     global_vars = [sp.Symbol(name) for name in CarKalman.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
+    m, j, aF, aR, cF_orig, cR_orig = global_vars
 
     # make functions and jacobians with sympy
     # state variables
@@ -168,7 +166,8 @@ 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, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
+  def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
+    self.filter.set_global("mass", mass)
     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,
-                       0.1**2, 0.1**2, 0.1**2,
+                       1**2, 1**2, 1**2,
                        100**2, 100**2, 100**2,
                        0.01**2, 0.01**2, 0.01**2])
 

openpilot/selfdrive/locationd/paramsd.py

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

openpilot/selfdrive/modeld/SConscript

@@ -1,20 +1,14 @@
-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)
 
@@ -30,13 +24,33 @@ 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
-# build on all cores
-taskset = 'taskset -c 0-7 ' if arch == 'comma_arm64' else ''
+# 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")
 
 def compile_model(onnx_path, pkl_path):
   onnx_path, target_pkl_path = File(onnx_path).abspath, File(pkl_path).abspath
@@ -52,14 +66,19 @@ compile_model('models/dmonitoring_model.onnx', 'models/dmonitoring_model_tinygra
 compile_model('models/driving_supercombo.onnx', 'models/driving_tinygrad.pkl')
 
 model_w, model_h = MEDMODEL_INPUT_SIZE
-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)
+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)
 
 dm_w, dm_h = DM_INPUT_SIZE
 for cam_w, cam_h in camera_configs:

openpilot/selfdrive/modeld/helpers.py

@@ -1,7 +1,11 @@
+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
 
@@ -13,11 +17,18 @@ def modeld_pkl_path(chestnut: bool):
   return MODELS_DIR / f'{prefix}driving_tinygrad.pkl'
 
 def load_oob(path, chestnut=False):
-  from tinygrad import Context
+  from tinygrad import Context, Device
+  from tinygrad.device import Buffer
   device = 'USB+AMD:LLVM' if chestnut else 'QCOM' if AGNOS else 'METAL' if sys.platform == 'darwin' else 'CPU:LLVM'
-  with Context(DEV=device):
-    from tinygrad_repo.examples.openpilot.helpers import load_pickle
-    return load_pickle(path, out_of_band=True)
+  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()
 
 def chestnut_present() -> bool:
   for d in USB_DEVICES_PATH.glob("*"):
@@ -30,15 +41,7 @@ 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() and all(
+  return (path.is_file() or Path(get_manifest_path(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,14 +2,12 @@
 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')
 from tinygrad.device import Buffer, Device
-from tinygrad.dtype import DType, dtypes
 from tinygrad.engine.realize import lower_and_compile
+from tinygrad.dtype import DType, dtypes
 from tinygrad.tensor import Tensor
 from tinygrad.helpers import round_up
 from tinygrad.uop.ops import UOp
@@ -28,7 +26,6 @@ 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
@@ -39,7 +36,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, wait_for_chestnut
+from openpilot.selfdrive.modeld.helpers import MODELS_DIR, chestnut_present, chestnut_compiled, modeld_pkl_path, load_oob
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
@@ -208,8 +205,6 @@ 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]
@@ -235,7 +230,7 @@ class ModelState(ModelStateBase):
 def main(demo=False):
   cloudlog.warning("modeld init")
 
-  CHESTNUT = chestnut_compiled() and (chestnut_present() or cable_connected())
+  CHESTNUT = chestnut_present() and chestnut_compiled()
   if CHESTNUT:
     from tinygrad.runtime.ops_amd import AMDDevice
     AMDDevice.wait_timeout_ms = 3000
@@ -243,7 +238,7 @@ def main(demo=False):
   params.put_bool("ChestnutLoading", CHESTNUT)
   params.remove("ChestnutActive")
 
-  gc.disable()
+  config_realtime_process(7, 54)
 
   # visionipc clients
   while True:
@@ -276,7 +271,6 @@ 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
@@ -286,13 +280,7 @@ def main(demo=False):
     loader.start()
     loader.join(BIG_MODEL_TIMEOUT)
     model = big_model
-    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)
+    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:
@@ -301,13 +289,10 @@ 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", "controlsState", "lateralDelay"])
+  sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"])
 
   publish_state = PublishState()
   params = Params()
@@ -425,7 +410,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 model.chestnut:
+      if not params.get_bool("ChestnutActive"):
         raise
       # fallback to small model
       cloudlog.exception("big model failed, fall back to small")
@@ -445,8 +430,6 @@ 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:d4c7a3fcfc95813826ca871a80bea18df833aacb2eb6f5333e25a01e65b1a422
-size 799941965
+oid sha256:76cc0a9bc3af7a318889483dcbe126337f8d338f5abcbe664a8988c9b18b6639
+size 776634338

openpilot/selfdrive/modeld/models/big_driving_warp_1344x760_tinygrad.pkl

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

openpilot/selfdrive/modeld/models/big_driving_warp_1928x1208_tinygrad.pkl

@@ -1,3 +0,0 @@
-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:dee5a294e8afaacc9295ac5d100e00733ecac278e79264b96331e40a3ede1b04
-size 7844499
+oid sha256:dd299afabe7a3e0d04cbe2bd97fdb0c93bba8ad6d3cc3663a0e0ededaf243ac2
+size 7497335

openpilot/selfdrive/monitoring/policy.py

@@ -51,7 +51,6 @@ 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
@@ -139,7 +138,6 @@ 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
@@ -242,7 +240,6 @@ 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
@@ -280,7 +277,6 @@ 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
@@ -410,7 +406,6 @@ 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,7 +28,6 @@ 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

@@ -1,9 +0,0 @@
-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

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

openpilot/selfdrive/navigation/navd.py

@@ -1,225 +0,0 @@
-#!/usr/bin/env python3
-import os
-
-import numpy as np
-import requests
-
-import openpilot.cereal.messaging as messaging
-from openpilot.common.api import API_HOST, Api
-from openpilot.common.params import Params
-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.route import along_path, local_meters
-
-REROUTE_DISTANCE = 25
-MANEUVER_TRANSITION_THRESHOLD = 10
-REROUTE_COUNTER_MIN = 3
-ROUTE_RESEND_PERIOD = 10  # updates (seconds); a restarted mapsd gets the route within this
-RECOMPUTE_HOLD = 10  # updates (seconds) between route requests
-
-
-def set_banner(instruction, banners, distance_to_maneuver):
-  """Fill the banner fields from the banner to show now: the last one still ahead of the ego, else the first."""
-  if not banners:
-    return
-  banner = next((b for b in reversed(banners) if distance_to_maneuver < b['distanceAlongGeometry']), banners[0])
-  instruction.showFull = distance_to_maneuver < banner['distanceAlongGeometry']
-  primary = banner['primary']
-  instruction.maneuverPrimaryText = primary.get('text') or ''
-  instruction.maneuverType = primary.get('type') or ''
-  instruction.maneuverModifier = primary.get('modifier') or ''
-  if banner.get('secondary') is not None:
-    instruction.maneuverSecondaryText = banner['secondary']['text']
-
-
-class RouteEngine:
-  def __init__(self, sm, pm, params=None):
-    self.sm = sm
-    self.pm = pm
-    self.params = params if params is not None else Params()
-
-    self.last_position = None  # (longitude, latitude)
-    self.last_bearing = None
-
-    self.nav_destination = None
-    self.step_idx = None
-    self.route = None
-    self.route_geometry = None  # an (n, 2) array of (longitude, latitude) per step
-
-    self.recompute_hold = 0
-    self.reroute_counter = 0
-
-    self.mapbox_token = os.environ.get("MAPBOX_TOKEN")
-    self.api = Api(self.params.get("DongleId")) if self.mapbox_token is None else None
-    self.mapbox_host = "https://api.mapbox.com" if self.mapbox_token is not None else API_HOST.rstrip('/') + '/v1/nav'
-
-  def update(self):
-    self.sm.update(0)
-
-    source = latest_gps_source(self.sm)
-    if source is not None:
-      gps = self.sm[source]
-      self.last_position, self.last_bearing = (gps.longitude, gps.latitude), gps.bearingDeg
-    route = self.route
-    try:
-      self.recompute_route(gps_ok=source is not None)
-      self.send_instruction()
-    except Exception:
-      cloudlog.exception("navd.failed_to_compute")
-    # mapsd dedupes repeats, so a periodic resend is how a restarted mapsd gets the route
-    if self.route is not route or self.sm.frame % ROUTE_RESEND_PERIOD == 0:
-      self.send_route()
-
-  def recompute_route(self, gps_ok: bool):
-    if self.last_position is None:
-      return
-
-    destination = self.params.get("NavDestination")
-    if destination is None:
-      self.clear_route()
-      self.recompute_hold = 0
-      return
-    new_destination = (destination['longitude'], destination['latitude'])
-
-    # Don't recompute when GPS drifts in tunnels
-    if not gps_ok and self.step_idx is not None:
-      return
-
-    self.recompute_hold -= 1
-    if (self.should_recompute() or new_destination != self.nav_destination) and self.recompute_hold <= 0:
-      self.recompute_hold = RECOMPUTE_HOLD
-      self.calculate_route(new_destination)
-      self.reroute_counter = 0
-
-  def calculate_route(self, destination):
-    assert self.last_position is not None
-    cloudlog.warning(f"Calculating route {self.last_position} -> {destination}")
-    self.nav_destination = destination
-
-    lang = self.params.get('LanguageSetting')
-    if lang is not None:
-      lang = lang.replace('main_', '')
-
-    token = self.mapbox_token
-    if token is None:
-      token = self.api.get_token()
-
-    params = {
-      'access_token': token,
-      'geometries': 'geojson',
-      'overview': 'full',
-      'steps': 'true',
-      'banner_instructions': 'true',
-      'alternatives': 'false',
-      'language': lang,
-    }
-    if self.last_bearing is not None:
-      params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90;"
-
-    coords_str = ';'.join(f'{lon},{lat}' for lon, lat in (self.last_position, destination))
-    url = self.mapbox_host + '/directions/v5/mapbox/driving-traffic/' + coords_str
-    try:
-      resp = requests.get(url, params=params, timeout=10)
-      if resp.status_code != 200:
-        cloudlog.event("API request failed", status_code=resp.status_code, text=resp.text, error=True)
-      resp.raise_for_status()
-
-      r = resp.json()
-      if len(r['routes']):
-        self.route = r['routes'][0]['legs'][0]['steps']
-        self.route_geometry = [np.array(step['geometry']['coordinates'], dtype=np.float64).reshape((-1, 2)) for step in self.route]
-        self.step_idx = 0
-      else:
-        cloudlog.warning("Got empty route response")
-        self.clear_route()
-
-    except requests.exceptions.RequestException:
-      cloudlog.exception("failed to get route")
-      self.clear_route()
-
-  def send_instruction(self):
-    msg = messaging.new_message('navInstruction', valid=True)
-
-    if self.step_idx is None:
-      msg.valid = False
-      self.pm.send('navInstruction', msg)
-      return
-
-    assert self.route is not None and self.route_geometry is not None and self.last_position is not None
-    step = self.route[self.step_idx]
-    along_geometry, _ = along_path(self.route_geometry[self.step_idx], self.last_position)
-    distance_to_maneuver_along_geometry = step['distance'] - along_geometry
-
-    # Banner instructions are for the following maneuver step, don't use empty last step
-    banner_step = step
-    if not len(banner_step['bannerInstructions']) and self.step_idx == len(self.route) - 1:
-      banner_step = self.route[max(self.step_idx - 1, 0)]
-
-    # Current instruction
-    msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry
-    set_banner(msg.navInstruction, banner_step['bannerInstructions'], distance_to_maneuver_along_geometry)
-
-    # Total remaining distance and time: the rest of this step, then every step after it
-    remaining = 1.0 - along_geometry / max(step['distance'], 1)
-    future = self.route[self.step_idx + 1:]
-    msg.navInstruction.distanceRemaining = step['distance'] * remaining + sum(s['distance'] for s in future)
-    msg.navInstruction.timeRemaining = step['duration'] * remaining + sum(s['duration'] for s in future)
-
-    self.pm.send('navInstruction', msg)
-
-    # Transition to next route segment
-    if distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD:
-      if self.step_idx + 1 < len(self.route):
-        self.step_idx += 1
-        self.recompute_hold = 0
-      else:
-        cloudlog.warning("Destination reached")
-
-        # Clear route if driving away from destination
-        if np.linalg.norm(local_meters(self.last_position, self.nav_destination)) > REROUTE_DISTANCE:
-          self.params.remove("NavDestination")
-          self.clear_route()
-
-  def send_route(self):
-    coords = np.concatenate(self.route_geometry) if self.route_geometry is not None else np.empty((0, 2))
-    msg = messaging.new_message('navRoute', valid=True)
-    msg.navRoute.coordinates = [{'longitude': lon, 'latitude': lat} for lon, lat in coords.tolist()]
-    self.pm.send('navRoute', msg)
-
-  def clear_route(self):
-    self.route = None
-    self.route_geometry = None
-    self.step_idx = None
-    self.nav_destination = None
-
-  def should_recompute(self):
-    if self.step_idx is None:
-      return True
-
-    assert self.route is not None and self.route_geometry is not None and self.last_position is not None
-    # Don't recompute in last segment, assume destination is reached
-    if self.step_idx == len(self.route) - 1:
-      return False
-
-    # TODO: Check for going wrong way in segment
-    if along_path(self.route_geometry[self.step_idx], self.last_position)[1] > REROUTE_DISTANCE:
-      self.reroute_counter += 1
-    else:
-      self.reroute_counter = 0
-    return self.reroute_counter > REROUTE_COUNTER_MIN
-
-
-def main():
-  pm = messaging.PubMaster(['navInstruction', 'navRoute'])
-  sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal'])
-
-  rk = Ratekeeper(1.0)
-  route_engine = RouteEngine(sm, pm)
-  while True:
-    route_engine.update()
-    rk.keep_time()
-
-
-if __name__ == "__main__":
-  main()

openpilot/selfdrive/navigation/pmtiles.py

@@ -1,136 +0,0 @@
-"""PMTiles v3 reader for the gzip-compressed Protomaps MVT basemap, over any byte-range source."""
-import gzip
-import struct
-from bisect import bisect_right
-from functools import lru_cache
-
-HEADER_LENGTH = 127
-
-
-def read_varint(data: bytes, offset: int) -> tuple[int, int]:
-  value = 0
-  for shift in range(0, 70, 7):
-    if offset >= len(data):
-      raise ValueError("Truncated varint")
-    byte = data[offset]
-    offset += 1
-    value |= (byte & 127) << shift
-    if byte < 128:
-      return value, offset
-  raise ValueError("Varint exceeds 64 bits")
-
-
-def tile_id(z: int, x: int, y: int) -> int:
-  """PMTiles orders each zoom level along a Hilbert curve."""
-  if not 0 <= z <= 26 or not 0 <= x < 1 << z or not 0 <= y < 1 << z:
-    raise ValueError("Invalid tile coordinates")
-  distance = 0
-  size = (1 << z) // 2
-  while size:
-    rx, ry = int(bool(x & size)), int(bool(y & size))
-    distance += size * size * ((3 * rx) ^ ry)
-    if not ry:
-      if rx:
-        x, y = size - 1 - x, size - 1 - y
-      x, y = y, x
-    size //= 2
-  return ((1 << (2 * z)) - 1) // 3 + distance
-
-
-def decode_directory(data: bytes) -> tuple[list[int], list[tuple[int, int, int]]]:
-  count, offset = read_varint(data, 0)
-  if count > len(data):
-    raise ValueError("Invalid directory entry count")
-  columns = []
-  for _ in range(4):
-    column = []
-    for _ in range(count):
-      value, offset = read_varint(data, offset)
-      column.append(value)
-    columns.append(column)
-  ids, runs, lengths, offsets = columns
-  for i in range(count):
-    if i:
-      ids[i] += ids[i - 1]
-    if offsets[i] == 0:
-      if i == 0:
-        raise ValueError("First directory offset cannot be implicit")
-      offsets[i] = offsets[i - 1] + lengths[i - 1]
-    else:
-      offsets[i] -= 1
-    if lengths[i] == 0 or (i and ids[i] <= ids[i - 1]):
-      raise ValueError("Invalid directory entry")
-  if offset != len(data):
-    raise ValueError("Trailing directory data")
-  return ids, list(zip(runs, lengths, offsets, strict=True))
-
-
-class PMTiles:
-  def __init__(self, source):
-    """source has size, read(offset, length), prefetch(ranges) and close()."""
-    self.source = source
-    self.size = source.size
-    try:
-      header = self.read(0, HEADER_LENGTH)
-      if header[:8] != b'PMTiles\x03':
-        raise ValueError("Expected a PMTiles v3 archive")
-      self.root_offset, self.root_length, _, _, self.leaf_offset, self.leaf_length, self.data_offset, self.data_length = struct.unpack_from('<8Q', header, 8)
-      compression, tile_type, self.min_zoom, self.max_zoom = header[97:99], header[99], header[100], header[101]
-      if compression != b'\x02\x02' or tile_type != 1:
-        raise ValueError("Expected gzip-compressed MVT vector tiles")
-      if not 0 <= self.min_zoom <= self.max_zoom <= 26:
-        raise ValueError("Unsupported PMTiles zoom range")
-      for offset, length in ((self.root_offset, self.root_length), (self.leaf_offset, self.leaf_length), (self.data_offset, self.data_length)):
-        if offset + length > self.size:
-          raise ValueError("PMTiles section outside archive")
-      self.directory = lru_cache(maxsize=32)(self._read_directory)
-      self.directory(self.root_offset, self.root_length)
-    except Exception:
-      self.source.close()
-      raise
-
-  def read(self, offset: int, length: int) -> bytes:
-    if offset < 0 or length < 0 or offset + length > self.size:
-      raise ValueError("PMTiles read outside archive")
-    data = self.source.read(offset, length)
-    if len(data) != length:
-      raise ValueError("Truncated PMTiles archive")
-    return data
-
-  def _read_directory(self, offset: int, length: int):
-    return decode_directory(gzip.decompress(self.read(offset, length)))
-
-  def locate(self, z: int, x: int, y: int) -> tuple[int, int] | None:
-    """The byte range of a tile in the archive, or None when there is no such tile."""
-    if not self.min_zoom <= z <= self.max_zoom:
-      return None
-    target = tile_id(z, x, y)
-    offset, length = self.root_offset, self.root_length
-    for _ in range(4):
-      ids, entries = self.directory(offset, length)
-      index = bisect_right(ids, target) - 1
-      if index < 0:
-        return None
-      run, length, offset = entries[index]
-      if run:
-        if target >= ids[index] + run:
-          return None
-        if offset + length > self.data_length:
-          raise ValueError("Tile outside PMTiles data section")
-        return self.data_offset + offset, length
-      if offset + length > self.leaf_length:
-        raise ValueError("Directory outside PMTiles leaf section")
-      offset += self.leaf_offset
-    raise ValueError("PMTiles directory nesting exceeds four levels")
-
-  def get(self, z: int, x: int, y: int) -> bytes | None:
-    located = self.locate(z, x, y)
-    return gzip.decompress(self.read(*located)) if located else None
-
-  def prefetch(self, tiles):
-    """Cache the (z, x, y) tiles ahead of reads."""
-    self.source.prefetch([located for located in (self.locate(*tile) for tile in tiles) if located])
-
-  def close(self):
-    self.directory.cache_clear()
-    self.source.close()

openpilot/selfdrive/navigation/renderer.py

@@ -1,208 +0,0 @@
-"""Deterministic CPU rasterization of Protomaps roads and a supplied route.
-
-Styled after the training map renderer: 1 m per pixel, the ego three quarters
-down the frame, pure white roads whose casings are part of the stroke, bridges cutting over the roads beneath
-them, a grey route and a white ego arrow.
-"""
-import math
-from functools import lru_cache
-
-import numpy as np
-
-from openpilot.selfdrive.navigation.pmtiles import PMTiles
-from openpilot.selfdrive.navigation.route import cumulative_distance, nearest_segment
-from openpilot.selfdrive.navigation.vector_tile import Road, decode_roads
-
-SIZE = 256
-ZOOM = 15  # tile zoom; the style zoom is derived from the 1 m per pixel scale instead
-FORWARD_OFFSET = 64.0  # the ego sits three quarters down the frame
-EGO = np.array([SIZE / 2, SIZE / 2 + FORWARD_OFFSET])  # pixel position of the ego
-EARTH_CIRCUMFERENCE = 40075016.68557849  # equatorial, for Web Mercator
-BACKGROUND = 0
-ROAD_INTENSITY = 255
-ROUTE_INTENSITY = 128
-ROUTE_WIDTH = 5.0  # pixels
-CUT_WIDTH = 3.0  # black separator under a bridge, added to the road width
-MAX_ROAD_DISTANCE = 100.0  # meters from the ego to the nearest road centerline for a valid map
-ARROW = np.array([(0, -9), (7, 7), (0, 3), (-7, 7)], dtype=np.float64)  # ego marker, pixels around the ego
-ARROW_OUTLINE = 2.0  # pixels
-
-
-# The training style's road classes as (zoom, width, zoom, width) stops for the interior and for one side of the
-# casing, in pixels. Casings are white there too, so they only widen the stroke. Only each class's top interval is
-# kept: the style zoom at 1 m per pixel stays above 14 until 78 degrees of latitude, where the lower stops begin.
-MAJOR, MEDIUM, STREET, MINOR, TRACK = range(5)
-STOPS = np.array([[[5, 0.75, 18, 32], [10, 1, 18, 2]],  # MAJOR: motorway, trunk, primary
-                  [[5, 0.1, 18, 26], [10, 0.75, 18, 2]],  # MEDIUM: secondary, tertiary
-                  [[14, 2, 18, 18], [12, 0.75, 20, 2]],  # STREET: residential, unclassified, living streets, major links
-                  [[14, 0.5, 18, 12], [12, 0.75, 20, 2]],  # MINOR: service roads and minor links
-                  [[14, 1, 18, 12], [12, 0.75, 20, 2]]], dtype=np.float64)  # TRACK
-
-
-def stroke_widths(zoom: float, styles: np.ndarray, levels: np.ndarray) -> np.ndarray:
-  """Full stroke width of each segment: the style's exponential (base 1.5) interpolation between its stops, flat
-  beyond them, with the casing on both sides except for tunnels, which are drawn at their interior width."""
-  z0, w0, z1, w1 = np.moveaxis(STOPS, -1, 0)
-  z = np.clip(zoom, z0, z1)
-  table = w0 + (w1 - w0) * (1.5 ** (z - z0) - 1) / (1.5 ** (z1 - z0) - 1)  # (style, interior / casing)
-  return table[styles, 0] + np.where(levels < 0, 0, 2 * table[styles, 1])
-
-
-def road_style(road: Road) -> int | None:
-  """The style for a road, or None when hidden. Protomaps kind_detail is the underlying OSM highway type."""
-  detail = road.detail
-  if detail in ('platform', 'pier', 'raceway'):
-    return None
-  if detail in ('motorway', 'trunk', 'primary') or (not detail and road.kind == 'highway'):
-    return MAJOR
-  if detail in ('secondary', 'tertiary') or (not detail and road.kind == 'major_road'):
-    return MEDIUM
-  if detail == 'track':
-    return TRACK
-  if detail in ('secondary_link', 'tertiary_link', 'service', 'driveway', 'parking_aisle', 'alley') or (
-    road.kind == 'other' and detail != 'living_street'):
-    return MINOR
-  return STREET
-
-
-def road_level(road: Road) -> int:
-  """Draw order: tunnels, then ground roads, then bridges, each cutting over the levels beneath."""
-  return 1 if road.bridge else -1 if road.tunnel else 0
-
-
-def mercator(coordinates: np.ndarray) -> np.ndarray:
-  """Longitude/latitude -> normalized Web Mercator, x east / y south."""
-  longitude, latitude = coordinates.T
-  y = (1.0 - np.arcsinh(np.tan(np.deg2rad(latitude))) / np.pi) / 2.0
-  return np.column_stack(((longitude + 180.0) / 360.0, y))
-
-
-def project(points: np.ndarray, longitude: float, latitude: float, bearing: float) -> np.ndarray:
-  """Mercator points to pixels of a frame at a position, headed along a bearing in degrees clockwise from north."""
-  origin = mercator(np.array([[longitude, latitude]]))[0]
-  delta = points - origin
-  delta[:, 0] = (delta[:, 0] + 0.5) % 1.0 - 0.5  # wrap across the antimeridian
-  delta *= EARTH_CIRCUMFERENCE * math.cos(math.radians(latitude))  # meters, which are pixels
-  c, s = math.cos(math.radians(bearing)), math.sin(math.radians(bearing))
-  return EGO + delta @ np.array([[c, -s], [s, c]])  # x right, y down
-
-
-def draw_segments(image: np.ndarray, starts: np.ndarray, ends: np.ndarray, radii: np.ndarray | float, intensity: int,
-                  *, overlay: bool = False, caps: np.ndarray | None = None):
-  """Strokes with round joins and one pixel of analytic edge antialiasing.
-
-  caps is (segments, 2): whether each segment's start/end gets a round cap; without it every end is round.
-  Max blending (the default) makes draw order irrelevant; overlay replaces the pixels beneath.
-  """
-  radii = np.broadcast_to(np.asarray(radii, dtype=np.float64), (len(starts),))
-  reach = radii[:, None] + 1
-  low = np.maximum(np.floor(np.minimum(starts, ends) - reach), 0).astype(int)
-  high = np.minimum(np.ceil(np.maximum(starts, ends) + reach), [image.shape[1] - 1, image.shape[0] - 1]).astype(int)
-  # Cull offscreen segments in one pass, then loop over plain Python floats.
-  visible = np.flatnonzero((high >= low).all(axis=1))
-  round_caps = np.ones((len(starts), 2), dtype=bool) if caps is None else caps
-  for i, (sx, sy), (ex, ey), (x0, y0), (x1, y1), radius in zip(visible.tolist(), starts[visible].tolist(), ends[visible].tolist(),
-                                                               low[visible].tolist(), high[visible].tolist(), radii[visible].tolist(),
-                                                               strict=True):
-    dx, dy = ex - sx, ey - sy
-    px = np.arange(x0, x1 + 1) + (0.5 - sx)
-    py = (np.arange(y0, y1 + 1) + (0.5 - sy))[:, None]
-    along = (px * dx + py * dy) / max(dx * dx + dy * dy, 1e-12)
-    t = np.clip(along, 0, 1)
-    alpha = np.clip(radius + 0.5 - np.hypot(px - t * dx, py - t * dy), 0, 1)
-    if not round_caps[i, 0]:
-      alpha = alpha * (along >= 0)
-    if not round_caps[i, 1]:
-      alpha = alpha * (along <= 1)
-    view = image[y0:y1 + 1, x0:x1 + 1]
-    if overlay:
-      view[:] = np.rint(view * (1 - alpha) + intensity * alpha).astype(np.uint8)
-    else:
-      np.maximum(view, np.rint(alpha * intensity).astype(np.uint8), out=view)
-
-
-def draw_vehicle(image: np.ndarray):
-  """The training ego marker: a white arrow with a black outline, drawn over everything else."""
-  arrow = ARROW + EGO
-  (x0, y0), (x1, y1) = (np.floor(arrow.min(axis=0)) - 2).astype(int), (np.ceil(arrow.max(axis=0)) + 2).astype(int)
-  px, py = np.meshgrid(np.arange(x0, x1) + 0.5, np.arange(y0, y1) + 0.5)
-  inside = np.zeros(px.shape, dtype=bool)
-  for a, b, c in ((arrow[0], arrow[1], arrow[2]), (arrow[0], arrow[2], arrow[3])):  # the concave arrow as two triangles
-    sides = [(q[0] - p[0]) * (py - p[1]) - (q[1] - p[1]) * (px - p[0]) for p, q in ((a, b), (b, c), (c, a))]
-    inside |= ((sides[0] >= 0) & (sides[1] >= 0) & (sides[2] >= 0)) | ((sides[0] <= 0) & (sides[1] <= 0) & (sides[2] <= 0))
-  image[y0:y1, x0:x1][inside] = 255
-  draw_segments(image, arrow, np.roll(arrow, -1, axis=0), ARROW_OUTLINE / 2, 0, overlay=True)
-
-
-class MapRenderer:
-  def __init__(self, archive: PMTiles):
-    self.archive = archive
-    self.zoom = min(max(ZOOM, archive.min_zoom), archive.max_zoom)
-    self.roads = lru_cache(maxsize=128)(self._load_roads)
-
-  def _load_roads(self, x: int, y: int):
-    """A tile's visible road segments as flat arrays: mercator starts/ends, style index, level and polyline end flags."""
-    tile = self.archive.get(self.zoom, x, y)
-    if tile is None:
-      return None
-    starts, ends, styles, levels, caps = [np.empty((0, 2))], [np.empty((0, 2))], [np.empty(0, int)], [np.empty(0, int)], [np.empty((0, 2), bool)]
-    for road in decode_roads(tile):
-      style = road_style(road)
-      if style is None:
-        continue
-      points = (road.points + np.array([x, y])) / (1 << self.zoom)
-      n = len(points) - 1
-      starts.append(points[:-1])
-      ends.append(points[1:])
-      styles.append(np.full(n, style))
-      levels.append(np.full(n, road_level(road)))
-      ends_of_line = np.zeros((n, 2), dtype=bool)
-      ends_of_line[0, 0] = ends_of_line[-1, 1] = True
-      caps.append(ends_of_line)
-    return np.concatenate(starts), np.concatenate(ends), np.concatenate(styles), np.concatenate(levels), np.concatenate(caps)
-
-  def view_tiles(self, longitude: float, latitude: float) -> list[tuple[int, int]]:
-    """Tiles covering the view at a point for every heading."""
-    origin = mercator(np.array([[longitude, latitude]]))[0]
-    # A bounding circle includes all headings, forward offset and road stroke widths.
-    radius = (SIZE / math.sqrt(2) + FORWARD_OFFSET + 16) / (EARTH_CIRCUMFERENCE * math.cos(math.radians(latitude)))
-    count = 1 << self.zoom
-    low = np.floor((origin - radius) * count).astype(int)
-    high = np.floor((origin + radius) * count).astype(int)
-    return [(x % count, y) for y in range(low[1], high[1] + 1) for x in range(low[0], high[0] + 1) if 0 <= y < count]
-
-  def route_tiles(self, route: np.ndarray) -> set[tuple[int, int]]:
-    """Every tile the view can show while the pose is snapped onto the route, sampled every 100 m."""
-    distance = cumulative_distance(route)
-    samples = np.column_stack([np.interp(np.arange(0, distance[-1], 100), distance, route[:, axis]) for axis in (0, 1)])
-    return {tile for lon, lat in np.vstack((samples, route)).tolist() for tile in self.view_tiles(lon, lat)}
-
-  def prefetch(self, route: np.ndarray):
-    self.archive.prefetch((self.zoom, x, y) for x, y in self.route_tiles(route))
-
-  def render(self, longitude: float, latitude: float, bearing: float, route: np.ndarray) -> tuple[np.ndarray, bool]:
-    """The map image at a position and bearing, and whether every tile was present with a road near the ego."""
-    image = np.full((SIZE, SIZE), BACKGROUND, dtype=np.uint8)
-    loaded = [self.roads(x, y) for x, y in self.view_tiles(longitude, latitude)]
-    tiles = [tile for tile in loaded if tile is not None]
-    # The training renderer's latitude-dependent style zoom: the web mercator zoom with one meter per pixel
-    # at this latitude, under the 512-pixel tile convention. Tile zoom and style zoom are independent.
-    style_zoom = math.log2(math.cos(math.radians(latitude)) * EARTH_CIRCUMFERENCE / 256) - 1
-    road_distance = math.inf
-    if tiles:
-      starts, ends, styles, levels, caps = (np.concatenate(column) for column in zip(*tiles, strict=True))
-      starts, ends = project(starts, longitude, latitude, bearing), project(ends, longitude, latitude, bearing)
-      widths = stroke_widths(style_zoom, styles, levels)
-      if len(starts):
-        road_distance = nearest_segment(starts, ends, EGO)[2]
-      for level in np.unique(levels):
-        chosen = levels == level
-        if level > levels.min():
-          # Cut the roads beneath, open at way endpoints so a bridge does not end in a black oval.
-          draw_segments(image, starts[chosen], ends[chosen], (widths[chosen] + CUT_WIDTH) / 2, BACKGROUND,
-                        overlay=True, caps=~caps[chosen])
-        draw_segments(image, starts[chosen], ends[chosen], widths[chosen] / 2, ROAD_INTENSITY)
-    pixels = project(mercator(route), longitude, latitude, bearing)
-    draw_segments(image, pixels[:-1], pixels[1:], ROUTE_WIDTH / 2, ROUTE_INTENSITY, overlay=True)
-    draw_vehicle(image)
-    return image, len(tiles) == len(loaded) and road_distance <= MAX_ROAD_DISTANCE

openpilot/selfdrive/navigation/route.py

@@ -1,71 +0,0 @@
-"""Planar geometry for routes, and route-relative map pose and progress tracking."""
-import numpy as np
-
-METERS_PER_DEGREE = 2 * np.pi * 6371008.8 / 360  # mean Earth radius, as Mapbox measures distances
-
-# Training renders the map on the route rather than at the measured position:
-# the ego is projected onto the route, the frame is centered there and headed along the route's tangent.
-PROJECTION_WINDOW = (-30.0, 100.0)  # meters around the previous progress to search; a loop cannot capture the ego
-HEADING_HALF_WINDOW = 50.0  # meters; the heading is the chord of the route this far behind and ahead
-
-
-def local_meters(points, origin) -> np.ndarray:
-  """Equirectangular east/north meters of (longitude, latitude) points around an origin."""
-  origin = np.asarray(origin, dtype=np.float64)
-  scale = np.array([METERS_PER_DEGREE * np.cos(np.radians(origin[1])), METERS_PER_DEGREE])
-  return (np.asarray(points, dtype=np.float64) - origin) * scale
-
-
-def nearest_segment(starts: np.ndarray, ends: np.ndarray, point) -> tuple[int, float, float]:
-  """(index, fraction along it, distance) of the closest spot on the segments, in the inputs' units."""
-  ab = ends - starts
-  t = np.clip(((point - starts) * ab).sum(axis=1) / np.maximum((ab * ab).sum(axis=1), 1e-12), 0, 1)
-  d = np.linalg.norm(starts + t[:, None] * ab - point, axis=1)
-  i = int(d.argmin())
-  return i, float(t[i]), float(d[i])
-
-
-def cumulative_distance(route: np.ndarray) -> np.ndarray:
-  """Meters along [(longitude, latitude), ...]."""
-  (lon1, lat1), (lon2, lat2) = route[:-1].T, route[1:].T
-  steps = np.hypot((lon2 - lon1) * np.cos(np.radians((lat1 + lat2) / 2)), lat2 - lat1) * METERS_PER_DEGREE
-  return np.r_[0.0, np.cumsum(steps)]
-
-
-def along_path(path: np.ndarray, point) -> tuple[float, float]:
-  """(meters along a (longitude, latitude) path to a point, and the point's distance from the path):
-  the path up to the start of the nearest segment, then straight to the point."""
-  local, p = local_meters(path, path[0]), local_meters(point, path[0])
-  segment, _, lateral = nearest_segment(local[:-1], local[1:], p)
-  return float(cumulative_distance(path)[segment] + np.linalg.norm(local_meters(point, path[segment]))), lateral
-
-
-class RouteTracker:
-  """Progress of the ego along a route, in meters, and the map pose at that progress."""
-
-  def __init__(self, route: np.ndarray):
-    self.route = np.asarray(route, dtype=np.float64).reshape((-1, 2))
-    self.path = local_meters(self.route, self.route[0])
-    self.distance = cumulative_distance(self.route)
-    self.progress = -np.inf  # the first fix may land anywhere on the route
-
-  def project(self, longitude: float, latitude: float) -> float:
-    """Advance to the nearest spot on the route within the window ahead of the previous progress."""
-    near = np.flatnonzero((self.distance[1:] >= self.progress + PROJECTION_WINDOW[0]) & (self.distance[:-1] <= self.progress + PROJECTION_WINDOW[1]))
-    if not len(near):  # only before the first fix, when the progress is still -inf
-      near = np.arange(len(self.path) - 1)
-    point = local_meters([longitude, latitude], self.route[0])
-    segment, along, _ = nearest_segment(self.path[near], self.path[near + 1], point)
-    segment = near[segment]
-    self.progress = max(self.progress, self.distance[segment] + along * (self.distance[segment + 1] - self.distance[segment]))
-    return self.progress
-
-  def pose(self, progress: float) -> tuple[float, float, float]:
-    """(longitude, latitude, bearing) on the route at a progress, headed along the +-50m chord."""
-    ends = np.clip([progress - HEADING_HALF_WINDOW, progress, progress + HEADING_HALF_WINDOW], 0, self.distance[-1])
-    (lon0, lon, lon1), (lat0, lat, lat1) = (np.interp(ends, self.distance, self.route[:, axis]) for axis in (0, 1))
-    bearing = np.degrees(np.arctan2((lon1 - lon0) * np.cos(np.radians(lat0)), lat1 - lat0)) % 360
-    return float(lon), float(lat), float(bearing)
-
-  def snap(self, longitude: float, latitude: float) -> tuple[float, float, float]:
-    return self.pose(self.project(longitude, latitude))

openpilot/selfdrive/navigation/tests/test_navigation.py

@@ -1,485 +0,0 @@
-import gzip
-import re
-import struct
-import tempfile
-import threading
-import time
-import unittest
-from contextlib import closing, contextmanager
-from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
-from pathlib import Path
-from types import SimpleNamespace
-from unittest.mock import MagicMock, patch
-
-import numpy as np
-import requests
-
-import openpilot.cereal.messaging as messaging
-from openpilot.common.prefix import OpenpilotPrefix
-from openpilot.selfdrive.navigation.mapsd import MapsD
-from openpilot.selfdrive.navigation.navd import REROUTE_COUNTER_MIN, ROUTE_RESEND_PERIOD, RouteEngine, set_banner
-from openpilot.selfdrive.navigation.pmtiles import PMTiles, decode_directory, tile_id
-from openpilot.selfdrive.navigation.renderer import MAJOR, MEDIUM, MINOR, SIZE, STREET, TRACK, MapRenderer, draw_segments, mercator, project, stroke_widths
-from openpilot.selfdrive.navigation.route import RouteTracker
-from openpilot.selfdrive.navigation.tiles import RemoteTiles
-from openpilot.selfdrive.navigation.vector_tile import decode_lines, decode_roads
-
-METER = 1 / 111319.49079327358  # degrees of latitude
-
-
-def draw_line(image, points, intensity, width, **kwargs):
-  draw_segments(image, points[:-1], points[1:], width / 2, intensity, **kwargs)
-
-
-def varint(value):
-  output = bytearray()
-  while value > 127:
-    output.append((value & 127) | 128)
-    value >>= 7
-  output.append(value)
-  return bytes(output)
-
-
-def field(number, value):
-  if isinstance(value, bytes):
-    return varint(number << 3 | 2) + varint(len(value)) + value
-  return varint(number << 3) + varint(value)
-
-
-def road_tile(crossing=None):
-  # One vertical line, from (2048, 0) to (2048, 4096), and one non-road layer. With crossing='bridge' or 'tunnel',
-  # a horizontal line from (1024, 2048) to (3072, 2048) at that level.
-  geometry = b''.join(varint(v) for v in [9, 4096, 0, 10, 0, 8192])
-  feature = field(2, b'\x00\x00') + field(3, 2) + field(4, geometry)
-  roads = field(15, 2) + field(1, b'roads') + field(2, feature)
-  if crossing is not None:
-    geometry = b''.join(varint(v) for v in [9, 2048, 4096, 10, 4096, 0])
-    roads += field(2, field(2, b'\x00\x00\x01\x01') + field(3, 2) + field(4, geometry))
-  roads += field(3, b'kind') + field(3, f'is_{crossing}'.encode()) + field(4, field(1, b'minor_road')) + field(4, field(7, 1)) + field(5, 4096)
-  return field(3, field(1, b'buildings')) + field(3, roads)
-
-
-def make_archive(payload=None, leaf=False, run=1, max_zoom=0):
-  """A PMTiles archive with one zoom-zero tile, in the root directory or in a leaf directory."""
-  tile = gzip.compress(road_tile() if payload is None else payload)
-  directory = gzip.compress(varint(1) + varint(0) + varint(run) + varint(len(tile)) + varint(1))
-  child = directory if leaf else b''
-  root = gzip.compress(varint(1) + varint(0) + varint(0) + varint(len(child)) + varint(1)) if leaf else directory
-  metadata = gzip.compress(b'{"vector_layers":[{"id":"roads"}]}')
-  header = bytearray(127)
-  header[:8] = b'PMTiles\x03'
-  offsets = [127, 127 + len(root), 127 + len(root) + len(metadata), 127 + len(root) + len(metadata) + len(child)]
-  struct.pack_into('<11Q', header, 8, offsets[0], len(root), offsets[1], len(metadata), offsets[2], len(child), offsets[3], len(tile), 1, 1, 1)
-  header[96:102] = bytes([1, 2, 2, 1, 0, max_zoom])
-  return bytes(header + root + metadata + child + tile)
-
-
-@contextmanager
-def local_archive(data):
-  """A PMTiles reader over in-memory bytes, bypassing the tile server."""
-  source = MagicMock(size=len(data))
-  source.read.side_effect = lambda offset, length: data[offset:offset + length]
-  with closing(PMTiles(source)) as archive:
-    yield archive
-
-
-def mapbox_response(steps):
-  response = MagicMock(status_code=200)
-  response.json.return_value = {'routes': [{'legs': [{'steps': steps}]}]}
-  return response
-
-
-def route_engine(sm, pm, values):
-  params = MagicMock()
-  params.get.side_effect = values.get
-  params.remove.side_effect = lambda key: values.pop(key, None)
-  with patch('openpilot.selfdrive.navigation.navd.Api') as api:
-    api.return_value.get_token.return_value = 'test-token'
-    return RouteEngine(sm, pm, params=params)
-
-
-class RangeHandler(BaseHTTPRequestHandler):
-  def do_GET(self):
-    self.server.requests.append(dict(self.headers))
-    if self.headers.get('If-Match', self.server.etag) != self.server.etag:
-      self.send_error(412)
-      return
-    if self.server.ignore_range:
-      self.send_response(200)
-      self.end_headers()
-      return
-    offset, end = map(int, re.fullmatch(r'bytes=(\d+)-(\d+)', self.headers['Range']).groups())
-    data = self.server.archive[offset:end + 1]
-    self.send_response(206)
-    self.send_header('Content-Range', f'bytes {offset}-{end}/{len(self.server.archive)}')
-    self.send_header('Content-Length', str(len(data)))
-    self.send_header('ETag', self.server.etag)
-    self.end_headers()
-    self.wfile.write(data)
-
-  def log_message(self, format: str, *args: object) -> None:  # noqa: A002  # stdlib override
-    pass
-
-
-class TileServerCase(unittest.TestCase):
-  """Serves a leaf-directory archive over HTTP range requests, with the comma API and tile cache stubbed."""
-
-  def setUp(self):
-    self.directory = tempfile.TemporaryDirectory()
-    self.addCleanup(self.directory.cleanup)
-    self.cache = Path(self.directory.name) / 'navigation' / 'tiles'
-    self.server = ThreadingHTTPServer(('127.0.0.1', 0), RangeHandler)
-    self.server.archive = make_archive(leaf=True)
-    self.server.etag = '"test-version"'
-    self.server.ignore_range = False
-    self.server.requests = []
-    threading.Thread(target=self.server.serve_forever, kwargs={'poll_interval': 0.01}, daemon=True).start()
-    self.addCleanup(self.server.server_close)
-    self.addCleanup(self.server.shutdown)
-    self.api = MagicMock(dongle_id='test-dongle')
-    self.api.get_token.return_value = 'device-token'
-    self.url = f'http://127.0.0.1:{self.server.server_port}/planet.pmtiles?sig=secret'
-    self.api.get.return_value.json.return_value = {'url': self.url, 'expires': time.time() + 86400}  # noqa: TID251
-    for name, value in [('Api', MagicMock(return_value=self.api)), ('Params', MagicMock()), ('PC', True)]:
-      self.enterContext(patch(f'openpilot.selfdrive.navigation.tiles.{name}', value))
-    self.enterContext(patch('openpilot.selfdrive.navigation.tiles.Paths.comma_home', return_value=self.directory.name))
-
-
-class TestEndToEnd(TileServerCase):
-  def test_navd_to_map_frames(self):
-    """navd routes a destination over messaging; mapsd fetches tiles over HTTP and renders map frames."""
-    destination = {'latitude': 0.01, 'longitude': 0}
-    self.enterContext(patch('requests.get', return_value=mapbox_response([
-      {'geometry': {'coordinates': [[0, -0.01], [0, 0.01]]}, 'distance': 2224, 'duration': 100, 'bannerInstructions': []}])))
-    with OpenpilotPrefix():
-      pm = messaging.PubMaster(['navInstruction', 'navRoute', 'gpsLocationExternal'])
-      sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal', 'navRoute'])
-      values = {'NavDestination': dict(destination)}
-      navd = route_engine(messaging.SubMaster(['gpsLocation', 'gpsLocationExternal']), pm, values)
-      mapsd = MapsD()
-      self.addCleanup(mapsd.close)
-
-      def frame(fix=True):
-        gps = messaging.new_message('gpsLocationExternal', valid=True)
-        gps.gpsLocationExternal.hasFix = fix
-        pm.send('gpsLocationExternal', gps)
-        navd.update()
-        sm.update(0)
-        return mapsd.update(sm)
-
-      image, timestamp = frame()
-      # The route is centered over the white road, with the ego arrow three quarters down.
-      self.assertEqual((image[128, 128], image[128, 131], image[188, 128], image[195, 128]), (128, 255, 255, 0))
-      self.assertEqual(timestamp, sm.logMonoTime['gpsLocationExternal'])
-      fetched = len(self.server.requests)
-      tracker = mapsd.tracker
-      navd.send_route()  # the same route again, as a restarted mapsd would get it
-      frame()
-      self.assertIs(mapsd.tracker, tracker)  # progress along it is kept
-      values.pop('NavDestination')
-      self.assertFalse(frame()[0].any())  # black without a route
-      values['NavDestination'] = dict(destination)
-      self.assertEqual(frame()[0][128, 128], 128)
-      self.assertEqual(len(self.server.requests), fetched)  # the second route is served from the cache
-      image, timestamp = frame(fix=False)
-      self.assertFalse(image.any() or timestamp)  # black, not stale, without a fix
-      self.api.get.assert_called_once()
-      self.assertTrue(all('Range' in headers for headers in self.server.requests))
-      self.assertTrue(all(headers['If-Match'] == self.server.etag for headers in self.server.requests[1:]))
-      self.assertTrue(all(b'secret' not in p.read_bytes() for p in self.cache.iterdir()))
-      # The archive is replaced on the server and the cache evicted: the next fetch fails, the map goes black,
-      # and after a delay a fresh reader starts over on the new version.
-      self.server.etag = '"new-version"'
-      for cached in self.cache.iterdir():
-        cached.unlink()
-      values.pop('NavDestination')
-      frame()
-      values['NavDestination'] = dict(destination)
-      image, timestamp = frame()
-      self.assertFalse(image.any() or timestamp)
-      fetched = len(self.server.requests)
-      self.assertFalse(frame()[1])  # not retried yet
-      self.assertEqual(len(self.server.requests), fetched)
-      mapsd.retry_at = 0
-      self.assertEqual(frame()[0][128, 128], 128)
-      self.assertEqual((self.cache / 'version').read_text().split()[0], '"new-version"')
-
-
-class TestRemoteTiles(TileServerCase):
-  def test_shared_cache_and_refresh(self):
-    with closing(PMTiles(RemoteTiles())) as archive:
-      archive.source.expires = 0  # the signed URL is refreshed before the next fetch
-      self.assertEqual(archive.get(0, 0, 0), road_tile())
-      self.assertEqual(archive.get(0, 0, 0), road_tile())
-      self.assertEqual(self.api.get.call_count, 2)
-    count = len(self.server.requests)
-    with closing(PMTiles(RemoteTiles())) as archive:
-      self.assertEqual(archive.get(0, 0, 0), road_tile())
-    # A second reader revalidates only the header; directories and tiles are shared on disk.
-    self.assertEqual(len(self.server.requests), count + 1)
-    with patch('openpilot.selfdrive.navigation.tiles.CACHE_LIMIT', 1), closing(RemoteTiles()) as source:
-      source.read(1, 2)  # an uncached range is downloaded, then the cache is trimmed to the limit
-    self.assertEqual([p.name for p in self.cache.iterdir()], ['version'])
-
-  def test_prefetch_merges_nearby_ranges(self):
-    with patch('openpilot.selfdrive.navigation.tiles.MERGE_GAP', 16), closing(RemoteTiles()) as source:
-      count = len(self.server.requests)
-      source.prefetch([(20, 10), (0, 10), (60, 10), (20, 10)])
-      self.assertEqual(len(self.server.requests), count + 2)  # 0-29 in one request, 60-69 in another
-      self.assertEqual(self.server.requests[-2]['Range'], 'bytes=0-29')
-      for offset in (0, 20, 60):
-        self.assertEqual(source.read(offset, 10), self.server.archive[offset:offset + 10])
-      self.assertEqual(len(self.server.requests), count + 2)
-
-  def test_failures(self):
-    with closing(PMTiles(RemoteTiles())) as archive:
-      self.server.etag = '"new-version"'
-      with self.assertRaisesRegex(OSError, '412'):  # the archive changed under an open reader
-        archive.get(0, 0, 0)
-    with closing(PMTiles(RemoteTiles())) as archive:  # a new reader starts over on the new version
-      self.assertEqual(archive.get(0, 0, 0), road_tile())
-    self.server.ignore_range = True
-    with self.assertRaisesRegex(OSError, '200'):
-      RemoteTiles()
-    self.api.get.return_value.raise_for_status.side_effect = requests.HTTPError(self.url)
-    with self.assertRaisesRegex(OSError, '^Unable to fetch map tiles$'):  # the signed URL stays out of the error
-      RemoteTiles()
-
-
-class TestRouteEngine(unittest.TestCase):
-  def setUp(self):
-    self.pm = MagicMock()
-    self.values = {'NavDestination': {'latitude': 0, 'longitude': 0.002}, 'LanguageSetting': 'main_en'}
-    self.sm = MagicMock()
-    self.sm.valid = dict.fromkeys(('gpsLocation', 'gpsLocationExternal'), True)
-    self.sm.logMonoTime = dict.fromkeys(self.sm.valid, time.monotonic_ns())
-    self.sm.recv_time = dict.fromkeys(self.sm.valid, time.monotonic())
-    self.sm.__getitem__.return_value = SimpleNamespace(hasFix=True, latitude=0, longitude=0, bearingDeg=90)
-    self.engine = route_engine(self.sm, self.pm, self.values)
-    self.engine.last_position, self.engine.last_bearing = (0, 0), 90
-    distance = 111.195  # Mapbox's length of 0.001 degrees at the equator
-    self.response = mapbox_response([
-      {'geometry': {'coordinates': [[0, 0], [0.0005, 0], [0.001, 0]]}, 'distance': distance, 'duration': 10,
-       'bannerInstructions': [{'distanceAlongGeometry': 100, 'primary': {'text': 'Turn right', 'type': 'turn', 'modifier': 'right'}}]},
-      {'geometry': {'coordinates': [[0.001, 0], [0.002, 0]]}, 'distance': distance, 'duration': 10, 'bannerInstructions': []},
-    ])
-
-  def last_message(self, service):
-    return next(call.args[1] for call in reversed(self.pm.send.call_args_list) if call.args[0] == service)
-
-  def load_route(self):
-    with patch('requests.get', return_value=self.response) as get:
-      self.engine.calculate_route((0.002, 0))
-    self.engine.send_route()
-    return get
-
-  def test_request_and_instruction(self):
-    get = self.load_route()
-    self.assertEqual(get.call_args.args[0], 'https://api.commadotai.com/v1/nav/directions/v5/mapbox/driving-traffic/0,0;0.002,0')
-    options = get.call_args.kwargs['params']
-    self.assertEqual((options['access_token'], options['bearings'], options['language']), ('test-token', '90,90;', 'en'))
-    self.assertEqual(len(self.last_message('navRoute').navRoute.coordinates), 5)
-    self.engine.send_instruction()
-    message = self.last_message('navInstruction')
-    self.assertTrue(message.valid)
-    instruction = message.navInstruction.to_dict()
-    self.assertEqual((instruction['maneuverPrimaryText'], instruction['maneuverModifier'], instruction['timeRemaining']), ('Turn right', 'right', 20))
-    self.engine.last_position = (0.0007, 0)  # on the step's second segment
-    self.engine.send_instruction()
-    self.assertAlmostEqual(self.last_message('navInstruction').navInstruction.timeRemaining, 13, delta=0.05)
-    self.engine.last_position = (0.001, 0)  # at the maneuver
-    self.engine.send_instruction()
-    self.assertAlmostEqual(self.last_message('navInstruction').navInstruction.maneuverDistance, 0, delta=0.01)
-
-  def test_banner_selection(self):
-    banners = [{'distanceAlongGeometry': 100, 'primary': {'text': 'Turn right', 'type': 'turn', 'modifier': 'right'}},
-               {'distanceAlongGeometry': 50, 'primary': {'text': 'Turn right'}, 'secondary': {'text': 'onto Main St'}}]
-    # The last banner still ahead of the ego, else the first: 120 m, 70 m and 30 m from the maneuver.
-    for distance, expected in [(120, ('Turn right', '', False)), (70, ('Turn right', '', True)), (30, ('Turn right', 'onto Main St', True))]:
-      instruction = messaging.new_message('navInstruction').navInstruction
-      set_banner(instruction, banners, distance)
-      self.assertEqual((instruction.maneuverPrimaryText, instruction.maneuverSecondaryText, instruction.showFull), expected)
-
-  def test_route_sent_on_change_and_periodically(self):
-    sent = lambda: sum(call.args[0] == 'navRoute' for call in self.pm.send.call_args_list)  # noqa: E731
-    self.sm.frame = 1
-    with patch('requests.get', return_value=self.response):
-      self.engine.update()  # the route to a new destination is sent once
-      self.engine.update()
-    self.assertEqual(sent(), 1)
-    self.sm.frame = ROUTE_RESEND_PERIOD  # and again periodically, for a mapsd that started after it
-    self.engine.update()
-    self.assertEqual(sent(), 2)
-
-  def test_maneuvers_and_rerouting(self):
-    self.load_route()
-    self.engine.last_position = (0, 0.001)  # 111 m off route: reroute after a few consecutive misses
-    for _ in range(REROUTE_COUNTER_MIN):
-      self.assertFalse(self.engine.should_recompute())
-    self.assertTrue(self.engine.should_recompute())
-    with patch('requests.get') as get:
-      self.engine.recompute_route(gps_ok=False)  # but not while the GPS drifts in a tunnel
-      get.assert_not_called()
-    with patch('requests.get', return_value=self.response) as get:
-      self.engine.recompute_route(gps_ok=True)
-      self.engine.reroute_counter = REROUTE_COUNTER_MIN  # still off route, but an immediate second recompute is held back
-      self.engine.recompute_route(gps_ok=True)
-      self.assertEqual(get.call_count, 1)
-    self.engine.last_position = (0.0012, 0)  # past the first maneuver
-    self.engine.send_instruction()
-    self.assertEqual(self.engine.step_idx, 1)
-    self.engine.last_position = (0.00215, 0)  # 17 m past the destination: arrived, but the route is kept nearby
-    self.engine.send_instruction()
-    self.assertIsNotNone(self.engine.route)
-    self.engine.last_position = (0.0024, 0)  # driving away from the destination
-    self.engine.send_instruction()
-    self.assertIsNone(self.engine.route)
-    self.assertNotIn('NavDestination', self.values)
-
-  def test_failed_request_clears_route(self):
-    self.load_route()
-    self.response.raise_for_status.side_effect = requests.HTTPError('test failure')
-    self.load_route()
-    self.assertIsNone(self.engine.route)
-    self.assertEqual(len(self.last_message('navRoute').navRoute.coordinates), 0)
-    self.engine.send_instruction()
-    self.assertFalse(self.last_message('navInstruction').valid)
-
-  def test_gps_source(self):
-    fixes = {'gpsLocation': SimpleNamespace(hasFix=True, latitude=1, longitude=1, bearingDeg=180),
-             'gpsLocationExternal': SimpleNamespace(hasFix=True, latitude=2, longitude=2, bearingDeg=0)}
-    self.sm.__getitem__.side_effect = fixes.get
-    self.sm.logMonoTime = {'gpsLocation': 2, 'gpsLocationExternal': 1}  # replayed logs are judged by receipt time
-    with patch('requests.get', return_value=self.response):
-      self.engine.update()
-      self.assertEqual((self.engine.last_position, self.engine.last_bearing), ((1, 1), 180))  # the newest fix wins
-      self.sm.valid['gpsLocation'] = False
-      self.engine.update()
-      self.assertEqual(self.engine.last_position, (2, 2))  # unless it is invalid
-      fixes['gpsLocationExternal'] = SimpleNamespace(hasFix=True, latitude=3, longitude=3, bearingDeg=0)
-      self.sm.recv_time = dict.fromkeys(self.sm.valid, time.monotonic() - 3)  # or stale
-      self.engine.update()
-      self.assertEqual(self.engine.last_position, (2, 2))
-
-
-class TestTiles(unittest.TestCase):
-  def test_pmtiles_format(self):
-    self.assertEqual([tile_id(1, x, y) for x, y in [(0, 0), (0, 1), (1, 1), (1, 0)]], [1, 2, 3, 4])
-    self.assertEqual(sorted(tile_id(3, x, y) for x in range(8) for y in range(8)), list(range(21, 85)))
-    # IDs 5, 10; runs 3, 1; lengths 4, 6; the second offset follows the first blob.
-    data = bytes([2, 5, 5, 3, 1, 4, 6, 1, 0])
-    self.assertEqual(decode_directory(data), ([5, 10], [(3, 4, 0), (1, 6, 4)]))
-    for invalid in (b'\x80', b'\x01\x00\x01\x01\x00', data + b'\x00'):
-      with self.assertRaises(ValueError):
-        decode_directory(invalid)
-    with local_archive(make_archive(run=3, max_zoom=1)) as archive:  # the zoom-zero tile is also addressed by IDs 1 and 2
-      self.assertEqual(archive.get(1, 0, 1), road_tile())
-      self.assertIsNone(archive.get(1, 1, 1))
-    with self.assertRaises(ValueError), local_archive(make_archive()[:-1]):
-      pass
-
-  def test_vector_tiles(self):
-    roads = decode_roads(road_tile())
-    self.assertEqual((len(roads), roads[0].kind), (1, 'minor_road'))
-    np.testing.assert_array_equal(roads[0].points, [[0.5, 0], [0.5, 1]])
-    lines = decode_lines([9, 20, 20, 10, 10, 0, 9, 9, 9, 10, 0, 10])
-    np.testing.assert_array_equal(lines[0], [[10, 10], [15, 10]])
-    np.testing.assert_array_equal(lines[1], [[10, 5], [10, 10]])
-    for data in (b'\x1a\xff', b'\x00', b'\x1a\x02\x0a\x04'):
-      with self.assertRaises(ValueError):
-        decode_roads(data)
-    for commands in ([10, 1, 1], [9, 0], [9, 0, 0, 15]):
-      with self.assertRaises(ValueError):
-        decode_lines(commands)
-
-
-class TestRendering(unittest.TestCase):
-  def test_projection(self):
-    # At the equator, 64m ahead is the image center and the car is 64px below it, at 1m per pixel.
-    points = mercator(np.array([[0, 0], [0, 64 * METER], [64 * METER, 0]]))
-    np.testing.assert_allclose(project(points, 0, 0, 0), [[128, 192], [128, 128], [192, 192]], atol=1e-5)
-    np.testing.assert_allclose(project(points, 0, 0, 90), [[128, 192], [64, 192], [128, 128]], atol=1e-5)
-    pixels = project(mercator(np.array([[-179.999, 0]])), 179.999, 0, 90)  # across the antimeridian
-    self.assertAlmostEqual(pixels[0, 1], 192 - 222.6389816, places=5)
-
-  def test_strokes(self):
-    image = np.zeros((SIZE, SIZE), dtype=np.uint8)
-    draw_line(image, np.array([[-1000, 128], [1000, 128]]), 255, 8)
-    self.assertTrue(np.all(image[128] == 255))
-    draw_line(image, np.array([[128, 0], [128, 256]]), 128, 5, overlay=True)  # the grey route replaces the white road
-    self.assertEqual((image[128, 128], image[128, 131]), (128, 255))
-    before = image.copy()
-    draw_line(image, np.array([[-1000, -1000], [-500, -500]]), 255, 5)
-    np.testing.assert_array_equal(image, before)
-    draw_line(image, np.array([[40, 40], [40, 40]]), 255, 5)  # a zero-length segment is still a dot
-    self.assertEqual(image[40, 40], 255)
-
-  def test_crossings(self):
-    route = np.array([[0.01, 0], [0.01, 0.01]])  # off to the side, out of the way of the probes
-    images = {}
-    for crossing in ('bridge', 'tunnel'):
-      with local_archive(make_archive(road_tile(crossing))) as archive:
-        images[crossing] = MapRenderer(archive).render(0, -40 * METER, 0, route)[0]  # the crossing 40m ahead
-    # The horizontal road crosses the vertical one 40px above the ego, at row 152. A bridge severs the road beneath
-    # just outside its own stroke; a tunnel passes underneath untouched. The cut has butt caps, so the
-    # bridge's own end is not wrapped in black.
-    self.assertEqual((images['bridge'][146, 128], images['bridge'][152, 128], images['bridge'][152, 200], images['bridge'][140, 128]), (0, 255, 255, 255))
-    self.assertEqual((images['tunnel'][146, 128], images['tunnel'][152, 200]), (255, 255))
-    # The ground road cuts over the tunnel beneath it, and the tunnel is drawn without its casing.
-    self.assertEqual((images['tunnel'][152, 133], images['tunnel'][147, 200]), (0, 0))
-
-  def test_road_widths(self):
-    # Pinned at the style zoom of 1 m per pixel: 16.26 at the equator and 15.26 at 60 degrees. Tunnels have no casing.
-    styles, levels = np.array([MAJOR, MEDIUM, STREET, MINOR, TRACK, STREET]), np.array([0, 0, 0, 0, 0, -1])
-    np.testing.assert_allclose(stroke_widths(16.256, styles, levels), [19.02, 15.48, 9.86, 6.70, 7.02, 7.89], atol=0.01)
-    np.testing.assert_allclose(stroke_widths(15.256, styles, levels), [13.52, 10.78, 6.39, 4.16, 4.58, 4.62], atol=0.01)
-
-  def test_route_tiles(self):
-    route = np.array([[0, -0.01], [0, 0.01]])  # 2.2 km north along the prime meridian
-    with local_archive(make_archive(max_zoom=15)) as archive:
-      renderer = MapRenderer(archive)
-      tiles = renderer.route_tiles(route)
-      # Both columns beside the meridian, and the rows within the view radius of the route's ends.
-      self.assertEqual({x for x, _ in tiles}, {16383, 16384})
-      self.assertEqual({y for _, y in tiles}, set(range(16382, 16386)))
-
-  def test_validity_and_display(self):
-    route = np.array([[0, -0.01], [0, 0.01]])
-    with local_archive(make_archive()) as archive:
-      renderer = MapRenderer(archive)
-      self.assertTrue(renderer.render(0, 0, 0, route)[1])
-      self.assertFalse(renderer.render(0.01, 0, 0, route)[1])  # no road near the ego
-      with patch.object(archive, 'get', return_value=None):  # a missing tile
-        image, valid = MapRenderer(archive).render(0, 0, 0, route)
-        self.assertFalse(valid)
-        self.assertEqual(image[128, 128], 128)  # the route is still drawn
-    with local_archive(make_archive(field(3, field(1, b'buildings')))) as archive:  # tiles without roads
-      self.assertFalse(MapRenderer(archive).render(0, 0, 0, route)[1])
-
-
-class TestRouteTracking(unittest.TestCase):
-  def test_snap_heading_and_window(self):
-    m = METER
-    tracker = RouteTracker(np.array([[0, 0], [0, 200 * m], [200 * m, 200 * m]]))  # north 200m, then east 200m
-    longitude, latitude, bearing = tracker.snap(3 * m, 100 * m)  # 3m right of the first leg
-    self.assertAlmostEqual(tracker.progress, 100, delta=0.5)
-    self.assertAlmostEqual(longitude, 0)  # lateral error removed
-    self.assertAlmostEqual(latitude, 100 * m)
-    self.assertAlmostEqual(bearing, 0, places=3)
-    self.assertAlmostEqual(tracker.snap(0, 200 * m)[2], 45, places=2)  # at the corner the +-50m chord is mid-turn
-    self.assertAlmostEqual(tracker.snap(0, 150 * m)[2], 45, places=2)  # progress never decreases
-    tracker.snap(100 * m, 200 * m)  # progress 300: the window ahead no longer includes the first leg
-    self.assertAlmostEqual(tracker.snap(150 * m, 200 * m)[0], 150 * m)  # so the window's offset is applied
-    self.assertAlmostEqual(tracker.snap(200 * m, 200 * m)[2], 90, places=2)  # the chord clips at the route's end
-    # Out and back along the same road: a fix near the far leg stays on the outbound leg within the window,
-    # while a fresh tracker searches anywhere and lands on the return leg.
-    route = np.array([[0, 0], [0, 1000 * m], [10 * m, 1000 * m], [10 * m, 0]])
-    tracker = RouteTracker(route)
-    tracker.snap(1 * m, 100 * m)
-    self.assertAlmostEqual(tracker.snap(9 * m, 150 * m)[2], 0, places=3)
-    self.assertAlmostEqual(tracker.progress, 150, delta=0.5)
-    self.assertAlmostEqual(RouteTracker(route).snap(9 * m, 150 * m)[2], 180, places=3)
-
-
-if __name__ == '__main__':
-  unittest.main()

openpilot/selfdrive/navigation/tiles.py

@@ -1,117 +0,0 @@
-"""Byte ranges of the hosted PMTiles basemap, cached on disk."""
-import re
-import time
-from pathlib import Path
-
-import requests
-
-from openpilot.common.api import Api
-from openpilot.common.hardware import PC
-from openpilot.common.hardware.hw import Paths
-from openpilot.common.params import Params
-from openpilot.selfdrive.navigation.pmtiles import HEADER_LENGTH
-
-CACHE_LIMIT = 64 * 1024 * 1024
-MERGE_GAP = 256 * 1024  # tiles this close together in the archive are fetched in one range request
-
-
-class RemoteTiles:
-  def __init__(self):
-    self.cache = Path(Paths.comma_home() if PC else '/data') / 'navigation' / 'tiles'
-    self.cache.mkdir(parents=True, exist_ok=True, mode=0o700)
-    self.session = requests.Session()
-    self.api = Api(Params().get('DongleId'))
-    self.url = ''
-    self.expires = 0
-    self.etag = None
-    self.size = 0
-    version = self.cache / 'version'
-    try:
-      header = self._fetch(0, HEADER_LENGTH)  # learns the archive's ETag and size
-      if not version.exists() or version.read_text() != f'{self.etag} {self.size}':
-        for cached in self.cache.iterdir():  # a new archive has new offsets
-          cached.unlink()
-        version.write_text(f'{self.etag} {self.size}')
-      self._store(0, header)
-    except Exception:
-      self.close()
-      raise
-
-  def _fetch(self, offset: int, length: int) -> bytes:
-    try:
-      if time.time() >= self.expires - 60:  # noqa: TID251 - SAS expiry is a Unix timestamp
-        response = self.api.get(f'v1/{self.api.dongle_id}/nav/tiles', access_token=self.api.get_token(), timeout=10)
-        response.raise_for_status()
-        metadata = response.json()
-        self.url, self.expires = metadata['url'], metadata['expires']
-      headers = {'Range': f'bytes={offset}-{offset + length - 1}', 'Accept-Encoding': 'identity'}
-      if self.etag is not None:
-        headers['If-Match'] = self.etag
-      with self.session.get(self.url, headers=headers, timeout=10, stream=True) as response:
-        if response.status_code != 206:
-          raise OSError(f'Map tile range request failed ({response.status_code})')
-        match = re.fullmatch(r'bytes (\d+)-(\d+)/(\d+)', response.headers.get('Content-Range', ''))
-        if match is None or tuple(map(int, match.groups()[:2])) != (offset, offset + length - 1):
-          raise OSError('Invalid map tile range response')
-        size = int(match[3])
-        etag = response.headers.get('ETag')
-        if not etag or (self.etag is not None and (etag != self.etag or size != self.size)):
-          raise OSError('Map archive changed during read')
-        data = bytearray()
-        for chunk in response.iter_content(chunk_size=min(length + 1, 65536)):
-          data.extend(chunk)
-          if len(data) > length:
-            raise OSError('Oversized map tile range response')
-        if len(data) != length:
-          raise OSError('Truncated map tile range response')
-        self.size, self.etag = size, etag
-        return bytes(data)
-    except (requests.RequestException, ValueError, KeyError, TypeError, RuntimeError):
-      # requests exceptions contain the signed URL; keep it out of logs.
-      raise OSError('Unable to fetch map tiles') from None
-
-  def _store(self, offset: int, data: bytes):
-    (self.cache / f'{offset}-{len(data)}').write_bytes(data)
-
-  def _trim(self):
-    files = [(cached.stat().st_mtime, cached.stat().st_size, cached) for cached in self.cache.iterdir() if re.fullmatch(r'\d+-\d+', cached.name)]
-    size = sum(cached_size for _, cached_size, _ in files)
-    for _, cached_size, cached in sorted(files):
-      if size <= CACHE_LIMIT:
-        break
-      cached.unlink(missing_ok=True)
-      size -= cached_size
-
-  def read(self, offset: int, length: int) -> bytes:
-    try:
-      data = (self.cache / f'{offset}-{length}').read_bytes()
-      if len(data) == length:
-        return data
-    except FileNotFoundError:
-      pass
-    data = self._fetch(offset, length)
-    self._store(offset, data)
-    self._trim()
-    return data
-
-  def prefetch(self, ranges):
-    """Cache (offset, length) ranges ahead of reads, fetching neighboring ones in a single request."""
-    missing = sorted((offset, length) for offset, length in set(ranges) if not (self.cache / f'{offset}-{length}').exists())
-    if not missing:
-      return
-    groups = [[missing[0]]]
-    for offset, length in missing[1:]:
-      last_offset, last_length = groups[-1][-1]
-      if offset - (last_offset + last_length) <= MERGE_GAP:
-        groups[-1].append((offset, length))
-      else:
-        groups.append([(offset, length)])
-    for group in groups:
-      start, end = group[0][0], group[-1][0] + group[-1][1]
-      data = self._fetch(start, end - start)
-      for offset, length in group:
-        self._store(offset, data[offset - start:offset - start + length])
-    self._trim()
-
-  def close(self):
-    self.session.close()

openpilot/selfdrive/navigation/vector_tile.py

@@ -1,133 +0,0 @@
-"""Decode road polylines from Protomaps MVT tiles without a protobuf runtime."""
-from dataclasses import dataclass
-
-import numpy as np
-
-from openpilot.selfdrive.navigation.pmtiles import read_varint
-
-
-@dataclass
-class Road:
-  points: np.ndarray  # fractions of the tile, x right / y down
-  kind: str
-  detail: str
-  tunnel: bool
-  bridge: bool
-
-
-def fields(data: bytes):
-  offset = 0
-  while offset < len(data):
-    tag, offset = read_varint(data, offset)
-    number, wire = tag >> 3, tag & 7
-    if number == 0:
-      raise ValueError("Invalid protobuf field")
-    if wire == 0:
-      value, offset = read_varint(data, offset)
-    elif wire in (1, 2, 5):
-      if wire == 2:
-        length, offset = read_varint(data, offset)
-      else:
-        length = 8 if wire == 1 else 4
-      if offset + length > len(data):
-        raise ValueError("Truncated protobuf field")
-      value = data[offset:offset + length]
-      offset += length
-    else:
-      raise ValueError(f"Unsupported protobuf wire type: {wire}")
-    yield number, wire, value
-
-
-def integers(data: bytes) -> list[int]:
-  result = []
-  offset = 0
-  while offset < len(data):
-    value, offset = read_varint(data, offset)
-    result.append(value)
-  return result
-
-
-def unzigzag(value: int) -> int:
-  return (value >> 1) ^ -(value & 1)
-
-
-def decode_value(data: bytes):
-  """A string or boolean layer value. Other types are unused by the roads layer and decode to None."""
-  for number, wire, value in fields(data):
-    if number == 1 and wire == 2:
-      return value.decode('utf-8')
-    if number == 7 and wire == 0:
-      return bool(value)
-  return None
-
-
-def decode_lines(commands: list[int]) -> list[np.ndarray]:
-  lines = []
-  line = []
-  x = y = offset = 0
-  while offset < len(commands):
-    command = commands[offset]
-    offset += 1
-    kind, count = command & 7, command >> 3
-    if kind not in (1, 2) or count == 0 or offset + 2 * count > len(commands):
-      raise ValueError("Invalid MVT line geometry")
-    if kind == 1 and count != 1:
-      raise ValueError("LineString MoveTo must contain one point")
-    if kind == 2 and not line:
-      raise ValueError("LineTo before MoveTo")
-    for _ in range(count):
-      x += unzigzag(commands[offset])
-      y += unzigzag(commands[offset + 1])
-      offset += 2
-      if kind == 1:
-        if len(line) >= 2:
-          lines.append(np.array(line, dtype=np.float64))
-        line = []
-      line.append((x, y))
-  if len(line) >= 2:
-    lines.append(np.array(line, dtype=np.float64))
-  return lines
-
-
-def decode_roads(data: bytes) -> list[Road]:
-  roads = []
-  for number, wire, layer in fields(data):
-    if number != 3 or wire != 2:
-      continue
-    layer_fields = list(fields(layer))
-    name = next((v.decode('utf-8') for n, w, v in layer_fields if n == 1 and w == 2), '')
-    if name != 'roads':
-      continue
-    version = next((v for n, w, v in layer_fields if n == 15 and w == 0), 1)
-    extent = next((v for n, w, v in layer_fields if n == 5 and w == 0), 4096)
-    if version not in (1, 2) or extent <= 0:
-      raise ValueError("Unsupported MVT roads layer")
-    keys = [v.decode('utf-8') for n, w, v in layer_fields if n == 3 and w == 2]
-    values = [decode_value(v) for n, w, v in layer_fields if n == 4 and w == 2]
-    for n, w, feature in layer_fields:
-      if n != 2 or w != 2:
-        continue
-      tags, geometry, geometry_type = [], [], 0
-      for fn, fw, fv in fields(feature):
-        if fn in (2, 4):
-          target = tags if fn == 2 else geometry
-          if fw == 2:
-            target.extend(integers(fv))
-          elif fw == 0:
-            target.append(fv)
-        elif fn == 3 and fw == 0:
-          geometry_type = fv
-      if geometry_type != 2:
-        continue
-      if len(tags) % 2:
-        raise ValueError("Unpaired MVT tags")
-      properties = {}
-      for i in range(0, len(tags), 2):
-        if tags[i] >= len(keys) or tags[i + 1] >= len(values):
-          raise ValueError("MVT tag index out of bounds")
-        properties[keys[tags[i]]] = values[tags[i + 1]]
-      kind = properties.get('kind', '')
-      if kind in ('highway', 'major_road', 'minor_road', 'other'):
-        roads.extend(Road(points / extent, kind, properties.get('kind_detail', ''), properties.get('is_tunnel') is True,
-                          properties.get('is_bridge') is True) for points in decode_lines(geometry))
-  return roads

openpilot/selfdrive/pandad/panda_comms.h

@@ -8,7 +8,7 @@
 
 
 #define TIMEOUT 0
-#define SPI_BUF_SIZE 4096
+#define SPI_BUF_SIZE 2048
 
 
 class PandaSpiHandle {
@@ -35,18 +35,15 @@ private:
   inline static std::recursive_mutex hw_lock;
 
   struct __attribute__((packed)) spi_header {
+    uint8_t sync;
     uint8_t endpoint;
-    uint32_t id;
     uint16_t tx_len;
-    uint16_t rx_len;
-  };
-  struct __attribute__((packed)) spi_resp_header {
-    uint8_t status;
-    uint32_t id;
-    uint16_t len;
+    uint16_t max_rx_len;
   };
 
-  int spi_transfer(uint8_t endpoint, uint32_t id, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, double deadline);
+  int wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, unsigned int length);
+  int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout);
+  int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout);
   int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout);
   int lltransfer(struct spi_ioc_transfer &t);
 

openpilot/selfdrive/pandad/pandad.cc

@@ -139,8 +139,6 @@ void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::Panda
   ps.setInterruptLoad(health.interrupt_load_pkt / 255.0f);
   ps.setFanPower(health.fan_power);
   ps.setSafetyRxChecksInvalid((health.flags_pkt & HEALTH_FLAG_SAFETY_RX_CHECKS_INVALID) != 0U);
-  ps.setNmiReset((health.flags_pkt & HEALTH_FLAG_NMI_RESET) != 0U);
-  ps.setHardfaultReset((health.flags_pkt & HEALTH_FLAG_HARDFAULT_RESET) != 0U);
   ps.setSpiErrorCount(health.spi_error_count_pkt);
   ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f);
   ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f);

openpilot/selfdrive/pandad/spi.cc

@@ -2,12 +2,10 @@
 #include <sys/ioctl.h>
 #include <linux/spi/spidev.h>
 
-#include <array>
 #include <cassert>
 #include <cmath>
 #include <cstring>
 #include <iomanip>
-#include <random>
 #include <sstream>
 
 #include "common/util.h"
@@ -17,26 +15,26 @@
 #include "selfdrive/pandad/panda_comms.h"
 
 
-// protocol, see panda/board/drivers/spi.h
-#define SPI_SLAVE_READY 0x3CU
-#define SPI_ACK 0xC3U
-#define SPI_NACK 0x5AU
-#define SPI_POLL 0xFFU  // filler while reading the response, never a valid request
-
-#define SPI_CRC_SIZE 4U
-#define SPI_XFER_SIZE (SPI_BUF_SIZE - 0x40)  // bulk chunk size, leaves room for the protocol overhead
-#define SPI_FIRST_READ_SIZE (1024 - sizeof(spi_resp_header) - SPI_CRC_SIZE)  // reads are usually small, start small
+#define SPI_SYNC 0x5AU
+#define SPI_HACK 0x79U
+#define SPI_DACK 0x85U
+#define SPI_NACK 0x1FU
+#define SPI_CHECKSUM_START 0xABU
 
 
 enum SpiError {
   NACK = -2,
   ACK_TIMEOUT = -3,
-  SILENT = -4,
 };
 
-const unsigned int SPI_MIN_TIMEOUT = 100; // milliseconds
-const double SPI_SILENT_TIMEOUT = 1000;  // milliseconds, longer than the slowest request takes on the panda
+const unsigned int SPI_ACK_TIMEOUT = 500; // milliseconds
 const std::string SPI_DEVICE = "/dev/spidev0.0";
+// TODO: fix SPI turnaround synchronization at the protocol level.
+static uint64_t spi_last_bus_activity_ns = 0;  // protected by hw_lock
+
+static void wait_for_spi_turnaround(uint64_t start_ns) {
+  while ((nanos_since_boot() - start_ns) < 400000) {}
+}
 
 class LockEx {
 public:
@@ -58,8 +56,8 @@ private:
 #define SPILOG(fn, fmt, ...) do {  \
       fn(fmt, ## __VA_ARGS__);     \
       fn("  %d / 0x%x / %d / %d / tx: %s", \
-         xfer_count, header.endpoint, header.tx_len, header.rx_len, \
-         util::hexdump(tx_buf + sizeof(header), std::min((int)header.tx_len, 8)).c_str()); \
+         xfer_count, header.endpoint, header.tx_len, header.max_rx_len, \
+         util::hexdump(tx_buf, std::min((int)header.tx_len, 8)).c_str()); \
       } while (0)
 
 PandaSpiHandle::PandaSpiHandle(std::string serial) {
@@ -90,7 +88,7 @@ PandaSpiHandle::PandaSpiHandle(std::string serial) {
     util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word, "failed setting SPI bits per word");
 
     // get hw UID/serial
-    ret = control_read(0xc3, 0, 0, uid, uid_len, 500);
+    ret = control_read(0xc3, 0, 0, uid, uid_len, 100);
     if (ret == uid_len) {
       std::stringstream stream;
       for (int i = 0; i < uid_len; i++) {
@@ -148,31 +146,40 @@ int PandaSpiHandle::control_read(uint8_t request, uint16_t param1, uint16_t para
 }
 
 int PandaSpiHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
-  for (int pos = 0; pos < length; pos += SPI_XFER_SIZE) {
-    int ret = spi_transfer_retry(endpoint, data + pos, std::min(SPI_XFER_SIZE, length - pos), NULL, 0, timeout);
-    if (ret < 0) {
-      comms_healthy = false;
-      return ret;
-    }
-  }
-  return length;
+  return bulk_transfer(endpoint, data, length, NULL, 0, timeout);
+}
+int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
+  return bulk_transfer(endpoint, NULL, 0, data, length, timeout);
 }
 
-int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
-  int pos = 0;
-  while (pos < length) {
-    int to_read = std::min(pos == 0 ? (int)SPI_FIRST_READ_SIZE : SPI_XFER_SIZE, length - pos);
-    int ret = spi_transfer_retry(endpoint, NULL, 0, data + pos, to_read, timeout);
-    if (ret < 0) {
-      comms_healthy = false;
-      return ret;
+int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout) {
+  const int xfer_size = SPI_BUF_SIZE - 0x40;
+
+  int ret = 0;
+  uint16_t length = (tx_data != NULL) ? tx_len : rx_len;
+  for (int i = 0; i < (int)std::ceil((float)length / xfer_size); i++) {
+    int d;
+    if (tx_data != NULL) {
+      int len = std::min(xfer_size, tx_len - (xfer_size * i));
+      d = spi_transfer_retry(endpoint, tx_data + (xfer_size * i), len, NULL, 0, timeout);
+    } else {
+      uint16_t to_read = std::min(xfer_size, rx_len - ret);
+      d = spi_transfer_retry(endpoint, NULL, 0, rx_data + (xfer_size * i), to_read, timeout);
     }
-    pos += ret;
-    if (ret < to_read) {
+
+    if (d < 0) {
+      SPILOG(LOGE, "SPI: bulk transfer failed with %d", d);
+      comms_healthy = false;
+      return d;
+    }
+
+    ret += d;
+    if ((rx_data != NULL) && d < xfer_size) {
       break;
     }
   }
-  return pos;
+
+  return ret;
 }
 
 std::vector<std::string> PandaSpiHandle::list() {
@@ -185,202 +192,228 @@ std::vector<std::string> PandaSpiHandle::list() {
   return {};
 }
 
-// random, so it's never the same as another request the panda just handled, even from another client
-static uint32_t random_id() {
-  static thread_local std::mt19937 rng{std::random_device{}()};
-  return rng();
-}
-
-// standard CRC-32, same as zlib's crc32()
-static uint32_t crc32(const uint8_t *data, uint32_t len) {
-  static const auto table = [] {
-    std::array<uint32_t, 256> t{};
-    for (uint32_t i = 0; i < 256; i++) {
-      uint32_t c = i;
-      for (int j = 0; j < 8; j++) {
-        c = (c & 1) ? (0xEDB88320U ^ (c >> 1)) : (c >> 1);
-      }
-      t[i] = c;
-    }
-    return t;
-  }();
-
-  uint32_t crc = 0xFFFFFFFFU;
-  for (uint32_t i = 0; i < len; i++) {
-    crc = table[(crc ^ data[i]) & 0xFFU] ^ (crc >> 8);
+void add_checksum(uint8_t *data, int data_len) {
+  data[data_len] = SPI_CHECKSUM_START;
+  for (int i=0; i < data_len; i++) {
+    data[data_len] ^= data[i];
   }
-  return crc ^ 0xFFFFFFFFU;
 }
 
-static void put_crc(uint8_t *data, uint32_t len) {
-  uint32_t crc = crc32(data, len);
-  memcpy(&data[len], &crc, sizeof(crc));
-}
-
-static bool check_crc(const uint8_t *data, uint32_t len) {
-  uint32_t crc;
-  memcpy(&crc, &data[len], sizeof(crc));
-  return crc == crc32(data, len);
+bool check_checksum(uint8_t *data, int data_len) {
+  uint8_t checksum = SPI_CHECKSUM_START;
+  for (uint16_t i = 0U; i < data_len; i++) {
+    checksum ^= data[i];
+  }
+  return checksum == 0U;
 }
 
 
 int PandaSpiHandle::spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout) {
-  // timeout of 0 means forever
-  const double deadline = (timeout == 0) ? INFINITY : millis_since_boot() + std::max(timeout, SPI_MIN_TIMEOUT);
-
-  // the id stays the same when we send the request again, so the panda never runs it twice
-  uint32_t id = random_id();
   int ret;
   int nack_count = 0;
-  while (true) {
-    ret = spi_transfer(endpoint, id, tx_data, tx_len, rx_data, max_rx_len, deadline);
-    if (ret >= 0 || !connected) {
-      break;
-    }
+  int timeout_count = 0;
+  bool timed_out = false;
+  double start_time = millis_since_boot();
 
-    if (ret == SpiError::NACK) {
-      // the panda NACKs while its CAN TX buffers are full. that's not a comms
-      // failure, so keep trying regardless of the timeout, without busy waiting.
-      // a new id, since the panda would answer the same id with the same NACK
-      id = random_id();
-      nack_count += 1;
-      if (nack_count > 3) {
-        usleep(std::clamp(nack_count*10, 200, 2000));
+  do {
+    ret = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len, timeout);
+
+    if (ret < 0) {
+      timed_out = (timeout != 0) && (timeout_count > 5);
+      timeout_count += ret == SpiError::ACK_TIMEOUT;
+
+      // give other threads a chance to run
+      std::this_thread::yield();
+
+      if (ret == SpiError::NACK) {
+        // prevent busy waiting while the panda is NACK'ing
+        // due to full TX buffers
+        nack_count += 1;
+        if (nack_count > 3) {
+          SPILOG(LOGD, "NACK sleep %d", nack_count);
+          usleep(std::clamp(nack_count*10, 200, 2000));
+        }
       }
-    } else if (ret == SpiError::SILENT) {
-      // the panda went quiet. let go of the bus for a bit, someone else might need it
-      LOGW("SPI: panda isn't responding");
-      usleep(10000);
-    } else if (millis_since_boot() > deadline) {
-      break;
     }
-
-    // give other threads a chance to run
-    std::this_thread::yield();
-  }
+  } while (ret < 0 && connected && !timed_out);
 
   if (ret < 0) {
-    SPILOG(LOGE, "SPI: transfer failed with %d", ret);
+    SPILOG(LOGE, "transfer failed, after %d tries, %.2fms", timeout_count, millis_since_boot() - start_time);
   }
+
   return ret;
 }
 
+int PandaSpiHandle::wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, unsigned int length) {
+  double start_millis = millis_since_boot();
+  if (timeout == 0) {
+    timeout = SPI_ACK_TIMEOUT;
+  }
+  timeout = std::clamp(timeout, 20U, SPI_ACK_TIMEOUT);
+
+  spi_ioc_transfer transfer = {
+    .tx_buf = (uint64_t)tx_buf,
+    .rx_buf = (uint64_t)rx_buf,
+    .len = length,
+  };
+  memset(tx_buf, tx, length);
+
+  while (true) {
+    int ret = lltransfer(transfer);
+    if (ret < 0) {
+      SPILOG(LOGE, "SPI: failed to send ACK request");
+      return ret;
+    }
+
+    if (rx_buf[0] == ack) {
+      break;
+    } else if (rx_buf[0] == SPI_NACK) {
+      SPILOG(LOGD, "SPI: got NACK, waiting for 0x%x", ack);
+      return SpiError::NACK;
+    }
+
+    // handle timeout
+    if (millis_since_boot() - start_millis > timeout) {
+      SPILOG(LOGW, "SPI: timed out waiting for ACK, waiting for 0x%x", ack);
+      return SpiError::ACK_TIMEOUT;
+    }
+  }
+
+  return 0;
+}
+
 int PandaSpiHandle::lltransfer(spi_ioc_transfer &t) {
   static const double err_prob = std::stod(util::getenv("SPI_ERR_PROB", "-1"));
-  auto chance = [](double p) { return (static_cast<double>(rand()) / RAND_MAX) < p; };
-  auto corrupt = [&](uint64_t buf) {
-    for (uint32_t i = 0; i < t.len; i++) {
-      if (chance(0.1)) ((uint8_t*)buf)[i] = (uint8_t)(rand() % 256);
-    }
-  };
 
-  if (err_prob > 0 && chance(err_prob)) {
-    printf("transfer len error\n");
-    t.len = rand() % SPI_BUF_SIZE;
-  }
-  if (err_prob > 0 && chance(err_prob) && t.tx_buf == (uint64_t)tx_buf) {
-    printf("corrupting TX\n");
-    corrupt(t.tx_buf);
+  if (err_prob > 0) {
+    if ((static_cast<double>(rand()) / RAND_MAX) < err_prob) {
+      printf("transfer len error\n");
+      t.len = rand() % SPI_BUF_SIZE;
+    }
+    if ((static_cast<double>(rand()) / RAND_MAX) < err_prob && t.tx_buf != (uint64_t)NULL) {
+      printf("corrupting TX\n");
+      for (int i = 0; i < t.len; i++) {
+        if ((static_cast<double>(rand()) / RAND_MAX) > 0.9) {
+          ((uint8_t*)t.tx_buf)[i] = (uint8_t)(rand() % 256);
+        }
+      }
+    }
   }
 
   int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &t);
-  if (err_prob > 0 && chance(err_prob)) {
-    printf("ioctl error\n");
-    ret = -1;
+
+  if (err_prob > 0) {
+    if ((static_cast<double>(rand()) / RAND_MAX) < err_prob && t.rx_buf != (uint64_t)NULL) {
+      printf("corrupting RX\n");
+      for (int i = 0; i < t.len; i++) {
+        if ((static_cast<double>(rand()) / RAND_MAX) > 0.9) {
+          ((uint8_t*)t.rx_buf)[i] = (uint8_t)(rand() % 256);
+        }
+      }
+    }
   }
 
-  if (err_prob > 0 && chance(err_prob)) {
-    printf("corrupting RX\n");
-    corrupt(t.rx_buf);
-  }
   return ret;
 }
 
-// One request and its response. Resends until the panda got the request and we got its response,
-// or the deadline. Every transfer either sees all of what the panda queued from its first byte, or
-// nothing it can mistake for a response. Returns the response length, or a negative SpiError
-int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint32_t id, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, double deadline) {
-  static const auto poll = [] {
-    std::array<uint8_t, SPI_BUF_SIZE> p;
-    p.fill(SPI_POLL);
-    return p;
-  }();
-
+int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout) {
+  int ret;
+  uint16_t rx_data_len;
   LockEx lock(spi_fd, hw_lock);
 
-  // the response is always read in one transfer of rx_len
-  const uint32_t rx_len = sizeof(spi_resp_header) + max_rx_len + SPI_CRC_SIZE;
-  const uint32_t req_len = sizeof(header) + tx_len;
-  assert(req_len + SPI_CRC_SIZE <= SPI_BUF_SIZE);
-  assert(rx_len <= SPI_BUF_SIZE);
+  // needs to be less, since we need to have space for the checksum
+  assert(tx_len < SPI_BUF_SIZE);
+  assert(max_rx_len < SPI_BUF_SIZE);
+
+  wait_for_spi_turnaround(spi_last_bus_activity_ns);
 
   xfer_count++;
   header = {
+    .sync = SPI_SYNC,
     .endpoint = endpoint,
-    .id = id,
     .tx_len = tx_len,
-    .rx_len = (uint16_t)rx_len,
+    .max_rx_len = max_rx_len
   };
 
-  auto xfer = [&](const uint8_t *tx, uint32_t len) {
-    spi_ioc_transfer t = {.tx_buf = (uint64_t)tx, .rx_buf = (uint64_t)rx_buf, .len = len};
-    int ret = lltransfer(t);
-    if (ret < 0) {
-      SPILOG(LOGE, "SPI: transfer failed");
-    }
-    return ret;
+  spi_ioc_transfer transfer = {
+    .tx_buf = (uint64_t)tx_buf,
+    .rx_buf = (uint64_t)rx_buf
   };
 
-  // when the panda last showed it's there. while it's quiet, we let go of the bus
-  double alive = millis_since_boot();
-  while (true) {
-    // send the request, until the panda was ready for it. it's rebuilt every time,
-    // since SPI_ERR_PROB corrupts tx_buf
-    memcpy(tx_buf, &header, sizeof(header));
-    if (tx_data != NULL) {
-      memcpy(tx_buf + sizeof(header), tx_data, tx_len);
-    }
-    put_crc(tx_buf, req_len);
-    // a failed transfer is sent again like any other, keeping the bus, so nothing can run in between
-    if (xfer(tx_buf, req_len + SPI_CRC_SIZE) >= 0 && rx_buf[0] == SPI_SLAVE_READY) {
-      alive = millis_since_boot();
-      // read until the response. if the panda is ready again, it never got the request
-      do {
-        if (xfer(poll.data(), rx_len) < 0) {
-          break;
-        }
-        if (rx_buf[0] == SPI_SLAVE_READY) {
-          alive = millis_since_boot();
-          break;
-        }
+  // Send header
+  memcpy(tx_buf, &header, sizeof(header));
+  add_checksum(tx_buf, sizeof(header));
+  transfer.len = sizeof(header) + 1;
+  ret = lltransfer(transfer);
+  if (ret < 0) {
+    SPILOG(LOGE, "SPI: failed to send header");
+    goto fail;
+  }
 
-        // a response that got cut off or corrupted isn't one, keep reading
-        spi_resp_header resp;
-        memcpy(&resp, rx_buf, sizeof(resp));
-        if ((resp.status == SPI_ACK || resp.status == SPI_NACK) && resp.len <= max_rx_len && check_crc(rx_buf, sizeof(resp) + resp.len)) {
-          // otherwise it's the response to an old request, and we send ours again
-          if (resp.id == header.id) {
-            if (resp.status == SPI_NACK) {
-              SPILOG(LOGD, "SPI: got NACK");
-              return SpiError::NACK;
-            }
-            if (rx_data != NULL) {
-              memcpy(rx_data, rx_buf + sizeof(resp), resp.len);
-            }
-            return resp.len;
-          }
-          alive = millis_since_boot();
-          break;
-        }
-      } while (millis_since_boot() < std::min(deadline, alive + SPI_SILENT_TIMEOUT));
-    }
+  // Wait for (N)ACK
+  ret = wait_for_ack(SPI_HACK, 0x11, timeout, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+  wait_for_spi_turnaround(nanos_since_boot());
 
-    if (millis_since_boot() > deadline) {
-      SPILOG(LOGW, "SPI: timed out");
-      return SpiError::ACK_TIMEOUT;
-    }
-    if (millis_since_boot() > alive + SPI_SILENT_TIMEOUT) {
-      return SpiError::SILENT;
+  // Send data
+  if (tx_data != NULL) {
+    memcpy(tx_buf, tx_data, tx_len);
+  }
+  add_checksum(tx_buf, tx_len);
+  transfer.len = tx_len + 1;
+  ret = lltransfer(transfer);
+  if (ret < 0) {
+    SPILOG(LOGE, "SPI: failed to send data");
+    goto fail;
+  }
+
+  // Wait for (N)ACK
+  ret = wait_for_ack(SPI_DACK, 0x13, timeout, 3);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // Read data
+  rx_data_len = *(uint16_t *)(rx_buf+1);
+  if (rx_data_len >= SPI_BUF_SIZE) {
+    SPILOG(LOGE, "SPI: RX data len larger than buf size %d", rx_data_len);
+    goto fail;
+  }
+
+  transfer.len = rx_data_len + 1;
+  transfer.rx_buf = (uint64_t)(rx_buf + 2 + 1);
+  ret = lltransfer(transfer);
+  if (ret < 0) {
+    SPILOG(LOGE, "SPI: failed to read rx data");
+    goto fail;
+  }
+  if (!check_checksum(rx_buf, rx_data_len + 4)) {
+    SPILOG(LOGE, "SPI: bad checksum");
+    goto fail;
+  }
+
+  if (rx_data != NULL) {
+    memcpy(rx_data, rx_buf + 3, rx_data_len);
+  }
+
+  spi_last_bus_activity_ns = nanos_since_boot();
+  return rx_data_len;
+
+fail:
+  // ensure slave is in a consistent state
+  // and ready for the next transfer
+  int nack_cnt = 0;
+  while (nack_cnt < 3) {
+    if (wait_for_ack(SPI_NACK, 0x14, 1, SPI_BUF_SIZE/2) == 0) {
+      nack_cnt += 1;
+    } else {
+      nack_cnt = 0;
     }
   }
+
+  spi_last_bus_activity_ns = nanos_since_boot();
+  if (ret >= 0) ret = -1;
+  return ret;
 }

openpilot/selfdrive/pandad/tests/test_pandad_spi.py

@@ -45,10 +45,9 @@ class TestBoarddSpi(OpenpilotTestCase):
 
     st = time.monotonic()
     ts = {s: [] for s in socks.keys()}
-    test_time = int(os.getenv("TEST_TIME", "20"))
-    for i in range(test_time + 2):
-      # send some CAN messages, then give the last ones time to come back
-      if not JUNGLE_SPAM and i < test_time:
+    for _ in range(int(os.getenv("TEST_TIME", "20"))):
+      # send some CAN messages
+      if not JUNGLE_SPAM:
         sent = send_random_can_messages(sendcan, random.randrange(2, 20))
         for k, v in sent.items():
           sent_msgs[k].extend(list(v))
@@ -75,7 +74,8 @@ class TestBoarddSpi(OpenpilotTestCase):
                 continue
               key = (msg.address, msg.dat)
               assert key in sent_msgs[msg.src], f"got unexpected msg: {msg.src=} {msg.address=} {msg.dat=}"
-              sent_msgs[msg.src].remove(key)
+              # TODO: enable this
+              #sent_msgs[msg.src].remove(key)
               total_recv_count += 1
           elif service == "pandaStates":
             assert len(m.pandaStates) == 1
@@ -107,8 +107,6 @@ class TestBoarddSpi(OpenpilotTestCase):
     with subtests.test(msg="CAN traffic"):
       print(f"Sent {total_sent_count} CAN messages, got {total_recv_count} back. {total_recv_count/(total_sent_count+1e-4):.2%} received")
       assert total_recv_count > 20
-      # every message came back exactly once, despite the injected errors
-      assert all(len(v) == 0 for v in sent_msgs.values())
 
 
 if __name__ == "__main__":

openpilot/selfdrive/selfdrived/alerts_offroad.json

@@ -21,22 +21,22 @@
     "text": "Chestnut detected! Switch to the %1 branch to use chestnut-class models.",
     "severity": -1
   },
-  "Offroad_ChestnutModelError": {
-    "text": "Chestnut model error. Restart the car to retry.",
-    "severity": 0
-  },
   "Offroad_ChestnutNotDetected": {
-    "text": "Chestnut not detected. Check USB and 12V connections, then restart the car.",
+    "text": "Chestnut not detected. Check USB and 12V connections.",
     "severity": 0
   },
   "Offroad_ChestnutOverheated": {
-    "text": "Chestnut overheated. Ensure good airflow.",
+    "text": "Chestnut overheated. Ensure good airflow. Current GPU temperature is %1.",
     "severity": 0
   },
   "Offroad_ChestnutPcieUnavailable": {
     "text": "%1",
     "severity": 0
   },
+  "Offroad_ChestnutUncompiled": {
+    "text": "Chestnut model not compiled. Keep ignition on and reboot the comma.",
+    "severity": 0
+  },
   "Offroad_ChestnutUpdateFailed": {
     "text": "Chestnut update failed. Check the USB cable.",
     "severity": 0
@@ -49,10 +49,6 @@
     "text": "Failed to register with comma.ai backend. It will not connect or upload to comma.ai servers, and receives no support from comma.ai. If this is a device purchased at comma.ai/shop, open a ticket at https://comma.ai/support.",
     "severity": 1
   },
-  "Offroad_UploadsIgnored": {
-    "text": "Uploads ignored due to a potential detected safety policy violation. Contact comma.ai/support if you believe this was made in error.",
-    "severity": 1
-  },
   "Offroad_CarUnrecognized": {
     "text": "sunnypilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please select your vehicle manually at https://www.sunnylink.ai/. Need help? Visit https://community.sunnypilot.ai/",
     "severity": 0
@@ -78,10 +74,5 @@
     "text": "<b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> in the branch name for the comma three.",
     "severity": 1,
     "_comment": "Set extra field to the current branch name."
-  },
-  "Offroad_Pairing": {
-    "text": "Finish setup. Pair your device with comma connect to view recent drives and bookmarked routes.",
-    "severity": -2,
-    "icon": "icons_mici/settings/device/green_settings.png"
   }
 }

openpilot/selfdrive/selfdrived/events.py

@@ -194,7 +194,7 @@ def longitudinal_maneuver_alert(CP: car.CarParams, CS: car.CarState, sm: messagi
 
 def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
   personality = str(personality).title()
-  return NormalPermanentAlert(f"Following Distance: {personality}", duration=1.5)
+  return NormalPermanentAlert(f"Driving Personality: {personality}", duration=1.5)
 
 
 def invalid_lkas_setting_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
@@ -833,10 +833,6 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
   EventName.userBookmark: {
     ET.PERMANENT: NormalPermanentAlert("Bookmark Saved", duration=1.5),
   },
-
-  EventName.userBookmarkNotPaired: {
-    ET.PERMANENT: NormalPermanentAlert("Bookmark Saved", "pair to connect\nto view", duration=1.5),
-  },
 }
 
 

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -29,6 +29,7 @@ from openpilot.common.hardware import HARDWARE
 from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem
 from openpilot.sunnypilot import get_sanitize_int_param
 from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP
+from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper
 from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement
 from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
@@ -54,7 +55,7 @@ TurnDirection = custom.ModelDataV2SP.TurnDirection
 IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
 
 
-class SelfdriveD:
+class SelfdriveD(CruiseHelper):
   def __init__(self, CP=None, CP_SP=None):
     self.params = Params()
 
@@ -122,6 +123,8 @@ class SelfdriveD:
     # cleanup old params
     if not self.CP.alphaLongitudinalAvailable:
       self.params.remove("AlphaLongitudinalEnabled")
+    if not self.CP.openpilotLongitudinalControl:
+      self.params.remove("ExperimentalMode")
 
     self.CS_prev = car.CarState.new_message()
     self.AM = AlertManager()
@@ -138,6 +141,7 @@ class SelfdriveD:
     self.events_prev = []
     self.logged_comm_issue = None
     self.not_running_prev = None
+    self.experimental_mode = False
     self.personality = get_sanitize_int_param(
       "LongitudinalPersonality",
       min(log.LongitudinalPersonality.schema.enumerants.values()),
@@ -178,6 +182,7 @@ class SelfdriveD:
 
     self.car_events_sp = CarSpecificEventsSP(self.CP, self.CP_SP)
 
+    CruiseHelper.__init__(self, self.CP)
     self.button_state_tracker = ButtonStateTracker()
 
   def update_events(self, CS):
@@ -204,7 +209,6 @@ class SelfdriveD:
     big_failed = big_active is False or model_unavailable or (self.big_model_active and not chestnut_present)
     if big_failed and not self.big_model_failed:
       self.events.add(EventName.bigModelFailed)
-      self.big_model_ready_t = time.monotonic()
     self.big_model_failed = big_failed
 
     # soft disable if the big model fails
@@ -232,9 +236,7 @@ class SelfdriveD:
 
     # Check for user bookmark press
     if self.sm.updated['userBookmark']:
-      prime_type = self.params.get("PrimeType")
-      paired = prime_type is not None and int(prime_type) >= 0
-      self.events.add(EventName.userBookmark if paired else EventName.userBookmarkNotPaired)
+      self.events.add(EventName.userBookmark)
 
     # Don't add any more events while in dashcam mode
     if self.CP.passive:
@@ -504,19 +506,23 @@ class SelfdriveD:
 
     # TODO: fix simulator
     if not SIMULATION or REPLAY:
-      if self.sm['modelV2'].frameDropPerc > 1 and not big_model_settling:
+      if self.sm['modelV2'].frameDropPerc > 1:
         self.events.add(EventName.modeldLagging)
 
     # mute canBusMissing event if in Park, as it sometimes may trigger a false alarm with MADS in Paused state
     if CS.gearShifter == car.CarState.GearShifter.park and self.mads.enabled:
       self.events.remove(EventName.canBusMissing)
 
-    # Decrement personality on distance button press
+    CruiseHelper.update(self, CS, self.events_sp, self.experimental_mode)
+
+    # decrement personality on distance button press
     if self.CP.openpilotLongitudinalControl:
       if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
-        self.personality = (self.personality - 1) % 3
-        self.params.put('LongitudinalPersonality', self.personality)
-        self.events.add(EventName.personalityChanged)
+        if not self.experimental_mode_switched:
+          self.personality = (self.personality - 1) % 3
+          self.params.put('LongitudinalPersonality', self.personality)
+          self.events.add(EventName.personalityChanged)
+        self.experimental_mode_switched = False
 
     self.icbm.run(CS, self.sm['carControl'], self.sm['longitudinalPlanSP'], self.is_metric)
 
@@ -593,6 +599,7 @@ class SelfdriveD:
     ss.active = self.active
     ss.state = self.state_machine.state
     ss.engageable = not self.events.contains(ET.NO_ENTRY)
+    ss.experimentalMode = self.experimental_mode
     ss.personality = self.personality
 
     ss.alertText1 = self.AM.current_alert.alert_text_1
@@ -659,6 +666,7 @@ class SelfdriveD:
       self.is_metric = self.params.get_bool("IsMetric")
       self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
       self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
+      self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
       self.personality = self.params.get("LongitudinalPersonality", return_default=True)
 
       self.mads.read_params()

openpilot/selfdrive/test/helpers.py

@@ -105,6 +105,17 @@ def with_processes(processes, init_time=0, ignore_stopped=None):
   return wrapper
 
 
+def noop(*args, **kwargs):
+  pass
+
+
+def read_segment_list(segment_list_path):
+  with open(segment_list_path) as f:
+    seg_list = f.read().splitlines()
+
+  return [(platform[2:], segment) for platform, segment in zip(seg_list[::2], seg_list[1::2], strict=True)]
+
+
 @contextlib.contextmanager
 def http_server_context(handler, setup=None):
   host = '127.0.0.1'
@@ -122,3 +133,32 @@ def http_server_context(handler, setup=None):
     server.shutdown()
     server.server_close()
     t.join()
+
+
+def with_http_server(func, handler=http.server.BaseHTTPRequestHandler, setup=None):
+  @wraps(func)
+  def inner(*args, **kwargs):
+    with http_server_context(handler, setup) as (host, port):
+      return func(*args, f"http://{host}:{port}", **kwargs)
+  return inner
+
+
+def DirectoryHttpServer(directory) -> type[http.server.SimpleHTTPRequestHandler]:
+  # creates an http server that serves files from directory
+  class Handler(http.server.SimpleHTTPRequestHandler):
+    API_NO_RESPONSE = False
+    API_BAD_RESPONSE = False
+
+    def do_GET(self):
+      if self.API_NO_RESPONSE:
+        return
+
+      if self.API_BAD_RESPONSE:
+        self.send_response(500, "")
+        return
+      super().do_GET()
+
+    def __init__(self, *args, **kwargs):
+      super().__init__(*args, directory=str(directory), **kwargs)
+
+  return Handler