Merge branch 'upstream/master' into sync-20260517-new-new

.gitattributes

@@ -11,13 +11,4 @@
 *.wav filter=lfs diff=lfs merge=lfs -text
 
 selfdrive/car/tests/test_models_segs.txt filter=lfs diff=lfs merge=lfs -text
-system/hardware/tici/updater_weston filter=lfs diff=lfs merge=lfs -text
-system/hardware/tici/updater_magic filter=lfs diff=lfs merge=lfs -text
-third_party/**/*.a filter=lfs diff=lfs merge=lfs -text
-third_party/**/*.so filter=lfs diff=lfs merge=lfs -text
-third_party/**/*.so.* filter=lfs diff=lfs merge=lfs -text
-third_party/**/*.dylib filter=lfs diff=lfs merge=lfs -text
-third_party/acados/*/t_renderer filter=lfs diff=lfs merge=lfs -text
-third_party/qt5/larch64/bin/lrelease filter=lfs diff=lfs merge=lfs -text
-third_party/qt5/larch64/bin/lupdate filter=lfs diff=lfs merge=lfs -text
-third_party/catch2/include/catch2/catch.hpp filter=lfs diff=lfs merge=lfs -text
+system/hardware/tici/updater filter=lfs diff=lfs merge=lfs -text

.github/ISSUE_TEMPLATE/enhancement.md

@@ -1,8 +0,0 @@
----
-name: Enhancement
-about: For openpilot enhancement suggestions
-title: ''
-labels: 'enhancement'
-assignees: ''
----
-

.github/workflows/tests.yaml

@@ -123,7 +123,7 @@ jobs:
         submodules: true
     - run: ./tools/op.sh setup
     - name: Build openpilot
-      run: scons -j$(nproc)
+      run: scons
     - name: Run unit tests
       timeout-minutes: ${{ contains(runner.name, 'nsc') && 2 || 999 }}
       run: |
@@ -147,7 +147,7 @@ jobs:
         submodules: true
     - run: ./tools/op.sh setup
     - name: Build openpilot
-      run: scons -j$(nproc)
+      run: scons
     - name: Run replay
       timeout-minutes: ${{ contains(runner.name, 'nsc') && 2 || 20 }}
       continue-on-error: ${{ github.ref == 'refs/heads/master' }}
@@ -179,7 +179,7 @@ jobs:
         repository: commaai/ci-artifacts
         ssh-key: ${{ secrets.CI_ARTIFACTS_DEPLOY_KEY }}
         path: ${{ github.workspace }}/ci-artifacts
-    - name: Push refs
+    - name: Prepare refs
       if: github.repository == 'commaai/openpilot' && github.ref == 'refs/heads/master'
       working-directory: ${{ github.workspace }}/ci-artifacts
       run: |
@@ -191,7 +191,13 @@ jobs:
         echo "${{ github.sha }}" > ref_commit
         git add .
         git commit -m "process-replay refs for ${{ github.repository }}@${{ github.sha }}" || echo "No changes to commit"
-        git push origin process-replay --force
+    - name: Push refs
+      if: github.repository == 'commaai/openpilot' && github.ref == 'refs/heads/master'
+      uses: nick-fields/retry@7152eba30c6575329ac0576536151aca5a72780e
+      with:
+        timeout_minutes: 2
+        max_attempts: 3
+        command: cd ${{ github.workspace }}/ci-artifacts && git push origin process-replay --force
     - name: Run regen
       if: false
       timeout-minutes: 4
@@ -214,7 +220,7 @@ jobs:
         submodules: true
     - run: ./tools/op.sh setup
     - name: Build openpilot
-      run: scons -j$(nproc)
+      run: scons
     - name: Driving test
       timeout-minutes: 2
       run: |
@@ -235,7 +241,7 @@ jobs:
           submodules: true
       - run: ./tools/op.sh setup
       - name: Build openpilot
-        run: scons -j$(nproc)
+        run: scons
       - name: Create UI Report
         run: |
           source selfdrive/test/setup_xvfb.sh

.gitignore

@@ -44,7 +44,7 @@ bin/
 config.json
 compile_commands.json
 compare_runtime*.html
-selfdrive/modeld/models/tg_compiled_flags.json
+selfdrive/modeld/models/tg_input_devices.json
 
 # build artifacts
 docs_site/

.vscode/settings.json

@@ -21,7 +21,6 @@
     "common/**",
     "selfdrive/**",
     "system/**",
-    "third_party/**",
     "tools/**",
   ]
 }

Jenkinsfile

@@ -166,8 +166,8 @@ node {
   env.GIT_BRANCH = checkout(scm).GIT_BRANCH
   env.GIT_COMMIT = checkout(scm).GIT_COMMIT
 
-  def excludeBranches = ['__nightly', 'devel', 'devel-staging', 'release3', 'release3-staging',
-                         'release-tici', 'release-tizi', 'release-tizi-staging', 'release-mici-staging', 'testing-closet*', 'hotfix-*']
+  def excludeBranches = ['__nightly', 'devel', 'devel-staging',
+                         'release-tizi', 'release-tizi-staging', 'release-mici', 'release-mici-staging', 'testing-closet*', 'hotfix-*']
   def excludeRegex = excludeBranches.join('|').replaceAll('\\*', '.*')
 
   if (env.BRANCH_NAME != 'master' && !env.BRANCH_NAME.contains('__jenkins_loop_')) {
@@ -179,7 +179,7 @@ node {
   try {
     if (env.BRANCH_NAME == 'devel-staging') {
       deviceStage("build release-tizi-staging", "tizi-needs-can", [], [
-        step("build release-tizi-staging", "RELEASE_BRANCH=release-tizi-staging $SOURCE_DIR/release/build_release.sh && git push -f origin release-tizi-staging:release-mici-staging"),
+        step("build release-tizi-staging", "RELEASE_BRANCH=release-tizi-staging,release-mici-staging $SOURCE_DIR/release/build_release.sh"),
       ])
     }
 
@@ -247,7 +247,6 @@ node {
           step("test pandad loopback", "pytest selfdrive/pandad/tests/test_pandad_loopback.py"),
           step("test pandad spi", "pytest selfdrive/pandad/tests/test_pandad_spi.py"),
           step("test amp", "pytest system/hardware/tici/tests/test_amplifier.py"),
-          step("test qcomgpsd", "pytest system/qcomgpsd/tests/test_qcomgpsd.py", [diffPaths: ["system/qcomgpsd/"]]),
         ])
       },
 

RELEASES.md

@@ -1,7 +1,13 @@
-Version 0.11.1 (2026-04-22)
+Version 0.11.2 (2026-06-15)
+========================
+
+
+Version 0.11.1 (2026-05-18)
 ========================
 * New driver monitoring model
 * Improved image processing pipeline for driver camera
+* Improved thermal policy for comma four
+* Acura MDX 2022-24 support thanks to mvl-boston!
 * Rivian R1S and R1T 2025 support thanks to lukasloetkolben!
 
 Version 0.11.0 (2026-03-17)

SConstruct

@@ -10,25 +10,28 @@ import numpy as np
 import SCons.Errors
 from SCons.Defaults import _stripixes
 
+TICI = os.path.isfile('/TICI')
+
 SCons.Warnings.warningAsException(True)
 
 Decider('MD5-timestamp')
 
-SetOption('num_jobs', max(1, int(os.cpu_count()/2)))
+SetOption('num_jobs', max(1, int(os.cpu_count()/(1 if "CI" in os.environ else 2))))
 
 AddOption('--ccflags', action='store', type='string', default='', help='pass arbitrary flags over the command line')
 AddOption('--verbose', action='store_true', default=False, help='show full build commands')
+release = not os.path.exists(File('#.gitattributes').abspath) # file absent on release branch, see release_files.py
 AddOption('--minimal',
           action='store_false',
           dest='extras',
-          default=os.path.exists(File('#.gitattributes').abspath), # minimal by default on release branch (where there's no LFS)
+          default=(not TICI and not release),
           help='the minimum build to run openpilot. no tests, tools, etc.')
 
 # Detect platform
 arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
 if platform.system() == "Darwin":
   arch = "Darwin"
-elif arch == "aarch64" and os.path.isfile('/TICI'):
+elif arch == "aarch64" and TICI:
   arch = "larch64"
 assert arch in [
   "larch64",  # linux tici arm64
@@ -37,8 +40,14 @@ assert arch in [
   "Darwin",   # macOS arm64 (x86 not supported)
 ]
 
-pkg_names = ['bzip2', 'capnproto', 'eigen', 'ffmpeg', 'libjpeg', 'libyuv', 'ncurses', 'zeromq', 'zstd']
+pkg_names = ['acados', 'bzip2', 'capnproto', 'catch2', 'eigen', 'ffmpeg', 'json11', 'libjpeg', 'libyuv', 'ncurses', 'zeromq', 'zstd']
 pkgs = [importlib.import_module(name) for name in pkg_names]
+acados = pkgs[pkg_names.index('acados')]
+acados_include_dirs = [
+  acados.INCLUDE_DIR,
+  os.path.join(acados.INCLUDE_DIR, "blasfeo", "include"),
+  os.path.join(acados.INCLUDE_DIR, "hpipm", "include"),
+]
 
 
 # ***** enforce a whitelist of system libraries *****
@@ -82,10 +91,10 @@ def _libflags(target, source, env, for_signature):
 env = Environment(
   ENV={
     "PATH": os.environ['PATH'],
-    "PYTHONPATH": Dir("#").abspath + ':' + Dir(f"#third_party/acados").abspath,
-    "ACADOS_SOURCE_DIR": Dir("#third_party/acados").abspath,
-    "ACADOS_PYTHON_INTERFACE_PATH": Dir("#third_party/acados/acados_template").abspath,
-    "TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer"
+    "PYTHONPATH": Dir("#").abspath,
+    "ACADOS_SOURCE_DIR": acados.DIR,
+    "ACADOS_PYTHON_INTERFACE_PATH": acados.TEMPLATE_DIR,
+    "TERA_PATH": acados.TERA_PATH
   },
   CCFLAGS=[
     "-g",
@@ -105,22 +114,14 @@ env = Environment(
   CPPPATH=[
     "#",
     "#msgq",
-    "#third_party",
-    "#third_party/json11",
-    "#third_party/linux/include",
-    "#third_party/acados/include",
-    "#third_party/acados/include/blasfeo/include",
-    "#third_party/acados/include/hpipm/include",
-    "#third_party/catch2/include",
+    acados_include_dirs,
     [x.INCLUDE_DIR for x in pkgs],
   ],
   LIBPATH=[
     "#common",
     "#msgq_repo",
-    "#third_party",
     "#selfdrive/pandad",
     "#rednose/helpers",
-    f"#third_party/acados/{arch}/lib",
     [x.LIB_DIR for x in pkgs],
   ],
   RPATH=[],
@@ -174,16 +175,6 @@ if not GetOption('verbose'):
   ):
     env[f"{action}COMSTR"] = f"  [{short}] $TARGET"
 
-# progress output
-node_interval = 5
-node_count = 0
-def progress_function(node):
-  global node_count
-  node_count += node_interval
-  sys.stderr.write("progress: %d\n" % node_count)
-if os.environ.get('SCONS_PROGRESS'):
-  Progress(progress_function, interval=node_interval)
-
 # ********** Cython build environment **********
 envCython = env.Clone()
 envCython["CPPPATH"] += [sysconfig.get_paths()['include'], np.get_include()]
@@ -199,14 +190,24 @@ else:
 np_version = SCons.Script.Value(np.__version__)
 Export('envCython', 'np_version')
 
-Export('env', 'arch')
+Export('env', 'arch', 'acados')
 
 # Setup cache dir
 default_cache_dir = '/data/scons_cache' if arch == "larch64" else '/tmp/scons_cache'
 cache_dir = ARGUMENTS.get('cache_dir', default_cache_dir)
+cache_size_limit = 4e9 if "CI" in os.environ else 2e9
 CacheDir(cache_dir)
 Clean(["."], cache_dir)
 
+def prune_cache_dir(target=None, source=None, env=None):
+  cache_files = sorted((os.path.join(root, f) for root, _, files in os.walk(cache_dir) for f in files), key=os.path.getmtime)
+  cache_size = sum(os.path.getsize(f) for f in cache_files)
+  for f in cache_files:
+    if cache_size < cache_size_limit:
+      break
+    cache_size -= os.path.getsize(f)
+    os.unlink(f)
+
 # ********** start building stuff **********
 
 # Build common module
@@ -242,9 +243,6 @@ SConscript([
 if arch == "larch64":
   SConscript(['system/camerad/SConscript'])
 
-# Build openpilot
-SConscript(['third_party/SConscript'])
-
 # Build selfdrive
 SConscript([
   'selfdrive/pandad/SConscript',
@@ -257,8 +255,8 @@ SConscript([
 
 SConscript(['sunnypilot/SConscript'])
 
-# Build tools
-if arch != "larch64":
+# Build desktop-only tools
+if GetOption('extras') and arch != "larch64":
   SConscript([
     'tools/replay/SConscript',
     'tools/cabana/SConscript',
@@ -267,3 +265,37 @@ if arch != "larch64":
 
 
 env.CompilationDatabase('compile_commands.json')
+
+# progress output
+def count_scons_nodes(nodes):
+  seen = set()
+  stack = list(nodes)
+
+  while stack:
+    node = stack.pop().disambiguate()
+    if node in seen:
+      continue
+    seen.add(node)
+    executor = node.get_executor()
+    if executor is not None:
+      stack += executor.get_all_prerequisites() + executor.get_all_children()
+
+  return len(seen)
+
+progress_interval = 5
+progress_count = 0
+progress_total = max(1, count_scons_nodes(env.arg2nodes(BUILD_TARGETS or [Dir('.')], env.fs.Entry)))
+
+def progress_function(node):
+  global progress_count
+  if progress_count >= progress_total:
+    return
+  progress_count = min(progress_count + progress_interval, progress_total)
+  progress = round(100. * progress_count / progress_total, 1)
+  sys.stderr.write("\rBuilding: %5.1f%%" % progress if sys.stderr.isatty() else "progress: %.1f\n" % progress)
+  if progress == 100. and sys.stderr.isatty():
+    sys.stderr.write("\n")
+  sys.stderr.flush()
+
+Progress(progress_function, interval=progress_interval)
+AddPostAction(BUILD_TARGETS or [Dir('.')], prune_cache_dir)

cereal/log.capnp

@@ -273,11 +273,7 @@ struct GPSNMEAData {
   nmea @2 :Text;
 }
 
-# android sensor_event_t
 struct SensorEventData {
-  version @0 :Int32;
-  sensor @1 :Int32;
-  type @2 :Int32;
   timestamp @3 :Int64;
 
   union {
@@ -296,8 +292,11 @@ struct SensorEventData {
 
   struct SensorVec {
     v @0 :List(Float32);
+
+    deprecated :group {
       status @1 :Int8;
     }
+  }
 
   enum SensorSource {
     android @0;
@@ -314,7 +313,11 @@ struct SensorEventData {
     mmc5603nj @11;
   }
 
+  # formerly based on android sensor_event_t
   deprecated :group {
+    version @0 :Int32;
+    sensor @1 :Int32;
+    type @2 :Int32;
     uncalibrated @10 :Bool;
   }
 }
@@ -457,10 +460,10 @@ struct DeviceState @0xa4d8b5af2aa492eb {
   }
 
   enum ThermalStatus {
-    green @0;
-    yellow @1;
-    red @2;
-    danger @3;
+    ok @0;
+    warmDEPRECATED @1;
+    overheated @2;
+    critical @3;
   }
 
   enum NetworkType {
@@ -2060,6 +2063,7 @@ struct DriverStateV2 {
     rightBlinkProb @8 :Float32;
     sunglassesProb @9 :Float32;
     phoneProb @13 :Float32;
+    sleepProb @14 :Float32;
 
     deprecated :group {
       notReadyProb @12 :List(Float32);
@@ -2074,7 +2078,7 @@ struct DriverStateV2 {
   }
 }
 
-struct DriverMonitoringState @0xb83cda094a1da284 {
+struct DriverMonitoringStateDEPRECATED @0xb83cda094a1da284 {
   events @18 :List(OnroadEvent);
   faceDetected @1 :Bool;
   isDistracted @2 :Bool;
@@ -2102,6 +2106,75 @@ struct DriverMonitoringState @0xb83cda094a1da284 {
   }
 }
 
+struct DriverMonitoringState {
+  lockout @0 :Bool;
+  alertCountLockoutPercent @1 :Int8;
+  alertTimeLockoutPercent @2 :Int8;
+
+  alwaysOn @3 :Bool;
+  alwaysOnLockout @4 :Bool;
+
+  alertLevel @5 :AlertLevel;
+  activePolicy @6 :MonitoringPolicy;
+  isRHD @7 :Bool;
+  rhdCalibration @8 :CalibrationState;
+
+  visionPolicyState @9 :VisionPolicyState;
+  wheeltouchPolicyState @10 :WheeltouchPolicyState;
+
+  enum AlertLevel {
+    # ordinal must match the name to prevent bugs
+    # comparing against the raw ordinal value
+    none @0;
+    one @1;
+    two @2;
+    three @3;
+  }
+
+  enum MonitoringPolicy {
+    wheeltouch @0;
+    vision @1;
+  }
+
+  struct VisionPolicyState {
+    awarenessPercent @0 :Int8;
+    awarenessStep @1 :Float32;
+    isDistracted @2 :Bool;
+    distractedTypes @3 :DistractedTypes;
+
+    faceDetected @4 :Bool;
+    pose @5 :Pose;
+    wheeltouchFallbackPercent @6 :Int8;
+    uncertainOffroadAlertPercent @7 :Int8;
+
+    struct DistractedTypes {
+      pose @0: Bool;
+      eye @1: Bool;
+      phone @2: Bool;
+    }
+
+    struct Pose {
+      pitch @0 :Float32;
+      yaw @1 :Float32;
+      pitchCalib @2 :CalibrationState;
+      yawCalib @3 :CalibrationState;
+      calibrated @4 :Bool;
+      uncertainty @5 :Float32;
+    }
+  }
+
+  struct WheeltouchPolicyState {
+    awarenessPercent @0 :Int8;
+    awarenessStep @1 :Float32;
+    driverInteracting @2 :Bool;
+  }
+
+  struct CalibrationState {
+    calibratedPercent @0 :Int8;
+    offset @1 :Float32;
+  }
+}
+
 struct Boot {
   wallTimeNanos @0 :UInt64;
   pstore @4 :Map(Text, Data);
@@ -2227,7 +2300,8 @@ struct Sentinel {
 }
 
 struct UIDebug {
-  drawTimeMillis @0 :Float32;
+  cpuTimeMillis @0 :Float32;
+  frameTimeMillis @1 :Float32;
 }
 
 struct ManagerState {
@@ -2375,7 +2449,6 @@ struct Event {
     boot @60 :Boot;
 
     # ********** openpilot daemon msgs **********
-    gpsNMEA @3 :GPSNMEAData;
     can @5 :List(CanData);
     controlsState @7 :ControlsState;
     selfdriveState @130 :SelfdriveState;
@@ -2400,7 +2473,6 @@ struct Event {
     qcomGnss @31 :QcomGnss;
     gpsLocationExternal @48 :GpsLocationData;
     gpsLocation @21 :GpsLocationData;
-    gnssMeasurements @91 :GnssMeasurements;
     liveParameters @61 :LiveParametersData;
     liveTorqueParameters @94 :LiveTorqueParametersData;
     liveDelay @146 : LiveDelayData;
@@ -2408,7 +2480,7 @@ struct Event {
     thumbnail @66: Thumbnail;
     onroadEvents @134: List(OnroadEvent);
     carParams @69: Car.CarParams;
-    driverMonitoringState @71: DriverMonitoringState;
+    driverMonitoringState @151 :DriverMonitoringState;
     livePose @129 :LivePose;
     modelV2 @75 :ModelDataV2;
     drivingModelData @128 :DrivingModelData;
@@ -2434,7 +2506,6 @@ struct Event {
     # systems stuff
     androidLog @20 :AndroidLogEntry;
     managerState @78 :ManagerState;
-    uploaderState @79 :UploaderState;
     procLog @33 :ProcLog;
     clocks @35 :Clocks;
     deviceState @6 :DeviceState;
@@ -2444,12 +2515,6 @@ struct Event {
     # touch frame
     touch @135 :List(Touch);
 
-    # navigation
-    navInstruction @82 :NavInstruction;
-    navRoute @83 :NavRoute;
-    navThumbnail @84: Thumbnail;
-    mapRenderState @105: MapRenderState;
-
     # UI services
     uiDebug @102 :UIDebug;
 
@@ -2551,5 +2616,13 @@ struct Event {
     gyroscope2DEPRECATED @100 :SensorEventData;
     accelerometer2DEPRECATED @101 :SensorEventData;
     temperatureSensor2DEPRECATED @123 :SensorEventData;
+    driverMonitoringStateDEPRECATED @71 :DriverMonitoringStateDEPRECATED;
+    gpsNMEADEPRECATED @3 :GPSNMEAData;
+    uploaderStateDEPRECATED @79 :UploaderState;
+    navInstructionDEPRECATED @82 :NavInstruction;
+    navRouteDEPRECATED @83 :NavRoute;
+    navThumbnailDEPRECATED @84 :Thumbnail;
+    gnssMeasurementsDEPRECATED @91 :GnssMeasurements;
+    mapRenderStateDEPRECATED @105: MapRenderState;
   }
 }

cereal/messaging/__init__.py

@@ -259,11 +259,11 @@ class PubMaster:
     self.sock[s].send(dat)
 
   def wait_for_readers_to_update(self, s: str, timeout: int, dt: float = 0.05) -> bool:
-    for _ in range(int(timeout*(1./dt))):
-      if self.sock[s].all_readers_updated():
+    try:
+      self.sock[s].wait_for_readers(timeout=timeout, interval=dt)
       return True
-      time.sleep(dt)
+    except TimeoutError:
       return False
 
   def all_readers_updated(self, s: str) -> bool:
-    return self.sock[s].all_readers_updated()  # type: ignore
+    return self.sock[s].all_readers_updated()

cereal/messaging/tests/test_messaging.py

@@ -30,7 +30,7 @@ def zmq_sleep(t=1):
 
 # TODO: this should take any capnp struct and returrn a msg with random populated data
 def random_carstate():
-  fields = ["vEgo", "aEgo", "brake", "steeringAngleDeg"]
+  fields = ["vEgo", "aEgo", "steeringTorque", "steeringAngleDeg"]
   msg = messaging.new_message("carState")
   cs = msg.carState
   for f in fields:

cereal/services.py

@@ -24,10 +24,7 @@ _services: dict[str, tuple] = {
   # note: the "EncodeIdx" packets will still be in the log
   "gyroscope": (True, 104., 104),
   "accelerometer": (True, 104., 104),
-  "magnetometer": (True, 25.),
-  "lightSensor": (True, 100., 100),
   "temperatureSensor": (True, 2., 200),
-  "gpsNMEA": (True, 9.),
   "deviceState": (True, 2., 1),
   "touch": (True, 20., 1),
   "can": (True, 100., 2053, QueueSize.BIG),  # decimation gives ~3 msgs in a full segment
@@ -56,7 +53,6 @@ _services: dict[str, tuple] = {
   "gpsLocation": (True, 1., 1),
   "ubloxGnss": (True, 10.),
   "qcomGnss": (True, 2.),
-  "gnssMeasurements": (True, 10., 10),
   "clocks": (True, 0.1, 1),
   "ubloxRaw": (True, 20.),
   "livePose": (True, 20., 4),
@@ -75,10 +71,6 @@ _services: dict[str, tuple] = {
   "drivingModelData": (True, 20., 10),
   "modelV2": (True, 20., None, QueueSize.BIG),
   "managerState": (True, 2., 1),
-  "uploaderState": (True, 0., 1),
-  "navInstruction": (True, 1., 10),
-  "navRoute": (True, 0.),
-  "navThumbnail": (True, 0.),
   "qRoadEncodeIdx": (False, 20.),
   "userBookmark": (True, 0., 1),
   "soundPressure": (True, 10., 10),
@@ -114,8 +106,6 @@ _services: dict[str, tuple] = {
   "livestreamRoadEncodeData": (False, 20., None, QueueSize.MEDIUM),
   "livestreamDriverEncodeData": (False, 20., None, QueueSize.MEDIUM),
   "customReservedRawData0": (True, 0.),
-  "customReservedRawData1": (True, 0.),
-  "customReservedRawData2": (True, 0.),
 }
 SERVICE_LIST = {name: Service(*vals) for
                 idx, (name, vals) in enumerate(_services.items())}

common/file_chunker.py

@@ -1,3 +1,5 @@
+#!/usr/bin/env python3
+import sys
 import math
 import os
 from pathlib import Path
@@ -10,10 +12,13 @@ def get_chunk_name(name, idx, num_chunks):
 def get_manifest_path(name):
   return f"{name}.chunkmanifest"
 
-def get_chunk_paths(path, file_size):
-  num_chunks = math.ceil(file_size / CHUNK_SIZE)
+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
   with open(path, 'rb') as f:
@@ -26,6 +31,13 @@ def chunk_file(path, targets):
   Path(manifest_path).write_text(str(len(chunk_paths)))
   os.remove(path)
 
+def get_existing_chunks(path):
+  if os.path.isfile(path):
+    return [path]
+  if os.path.isfile(manifest := get_manifest_path(path)):
+    num_chunks = int(Path(manifest).read_text().strip())
+    return _chunk_paths(path, num_chunks)
+  raise FileNotFoundError(path)
 
 def read_file_chunked(path):
   manifest_path = get_manifest_path(path)
@@ -35,3 +47,9 @@ def read_file_chunked(path):
   if os.path.isfile(path):
     return Path(path).read_bytes()
   raise FileNotFoundError(path)
+
+
+if __name__ == "__main__":
+  path = sys.argv[1]
+  chunk_paths = get_chunk_targets(path, os.path.getsize(path))
+  chunk_file(path, chunk_paths)

common/parameterized.py

@@ -1,8 +1,13 @@
+import re
 import sys
 import pytest
 import inspect
 
 
+def _to_safe_name(s):
+  return re.sub(r"[^a-zA-Z0-9_]+", "_", str(s)).strip("_")
+
+
 class parameterized:
   @staticmethod
   def expand(cases):
@@ -34,7 +39,9 @@ def parameterized_class(attrs, input_list=None):
   def decorator(cls):
     globs = sys._getframe(1).f_globals
     for i, params in enumerate(params_list):
-      name = f"{cls.__name__}_{i}"
+      # append sanitized string param values so pytest -k can filter by them
+      suffix = "_".join(filter(None, (_to_safe_name(v) for v in params.values() if isinstance(v, str))))
+      name = f"{cls.__name__}_{i}" + (f"_{suffix}" if suffix else "")
       new_cls = type(name, (cls,), dict(params))
       new_cls.__module__ = cls.__module__
       new_cls.__test__ = True  # override inherited False so pytest collects this subclass

common/params.py

@@ -14,6 +14,6 @@ if __name__ == "__main__":
   if len(sys.argv) == 3:
     val = sys.argv[2]
     print(f"SET: {key} = {val}")
-    params.put(key, val)
+    params.put(key, val, block=True)
   elif len(sys.argv) == 2:
     print(f"GET: {key} = {params.get(key)}")

common/params_keys.h

@@ -80,6 +80,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LiveDelay", {PERSISTENT | BACKUP, BYTES}},
     {"LiveParameters", {PERSISTENT, JSON}},
     {"LiveParametersV2", {PERSISTENT, BYTES}},
+    {"LivestreamEncoderBitrate", {CLEAR_ON_MANAGER_START | DONT_LOG, INT}},
     {"LiveTorqueParameters", {PERSISTENT | DONT_LOG, BYTES}},
     {"LocationFilterInitialState", {PERSISTENT, BYTES}},
     {"LateralManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
@@ -103,8 +104,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"OnroadCycleRequested", {CLEAR_ON_MANAGER_START, BOOL}},
     {"OpenpilotEnabledToggle", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"PandaHeartbeatLost", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
-    {"PandaSomResetTriggered", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
-    {"PandaSignatures", {CLEAR_ON_MANAGER_START, BYTES}},
     {"PrimeType", {PERSISTENT, INT}},
     {"RecordAudio", {PERSISTENT | BACKUP, BOOL}},
     {"RecordAudioFeedback", {PERSISTENT | BACKUP, BOOL, "0"}},
@@ -132,6 +131,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"UpdaterLastFetchTime", {PERSISTENT, TIME}},
     {"UptimeOffroad", {PERSISTENT, FLOAT, "0.0"}},
     {"UptimeOnroad", {PERSISTENT, FLOAT, "0.0"}},
+    {"UsbGpuPresent", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
+    {"UsbGpuCompiled", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
     {"Version", {PERSISTENT, STRING}},
 
     // --- sunnypilot params --- //

common/params_pyx.pyx

@@ -142,32 +142,27 @@ cdef class Params:
     cdef ParamKeyType t = self.p.getKeyType(k)
     return ensure_bytes(self.python2cpp(type(dat), t, dat, key))
 
-  def put(self, key, dat):
+  def put(self, key, dat, bool block = False):
     """
-    Warning: This function blocks until the param is written to disk!
+    Warning: block=True blocks until the param is written to disk!
     In very rare cases this can take over a second, and your code will hang.
-    Use the put_nonblocking, put_bool_nonblocking in time sensitive code, but
-    in general try to avoid writing params as much as possible.
+    Use block=False in time sensitive code, but in general try to avoid
+    writing params as much as possible.
     """
     cdef string k = self.check_key(key)
     cdef string dat_bytes = self._put_cast(key, dat)
     with nogil:
+      if block:
         self.p.put(k, dat_bytes)
-
-  def put_bool(self, key, bool val):
-    cdef string k = self.check_key(key)
-    with nogil:
-      self.p.putBool(k, val)
-
-  def put_nonblocking(self, key, dat):
-    cdef string k = self.check_key(key)
-    cdef string dat_bytes = self._put_cast(key, dat)
-    with nogil:
+      else:
         self.p.putNonBlocking(k, dat_bytes)
 
-  def put_bool_nonblocking(self, key, bool val):
+  def put_bool(self, key, bool val, bool block = False):
     cdef string k = self.check_key(key)
     with nogil:
+      if block:
+        self.p.putBool(k, val)
+      else:
         self.p.putBoolNonBlocking(k, val)
 
   def remove(self, key):

common/realtime.py

@@ -28,6 +28,11 @@ class Priority:
   CTRL_HIGH = 53
 
 
+def drop_realtime() -> None:
+  if sys.platform == 'linux' and not PC:
+    os.sched_setscheduler(0, os.SCHED_OTHER, os.sched_param(0))
+
+
 def set_core_affinity(cores: list[int]) -> None:
   if sys.platform == 'linux' and not PC:
     os.sched_setaffinity(0, cores)

common/swaglog.cc

@@ -11,7 +11,7 @@
 
 #include <zmq.h>
 #include <stdarg.h>
-#include "third_party/json11/json11.hpp"
+#include "json11/json11.hpp"
 #include "common/version.h"
 #include "system/hardware/hw.h"
 

common/tests/test_params.py

@@ -12,17 +12,17 @@ class TestParams:
     self.params = Params()
 
   def test_params_put_and_get(self):
-    self.params.put("DongleId", "cb38263377b873ee")
+    self.params.put("DongleId", "cb38263377b873ee", block=True)
     assert self.params.get("DongleId") == "cb38263377b873ee"
 
   def test_params_non_ascii(self):
     st = b"\xe1\x90\xff"
-    self.params.put("CarParams", st)
+    self.params.put("CarParams", st, block=True)
     assert self.params.get("CarParams") == st
 
   def test_params_get_cleared_manager_start(self):
-    self.params.put("CarParams", b"test")
-    self.params.put("DongleId", "cb38263377b873ee")
+    self.params.put("CarParams", b"test", block=True)
+    self.params.put("DongleId", "cb38263377b873ee", block=True)
     assert self.params.get("CarParams") == b"test"
 
     undefined_param = self.params.get_param_path(uuid.uuid4().hex)
@@ -36,15 +36,15 @@ class TestParams:
     assert not os.path.isfile(undefined_param)
 
   def test_params_two_things(self):
-    self.params.put("DongleId", "bob")
-    self.params.put("AthenadPid", 123)
+    self.params.put("DongleId", "bob", block=True)
+    self.params.put("AthenadPid", 123, block=True)
     assert self.params.get("DongleId") == "bob"
     assert self.params.get("AthenadPid") == 123
 
   def test_params_get_block(self):
     def _delayed_writer():
       time.sleep(0.1)
-      self.params.put("CarParams", b"test")
+      self.params.put("CarParams", b"test", block=True)
     threading.Thread(target=_delayed_writer).start()
     assert self.params.get("CarParams") is None
     assert self.params.get("CarParams", block=True) == b"test"
@@ -57,10 +57,10 @@ class TestParams:
       self.params.get_bool("swag")
 
     with pytest.raises(UnknownKeyName):
-      self.params.put("swag", "abc")
+      self.params.put("swag", "abc", block=True)
 
     with pytest.raises(UnknownKeyName):
-      self.params.put_bool("swag", True)
+      self.params.put_bool("swag", True, block=True)
 
   def test_remove_not_there(self):
     assert self.params.get("CarParams") is None
@@ -71,23 +71,23 @@ class TestParams:
     self.params.remove("IsMetric")
     assert not self.params.get_bool("IsMetric")
 
-    self.params.put_bool("IsMetric", True)
+    self.params.put_bool("IsMetric", True, block=True)
     assert self.params.get_bool("IsMetric")
 
-    self.params.put_bool("IsMetric", False)
+    self.params.put_bool("IsMetric", False, block=True)
     assert not self.params.get_bool("IsMetric")
 
-    self.params.put("IsMetric", True)
+    self.params.put("IsMetric", True, block=True)
     assert self.params.get_bool("IsMetric")
 
-    self.params.put("IsMetric", False)
+    self.params.put("IsMetric", False, block=True)
     assert not self.params.get_bool("IsMetric")
 
   def test_put_non_blocking_with_get_block(self):
     q = Params()
     def _delayed_writer():
       time.sleep(0.1)
-      Params().put_nonblocking("CarParams", b"test")
+      Params().put("CarParams", b"test")
     threading.Thread(target=_delayed_writer).start()
     assert q.get("CarParams") is None
     assert q.get("CarParams", True) == b"test"
@@ -96,7 +96,7 @@ class TestParams:
     q = Params()
     def _delayed_writer():
       time.sleep(0.1)
-      Params().put_bool_nonblocking("CarParams", True)
+      Params().put_bool("CarParams", True)
     threading.Thread(target=_delayed_writer).start()
     assert q.get("CarParams") is None
     assert q.get("CarParams", True) == b"1"
@@ -123,19 +123,19 @@ class TestParams:
 
   def test_params_get_type(self):
     # json
-    self.params.put("ApiCache_FirehoseStats", {"a": 0})
+    self.params.put("ApiCache_FirehoseStats", {"a": 0}, block=True)
     assert self.params.get("ApiCache_FirehoseStats") == {"a": 0}
 
     # int
-    self.params.put("BootCount", 1441)
+    self.params.put("BootCount", 1441, block=True)
     assert self.params.get("BootCount") == 1441
 
     # bool
-    self.params.put("AdbEnabled", True)
+    self.params.put("AdbEnabled", True, block=True)
     assert self.params.get("AdbEnabled")
     assert isinstance(self.params.get("AdbEnabled"), bool)
 
     # time
     now = datetime.datetime.now(datetime.UTC)
-    self.params.put("InstallDate", now)
+    self.params.put("InstallDate", now, block=True)
     assert self.params.get("InstallDate") == now

common/tests/test_swaglog.cc

@@ -7,7 +7,7 @@
 #include "common/util.h"
 #include "common/version.h"
 #include "system/hardware/hw.h"
-#include "third_party/json11/json11.hpp"
+#include "json11/json11.hpp"
 
 #include "sunnypilot/common/version.h"
 

common/utils.py

@@ -48,7 +48,7 @@ def sudo_write(val: str, path: str) -> None:
 
 def sudo_read(path: str) -> str:
   try:
-    return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip()
+    return subprocess.check_output(["sudo", "cat", "--", path], encoding='utf8').strip()
   except Exception:
     return ""
 

common/version.h

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

conftest.py

@@ -7,25 +7,19 @@ from openpilot.common.prefix import OpenpilotPrefix
 from openpilot.system.manager import manager
 from openpilot.system.hardware import TICI, HARDWARE
 
-# TODO: pytest-cpp doesn't support FAIL, and we need to create test translations in sessionstart
-# pending https://github.com/pytest-dev/pytest-cpp/pull/147
+# these are heavy CI-only tests, invoked explicitly in .github/workflows/tests.yaml
 collect_ignore = [
   "selfdrive/test/process_replay/test_processes.py",
   "selfdrive/test/process_replay/test_regen.py",
+  # tinygrad JIT has process-global state. Other test files import modeld → tinygrad,
+  # which corrupts JIT captures for test_warp.py in the same process. Run separately in CI.
+  "sunnypilot/modeld_v2/tests/test_warp.py",
 ]
 collect_ignore_glob = [
   "selfdrive/debug/*.py",
-  "selfdrive/modeld/*.py",
-  "sunnypilot/modeld*/*.py",
 ]
 
 
-def pytest_sessionstart(session):
-  # TODO: fix tests and enable test order randomization
-  if session.config.pluginmanager.hasplugin('randomly'):
-    session.config.option.randomly_reorganize = False
-
-
 @pytest.hookimpl(hookwrapper=True, trylast=True)
 def pytest_runtest_call(item):
   # ensure we run as a hook after capturemanager's
@@ -97,15 +91,3 @@ def pytest_collection_modifyitems(config, items):
       class_property_name = item.get_closest_marker('xdist_group_class_property').args[0]
       class_property_value = getattr(item.cls, class_property_name)
       item.add_marker(pytest.mark.xdist_group(class_property_value))
-
-
-@pytest.hookimpl(trylast=True)
-def pytest_configure(config):
-  config_line = "xdist_group_class_property: group tests by a property of the class that contains them"
-  config.addinivalue_line("markers", config_line)
-
-  config_line = "nocapture: don't capture test output"
-  config.addinivalue_line("markers", config_line)
-
-  config_line = "shared_download_cache: share download cache between tests"
-  config.addinivalue_line("markers", config_line)

docs/SAFETY.md

@@ -4,8 +4,8 @@ openpilot is an Adaptive Cruise Control (ACC) and Automated Lane Centering (ALC)
 Like other ACC and ALC systems, openpilot is a failsafe passive system and it requires the
 driver to be alert and to pay attention at all times.
 
-In order to enforce driver alertness, openpilot includes a driver monitoring feature
-that alerts the driver when distracted.
+To assist the driver in maintaining alertness, openpilot includes a driver monitoring feature
+that alerts when it detects driver distraction.
 
 However, even with an attentive driver, we must make further efforts for the system to be
 safe. We repeat, **driver alertness is necessary, but not sufficient, for openpilot to be

docs/ext/glossary.py

@@ -8,7 +8,7 @@ from markdown.extensions import Extension
 from markdown.preprocessors import Preprocessor
 from markdown.treeprocessors import Treeprocessor
 
-from zensical.extensions.links import LinksProcessor
+from zensical.extensions.links import LinksTreeprocessor
 
 GlossaryTerm = tuple[str, re.Pattern[str], str]
 
@@ -78,7 +78,7 @@ class GlossaryTreeprocessor(Treeprocessor):
   def run(self, root: ET.Element) -> None:
     at = self.md.treeprocessors.get_index_for_name("zrelpath")
     processor = self.md.treeprocessors[at]
-    if not isinstance(processor, LinksProcessor):
+    if not isinstance(processor, LinksTreeprocessor):
       raise TypeError("Links processor not registered")
     if processor.path == GLOSSARY_PAGE:
       return

docs/how-to/turn-the-speed-blue.md

@@ -20,7 +20,7 @@ source .venv/bin/activate
 
 Then, compile openpilot:
 ```bash
-scons -j$(nproc)
+scons
 ```
 
 ## 2. Run replay

launch_env.sh

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

openpilot/third_party

@@ -1 +0,0 @@
-../third_party

pyproject.toml

@@ -20,14 +20,17 @@ dependencies = [
   # core
   "cffi",
   "scons",
-  "pycapnp",
+  "pycapnp==2.1.0",  # 2.2 introduces a memory leak due to cyclic references
   "Cython",
   "setuptools",
   "numpy >=2.0",
 
   # vendored native dependencies
   "bzip2 @ git+https://github.com/commaai/dependencies.git@release-bzip2#subdirectory=bzip2",
+  "bootstrap-icons @ git+https://github.com/commaai/dependencies.git@release-bootstrap-icons#subdirectory=bootstrap-icons",
   "capnproto @ git+https://github.com/commaai/dependencies.git@release-capnproto#subdirectory=capnproto",
+  "catch2 @ git+https://github.com/commaai/dependencies.git@release-catch2#subdirectory=catch2",
+  "acados @ git+https://github.com/commaai/dependencies.git@release-acados#subdirectory=acados",
   "eigen @ git+https://github.com/commaai/dependencies.git@release-eigen#subdirectory=eigen",
   "ffmpeg @ git+https://github.com/commaai/dependencies.git@release-ffmpeg#subdirectory=ffmpeg",
   "libjpeg @ git+https://github.com/commaai/dependencies.git@release-libjpeg#subdirectory=libjpeg",
@@ -36,8 +39,10 @@ dependencies = [
   "ncurses @ git+https://github.com/commaai/dependencies.git@release-ncurses#subdirectory=ncurses",
   "zeromq @ git+https://github.com/commaai/dependencies.git@release-zeromq#subdirectory=zeromq",
   "libusb @ git+https://github.com/commaai/dependencies.git@release-libusb#subdirectory=libusb",
+  "json11 @ git+https://github.com/commaai/dependencies.git@release-json11#subdirectory=json11",
   "git-lfs @ git+https://github.com/commaai/dependencies.git@release-git-lfs#subdirectory=git-lfs",
   "gcc-arm-none-eabi @ git+https://github.com/commaai/dependencies.git@release-gcc-arm-none-eabi#subdirectory=gcc-arm-none-eabi",
+  "xvfb @ git+https://github.com/commaai/dependencies.git@release-xvfb#subdirectory=xvfb",
 
   # body / webrtcd
   "av",
@@ -58,9 +63,6 @@ dependencies = [
   "json-rpc",
   "websocket_client",
 
-  # acados deps
-  "casadi >=3.6.6",  # 3.12 fixed in 3.6.6
-
   # joystickd
   "inputs",
 
@@ -73,7 +75,7 @@ dependencies = [
   "zstandard",
 
   # ui
-  "raylib > 5.5.0.3",
+  "raylib @ git+https://github.com/commaai/dependencies.git@release-raylib#subdirectory=raylib",
   "qrcode",
   "jeepney",
   "pillow",
@@ -94,7 +96,6 @@ testing = [
   "pytest-subtests",
   # https://github.com/pytest-dev/pytest-xdist/pull/1229
   "pytest-xdist @ git+https://github.com/sshane/pytest-xdist@2b4372bd62699fb412c4fe2f95bf9f01bd2018da",
-  "pytest-asyncio",
   "pytest-mock",
   "ruff",
   "codespell",
@@ -130,11 +131,13 @@ addopts = "--ignore=openpilot/ --ignore=opendbc/ --ignore=panda/ --ignore=rednos
 cpp_files = "test_*"
 cpp_harness = "selfdrive/test/cpp_harness.py"
 python_files = "test_*.py"
-asyncio_default_fixture_loop_scope = "function"
 markers = [
   "slow: tests that take awhile to run and can be skipped with -m 'not slow'",
   "tici: tests that are only meant to run on the C3/C3X",
-  "skip_tici_setup: mark test to skip tici setup fixture"
+  "skip_tici_setup: mark test to skip tici setup fixture",
+  "nocapture: don't capture test output",
+  "shared_download_cache: share download cache between tests",
+  "xdist_group_class_property: group tests by a property of the class that contains them",
 ]
 testpaths = [
   "common",
@@ -175,9 +178,7 @@ lint.ignore = [
   "UP045", "UP007",  # these don't play nice with raylib atm
 ]
 line-length = 160
-target-version ="py311"
 exclude = [
-  "body",
   "cereal",
   "panda",
   "opendbc",
@@ -186,7 +187,7 @@ exclude = [
   "tinygrad_repo",
   "teleoprtc",
   "teleoprtc_repo",
-  "third_party",
+  "third_party/copyparty",
   "*.ipynb",
   "generated",
 ]
@@ -196,7 +197,6 @@ lint.flake8-implicit-str-concat.allow-multiline = false
 "selfdrive".msg = "Use openpilot.selfdrive"
 "common".msg = "Use openpilot.common"
 "system".msg = "Use openpilot.system"
-"third_party".msg = "Use openpilot.third_party"
 "tools".msg = "Use openpilot.tools"
 "pytest.main".msg = "pytest.main requires special handling that is easy to mess up!"
 "unittest".msg = "Use pytest"
@@ -214,7 +214,6 @@ quote-style = "preserve"
 
 [tool.ty.src]
 exclude = [
-  "cereal/",
   "msgq/",
   "msgq_repo/",
   "opendbc/",
@@ -230,27 +229,16 @@ exclude = [
 ]
 
 [tool.ty.rules]
-# Ignore unresolved imports for Cython-compiled modules (.pyx)
-unresolved-import = "ignore"
-# Ignore unresolved attributes - many from capnp and Cython modules
-unresolved-attribute = "ignore"
-# Ignore invalid method overrides - signature variance issues
-invalid-method-override = "ignore"
-# Ignore possibly-missing-attribute - too many false positives
-possibly-missing-attribute = "ignore"
-# Ignore invalid assignment - often intentional monkey-patching
-invalid-assignment = "ignore"
-# Ignore no-matching-overload - numpy/ctypes overload matching issues
-no-matching-overload = "ignore"
-# Ignore invalid-argument-type - many false positives from raylib, ctypes, numpy
-invalid-argument-type = "ignore"
-# Ignore call-non-callable - false positives from dynamic types
-call-non-callable = "ignore"
-# Ignore unsupported-operator - false positives from dynamic types
-unsupported-operator = "ignore"
-# Ignore not-subscriptable - false positives from dynamic types
-not-subscriptable = "ignore"
-# not-iterable errors are now fixed
+unresolved-import = "ignore"  # Cython-compiled modules (.pyx)
+unresolved-attribute = "ignore"  # many from capnp and Cython modules
+invalid-method-override = "ignore"  # signature variance issues
+possibly-missing-attribute = "ignore"  # too many false positives
+invalid-assignment = "ignore"  # often intentional monkey-patching
+no-matching-overload = "ignore"  # numpy/ctypes overload matching issues
+invalid-argument-type = "ignore"  # many false positives from raylib, ctypes, numpy
+call-non-callable = "ignore"  # false positives from dynamic types
+unsupported-operator = "ignore" # false positives from dynamic types
+not-subscriptable = "ignore" # false positives from dynamic types
 
 [tool.uv]
 python-preference = "only-managed"

release/build_release.sh

@@ -16,6 +16,8 @@ if [ -z "$RELEASE_BRANCH" ]; then
   exit 1
 fi
 
+BUILD_BRANCH=release-mici-staging
+
 
 # set git identity
 source $DIR/identity.sh
@@ -26,7 +28,7 @@ mkdir -p $BUILD_DIR
 cd $BUILD_DIR
 git init
 git remote add origin git@github.com:commaai/openpilot.git
-git checkout --orphan $RELEASE_BRANCH
+git checkout --orphan $BUILD_BRANCH
 
 # do the files copy
 echo "[-] copying files T=$SECONDS"
@@ -46,14 +48,14 @@ git commit -a -m "openpilot v$VERSION release"
 
 # Build
 export PYTHONPATH="$BUILD_DIR"
-scons -j$(nproc) --minimal
+scons
 
 if [ -z "$PANDA_DEBUG_BUILD" ]; then
   # release panda fw
-  CERT=/data/pandaextra/certs/release RELEASE=1 scons -j$(nproc) panda/
+  CERT=/data/pandaextra/certs/release RELEASE=1 scons panda/
 else
   # build with ALLOW_DEBUG=1 to enable features like experimental longitudinal
-  scons -j$(nproc) panda/
+  scons panda/
 fi
 
 # Ensure no submodules in release
@@ -72,8 +74,8 @@ find . -name '*.pyc' -delete
 find . -name 'moc_*' -delete
 find . -name '__pycache__' -delete
 rm -rf .sconsign.dblite Jenkinsfile release/
-rm -f selfdrive/modeld/models/*.onnx
-rm -f sunnypilot/modeld*/models/*.onnx
+rm -f selfdrive/modeld/models/*.onnx*
+rm -f sunnypilot/modeld*/models/*.onnx*
 
 find third_party/ -name '*x86*' -exec rm -r {} +
 find third_party/ -name '*Darwin*' -exec rm -r {} +
@@ -94,9 +96,11 @@ cd $BUILD_DIR
 RELEASE=1 pytest -n0 -s selfdrive/test/test_onroad.py
 #pytest selfdrive/car/tests/test_car_interfaces.py
 
-if [ ! -z "$RELEASE_BRANCH" ]; then
-  echo "[-] pushing release T=$SECONDS"
-  git push -f origin $RELEASE_BRANCH:$RELEASE_BRANCH
-fi
+echo "[-] pushing release T=$SECONDS"
+REFS=()
+for branch in ${RELEASE_BRANCH//,/ }; do
+  REFS+=("$BUILD_BRANCH:$branch")
+done
+git push -f origin "${REFS[@]}"
 
 echo "[-] done T=$SECONDS"

release/build_stripped.sh

@@ -45,6 +45,8 @@ cd $TARGET_DIR
 rm -rf .git/modules/
 rm -f panda/board/obj/panda.bin.signed
 
+find selfdrive/modeld/models -name '*.onnx' -size +95M -exec ./common/file_chunker.py {} \;
+
 # include source commit hash and build date in commit
 GIT_HASH=$(git --git-dir=$SOURCE_DIR/.git rev-parse HEAD)
 GIT_COMMIT_DATE=$(git --git-dir=$SOURCE_DIR/.git show --no-patch --format='%ct %ci' HEAD)

release/pack.py

@@ -13,7 +13,7 @@ from openpilot.common.basedir import BASEDIR
 
 DIRS = ['cereal', 'openpilot']
 EXTS = ['.png', '.py', '.ttf', '.capnp', '.json', '.fnt', '.mo', '.po']
-EXCLUDE = ['selfdrive/assets/training', 'third_party/raylib/raylib_repo/examples']
+EXCLUDE = ['selfdrive/assets/training']
 INTERPRETER = '/usr/bin/env python3'
 
 

scripts/lint/check_raylib_includes.sh

@@ -1,10 +0,0 @@
-#!/usr/bin/env bash
-
-FAIL=0
-
-if grep -n '#include "third_party/raylib/include/raylib\.h"' $@ | grep -v '^system/ui/raylib/raylib\.h'; then
-  echo -e "Bad raylib include found! Use '#include \"system/ui/raylib/raylib.h\"' instead\n"
-  FAIL=1
-fi
-
-exit $FAIL

scripts/lint/lint.sh

@@ -14,7 +14,7 @@ cd $ROOT
 FAILED=0
 
 IGNORED_FILES="uv\.lock|docs\/CARS.md|LICENSE\.md"
-IGNORED_DIRS="^third_party.*|^msgq.*|^msgq_repo.*|^opendbc.*|^opendbc_repo.*|^cereal.*|^panda.*|^rednose.*|^rednose_repo.*|^tinygrad.*|^tinygrad_repo.*|^teleoprtc.*|^teleoprtc_repo.*"
+IGNORED_DIRS="^msgq.*|^msgq_repo.*|^opendbc.*|^opendbc_repo.*|^cereal.*|^panda.*|^rednose.*|^rednose_repo.*|^tinygrad.*|^tinygrad_repo.*|^teleoprtc.*|^teleoprtc_repo.*|^third_party.*"
 
 function run() {
   shopt -s extglob

scripts/reporter.py

@@ -34,6 +34,11 @@ if __name__ == "__main__":
 
   for f in glob.glob(BASEDIR + MODEL_PATH + "/*.onnx"):
     fn = os.path.basename(f)
-    master = get_checkpoint(MASTER_PATH + MODEL_PATH + fn)
+    master_path = MASTER_PATH + MODEL_PATH + fn
+    if os.path.exists(master_path):
+      master = get_checkpoint(master_path)
+      master_col = f"[{master}](https://reporter.comma.life/experiment/{master})"
+    else:
+      master_col = "N/A (new model)"
     pr = get_checkpoint(BASEDIR + MODEL_PATH + fn)
-    print("|", fn, "|", f"[{master}](https://reporterv2.comma.life/{master})", "|", f"[{pr}](https://reporterv2.comma.life/{pr})", "|")
+    print("|", fn, "|", master_col, "|", f"[{pr}](https://reporter.comma.life/experiment/{pr})", "|")

selfdrive/assets/assets.qrc

@@ -1,6 +1,6 @@
 <!DOCTYPE RCC><RCC version="1.0">
 <qresource>
-  <file alias="bootstrap-icons.svg">../../third_party/bootstrap/bootstrap-icons.svg</file>
+  <file alias="bootstrap-icons.svg">@BOOTSTRAP_ICONS_SVG@</file>
   <file>images/button_continue_triangle.svg</file>
   <file>icons/circled_check.svg</file>
   <file>icons/circled_slash.svg</file>

selfdrive/assets/fonts/process.py

@@ -37,10 +37,10 @@ def _char_sets():
   return tuple(sorted(ord(c) for c in base)), tuple(sorted(ord(c) for c in unifont))
 
 
-def _glyph_metrics(glyphs, rects, codepoints):
+def _glyph_metrics(glyphs, rects, glyph_count: int):
   entries = []
   min_offset_y, max_extent = None, 0
-  for idx, codepoint in enumerate(codepoints):
+  for idx in range(glyph_count):
     glyph = glyphs[idx]
     rect = rects[idx]
     width = int(round(rect.width))
@@ -49,7 +49,7 @@ def _glyph_metrics(glyphs, rects, codepoints):
     min_offset_y = offset_y if min_offset_y is None else min(min_offset_y, offset_y)
     max_extent = max(max_extent, offset_y + height)
     entries.append({
-      "id": codepoint,
+      "id": glyph.value,
       "x": int(round(rect.x)),
       "y": int(round(rect.y)),
       "width": width,
@@ -97,19 +97,23 @@ def _process_font(font_path: Path, codepoints: tuple[int, ...]):
   file_buf = rl.ffi.new("unsigned char[]", data)
   cp_buffer = rl.ffi.new("int[]", codepoints)
   cp_ptr = rl.ffi.cast("int *", cp_buffer)
-  glyphs = rl.load_font_data(rl.ffi.cast("unsigned char *", file_buf), len(data), font_size, cp_ptr, len(codepoints), rl.FontType.FONT_DEFAULT)
+  glyph_count = rl.ffi.new("int *", len(codepoints))
+  glyphs = rl.load_font_data(
+    rl.ffi.cast("unsigned char *", file_buf), len(data), font_size, cp_ptr, len(codepoints),
+    rl.FontType.FONT_DEFAULT, glyph_count
+  )
   if glyphs == rl.ffi.NULL:
     raise RuntimeError("raylib failed to load font data")
 
   rects_ptr = rl.ffi.new("Rectangle **")
-  image = rl.gen_image_font_atlas(glyphs, rects_ptr, len(codepoints), font_size, GLYPH_PADDING, 0)
+  image = rl.gen_image_font_atlas(glyphs, rects_ptr, glyph_count[0], font_size, GLYPH_PADDING, 0)
   if image.width == 0 or image.height == 0:
     raise RuntimeError("raylib returned an empty atlas")
 
   rects = rects_ptr[0]
   atlas_name = f"{font_path.stem}.png"
   atlas_path = FONT_DIR / atlas_name
-  entries, line_height, base = _glyph_metrics(glyphs, rects, codepoints)
+  entries, line_height, base = _glyph_metrics(glyphs, rects, glyph_count[0])
 
   if not rl.export_image(image, atlas_path.as_posix()):
     raise RuntimeError("Failed to export atlas image")

selfdrive/assets/icons_mici/egpu.png

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

selfdrive/assets/icons_mici/egpu_gray.png

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

selfdrive/assets/prep-svg.sh

@@ -3,7 +3,7 @@ set -e
 
 DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
 ICONS_DIR="$DIR/icons"
-BOOTSTRAP_SVG="$DIR/../../third_party/bootstrap/bootstrap-icons.svg"
+BOOTSTRAP_SVG="$(python3 -c 'import bootstrap_icons; print(bootstrap_icons.SVG_PATH)')"
 
 ICON_IDS=(
   arrow-right

selfdrive/car/car_specific.py

@@ -73,7 +73,7 @@ class CarSpecificEvents:
     elif self.CP.brand == 'gm':
       # Enabling at a standstill with brake is allowed
       # TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs
-      if CS.vEgo < self.CP.minEnableSpeed and not (CS.standstill and CS.brake >= 20 and
+      if CS.vEgo < self.CP.minEnableSpeed and not (CS.standstill and CS.brakePressed and
                                                    self.CP.networkLocation == NetworkLocation.fwdCamera):
         events.add(EventName.belowEngageSpeed)
       if CS.cruiseState.standstill:

selfdrive/car/card.py

@@ -37,7 +37,7 @@ def obd_callback(params: Params) -> ObdCallback:
     if params.get_bool("ObdMultiplexingEnabled") != obd_multiplexing:
       cloudlog.warning(f"Setting OBD multiplexing to {obd_multiplexing}")
       params.remove("ObdMultiplexingChanged")
-      params.put_bool("ObdMultiplexingEnabled", obd_multiplexing)
+      params.put_bool("ObdMultiplexingEnabled", obd_multiplexing, block=True)
       params.get_bool("ObdMultiplexingChanged", block=True)
       cloudlog.warning("OBD multiplexing set successfully")
   return set_obd_multiplexing
@@ -116,7 +116,7 @@ class Car:
       self.CP_SP = self.CI.CP_SP
 
       # continue onto next fingerprinting step in pandad
-      self.params.put_bool("FirmwareQueryDone", True)
+      self.params.put_bool("FirmwareQueryDone", True, block=True)
     else:
       self.CI, self.CP, self.CP_SP = CI, CI.CP, CI.CP_SP
       self.RI = RI
@@ -143,7 +143,7 @@ class Car:
         with open("/cache/params/SecOCKey") as f:
           user_key = f.readline().strip()
           if len(user_key) == 32:
-            self.params.put("SecOCKey", user_key)
+            self.params.put("SecOCKey", user_key, block=True)
       except Exception:
         pass
 
@@ -161,21 +161,21 @@ class Car:
     # Write previous route's CarParams
     prev_cp = self.params.get("CarParamsPersistent")
     if prev_cp is not None:
-      self.params.put("CarParamsPrevRoute", prev_cp)
+      self.params.put("CarParamsPrevRoute", prev_cp, block=True)
 
     # Write CarParams for controls and radard
     cp_bytes = self.CP.to_bytes()
-    self.params.put("CarParams", cp_bytes)
-    self.params.put_nonblocking("CarParamsCache", cp_bytes)
-    self.params.put_nonblocking("CarParamsPersistent", cp_bytes)
+    self.params.put("CarParams", cp_bytes, block=True)
+    self.params.put("CarParamsCache", cp_bytes)
+    self.params.put("CarParamsPersistent", cp_bytes)
 
     # Write CarParamsSP for controls
     # convert to pycapnp representation for caching and logging
     self.CP_SP_capnp = convert_to_capnp(self.CP_SP)
     cp_sp_bytes = self.CP_SP_capnp.to_bytes()
-    self.params.put("CarParamsSP", cp_sp_bytes)
-    self.params.put_nonblocking("CarParamsSPCache", cp_sp_bytes)
-    self.params.put_nonblocking("CarParamsSPPersistent", cp_sp_bytes)
+    self.params.put("CarParamsSP", cp_sp_bytes, block=True)
+    self.params.put("CarParamsSPCache", cp_sp_bytes)
+    self.params.put("CarParamsSPPersistent", cp_sp_bytes)
 
     self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP)
 
@@ -274,7 +274,7 @@ class Car:
       # TODO: this can make us miss at least a few cycles when doing an ECU knockout
       self.CI.init(self.CP, self.CP_SP, *self.can_callbacks)
       # signal pandad to switch to car safety mode
-      self.params.put_bool_nonblocking("ControlsReady", True)
+      self.params.put_bool("ControlsReady", True)
 
     if self.sm.all_alive(['carControl']):
       # send car controls over can

selfdrive/car/tests/test_cruise_speed.py

@@ -94,7 +94,7 @@ class TestVCruiseHelper:
         self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, 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
+      assert (not pressed) == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
 
   def test_resume_in_standstill(self):
     """

selfdrive/car/tests/test_models.py

@@ -13,7 +13,7 @@ from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs
 from opendbc.car.can_definitions import CanData
 from opendbc.car.car_helpers import FRAME_FINGERPRINT, interfaces
 from opendbc.car.fingerprints import MIGRATION
-from opendbc.car.honda.values import CAR as HONDA, HondaFlags
+from opendbc.car.honda.values import HondaFlags
 from opendbc.car.structs import car
 from opendbc.car.tests.routes import non_tested_cars, routes, CarTestRoute
 from opendbc.car.values import Platform, PLATFORMS
@@ -28,6 +28,14 @@ from openpilot.tools.lib.route import SegmentName
 SafetyModel = car.CarParams.SafetyModel
 SteerControlType = structs.CarParams.SteerControlType
 
+# panda safety stores angle_meas in brand-specific CAN units (angle_deg_to_can in opendbc/safety/modes/*.h).
+ANGLE_DEG_TO_CAN = {
+  "tesla": -10,
+  "toyota": 17.452007,
+  "nissan": 100,
+  "psa": 10,
+}
+
 NUM_JOBS = int(os.environ.get("NUM_JOBS", "1"))
 JOB_ID = int(os.environ.get("JOB_ID", "0"))
 INTERNAL_SEG_LIST = os.environ.get("INTERNAL_SEG_LIST", "")
@@ -350,13 +358,7 @@ class TestCarModelBase(unittest.TestCase):
         self.assertEqual(CS.gasPressed, self.safety.get_gas_pressed_prev())
 
       if self.safety.get_brake_pressed_prev() != prev_panda_brake:
-        # TODO: remove this exception once this mismatch is resolved
-        brake_pressed = CS.brakePressed
-        if CS.brakePressed and not self.safety.get_brake_pressed_prev():
-          if self.CP.carFingerprint in (HONDA.HONDA_PILOT, HONDA.HONDA_RIDGELINE) and CS.brake > 0.05:
-            brake_pressed = False
-
-        self.assertEqual(brake_pressed, self.safety.get_brake_pressed_prev())
+        self.assertEqual(CS.brakePressed, self.safety.get_brake_pressed_prev())
 
       if self.safety.get_regen_braking_prev() != prev_panda_regen_braking:
         self.assertEqual(CS.regenBraking, self.safety.get_regen_braking_prev())
@@ -433,12 +435,14 @@ class TestCarModelBase(unittest.TestCase):
         checks['vEgoRaw'] += (v_ego_raw > (self.safety.get_vehicle_speed_max() + 1e-3) or
                               v_ego_raw < (self.safety.get_vehicle_speed_min() - 1e-3))
 
-      # TODO: remove this exception once this mismatch is resolved
-      brake_pressed = CS.brakePressed
-      if CS.brakePressed and not self.safety.get_brake_pressed_prev():
-        if self.CP.carFingerprint in (HONDA.HONDA_PILOT, HONDA.HONDA_RIDGELINE) and CS.brake > 0.05:
-          brake_pressed = False
-      checks['brakePressed'] += brake_pressed != self.safety.get_brake_pressed_prev()
+      # check steering angle for angle control cars (panda stores angle_meas in CAN units)
+      # ford excluded since it tracks curvature, not steering angle
+      if self.CP.steerControlType == SteerControlType.angle and not self.CP.notCar and self.CP.brand != "ford":
+        angle_can = (CS.steeringAngleDeg + CS.steeringAngleOffsetDeg) * ANGLE_DEG_TO_CAN[self.CP.brand]
+        checks['steeringAngleDeg'] += (angle_can > (self.safety.get_angle_meas_max() + 1) or
+                                       angle_can < (self.safety.get_angle_meas_min() - 1))
+
+      checks['brakePressed'] += CS.brakePressed != self.safety.get_brake_pressed_prev()
       checks['regenBraking'] += CS.regenBraking != self.safety.get_regen_braking_prev()
       checks['steeringDisengage'] += CS.steeringDisengage != self.safety.get_steering_disengage_prev()
 

selfdrive/controls/controlsd.py

@@ -212,7 +212,7 @@ class Controls(ControlsExt):
     cs.upAccelCmd = float(self.LoC.pid.p)
     cs.uiAccelCmd = float(self.LoC.pid.i)
     cs.ufAccelCmd = float(self.LoC.pid.f)
-    cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or
+    cs.forceDecel = bool((self.sm['driverMonitoringState'].alertLevel == log.DriverMonitoringState.AlertLevel.three) or
                          (self.sm['selfdriveState'].state == State.softDisabling))
 
     lat_tuning = self.CP.lateralTuning.which()

selfdrive/controls/lib/drive_helpers.py

@@ -8,6 +8,7 @@ CAR_ROTATION_RADIUS = 0.0
 # This is a turn radius smaller than most cars can achieve
 MAX_CURVATURE = 0.2
 MAX_VEL_ERR = 5.0  # m/s
+MIN_STABLE_DELAY = 0.3
 
 # EU guidelines
 MAX_LATERAL_JERK = 5.0  # m/s^3
@@ -43,6 +44,9 @@ def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.
   if len(speeds) == len(t_idxs):
     v_now = speeds[0]
     a_now = accels[0]
+    if action_t < MIN_STABLE_DELAY:
+      v_target = v_now + (action_t / MIN_STABLE_DELAY) * (np.interp(MIN_STABLE_DELAY, t_idxs, speeds) - v_now)
+    else:
       v_target = np.interp(action_t, t_idxs, speeds)
     a_target = 2 * (v_target - v_now) / (action_t) - a_now
   else:
@@ -58,6 +62,9 @@ def curv_from_psis(psi_target, psi_rate, vego, action_t):
   return 2*curv_from_psi - psi_rate / vego
 
 def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t):
+  if action_t < MIN_STABLE_DELAY:
+    psi_target = (action_t / MIN_STABLE_DELAY) * np.interp(MIN_STABLE_DELAY, t_idxs, yaws)
+  else:
     psi_target = np.interp(action_t, t_idxs, yaws)
   psi_rate = yaw_rates[0]
   return curv_from_psis(psi_target, psi_rate, vego, action_t)

selfdrive/controls/lib/lateral_mpc_lib/SConscript

@@ -1,4 +1,4 @@
-Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'np_version')
+Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'np_version', 'acados')
 
 gen = "c_generated_code"
 
@@ -45,18 +45,24 @@ generated_files = [
   f'{gen}/lat_cost/lat_cost.h',
 ] + build_files
 
-acados_dir = '#third_party/acados'
-acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera'
+acados_include_dir = Dir(acados.INCLUDE_DIR)
+acados_template_dir = Dir(acados.TEMPLATE_DIR)
 
 source_list = ['lat_mpc.py',
   '#selfdrive/modeld/constants.py',
-  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
-  f'{acados_templates_dir}/acados_solver.in.c',
+  acados_include_dir.File('acados_c/ocp_nlp_interface.h'),
+  acados_template_dir.File('c_templates_tera/acados_solver.in.c'),
 ]
 
 lenv = env.Clone()
-acados_rel_path = Dir(gen).rel_path(Dir(f"#third_party/acados/{arch}/lib"))
-lenv["RPATH"] += [lenv.Literal(f'\\$$ORIGIN/{acados_rel_path}')]
+copied_acados_libs = []
+if arch != "Darwin":
+  for lib in ["libacados.so", "libblasfeo.so", "libhpipm.so", "libqpOASES_e.so.3.1"]:
+    copied_acados_libs += lenv.Command(f"{gen}/{lib}", Dir(acados.LIB_DIR).File(lib), [Mkdir(Dir(gen)), Copy("$TARGET", "$SOURCE")])
+  lenv["RPATH"] += [lenv.Literal('\\$$ORIGIN')]
+else:
+  acados_rel_path = Dir(gen).rel_path(Dir(acados.LIB_DIR))
+  lenv["RPATH"] += [lenv.Literal(f'\\$$ORIGIN/{acados_rel_path}')]
 lenv.Clean(generated_files, Dir(gen))
 
 generated_lat = lenv.Command(generated_files,
@@ -77,8 +83,8 @@ lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat",
                                 LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
 
 # generate cython stuff
-acados_ocp_solver_pyx = File("#third_party/acados/acados_template/acados_ocp_solver_pyx.pyx")
-acados_ocp_solver_common = File("#third_party/acados/acados_template/acados_solver_common.pxd")
+acados_ocp_solver_pyx = acados_template_dir.File('acados_ocp_solver_pyx.pyx')
+acados_ocp_solver_common = acados_template_dir.File('acados_solver_common.pxd')
 libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd')
 libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c')
 
@@ -94,4 +100,5 @@ lenv2.Command(libacados_ocp_solver_c,
   f' {acados_ocp_solver_pyx.get_labspath()}')
 lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c], LIBS=['acados_ocp_solver_lat'])
 lenv2.Depends(lib_cython, lib_solver)
+lenv2.Depends(lib_cython, copied_acados_libs)
 lenv2.Depends(libacados_ocp_solver_c, np_version)

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -8,7 +8,7 @@ from casadi import SX, vertcat, sin, cos
 from openpilot.selfdrive.modeld.constants import ModelConstants
 
 if __name__ == '__main__':  # generating code
-  from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+  from acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
   from openpilot.selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
 

selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -1,4 +1,4 @@
-Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'np_version')
+Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'np_version', 'acados')
 
 gen = "c_generated_code"
 
@@ -51,18 +51,24 @@ generated_files = [
   f'{gen}/long_cost/long_cost.h',
 ] + build_files
 
-acados_dir = '#third_party/acados'
-acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera'
+acados_include_dir = Dir(acados.INCLUDE_DIR)
+acados_template_dir = Dir(acados.TEMPLATE_DIR)
 
 source_list = ['long_mpc.py',
   '#selfdrive/modeld/constants.py',
-  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
-  f'{acados_templates_dir}/acados_solver.in.c',
+  acados_include_dir.File('acados_c/ocp_nlp_interface.h'),
+  acados_template_dir.File('c_templates_tera/acados_solver.in.c'),
 ]
 
 lenv = env.Clone()
-acados_rel_path = Dir(gen).rel_path(Dir(f"#third_party/acados/{arch}/lib"))
-lenv["RPATH"] += [lenv.Literal(f'\\$$ORIGIN/{acados_rel_path}')]
+copied_acados_libs = []
+if arch != "Darwin":
+  for lib in ["libacados.so", "libblasfeo.so", "libhpipm.so", "libqpOASES_e.so.3.1"]:
+    copied_acados_libs += lenv.Command(f"{gen}/{lib}", Dir(acados.LIB_DIR).File(lib), [Mkdir(Dir(gen)), Copy("$TARGET", "$SOURCE")])
+  lenv["RPATH"] += [lenv.Literal('\\$$ORIGIN')]
+else:
+  acados_rel_path = Dir(gen).rel_path(Dir(acados.LIB_DIR))
+  lenv["RPATH"] += [lenv.Literal(f'\\$$ORIGIN/{acados_rel_path}')]
 lenv.Clean(generated_files, Dir(gen))
 generated_long = lenv.Command(generated_files,
                               source_list,
@@ -82,8 +88,8 @@ lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_long",
                                 LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
 
 # generate cython stuff
-acados_ocp_solver_pyx = File("#third_party/acados/acados_template/acados_ocp_solver_pyx.pyx")
-acados_ocp_solver_common = File("#third_party/acados/acados_template/acados_solver_common.pxd")
+acados_ocp_solver_pyx = acados_template_dir.File('acados_ocp_solver_pyx.pyx')
+acados_ocp_solver_common = acados_template_dir.File('acados_solver_common.pxd')
 libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd')
 libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c')
 
@@ -99,4 +105,5 @@ lenv2.Command(libacados_ocp_solver_c,
   f' {acados_ocp_solver_pyx.get_labspath()}')
 lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c], LIBS=['acados_ocp_solver_long'])
 lenv2.Depends(lib_cython, lib_solver)
+lenv2.Depends(lib_cython, copied_acados_libs)
 lenv2.Depends(libacados_ocp_solver_c, np_version)

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -11,7 +11,7 @@ from openpilot.selfdrive.modeld.constants import index_function
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
 if __name__ == '__main__':  # generating code
-  from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+  from acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
   from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
 

selfdrive/controls/radard.py

@@ -142,7 +142,7 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks
     return None
 
 
-def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float):
+def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float, lead_prob: float):
   lead_v_rel_pred = lead_msg.v[0] - model_v_ego
   return {
     "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
@@ -153,7 +153,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
     "aLeadK": float(lead_msg.a[0]),
     "aLeadTau": 0.3,
     "fcw": False,
-    "modelProb": float(lead_msg.prob),
+    "modelProb": float(lead_prob),
     "status": True,
     "radar": False,
     "radarTrackId": -1,
@@ -161,19 +161,20 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
 
 
 def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader,
-             model_v_ego: float, CP: structs.CarParams, CP_SP: structs.CarParamsSP, low_speed_override: bool = True) -> dict[str, Any]:
+             model_v_ego: float, lead_prob: float, CP: structs.CarParams, CP_SP: structs.CarParamsSP,
+             low_speed_override: bool = True) -> dict[str, Any]:
   # Determine leads, this is where the essential logic happens
-  if len(tracks) > 0 and ready and lead_msg.prob > .5:
+  if len(tracks) > 0 and ready and lead_prob > .5:
     track = match_vision_to_track(v_ego, lead_msg, tracks)
   else:
     track = None
 
   lead_dict = {'status': False}
   if track is not None:
-    lead_dict = track.get_RadarState(lead_msg.prob)
+    lead_dict = track.get_RadarState(lead_prob)
     lead_dict = get_custom_yrel(CP, CP_SP, lead_dict, lead_msg)
-  elif (track is None) and ready and (lead_msg.prob > .5):
-    lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
+  elif (track is None) and ready and (lead_prob > .5):
+    lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego, lead_prob)
 
   if low_speed_override:
     low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)]
@@ -205,6 +206,7 @@ class RadarD:
 
     self.tracks: dict[int, Track] = {}
     self.kalman_params = KalmanParams(DT_MDL)
+    self.lead_prob_filters = [FirstOrderFilter(0.0, 0.2, DT_MDL) for _ in range(2)]
 
     self.v_ego = 0.0
     self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1)
@@ -256,8 +258,18 @@ class RadarD:
       model_v_ego = self.v_ego
     leads_v3 = sm['modelV2'].leadsV3
     if len(leads_v3) > 1:
-      self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.CP, self.CP_SP, low_speed_override=True)
-      self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.CP, self.CP_SP, low_speed_override=False)
+      for i in range(2):
+        # Asymmetric filter on lead prob to keep lead when uncertain
+        lead_prob = leads_v3[i].prob
+        if lead_prob > self.lead_prob_filters[i].x:
+          self.lead_prob_filters[i].x = lead_prob
+        else:
+          self.lead_prob_filters[i].update(lead_prob)
+
+      self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.lead_prob_filters[0].x,
+                                          self.CP, self.CP_SP, low_speed_override=True)
+      self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.lead_prob_filters[1].x,
+                                          self.CP, self.CP_SP, low_speed_override=False)
 
   def publish(self, pm: messaging.PubMaster):
     assert self.radar_state is not None

selfdrive/debug/car/ecu_addrs.py

@@ -26,9 +26,9 @@ if __name__ == "__main__":
   # Set up params for pandad
   params = Params()
   params.remove("FirmwareQueryDone")
-  params.put_bool("IsOnroad", False)
+  params.put_bool("IsOnroad", False, block=True)
   time.sleep(0.2)  # thread is 10 Hz
-  params.put_bool("IsOnroad", True)
+  params.put_bool("IsOnroad", True, block=True)
 
   obd_callback(params)(not args.no_obd)
 

selfdrive/debug/car/fw_versions.py

@@ -30,9 +30,9 @@ if __name__ == "__main__":
   # Set up params for pandad
   params = Params()
   params.remove("FirmwareQueryDone")
-  params.put_bool("IsOnroad", False)
+  params.put_bool("IsOnroad", False, block=True)
   time.sleep(0.2)  # thread is 10 Hz
-  params.put_bool("IsOnroad", True)
+  params.put_bool("IsOnroad", True, block=True)
   set_obd_multiplexing = obd_callback(params)
 
   extra: Any = None

selfdrive/debug/set_car_params.py

@@ -19,4 +19,4 @@ if __name__ == "__main__":
 
   cp_bytes = CP.to_bytes()
   for p in ("CarParams", "CarParamsCache", "CarParamsPersistent"):
-    Params().put(p, cp_bytes)
+    Params().put(p, cp_bytes, block=True)

selfdrive/debug/uiview.py

@@ -9,7 +9,7 @@ from openpilot.system.hardware import HARDWARE
 if __name__ == "__main__":
   CP = car.CarParams(notCar=True, wheelbase=1, steerRatio=10)
   params = Params()
-  params.put("CarParams", CP.to_bytes())
+  params.put("CarParams", CP.to_bytes(), block=True)
 
   if use_tinygrad_modeld := is_tinygrad_model(False, params, CP):
     print("Using TinyGrad modeld")

selfdrive/locationd/calibrationd.py

@@ -167,7 +167,7 @@ class Calibrator:
 
     write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
     if self.param_put and write_this_cycle:
-      self.params.put_nonblocking("CalibrationParams", self.get_msg(True).to_bytes())
+      self.params.put("CalibrationParams", self.get_msg(True).to_bytes())
 
   def handle_v_ego(self, v_ego: float) -> None:
     self.v_ego = v_ego

selfdrive/locationd/lagd.py

@@ -414,7 +414,7 @@ def main():
       pm.send('liveDelay', lag_msg_dat)
 
       if sm.frame % 1200 == 0: # cache every 60 seconds
-        params.put_nonblocking("LiveDelay", lag_msg_dat)
+        params.put("LiveDelay", lag_msg_dat)
 
       if sm.frame % 60 == 0:  # read from and write to params every 3 seconds
         lagd_toggle.update(lag_msg)

selfdrive/locationd/paramsd.py

@@ -212,7 +212,7 @@ def migrate_cached_vehicle_params_if_needed(params: Params):
     last_parameters_msg.liveParameters.steerRatio = last_parameters_data_old['steerRatio']
     last_parameters_msg.liveParameters.stiffnessFactor = last_parameters_data_old['stiffnessFactor']
     last_parameters_msg.liveParameters.angleOffsetAverageDeg = last_parameters_data_old['angleOffsetAverageDeg']
-    params.put("LiveParametersV2", last_parameters_msg.to_bytes())
+    params.put("LiveParametersV2", last_parameters_msg.to_bytes(), block=True)
   except Exception as e:
     cloudlog.error(f"Failed to perform parameter migration: {e}")
     params.remove("LiveParameters")
@@ -290,7 +290,7 @@ def main():
 
       msg_dat = msg.to_bytes()
       if sm.frame % 1200 == 0:  # once a minute
-        params.put_nonblocking("LiveParametersV2", msg_dat)
+        params.put("LiveParametersV2", msg_dat)
 
       pm.send('liveParameters', msg_dat)
 

selfdrive/locationd/test/test_calibrationd.py

@@ -36,7 +36,7 @@ class TestCalibrationd:
     msg.liveCalibration.validBlocks = random.randint(1, 10)
     msg.liveCalibration.rpyCalib = [random.random() for _ in range(3)]
     msg.liveCalibration.height = [random.random() for _ in range(1)]
-    Params().put("CalibrationParams", msg.to_bytes())
+    Params().put("CalibrationParams", msg.to_bytes(), block=True)
     c = Calibrator(param_put=True)
 
     np.testing.assert_allclose(msg.liveCalibration.rpyCalib, c.rpy)

selfdrive/locationd/test/test_lagd.py

@@ -53,8 +53,8 @@ class TestLagd:
     msg = messaging.new_message('liveDelay')
     msg.liveDelay.lateralDelayEstimate = random.random()
     msg.liveDelay.validBlocks = random.randint(1, 10)
-    params.put("LiveDelay", msg.to_bytes())
-    params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
+    params.put("LiveDelay", msg.to_bytes(), block=True)
+    params.put("CarParamsPrevRoute", CP.as_builder().to_bytes(), block=True)
 
     saved_lag_params = retrieve_initial_lag(params, CP)
     assert saved_lag_params is not None

selfdrive/locationd/test/test_paramsd.py

@@ -27,8 +27,8 @@ class TestParamsd:
     CP = next(m for m in lr if m.which() == "carParams").carParams
 
     msg = get_random_live_parameters(CP)
-    params.put("LiveParametersV2", msg.to_bytes())
-    params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
+    params.put("LiveParametersV2", msg.to_bytes(), block=True)
+    params.put("CarParamsPrevRoute", CP.as_builder().to_bytes(), block=True)
 
     migrate_cached_vehicle_params_if_needed(params) # this is not tested here but should not mess anything up or throw an error
     sr, sf, offset, p_init = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
@@ -46,8 +46,8 @@ class TestParamsd:
     CP = next(m for m in lr if m.which() == "carParams").carParams
 
     msg = get_random_live_parameters(CP)
-    params.put("LiveParameters", msg.liveParameters.to_dict())
-    params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
+    params.put("LiveParameters", msg.liveParameters.to_dict(), block=True)
+    params.put("CarParamsPrevRoute", CP.as_builder().to_bytes(), block=True)
     params.remove("LiveParametersV2")
 
     migrate_cached_vehicle_params_if_needed(params)
@@ -59,7 +59,7 @@ class TestParamsd:
 
   def test_read_saved_params_corrupted_old_format(self):
     params = Params()
-    params.put("LiveParameters", {})
+    params.put("LiveParameters", {}, block=True)
     params.remove("LiveParametersV2")
 
     migrate_cached_vehicle_params_if_needed(params)

selfdrive/locationd/torqued.py

@@ -278,7 +278,7 @@ def main(demo=False):
     # Cache points every 60 seconds while onroad
     if sm.frame % 240 == 0:
       msg = estimator.get_msg(valid=sm.all_checks(), with_points=True)
-      params.put_nonblocking("LiveTorqueParameters", msg.to_bytes())
+      params.put("LiveTorqueParameters", msg.to_bytes())
 
 
 if __name__ == "__main__":

selfdrive/modeld/SConscript

@@ -1,9 +1,19 @@
 import glob
 import json
 import os
-from SCons.Script import Value
-from openpilot.common.file_chunker import chunk_file, get_chunk_paths
-from tinygrad import Device
+import sys, subprocess
+from SCons.Script import Action, Value
+from openpilot.common.file_chunker import chunk_file, get_chunk_targets, get_existing_chunks
+from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
+from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE, DM_INPUT_SIZE
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.modeld.helpers import TG_INPUT_DEVICES_PATH, usbgpu_present, modeld_pkl_path
+
+
+CAMERA_CONFIGS = [
+  (_ar_ox_fisheye.width, _ar_ox_fisheye.height),  # tici: 1928x1208
+  (_os_fisheye.width, _os_fisheye.height),        # mici: 1344x760
+]
 
 Import('env', 'arch')
 chunker_file = File("#common/file_chunker.py")
@@ -16,73 +26,116 @@ tinygrad_files = ["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + "
 def estimate_pickle_max_size(onnx_size):
   return 1.2 * onnx_size  + 10 * 1024 * 1024  # 20% + 10MB is plenty
 
-# THREADS=0 is need to prevent bug: https://github.com/tinygrad/tinygrad/issues/14689
 # get fastest TG config
-available = set(Device.get_available_devices())
-# FIXME-SP: reset when we bump tg
-if False:  # 'CUDA' in available:
+# probe in subprocess so usbgpu locks gets released on process exit
+def probe_devices():
+  return set(subprocess.run(
+    [sys.executable, '-c', 'from tinygrad import Device\nprint("\\n".join(Device.get_available_devices()))'],
+    capture_output=True, text=True, check=True).stdout.strip().splitlines())
+
+available = probe_devices()
+if 'CUDA' in available:
   tg_backend = 'CUDA'
   tg_flags = f'DEV={tg_backend}'
 elif 'QCOM' in available:
   tg_backend = 'QCOM'
-  tg_flags = f'DEV={tg_backend} FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0'
+  tg_flags = f'DEV={tg_backend} IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1'
 else:
-  tg_backend = 'CPU' if arch == 'Darwin' else 'CPU CPU_LLVM=1'  # FIXME-SP: reset when we bump tg
-  tg_flags = f'DEV={tg_backend} THREADS=0'
+  tg_backend = 'CPU'
+  tg_flags = f'DEV=CPU' if arch == 'Darwin' else 'DEV=CPU:LLVM'
 
-def write_tg_compiled_flags(target, source, env):
+tg_devices = { # which device to put jit inputs to at runtime
+  'selfdrive.modeld.modeld': {
+    'default': {'WARP_DEV': tg_backend, 'QUEUE_DEV': tg_backend},
+    'usbgpu': {'WARP_DEV': tg_backend, 'QUEUE_DEV': 'AMD'}
+  },
+  'selfdrive.modeld.dmonitoringmodeld': {
+    'default': {'DEV': tg_backend}
+  },
+}
+
+USBGPU = usbgpu_present() # or release # TODO always build big model on release
+if USBGPU:
+  usbgpu_tg_flags = f'DEBUG=2 DEV=USB+AMD:LLVM WARP_DEV={tg_backend} FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0'
+  # the USB+AMD GPU takes an exclusive flock; serialize all targets that touch it
+  usbgpu_lock = File("models/.usb_gpu.lock").abspath
+
+def write_tg_devices(target, source, env):
   with open(str(target[0]), "w") as f:
-    json.dump({"DEV": tg_backend}, f)
+    json.dump(tg_devices, f)
     f.write("\n")
 
-compiled_flags_node = lenv.Command(
-  File("models/tg_compiled_flags.json").abspath,
-  tinygrad_files + [Value(tg_backend)],
-  write_tg_compiled_flags,
+tg_devices_node = lenv.Command(
+  str(TG_INPUT_DEVICES_PATH),
+  [Value(tg_devices)],
+  write_tg_devices,
 )
 
 # tinygrad calls brew which needs a $HOME in the env
 mac_brew_string = f'HOME={os.path.expanduser("~")}' if arch == 'Darwin' else ''
 
-# Get model metadata
-for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']:
-  fn = File(f"models/{model_name}").abspath
-  script_files = [File(Dir("#selfdrive/modeld").File("get_model_metadata.py").abspath)]
-  cmd = f'{tg_flags} {mac_brew_string} python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx'
-  lenv.Command(fn + "_metadata.pkl", [fn + ".onnx"] + tinygrad_files + script_files + [compiled_flags_node], cmd)
+modeld_dir = Dir("#selfdrive/modeld").abspath
+compile_modeld_script = [
+  File(f"{modeld_dir}/compile_modeld.py"),
+  File(f"{modeld_dir}/get_model_metadata.py"),
+  File("#system/camerad/cameras/nv12_info.py"),
+  File("#system/hardware/hw.py"),
+]
+model_w, model_h = MEDMODEL_INPUT_SIZE
+frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
 
-image_flag = {
-     'larch64': 'IMAGE=2',
-}.get(arch, 'IMAGE=0')
-script_files = [File(Dir("#selfdrive/modeld").File("compile_warp.py").abspath)]
-compile_warp_cmd = f'{tg_flags} {mac_brew_string} python3 {Dir("#selfdrive/modeld").abspath}/compile_warp.py '
-from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
-warp_targets = []
-for cam in [_ar_ox_fisheye, _os_fisheye]:
-  w, h = cam.width, cam.height
-  warp_targets += [File(f"models/warp_{w}x{h}_tinygrad.pkl").abspath, File(f"models/dm_warp_{w}x{h}_tinygrad.pkl").abspath]
-lenv.Command(warp_targets, tinygrad_files + script_files + [compiled_flags_node], compile_warp_cmd)
+for usbgpu in [False, True] if USBGPU else [False]:
+  target_pkl_path = File(modeld_pkl_path(usbgpu)).abspath
+  file_prefix, cmd_flags = ('big_', usbgpu_tg_flags) if usbgpu else ('', tg_flags)
+  driving_onnx_deps = [p for m in [f'{file_prefix}driving_vision', f'{file_prefix}driving_on_policy']
+                       for p in get_existing_chunks(File(f"models/{m}.onnx").abspath)]
+  camera_res_args = ' '.join(f'{cw}x{ch}' for cw, ch in CAMERA_CONFIGS)
+  cmd = (f'{cmd_flags} {mac_brew_string} python3 {modeld_dir}/compile_modeld.py '
+        f'--model-size {model_w}x{model_h} '
+        f'--camera-resolutions {camera_res_args} '
+        f'--vision-onnx {File(f"models/{file_prefix}driving_vision.onnx").abspath} '
+        f'--on-policy-onnx {File(f"models/{file_prefix}driving_on_policy.onnx").abspath} '
+        f'--output {target_pkl_path} --frame-skip {frame_skip}')
+  onnx_sizes_sum = sum(os.path.getsize(f) for f in driving_onnx_deps)
+  chunk_targets = get_chunk_targets(target_pkl_path, estimate_pickle_max_size(onnx_sizes_sum))
+  def do_chunk(target, source, env, pkl=target_pkl_path, chunks=chunk_targets):
+    chunk_file(pkl, chunks)
+  node = lenv.Command(
+    chunk_targets,
+    tinygrad_files + compile_modeld_script + driving_onnx_deps + [Value(chunk_targets), chunker_file],
+    [cmd, Action(do_chunk, "  [CHUNK] $TARGET")],
+  )
+  if usbgpu:
+    lenv.SideEffect(usbgpu_lock, node)
+
+# get model metadata
+fn = File(f"models/dmonitoring_model").abspath
+script_files = [File(Dir("#selfdrive/modeld").File("get_model_metadata.py").abspath)]
+cmd = f'{tg_flags} {mac_brew_string} python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx'
+lenv.Command(fn + "_metadata.pkl", [fn + ".onnx"] + tinygrad_files + script_files + [tg_devices_node], cmd)
+
+dm_w, dm_h = DM_INPUT_SIZE
+compile_dm_warp_script = [File(f"{modeld_dir}/compile_dm_warp.py")]
+for cam_w, cam_h in CAMERA_CONFIGS:
+  dm_pkl_path = File(f"models/dm_warp_{cam_w}x{cam_h}_tinygrad.pkl").abspath
+  cmd = (f'{tg_flags} {mac_brew_string} python3 {modeld_dir}/compile_dm_warp.py '
+         f'--camera-resolution {cam_w}x{cam_h} --warp-to {dm_w}x{dm_h} '
+         f'--output {dm_pkl_path}')
+  lenv.Command(dm_pkl_path, tinygrad_files + compile_dm_warp_script + compile_modeld_script + [tg_devices_node], cmd)
 
 def tg_compile(flags, model_name):
   pythonpath_string = 'PYTHONPATH="${PYTHONPATH}:' + env.Dir("#tinygrad_repo").abspath + '"'
   fn = File(f"models/{model_name}").abspath
   pkl = fn + "_tinygrad.pkl"
   onnx_path = fn + ".onnx"
-  chunk_targets = get_chunk_paths(pkl, estimate_pickle_max_size(os.path.getsize(onnx_path)))
-  compile_node = lenv.Command(
-    pkl,
-    [onnx_path] + tinygrad_files + [chunker_file, compiled_flags_node],
-    f'{pythonpath_string} {flags} {image_flag} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {pkl}',
-  )
+  chunk_targets = get_chunk_targets(pkl, estimate_pickle_max_size(os.path.getsize(onnx_path)))
   def do_chunk(target, source, env):
     chunk_file(pkl, chunk_targets)
   return lenv.Command(
     chunk_targets,
-    compile_node,
-    do_chunk,
+    [onnx_path] + tinygrad_files + [Value(chunk_targets), chunker_file, tg_devices_node],
+    [f'{pythonpath_string} {flags} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {pkl}',
+     Action(do_chunk, "  [CHUNK] $TARGET")],
   )
 
-# Compile small models
-for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']:
-  tg_compile(tg_flags, model_name)
-
+tg_compile(tg_flags, 'dmonitoring_model')

selfdrive/modeld/compile_dm_warp.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python3
+import argparse
+import pickle
+import time
+
+from tinygrad.tensor import Tensor
+from tinygrad.device import Device
+from tinygrad.engine.jit import TinyJit
+
+from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
+from openpilot.selfdrive.modeld.compile_modeld import NV12Frame, warp_perspective_tinygrad, _parse_size
+
+
+def make_warp_dm(nv12: NV12Frame, dm_w, dm_h):
+  cam_w, cam_h, stride, _, _, _ = nv12
+  stride_pad = stride - cam_w
+
+  def warp_dm(input_frame, M_inv):
+    M_inv = M_inv.to(Device.DEFAULT).realize()
+    return warp_perspective_tinygrad(input_frame[:cam_h*stride], M_inv,
+                                     (dm_w, dm_h), (cam_h, cam_w), stride_pad, border_fill_val=16).reshape(-1, dm_h * dm_w) # Y
+  return warp_dm
+
+
+def compile_dm_warp(nv12: NV12Frame, dm_w, dm_h, pkl_path):
+  print(f"Compiling DM warp for {nv12.width}x{nv12.height} -> {dm_w}x{dm_h}...")
+
+  warp_dm_jit = TinyJit(make_warp_dm(nv12, dm_w, dm_h), prune=True)
+
+  for i in range(10):
+    frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
+    M_inv = Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')
+    Device.default.synchronize()
+    st = time.perf_counter()
+    warp_dm_jit(frame, M_inv).realize()
+    mt = time.perf_counter()
+    Device.default.synchronize()
+    et = time.perf_counter()
+    print(f"  [{i+1}/10] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
+
+  with open(pkl_path, "wb") as f:
+    pickle.dump(warp_dm_jit, f)
+  print(f"  Saved to {pkl_path}")
+
+
+if __name__ == "__main__":
+  p = argparse.ArgumentParser()
+  p.add_argument('--camera-resolution', type=_parse_size, required=True, help='camera resolution WxH')
+  p.add_argument('--warp-to', type=_parse_size, required=True, help='DM input WxH')
+  p.add_argument('--output', required=True)
+  args = p.parse_args()
+
+  cam_w, cam_h = args.camera_resolution
+  nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
+  dm_w, dm_h = args.warp_to
+  compile_dm_warp(nv12, dm_w, dm_h, args.output)

selfdrive/modeld/compile_modeld.py

@@ -0,0 +1,299 @@
+#!/usr/bin/env python3
+import argparse
+import atexit
+import os
+import pickle
+import time
+from functools import partial
+from collections import namedtuple, defaultdict
+
+import numpy as np
+
+def _patch_tinygrad_fetch_fw():
+  import hashlib
+  import pathlib
+  import zstandard
+  from tinygrad import helpers
+  _orig = helpers.fetch_fw
+  def fetch_fw(path, name, sha256):
+    p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
+    if p.is_file():
+      blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read()
+      if hashlib.sha256(blob).hexdigest() == sha256:
+        return blob
+    return _orig(path, name, sha256)
+  helpers.fetch_fw = fetch_fw
+_patch_tinygrad_fetch_fw()
+
+from tinygrad.tensor import Tensor
+from tinygrad.helpers import Context
+from tinygrad.device import Device
+from tinygrad.engine.jit import TinyJit
+
+
+NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size'])
+WARP_INPUTS = ['img_q', 'big_img_q', 'tfm', 'big_tfm']
+POLICY_INPUTS = ['feat_q', 'desire_q', 'desire', 'traffic_convention', 'action_t']
+
+UV_SCALE_MATRIX = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 1]], dtype=np.float32)
+UV_SCALE_MATRIX_INV = np.linalg.inv(UV_SCALE_MATRIX)
+
+WARP_DEV = os.getenv('WARP_DEV')
+
+
+def make_random_images(keys, shape, device=None):
+  return {k: Tensor.randint(shape, low=0, high=256, dtype='uint8', device=device).realize() for k in keys}
+
+
+def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad, border_fill_val=None):
+  w_dst, h_dst = dst_shape
+  h_src, w_src = src_shape
+
+  x = Tensor.arange(w_dst, device=WARP_DEV).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1)
+  y = Tensor.arange(h_dst, device=WARP_DEV).reshape(h_dst, 1).expand(h_dst, w_dst).reshape(-1)
+
+  # inline 3x3 matmul as elementwise to avoid reduce op (enables fusion with gather)
+  src_x = M_inv[0, 0] * x + M_inv[0, 1] * y + M_inv[0, 2]
+  src_y = M_inv[1, 0] * x + M_inv[1, 1] * y + M_inv[1, 2]
+  src_w = M_inv[2, 0] * x + M_inv[2, 1] * y + M_inv[2, 2]
+
+  src_x = src_x / src_w
+  src_y = src_y / src_w
+
+  x_round = Tensor.round(src_x)
+  y_round = Tensor.round(src_y)
+  x_nn_clipped = x_round.clip(0, w_src - 1).cast('int')
+  y_nn_clipped = y_round.clip(0, h_src - 1).cast('int')
+  idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped
+  sampled = src_flat[idx]
+
+  if border_fill_val is None:
+    return sampled
+
+  in_bounds = ((x_round >= 0) & (x_round <= w_src - 1) &
+               (y_round >= 0) & (y_round <= h_src - 1)).cast(sampled.dtype)
+  return sampled * in_bounds + Tensor(border_fill_val, dtype=sampled.dtype) * (1 - in_bounds)
+
+
+def frames_to_tensor(frames):
+  H = (frames.shape[0] * 2) // 3
+  W = frames.shape[1]
+  in_img1 = Tensor.cat(frames[0:H:2, 0::2],
+                       frames[1:H:2, 0::2],
+                       frames[0:H:2, 1::2],
+                       frames[1:H:2, 1::2],
+                       frames[H:H+H//4].reshape((H//2, W//2)),
+                       frames[H+H//4:H+H//2].reshape((H//2, W//2)), dim=0).reshape((6, H//2, W//2))
+  return in_img1
+
+
+def make_frame_prepare(nv12: NV12Frame, model_w, model_h):
+  cam_w, cam_h, stride, y_height, uv_height, _ = nv12
+  uv_offset = stride * y_height
+  stride_pad = stride - cam_w
+
+  def frame_prepare_tinygrad(input_frame, M_inv):
+    # UV_SCALE @ M_inv @ UV_SCALE_INV simplifies to elementwise scaling
+    M_inv_uv = M_inv * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=WARP_DEV)
+    # deinterleave NV12 UV plane (UVUV... -> separate U, V)
+    uv = input_frame[uv_offset:uv_offset + uv_height * stride].reshape(uv_height, stride)
+    with Context(SPLIT_REDUCEOP=0):
+      y = warp_perspective_tinygrad(input_frame[:cam_h*stride],
+                                    M_inv, (model_w, model_h),
+                                    (cam_h, cam_w), stride_pad).realize()
+      u = warp_perspective_tinygrad(uv[:cam_h//2, :cam_w:2].flatten(),
+                                    M_inv_uv, (model_w//2, model_h//2),
+                                    (cam_h//2, cam_w//2), 0).realize()
+      v = warp_perspective_tinygrad(uv[:cam_h//2, 1:cam_w:2].flatten(),
+                                    M_inv_uv, (model_w//2, model_h//2),
+                                    (cam_h//2, cam_w//2), 0).realize()
+    yuv = y.cat(u).cat(v).reshape((model_h * 3 // 2, model_w))
+    tensor = frames_to_tensor(yuv)
+    return tensor
+  return frame_prepare_tinygrad
+
+
+def make_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device):
+  img = vision_input_shapes['img']  # (1, 12, 128, 256)
+  n_frames = img[1] // 6
+  img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
+
+  fb = policy_input_shapes['features_buffer']  # (1, 25, 512)
+  dp = policy_input_shapes['desire_pulse']  # (1, 25, 8)
+  tc = policy_input_shapes['traffic_convention']  # (1, 2)
+  #TODO action_t is hardcoded to match tc for future compatibility
+  at = tc
+
+  npy = {
+    'desire': np.zeros(dp[2], dtype=np.float32),
+    'traffic_convention': np.zeros(tc, dtype=np.float32),
+    'tfm': np.zeros((3, 3), dtype=np.float32),
+    'big_tfm': np.zeros((3, 3), dtype=np.float32),
+    'action_t': np.zeros(at, dtype=np.float32),
+  }
+  input_queues = {
+    'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+    'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+    'feat_q': Tensor(np.zeros((frame_skip * (fb[1] - 1) + 1, fb[0], fb[2]), dtype=np.float32), device=device).contiguous().realize(),
+    'desire_q': Tensor(np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32), device=device).contiguous().realize(),
+    **{k: Tensor(v, device='NPY').realize() for k, v in npy.items()},
+  }
+  return input_queues, npy
+
+
+def shift_and_sample(buf, new_val, sample_fn):
+  buf.assign(buf[1:].cat(new_val, dim=0).contiguous())
+  return sample_fn(buf)
+
+
+def sample_skip(buf, frame_skip):
+  return buf[::frame_skip].contiguous().flatten(0, 1).unsqueeze(0)
+
+
+def sample_desire(buf, frame_skip):
+  return buf.reshape(-1, frame_skip, *buf.shape[1:]).max(1).flatten(0, 1).unsqueeze(0)
+
+
+def make_warp(nv12, model_w, model_h, frame_skip):
+  frame_prepare = make_frame_prepare(nv12, model_w, model_h)
+  sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
+
+  def warp_enqueue(img_q, big_img_q, tfm, big_tfm, frame, big_frame):
+    tfm = tfm.to(WARP_DEV)
+    big_tfm = big_tfm.to(WARP_DEV)
+    Tensor.realize(tfm, big_tfm)
+
+    warped_frame = frame_prepare(frame, tfm).unsqueeze(0).to(Device.DEFAULT)
+    warped_big_frame = frame_prepare(big_frame, big_tfm).unsqueeze(0).to(Device.DEFAULT)
+    img = shift_and_sample(img_q, warped_frame, sample_skip_fn)
+    big_img = shift_and_sample(big_img_q, warped_big_frame, sample_skip_fn)
+    return img, big_img
+  return warp_enqueue
+
+
+def make_run_policy(vision_runner, on_policy_runner, vision_features_slice, frame_skip):
+  sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
+  sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
+
+  def run_policy(img, big_img, feat_q, desire_q, desire, traffic_convention, action_t):
+    desire = desire.to(Device.DEFAULT)
+    traffic_convention = traffic_convention.to(Device.DEFAULT)
+    action_t = action_t.to(Device.DEFAULT)
+    Tensor.realize(desire, traffic_convention, action_t)
+    desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
+    vision_out = next(iter(vision_runner({'img': img, 'big_img': big_img}).values())).cast('float32')
+
+    new_feat = vision_out[:, vision_features_slice].reshape(1, -1).unsqueeze(0)
+    feat_buf = shift_and_sample(feat_q, new_feat, sample_skip_fn)
+
+    inputs = {
+      'features_buffer': feat_buf,
+      'desire_pulse': desire_buf,
+      'traffic_convention': traffic_convention,
+      'action_t': action_t,
+    }
+    on_policy_out = next(iter(on_policy_runner(inputs).values())).cast('float32')
+    #off_policy_out = next(iter(off_policy_runner(inputs).values())).cast('float32')
+    return vision_out, on_policy_out
+
+  return run_policy
+
+
+def compile_jit(jit, make_random_inputs, input_keys, frame_skip, vision_metadata, policy_metadata):
+  vision_input_shapes = vision_metadata['input_shapes']
+  policy_input_shapes = policy_metadata['input_shapes']
+
+  SEED = 42
+  def random_inputs_run(fn, seed, test_val=None, test_buffers=None, expect_match=True):
+    input_queues, npy = make_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, Device.DEFAULT)
+    np.random.seed(seed)
+    Tensor.manual_seed(seed)
+
+    testing = test_val is not None or test_buffers is not None
+    n_runs = 1 if testing else 3
+
+    for i in range(n_runs):
+      for v in npy.values():
+        v[:] = np.random.randn(*v.shape).astype(v.dtype)
+      Device.default.synchronize()
+      random_inputs = make_random_inputs()
+      st = time.perf_counter()
+      outs = fn(**{k: input_queues[k] for k in input_keys}, **random_inputs)
+      mt = time.perf_counter()
+      Device.default.synchronize()
+      et = time.perf_counter()
+      print(f"  [{i+1}/{n_runs}] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
+
+      if i == 0:
+        val = [np.copy(v.numpy()) for v in outs]
+        buffers = [np.copy(v.numpy().copy()) for v in input_queues.values()]
+
+    if test_val is not None:
+      match = all(np.array_equal(a, b) for a, b in zip(val, test_val, strict=True))
+      assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={seed})"
+    if test_buffers is not None:
+      match = all(np.array_equal(a, b) for a, b in zip(buffers, test_buffers, strict=True))
+      assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={seed})"
+    return val, buffers
+
+  print('capture + replay')
+  test_val, test_buffers = random_inputs_run(jit, SEED)
+  print('pickle round trip')
+  jit = pickle.loads(pickle.dumps(jit))
+  random_inputs_run(jit, SEED, test_val, test_buffers, expect_match=True)
+  random_inputs_run(jit, SEED+1, test_val, test_buffers, expect_match=False)
+  return jit
+
+
+def _parse_size(s):
+  w, h = s.lower().split('x')
+  return int(w), int(h)
+
+
+def read_file_chunked_to_shm(path):
+  from openpilot.common.file_chunker import read_file_chunked
+  from openpilot.system.hardware.hw import Paths
+  shm_path = os.path.join(Paths.shm_path(), os.path.basename(path))
+  atexit.register(lambda: os.path.exists(shm_path) and os.remove(shm_path))
+  with open(shm_path, 'wb') as f:
+    f.write(read_file_chunked(path))
+  return shm_path
+
+
+if __name__ == "__main__":
+  from tinygrad.nn.onnx import OnnxRunner
+  from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
+  from openpilot.selfdrive.modeld.get_model_metadata import make_metadata_dict
+  p = argparse.ArgumentParser()
+  p.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
+  p.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True,
+                 help='camera resolutions WxH (one or more)')
+  p.add_argument('--vision-onnx', required=True)
+  p.add_argument('--on-policy-onnx', required=True)
+  p.add_argument('--output', required=True)
+  p.add_argument('--frame-skip', type=int, required=True)
+  args = p.parse_args()
+
+  out = defaultdict(dict)
+  vision_path, on_policy_path = read_file_chunked_to_shm(args.vision_onnx), read_file_chunked_to_shm(args.on_policy_onnx)
+  model_w, model_h = args.model_size
+
+  vision_runner = OnnxRunner(vision_path)
+  on_policy_runner = OnnxRunner(on_policy_path)
+  vision_metadata, on_policy_metadata = make_metadata_dict(vision_path), make_metadata_dict(on_policy_path)
+
+  run_policy_jit = TinyJit(make_run_policy(vision_runner, on_policy_runner, vision_metadata['output_slices']['hidden_state'], args.frame_skip), prune=True)
+  out['metadata']['vision'], out['metadata']['on_policy'] = vision_metadata, on_policy_metadata
+
+  make_random_model_inputs = partial(make_random_images, keys=['img', 'big_img'], shape=vision_metadata['input_shapes']['img'])
+  out['run_policy'] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS, args.frame_skip, vision_metadata, on_policy_metadata)
+
+  for cam_w, cam_h in args.camera_resolutions:
+    nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
+    make_random_warp_inputs = partial(make_random_images, keys=['frame', 'big_frame'], shape=nv12.size, device=WARP_DEV)
+    warp_enqueue = TinyJit(make_warp(nv12, model_w, model_h, args.frame_skip), prune=True)
+    out[(cam_w,cam_h)] = compile_jit(warp_enqueue, make_random_warp_inputs, WARP_INPUTS, args.frame_skip, vision_metadata, on_policy_metadata)
+
+  with open(args.output, "wb") as f:
+    pickle.dump(out, f)
+  print(f"Saved JITs to {args.output} ({os.path.getsize(args.output) / 1e6:.2f} MB)")

selfdrive/modeld/constants.py

@@ -38,6 +38,7 @@ class ModelConstants:
   LANE_LINES_WIDTH = 2
   ROAD_EDGES_WIDTH = 2
   PLAN_WIDTH = 15
+  ACTION_WIDTH = 2
   DESIRE_PRED_WIDTH = 8
   LAT_PLANNER_SOLUTION_WIDTH = 4
   DESIRED_CURV_WIDTH = 1

selfdrive/modeld/dmonitoringmodeld.py

@@ -1,12 +1,6 @@
 #!/usr/bin/env python3
 import os
-from openpilot.selfdrive.modeld.tinygrad_helpers import MODELS_DIR, set_tinygrad_backend_from_compiled_flags
-set_tinygrad_backend_from_compiled_flags()
-
-# FIXME-SP: remove once we bump tg
-from openpilot.system.hardware import TICI
-os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
-
+from openpilot.selfdrive.modeld.helpers import MODELS_DIR, get_tg_input_devices
 from tinygrad.tensor import Tensor
 import time
 import pickle
@@ -28,11 +22,13 @@ SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 MODEL_PKL_PATH = MODELS_DIR / 'dmonitoring_model_tinygrad.pkl'
 METADATA_PATH = MODELS_DIR / 'dmonitoring_model_metadata.pkl'
 
+
 class ModelState:
   inputs: dict[str, np.ndarray]
   output: np.ndarray
 
-  def __init__(self):
+  def __init__(self, cam_w: int, cam_h: int):
+    self.DEV = get_tg_input_devices(PROCESS_NAME, usbgpu=False)['DEV']
     with open(METADATA_PATH, 'rb') as f:
       model_metadata = pickle.load(f)
       self.input_shapes = model_metadata['input_shapes']
@@ -44,31 +40,27 @@ class ModelState:
 
     self.warp_inputs_np = {'transform': np.zeros((3,3), dtype=np.float32)}
     self.warp_inputs = {k: Tensor(v, device='NPY') for k,v in self.warp_inputs_np.items()}
-    self.frame_buf_params = None
+    self.frame_buf_params = get_nv12_info(cam_w, cam_h)
     self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
     self._blob_cache : dict[int, Tensor] = {}
-    self.image_warp = None
     self.model_run = pickle.loads(read_file_chunked(str(MODEL_PKL_PATH)))
+    with open(MODELS_DIR / f'dm_warp_{cam_w}x{cam_h}_tinygrad.pkl', "rb") as f:
+      self.image_warp = pickle.load(f)
 
   def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]:
     self.numpy_inputs['calib'][0,:] = calib
 
     t1 = time.perf_counter()
 
-    if self.image_warp is None:
-      self.frame_buf_params = get_nv12_info(buf.width, buf.height)
-      warp_path = MODELS_DIR / f'dm_warp_{buf.width}x{buf.height}_tinygrad.pkl'
-      with open(warp_path, "rb") as f:
-        self.image_warp = pickle.load(f)
-    ptr = buf.data.ctypes.data
+    ptr = np.frombuffer(buf.data, dtype=np.uint8).ctypes.data
     # There is a ringbuffer of imgs, just cache tensors pointing to all of them
     if ptr not in self._blob_cache:
-      self._blob_cache[ptr] = Tensor.from_blob(ptr, (self.frame_buf_params[3],), dtype='uint8')
+      self._blob_cache[ptr] = Tensor.from_blob(ptr, (self.frame_buf_params[3],), dtype='uint8', device=self.DEV)
 
     self.warp_inputs_np['transform'][:] = transform[:]
-    self.tensor_inputs['input_img'] = self.image_warp(self._blob_cache[ptr], self.warp_inputs['transform']).realize()
+    self.tensor_inputs['input_img'] = self.image_warp(self._blob_cache[ptr], self.warp_inputs['transform'])
 
-    output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy().flatten()
+    output = self.model_run(**self.tensor_inputs).numpy().flatten()
 
     t2 = time.perf_counter()
     return output, t2 - t1
@@ -83,7 +75,7 @@ def parse_model_output(model_output):
     face_descs = model_output[f'face_descs_{ds_suffix}']
     parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
     parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
-    for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
+    for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob', 'sleep_prob']:
       parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
   return parsed
 
@@ -99,6 +91,7 @@ def fill_driver_data(msg, model_output, ds_suffix):
   msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
   msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
   msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
+  msg.sleepProb = model_output[f'sleep_prob_{ds_suffix}'][0, 0].item()
 
 def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
   msg = messaging.new_message('driverStateV2', valid=True)
@@ -116,9 +109,6 @@ def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_t
 def main():
   config_realtime_process(7, 5)
 
-  model = ModelState()
-  cloudlog.warning("models loaded, dmonitoringmodeld starting")
-
   cloudlog.warning("connecting to driver stream")
   vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True)
   while not vipc_client.connect(False):
@@ -126,6 +116,9 @@ def main():
   assert vipc_client.is_connected()
   cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}")
 
+  model = ModelState(vipc_client.width, vipc_client.height)
+  cloudlog.warning("models loaded, dmonitoringmodeld starting")
+
   sm = SubMaster(["liveCalibration"])
   pm = PubMaster(["driverStateV2"])
 

selfdrive/modeld/get_model_metadata.py

@@ -35,21 +35,21 @@ def get_metadata_value_by_name(model: dict[str, Any], name: str) -> str | Any:
   return None
 
 
-if __name__ == "__main__":
-  model_path = pathlib.Path(sys.argv[1])
+def make_metadata_dict(model_path):
   model = MetadataOnnxPBParser(model_path).parse()
   output_slices = get_metadata_value_by_name(model, 'output_slices')
   assert output_slices is not None, 'output_slices not found in metadata'
-
-  metadata = {
+  return {
     'model_checkpoint': get_metadata_value_by_name(model, 'model_checkpoint'),
     'output_slices': pickle.loads(codecs.decode(output_slices.encode(), "base64")),
     'input_shapes': dict(get_name_and_shape(x) for x in model["graph"]["input"]),
     'output_shapes': dict(get_name_and_shape(x) for x in model["graph"]["output"]),
   }
 
+
+if __name__ == "__main__":
+  model_path = pathlib.Path(sys.argv[1])
   metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl')
   with open(metadata_path, 'wb') as f:
-    pickle.dump(metadata, f)
-
+    pickle.dump(make_metadata_dict(model_path), f)
   print(f'saved metadata to {metadata_path}')

selfdrive/modeld/helpers.py

@@ -0,0 +1,26 @@
+import json
+from pathlib import Path
+
+MODELS_DIR = Path(__file__).resolve().parent / 'models'
+TG_INPUT_DEVICES_PATH = MODELS_DIR / 'tg_input_devices.json'
+USBGPU_VID = 0xADD1
+USBGPU_PID = 0x0001
+
+
+def get_tg_input_devices(process_name: str, usbgpu: bool):
+  with open(TG_INPUT_DEVICES_PATH) as f:
+    return json.load(f)[process_name]['default' if not usbgpu else 'usbgpu']
+
+def modeld_pkl_path(usbgpu: bool):
+  prefix = 'big_' if usbgpu else ''
+  return MODELS_DIR / f'{prefix}driving_tinygrad.pkl'
+
+def usbgpu_present() -> bool:
+  for d in Path("/sys/bus/usb/devices").glob("*"):
+    try:
+      if int((d / "idVendor").read_text(), 16) == USBGPU_VID and \
+          int((d / "idProduct").read_text(), 16) == USBGPU_PID:
+        return True
+    except Exception:
+      pass
+  return False

selfdrive/modeld/modeld.py

@@ -1,16 +1,6 @@
 #!/usr/bin/env python3
 import os
-from openpilot.selfdrive.modeld.tinygrad_helpers import MODELS_DIR, set_tinygrad_backend_from_compiled_flags
-set_tinygrad_backend_from_compiled_flags()
-
-# FIXME-SP: remove once we bump tg
-from openpilot.system.hardware import TICI
-os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
-
-USBGPU = "USBGPU" in os.environ
-if USBGPU:
-  os.environ['DEV'] = 'AMD'
-  os.environ['AMD_IFACE'] = 'USB'
+os.environ['GMMU'] = '0' # for usbgpu fast loading, noop for qcom
 from tinygrad.tensor import Tensor
 import time
 import pickle
@@ -30,44 +20,41 @@ from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value, get_curvature_from_plan
 from openpilot.selfdrive.modeld.parse_model_outputs import Parser
+from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, WARP_INPUTS, POLICY_INPUTS
 from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
-from openpilot.common.file_chunker import read_file_chunked
+from openpilot.common.file_chunker import read_file_chunked, get_manifest_path
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
+from openpilot.selfdrive.modeld.helpers import usbgpu_present, modeld_pkl_path, get_tg_input_devices
 
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 
-
 PROCESS_NAME = "selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
-VISION_PKL_PATH = MODELS_DIR / 'driving_vision_tinygrad.pkl'
-VISION_METADATA_PATH = MODELS_DIR / 'driving_vision_metadata.pkl'
-POLICY_PKL_PATH = MODELS_DIR / 'driving_policy_tinygrad.pkl'
-POLICY_METADATA_PATH = MODELS_DIR / 'driving_policy_metadata.pkl'
-
 LAT_SMOOTH_SECONDS = 0.0
 LONG_SMOOTH_SECONDS = 0.3
 MIN_LAT_CONTROL_SPEED = 0.3
 
-IMG_QUEUE_SHAPE = (6*(ModelConstants.MODEL_RUN_FREQ//ModelConstants.MODEL_CONTEXT_FREQ + 1), 128, 256)
-assert IMG_QUEUE_SHAPE[0] == 30
-
 
 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
                           lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
+  if 'action' not in model_output:
     plan = model_output['plan'][0]
     desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
                                                      plan[:,Plan.ACCELERATION][:,0],
                                                      ModelConstants.T_IDXS,
                                                      action_t=long_action_t)
-    desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
-
     desired_curvature = get_curvature_from_plan(plan[:,Plan.T_FROM_CURRENT_EULER][:,2],
                                                 plan[:,Plan.ORIENTATION_RATE][:,2],
                                                 ModelConstants.T_IDXS,
                                                 v_ego,
                                                 lat_action_t)
+  else:
+    desired_accel = model_output['action'][0,1]
+    desired_curvature = model_output['action'][0,0] / (max(1.0, v_ego))**2
+    should_stop = (v_ego < 0.3 and desired_accel < 0.1)
+  desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
   if v_ego > MIN_LAT_CONTROL_SPEED:
     desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
   else:
@@ -77,6 +64,7 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
                                 desiredAcceleration=float(desired_accel),
                                 shouldStop=bool(should_stop))
 
+
 class FrameMeta:
   frame_id: int = 0
   timestamp_sof: int = 0
@@ -86,108 +74,35 @@ class FrameMeta:
     if vipc is not None:
       self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof
 
-class InputQueues:
-  def __init__ (self, model_fps, env_fps, n_frames_input):
-    assert env_fps % model_fps == 0
-    assert env_fps >= model_fps
-    self.model_fps = model_fps
-    self.env_fps = env_fps
-    self.n_frames_input = n_frames_input
-
-    self.dtypes = {}
-    self.shapes = {}
-    self.q = {}
-
-  def update_dtypes_and_shapes(self, input_dtypes, input_shapes) -> None:
-    self.dtypes.update(input_dtypes)
-    if self.env_fps == self.model_fps:
-      self.shapes.update(input_shapes)
-    else:
-      for k in input_shapes:
-        shape = list(input_shapes[k])
-        if 'img' in k:
-          n_channels = shape[1] // self.n_frames_input
-          shape[1] = (self.env_fps // self.model_fps + (self.n_frames_input - 1)) * n_channels
-        else:
-          shape[1] = (self.env_fps // self.model_fps) * shape[1]
-        self.shapes[k] = tuple(shape)
-
-  def reset(self) -> None:
-    self.q = {k: np.zeros(self.shapes[k], dtype=self.dtypes[k]) for k in self.dtypes.keys()}
-
-  def enqueue(self, inputs:dict[str, np.ndarray]) -> None:
-    for k in inputs.keys():
-      if inputs[k].dtype != self.dtypes[k]:
-        raise ValueError(f'supplied input <{k}({inputs[k].dtype})> has wrong dtype, expected {self.dtypes[k]}')
-      input_shape = list(self.shapes[k])
-      input_shape[1] = -1
-      single_input = inputs[k].reshape(tuple(input_shape))
-      sz = single_input.shape[1]
-      self.q[k][:,:-sz] = self.q[k][:,sz:]
-      self.q[k][:,-sz:] = single_input
-
-  def get(self, *names) -> dict[str, np.ndarray]:
-    if self.env_fps == self.model_fps:
-      return {k: self.q[k] for k in names}
-    else:
-      out = {}
-      for k in names:
-        shape = self.shapes[k]
-        if 'img' in k:
-          n_channels = shape[1] // (self.env_fps // self.model_fps + (self.n_frames_input - 1))
-          out[k] = np.concatenate([self.q[k][:, s:s+n_channels] for s in np.linspace(0, shape[1] - n_channels, self.n_frames_input, dtype=int)], axis=1)
-        elif 'pulse' in k:
-          # any pulse within interval counts
-          out[k] = self.q[k].reshape((shape[0], shape[1] * self.model_fps // self.env_fps, self.env_fps // self.model_fps, -1)).max(axis=2)
-        else:
-          idxs = np.arange(-1, -shape[1], -self.env_fps // self.model_fps)[::-1]
-          out[k] = self.q[k][:, idxs]
-      return out
 
 class ModelState(ModelStateBase):
-  inputs: dict[str, np.ndarray]
-  output: np.ndarray
   prev_desire: np.ndarray  # for tracking the rising edge of the pulse
 
-  def __init__(self):
+  def __init__(self, cam_w: int, cam_h: int, usbgpu: bool):
     ModelStateBase.__init__(self)
     self.LAT_SMOOTH_SECONDS = LAT_SMOOTH_SECONDS
-    with open(VISION_METADATA_PATH, 'rb') as f:
-      vision_metadata = pickle.load(f)
+    input_devices = get_tg_input_devices(PROCESS_NAME, usbgpu)
+    self.WARP_DEV, self.QUEUE_DEV = input_devices['WARP_DEV'], input_devices['QUEUE_DEV']
+    jits = pickle.loads(read_file_chunked(modeld_pkl_path(usbgpu)))
+    vision_metadata = jits['metadata']['vision']
     self.vision_input_shapes = vision_metadata['input_shapes']
     self.vision_input_names = list(self.vision_input_shapes.keys())
     self.vision_output_slices = vision_metadata['output_slices']
-      vision_output_size = vision_metadata['output_shapes']['outputs'][1]
 
-    with open(POLICY_METADATA_PATH, 'rb') as f:
-      policy_metadata = pickle.load(f)
+    policy_metadata = jits['metadata']['on_policy']
     self.policy_input_shapes = policy_metadata['input_shapes']
     self.policy_output_slices = policy_metadata['output_slices']
-      policy_output_size = policy_metadata['output_shapes']['outputs'][1]
 
     self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
 
-    # policy inputs
-    self.numpy_inputs = {k: np.zeros(self.policy_input_shapes[k], dtype=np.float32) for k in self.policy_input_shapes}
-    self.full_input_queues = InputQueues(ModelConstants.MODEL_CONTEXT_FREQ, ModelConstants.MODEL_RUN_FREQ, ModelConstants.N_FRAMES)
-    for k in ['desire_pulse', 'features_buffer']:
-      self.full_input_queues.update_dtypes_and_shapes({k: self.numpy_inputs[k].dtype}, {k: self.numpy_inputs[k].shape})
-    self.full_input_queues.reset()
-
-    self.img_queues = {'img': Tensor.zeros(IMG_QUEUE_SHAPE, dtype='uint8').contiguous().realize(),
-                       'big_img': Tensor.zeros(IMG_QUEUE_SHAPE, dtype='uint8').contiguous().realize()}
-    self.full_frames : dict[str, Tensor] = {}
-    self._blob_cache : dict[int, Tensor] = {}
-    self.transforms_np = {k: np.zeros((3,3), dtype=np.float32) for k in self.img_queues}
-    self.transforms = {k: Tensor(v, device='NPY').realize() for k, v in self.transforms_np.items()}
-    self.vision_output = np.zeros(vision_output_size, dtype=np.float32)
-    self.policy_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
-    self.policy_output = np.zeros(policy_output_size, dtype=np.float32)
+    self.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
+    self.input_queues, self.npy = make_input_queues(self.vision_input_shapes, self.policy_input_shapes, self.frame_skip, device=self.QUEUE_DEV)
+    self.full_frames: dict[str, Tensor] = {}
+    self._blob_cache: dict[int, Tensor] = {}
     self.parser = Parser()
-    self.frame_buf_params : dict[str, tuple[int, int, int, int]] = {}
-    self.update_imgs = None
-    self.vision_run = pickle.loads(read_file_chunked(str(VISION_PKL_PATH)))
-    self.policy_run = pickle.loads(read_file_chunked(str(POLICY_PKL_PATH)))
+    self.frame_buf_params = {k: get_nv12_info(cam_w, cam_h) for k in ('img', 'big_img')}
+    self.run_policy = jits['run_policy']
+    self.warp_enqueue = jits[(cam_w,cam_h)]
 
   def slice_outputs(self, model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]:
     parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()}
@@ -195,66 +110,59 @@ class ModelState(ModelStateBase):
 
   def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
           inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
-    # Model decides when action is completed, so desire input is just a pulse triggered on rising edge
-    inputs['desire_pulse'][0] = 0
-    new_desire = np.where(inputs['desire_pulse'] - self.prev_desire > .99, inputs['desire_pulse'], 0)
-    self.prev_desire[:] = inputs['desire_pulse']
-    if self.update_imgs is None:
     for key in bufs.keys():
-        w, h = bufs[key].width, bufs[key].height
-        self.frame_buf_params[key] = get_nv12_info(w, h)
-      warp_path = MODELS_DIR / f'warp_{w}x{h}_tinygrad.pkl'
-      with open(warp_path, "rb") as f:
-        self.update_imgs = pickle.load(f)
-
-    for key in bufs.keys():
-      ptr = bufs[key].data.ctypes.data
+      ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data
       yuv_size = self.frame_buf_params[key][3]
       # There is a ringbuffer of imgs, just cache tensors pointing to all of them
       cache_key = (key, ptr)
       if cache_key not in self._blob_cache:
-        self._blob_cache[cache_key] = Tensor.from_blob(ptr, (yuv_size,), dtype='uint8')
+        self._blob_cache[cache_key] = Tensor.from_blob(ptr, (yuv_size,), dtype='uint8', device=self.WARP_DEV)
       self.full_frames[key] = self._blob_cache[cache_key]
-    for key in bufs.keys():
-      self.transforms_np[key][:,:] = transforms[key][:,:]
 
-    out = self.update_imgs(self.img_queues['img'], self.full_frames['img'], self.transforms['img'],
-                           self.img_queues['big_img'], self.full_frames['big_img'], self.transforms['big_img'])
-    vision_inputs = {'img': out[0], 'big_img': out[1]}
+    # Model decides when action is completed, so desire input is just a pulse triggered on rising edge
+    inputs['desire_pulse'][0] = 0
+    self.npy['desire'][:] = np.where(inputs['desire_pulse'] - self.prev_desire > .99, inputs['desire_pulse'], 0)
+    self.prev_desire[:] = inputs['desire_pulse']
+    self.npy['traffic_convention'][:] = inputs['traffic_convention']
+    self.npy['action_t'][:] = inputs['action_t']
+    self.npy['tfm'][:,:] = transforms['img'][:,:]
+    self.npy['big_tfm'][:,:] = transforms['big_img'][:,:]
+
+    img, big_img = self.warp_enqueue(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=self.full_frames['img'], big_frame=self.full_frames['big_img'])
 
     if prepare_only:
       return None
 
-    self.vision_output = self.vision_run(**vision_inputs).contiguous().realize().uop.base.buffer.numpy().flatten()
-    vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices))
+    vision_output, on_policy_output = self.run_policy(
+      **{k: self.input_queues[k] for k in POLICY_INPUTS}, img=img, big_img=big_img
+    )
 
-    self.full_input_queues.enqueue({'features_buffer': vision_outputs_dict['hidden_state'], 'desire_pulse': new_desire})
-    for k in ['desire_pulse', 'features_buffer']:
-      self.numpy_inputs[k][:] = self.full_input_queues.get(k)[k]
-    self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
-
-    self.policy_output = self.policy_run(**self.policy_inputs).contiguous().realize().uop.base.buffer.numpy().flatten()
-    policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices))
+    vision_output = vision_output.numpy().flatten()
+    on_policy_output = on_policy_output.numpy().flatten()
+    vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(vision_output, self.vision_output_slices))
+    policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(on_policy_output, self.policy_output_slices))
     combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict}
-    if SEND_RAW_PRED:
-      combined_outputs_dict['raw_pred'] = np.concatenate([self.vision_output.copy(), self.policy_output.copy()])
 
+    if SEND_RAW_PRED:
+      combined_outputs_dict['raw_pred'] = np.concatenate([vision_output.copy(), on_policy_output.copy()])
     return combined_outputs_dict
 
 
 def main(demo=False):
   cloudlog.warning("modeld init")
 
+  _present = usbgpu_present()
+  _compiled = os.path.isfile(get_manifest_path(modeld_pkl_path(usbgpu=True)))
+  USBGPU = _present and _compiled
+  params = Params()
+  params.put_bool("UsbGpuPresent", _present)
+  params.put_bool("UsbGpuCompiled", _compiled)
+
   if not USBGPU:
     # USB GPU currently saturates a core so can't do this yet,
     # also need to move the aux USB interrupts for good timings
     config_realtime_process(7, 54)
 
-  st = time.monotonic()
-  cloudlog.warning("loading model")
-  model = ModelState()
-  cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
-
   # visionipc clients
   while True:
     available_streams = VisionIpcClient.available_streams("camerad", block=False)
@@ -278,6 +186,11 @@ def main(demo=False):
   if use_extra_client:
     cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
 
+  st = time.monotonic()
+  cloudlog.warning("loading model")
+  model = ModelState(vipc_client_main.width, vipc_client_main.height, USBGPU)
+  cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
+
   # messaging
   pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"])
   sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
@@ -298,7 +211,6 @@ def main(demo=False):
   meta_main = FrameMeta()
   meta_extra = FrameMeta()
 
-
   if demo:
     CP = get_demo_car_params()
   else:
@@ -382,9 +294,14 @@ def main(demo=False):
 
     bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names}
     transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names}
-    inputs:dict[str, np.ndarray] = {
+    frame_delay = DT_MDL # compensate for time passed since the frame was captured: current_time - timestamp_eof is 50ms on average
+    action_delay = DT_MDL / 2 # middle of the interval between model output (current state) and next frame (expected state)
+    lat_action_t = lat_delay + frame_delay + action_delay
+    long_action_t = long_delay + frame_delay + action_delay
+    inputs: dict[str, np.ndarray] = {
       'desire_pulse': vec_desire,
       'traffic_convention': traffic_convention,
+      'action_t': np.array([lat_action_t, long_action_t], dtype=np.float32),
     }
 
     mt1 = time.perf_counter()
@@ -398,9 +315,7 @@ def main(demo=False):
       posenet_send = messaging.new_message('cameraOdometry')
       mdv2sp_send = messaging.new_message('modelDataV2SP')
 
-      frame_delay = DT_MDL # compensate for time passed since the frame was captured: current_time - timestamp_eof is 50ms on average
-      action_delay = DT_MDL / 2 # middle of the interval between model output (current state) and next frame (expected state)
-      action = get_action_from_model(model_output, prev_action, lat_delay + frame_delay + action_delay, long_delay + frame_delay + action_delay, v_ego)
+      action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
       prev_action = action
       fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,

selfdrive/modeld/models/big_driving_on_policy.onnx

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

selfdrive/modeld/models/big_driving_policy.onnx

@@ -1 +0,0 @@
-driving_policy.onnx

selfdrive/modeld/models/big_driving_vision.onnx

@@ -1 +0,0 @@
-driving_vision.onnx

selfdrive/modeld/models/big_driving_vision.onnx

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

selfdrive/modeld/parse_model_outputs.py

@@ -110,11 +110,7 @@ class Parser:
     return outs
 
   def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
-    plan_mhp = self.is_mhp(outs, 'plan', ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH)
-    plan_in_N, plan_out_N = (ModelConstants.PLAN_MHP_N, ModelConstants.PLAN_MHP_SELECTION) if plan_mhp else (0, 0)
-    self.parse_mdn('plan', outs, in_N=plan_in_N, out_N=plan_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
-    if 'planplus' in outs:
-      self.parse_mdn('planplus', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
+    self.parse_mdn('plan', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
     self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
     return outs
 

selfdrive/modeld/tinygrad_helpers.py

@@ -1,12 +0,0 @@
-import json
-import os
-from pathlib import Path
-
-MODELS_DIR = Path(__file__).parent / 'models'
-COMPILED_FLAGS_PATH = MODELS_DIR / 'tg_compiled_flags.json'
-
-
-def set_tinygrad_backend_from_compiled_flags() -> None:
-  if os.path.isfile(COMPILED_FLAGS_PATH):
-    with open(COMPILED_FLAGS_PATH) as f:
-      os.environ['DEV'] = str(json.load(f)['DEV'])

selfdrive/monitoring/README.md

@@ -1,15 +0,0 @@
-# driver monitoring (DM)
-
-Uploading driver-facing camera footage is opt-in, but it is encouraged to opt-in to improve the DM model. You can always change your preference using the "Record and Upload Driver Camera" toggle.
-
-## Troubleshooting
-
-Before creating a bug report, go through these troubleshooting steps.
-
-* Ensure the driver-facing camera has a good view of the driver in normal driving positions.
- * This can be checked in Settings -> Device -> Preview Driver Camera (when car is off).
-* If the camera can't see the driver, the device should be re-mounted.
-
-## Bug report
-
-In order for us to look into DM bug reports, we'll need the driver-facing camera footage. If you don't normally have this enabled, simply enable the toggle for a single drive. Also ensure the "Upload Raw Logs" toggle is enabled before going for a drive.

selfdrive/monitoring/dmonitoringd.py

@@ -2,7 +2,7 @@
 import cereal.messaging as messaging
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process
-from openpilot.selfdrive.monitoring.helpers import DriverMonitoring
+from openpilot.selfdrive.monitoring.policy import DriverMonitoring
 
 
 def dmonitoringd_thread():
@@ -25,7 +25,7 @@ def dmonitoringd_thread():
 
     valid = sm.all_checks()
     if demo_mode and sm.valid['driverStateV2']:
-      DM.run_step(sm, demo=demo_mode)
+      DM.run_step(sm, demo=True)
     elif valid:
       DM.run_step(sm, demo=demo_mode)
 
@@ -40,9 +40,9 @@ def dmonitoringd_thread():
 
     # save rhd virtual toggle every 5 mins
     if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and
-     DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and
-     DM.wheel_on_right == (DM.wheelpos.prob_offseter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)):
-      params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right)
+     DM.wheelpos_offsetter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and
+     DM.wheel_on_right == (DM.wheelpos_offsetter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)):
+      params.put_bool("IsRhdDetected", DM.wheel_on_right)
 
 def main():
   dmonitoringd_thread()

selfdrive/monitoring/policy.py

@@ -1,18 +1,20 @@
+from collections import defaultdict
 from math import atan2, radians
 import numpy as np
 
 from cereal import car, log
 import cereal.messaging as messaging
-from openpilot.selfdrive.selfdrived.events import Events
-from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
 from openpilot.common.realtime import DT_DMON
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.params import Params
 from openpilot.common.stat_live import RunningStatFilter
 from openpilot.common.transformations.camera import DEVICE_CAMERAS
-from openpilot.system.hardware import HARDWARE
 
-EventName = log.OnroadEvent.EventName
+AlertLevel = log.DriverMonitoringState.AlertLevel
+MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
+
+def to_percent(v):
+  return int(min(max(v * 100., 0.), 100.))
 
 # ******************************************************************************************
 #  NOTE: To fork maintainers.
@@ -21,22 +23,27 @@ EventName = log.OnroadEvent.EventName
 # ******************************************************************************************
 
 class DRIVER_MONITOR_SETTINGS:
-  def __init__(self, device_type):
-    self._DT_DMON = DT_DMON
-    # ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2
-    self._AWARENESS_TIME = 30. # passive wheeltouch total timeout
-    self._AWARENESS_PRE_TIME_TILL_TERMINAL = 15.
-    self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6.
-    self._DISTRACTED_TIME = 11. # active monitoring total timeout
-    self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 8.
-    self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
+  def __init__(self):
+    # https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2
+    self._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT = 15.
+    self._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT = 24.
+    self._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT = 30.
+    # https://cdn.euroncap.com/cars/assets/euro_ncap_protocol_safe_driving_driver_engagement_v11_a30e874152.pdf
+    self._VISION_POLICY_ALERT_1_TIMEOUT = 3.
+    self._VISION_POLICY_ALERT_2_TIMEOUT = 5.
+    self._VISION_POLICY_ALERT_3_TIMEOUT = 11.
+
+    self._TIMEOUT_RECOVERY_FACTOR_MAX = 5.
+    self._TIMEOUT_RECOVERY_FACTOR_MIN = 1.25
+
+    self._MAX_TERMINAL_ALERTS = 3  # not allowed to engage after 3 terminal alerts
+    self._MAX_TERMINAL_DURATION = int(30 / DT_DMON)  # not allowed to engage after 30s of terminal alerts
 
     self._FACE_THRESHOLD = 0.7
     self._EYE_THRESHOLD = 0.65
     self._SG_THRESHOLD = 0.9
     self._BLINK_THRESHOLD = 0.865
     self._PHONE_THRESH = 0.5
-
     self._POSE_PITCH_THRESHOLD = 0.3133
     self._POSE_PITCH_THRESHOLD_SLACK = 0.3237
     self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD
@@ -57,106 +64,79 @@ class DRIVER_MONITOR_SETTINGS:
     self._YAW_MIN_OFFSET = -0.0246
 
     self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1
-    self._DCAM_UNCERTAIN_ALERT_COUNT = int(60  / self._DT_DMON)
-    self._DCAM_UNCERTAIN_RESET_COUNT = int(20  / self._DT_DMON)
-    self._POSESTD_THRESHOLD = 0.3
-    self._HI_STD_FALLBACK_TIME = int(10  / self._DT_DMON)  # fall back to wheel touch if model is uncertain for 10s
+    self._DCAM_UNCERTAIN_ALERT_COUNT = int(60  / DT_DMON)
+    self._DCAM_UNCERTAIN_RESET_COUNT = int(2  / DT_DMON)
+    self._HI_STD_THRESHOLD = 0.3
+    self._HI_STD_FALLBACK_TIME = int(10  / DT_DMON)  # fall back to wheel touch if model is uncertain for 10s
     self._DISTRACTED_FILTER_TS = 0.25  # 0.6Hz
 
     self._POSE_CALIB_MIN_SPEED = 13  # 30 mph
-    self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON)  # valid data counts before calibration completes, 1min cumulative
-    self._POSE_OFFSET_MAX_COUNT = int(360 / self._DT_DMON)  # stop deweighting new data after 6 min, aka "short term memory"
-
+    self._POSE_OFFSET_MIN_COUNT = int(60 / DT_DMON)  # valid data counts before calibration completes, 1min cumulative
+    self._POSE_OFFSET_MAX_COUNT = int(360 / DT_DMON)  # stop deweighting new data after 6 min, aka "short term memory"
     self._WHEELPOS_CALIB_MIN_SPEED = 11
     self._WHEELPOS_THRESHOLD = 0.5
-    self._WHEELPOS_FILTER_MIN_COUNT = int(15 / self._DT_DMON) # allow 15 seconds to converge wheel side
+    self._WHEELPOS_FILTER_MIN_COUNT = int(15 / DT_DMON) # allow 15 seconds to converge wheel side
     self._WHEELPOS_DATA_AVG = 0.03
     self._WHEELPOS_DATA_VAR = 3*5.5e-5
     self._WHEELPOS_MAX_COUNT = -1
 
-    self._RECOVERY_FACTOR_MAX = 5.  # relative to minus step change
-    self._RECOVERY_FACTOR_MIN = 1.25  # relative to minus step change
-
-    self._MAX_TERMINAL_ALERTS = 3  # not allowed to engage after 3 terminal alerts
-    self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON)  # not allowed to engage after 30s of terminal alerts
-
-class DistractedType:
-
-  NOT_DISTRACTED = 0
-  DISTRACTED_POSE = 1 << 0
-  DISTRACTED_BLINK = 1 << 1
-  DISTRACTED_PHONE = 1 << 2
-
 class DriverPose:
   def __init__(self, settings):
     pitch_filter_raw_priors = (settings._PITCH_NATURAL_OFFSET, settings._PITCH_NATURAL_VAR, 2)
     yaw_filter_raw_priors = (settings._YAW_NATURAL_OFFSET, settings._YAW_NATURAL_VAR, 2)
     self.yaw = 0.
     self.pitch = 0.
-    self.roll = 0.
-    self.yaw_std = 0.
-    self.pitch_std = 0.
-    self.roll_std = 0.
-    self.pitch_offseter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
-    self.yaw_offseter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
+    self.pitch_offsetter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
+    self.yaw_offsetter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
     self.calibrated = False
     self.low_std = True
     self.cfactor_pitch = 1.
     self.cfactor_yaw = 1.
     self.steer_yaw_offset = 0.
 
-class DriverProb:
-  def __init__(self, raw_priors, max_trackable):
-    self.prob = 0.
-    self.prob_offseter = RunningStatFilter(raw_priors=raw_priors, max_trackable=max_trackable)
-    self.prob_calibrated = False
-
 class DriverBlink:
   def __init__(self):
     self.left = 0.
     self.right = 0.
 
-
 # model output refers to center of undistorted+leveled image
-EFL = 598.0 # focal length in K
-cam = DEVICE_CAMERAS[("tici", "ar0231")] # corrected image has same size as raw
-W, H = (cam.dcam.width, cam.dcam.height)  # corrected image has same size as raw
+ref_undistorted_cam = DEVICE_CAMERAS[("tici", "ar0231")].dcam
+dcam_undistorted_FL = 598.0
+dcam_undistorted_W, dcam_undistorted_H = (ref_undistorted_cam.width, ref_undistorted_cam.height)
 
-def face_orientation_from_net(angles_desc, pos_desc, rpy_calib):
-  # the output of these angles are in device frame
-  # so from driver's perspective, pitch is up and yaw is right
+def face_orientation_from_model(orient_model, pos_model, rpy_calib):
+  pitch_model = orient_model[0]
+  yaw_model = orient_model[1]
 
-  pitch_net, yaw_net, roll_net = angles_desc
+  face_pixel_position = ((pos_model[0]+0.5)*dcam_undistorted_W, (pos_model[1]+0.5)*dcam_undistorted_H)
+  yaw_focal_angle = atan2(face_pixel_position[0] - dcam_undistorted_W//2, dcam_undistorted_FL)
+  pitch_focal_angle = atan2(face_pixel_position[1] - dcam_undistorted_H//2, dcam_undistorted_FL)
 
-  face_pixel_position = ((pos_desc[0]+0.5)*W, (pos_desc[1]+0.5)*H)
-  yaw_focal_angle = atan2(face_pixel_position[0] - W//2, EFL)
-  pitch_focal_angle = atan2(face_pixel_position[1] - H//2, EFL)
+  pitch = pitch_model + pitch_focal_angle
+  yaw = -yaw_model + yaw_focal_angle
 
-  pitch = pitch_net + pitch_focal_angle
-  yaw = -yaw_net + yaw_focal_angle
-
-  # no calib for roll
   pitch -= rpy_calib[1]
   yaw -= rpy_calib[2]
-  return roll_net, pitch, yaw
+  return pitch, yaw
 
 
 class DriverMonitoring:
   def __init__(self, rhd_saved=False, settings=None, always_on=False):
     # init policy settings
-    self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type())
+    self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS()
 
     # init driver status
     wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2)
-    self.wheelpos = DriverProb(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT)
+    self.wheelpos_offsetter = RunningStatFilter(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT)
     self.pose = DriverPose(settings=self.settings)
     self.blink = DriverBlink()
     self.phone_prob = 0.
 
+    self.alert_level = AlertLevel.none
     self.always_on = always_on
-    self.distracted_types = []
+    self.distracted_types = defaultdict(bool)
     self.driver_distracted = False
-    self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON)
+    self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, DT_DMON)
     self.wheel_on_right = False
     self.wheel_on_right_last = None
     self.wheel_on_right_default = rhd_saved
@@ -164,61 +144,56 @@ class DriverMonitoring:
     self.terminal_alert_cnt = 0
     self.terminal_time = 0
     self.step_change = 0.
-    self.active_monitoring_mode = True
+    self.active_policy = MonitoringPolicy.vision
+    self.driver_interacting = False
     self.is_model_uncertain = False
     self.hi_stds = 0
-    self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
-    self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
+    self.model_std_max = 0.
+    self.threshold_alert_1 = 0.
+    self.threshold_alert_2 = 0.
     self.dcam_uncertain_cnt = 0
-    self.dcam_uncertain_alerted = False # once per drive
     self.dcam_reset_cnt = 0
-
-    self.params = Params()
-    self.too_distracted = self.params.get_bool("DriverTooDistracted")
+    self.too_distracted = Params().get_bool("DriverTooDistracted")
 
     self._reset_awareness()
-    self._set_timers(active_monitoring=True)
-    self._reset_events()
+    self._set_policy(MonitoringPolicy.vision)
 
   def _reset_awareness(self):
     self.awareness = 1.
-    self.awareness_active = 1.
-    self.awareness_passive = 1.
+    self.last_vision_awareness = 1.
+    self.last_wheeltouch_awareness = 1.
 
-  def _reset_events(self):
-    self.current_events = Events()
-
-  def _set_timers(self, active_monitoring):
-    if self.active_monitoring_mode and self.awareness <= self.threshold_prompt:
-      if active_monitoring:
-        self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME
+  def _set_policy(self, target_policy):
+    if self.active_policy == MonitoringPolicy.vision and self.awareness <= self.threshold_alert_2:
+      if target_policy == MonitoringPolicy.vision:
+        self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
       else:
         self.step_change = 0.
       return  # no exploit after orange alert
     elif self.awareness <= 0.:
       return
 
-    if active_monitoring:
+    if target_policy == MonitoringPolicy.vision:
       # when falling back from passive mode to active mode, reset awareness to avoid false alert
-      if not self.active_monitoring_mode:
-        self.awareness_passive = self.awareness
-        self.awareness = self.awareness_active
+      if self.active_policy != MonitoringPolicy.vision:
+        self.last_wheeltouch_awareness = self.awareness
+        self.awareness = self.last_vision_awareness
 
-      self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
-      self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
-      self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME
-      self.active_monitoring_mode = True
+      self.threshold_alert_1 = 1. - self.settings._VISION_POLICY_ALERT_1_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
+      self.threshold_alert_2 = 1. - self.settings._VISION_POLICY_ALERT_2_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
+      self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
+      self.active_policy = MonitoringPolicy.vision
     else:
-      if self.active_monitoring_mode:
-        self.awareness_active = self.awareness
-        self.awareness = self.awareness_passive
+      if self.active_policy == MonitoringPolicy.vision:
+        self.last_vision_awareness = self.awareness
+        self.awareness = self.last_wheeltouch_awareness
 
-      self.threshold_pre = self.settings._AWARENESS_PRE_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME
-      self.threshold_prompt = self.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME
-      self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME
-      self.active_monitoring_mode = False
+      self.threshold_alert_1 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
+      self.threshold_alert_2 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
+      self.step_change = DT_DMON / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
+      self.active_policy = MonitoringPolicy.wheeltouch
 
-  def _set_policy(self, brake_disengage_prob, car_speed):
+  def _set_pose_strictness(self, brake_disengage_prob, car_speed):
     bp = brake_disengage_prob
     k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
     bp_normal = max(min(bp / k1, 0.5),0)
@@ -230,15 +205,15 @@ class DriverMonitoring:
                                             self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
 
   def _get_distracted_types(self):
-    distracted_types = []
+    self.distracted_types = defaultdict(bool)
 
     if not self.pose.calibrated:
       pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET
       yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET
     else:
-      pitch_error = self.pose.pitch - min(max(self.pose.pitch_offseter.filtered_stat.mean(),
+      pitch_error = self.pose.pitch - min(max(self.pose.pitch_offsetter.filtered_stat.mean(),
                                                        self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET)
-      yaw_error = self.pose.yaw - min(max(self.pose.yaw_offseter.filtered_stat.mean(),
+      yaw_error = self.pose.yaw - min(max(self.pose.yaw_offsetter.filtered_stat.mean(),
                                                     self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET)
     pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit
 
@@ -250,28 +225,21 @@ class DriverMonitoring:
     pitch_threshold = self.settings._POSE_PITCH_THRESHOLD * self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD
     yaw_threshold = self.settings._POSE_YAW_THRESHOLD * self.pose.cfactor_yaw
 
-    if pitch_error > pitch_threshold or yaw_error > yaw_threshold:
-      distracted_types.append(DistractedType.DISTRACTED_POSE)
-
-    if (self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD:
-      distracted_types.append(DistractedType.DISTRACTED_BLINK)
-
-    if self.phone_prob > self.settings._PHONE_THRESH:
-      distracted_types.append(DistractedType.DISTRACTED_PHONE)
-
-    return distracted_types
+    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)
 
   def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False, steering_angle_deg=0.):
     rhd_pred = driver_state.wheelOnRightProb
     # calibrates only when there's movement and either face detected
     if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or
                                           driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD):
-      self.wheelpos.prob_offseter.push_and_update(rhd_pred)
+      self.wheelpos_offsetter.push_and_update(rhd_pred)
 
-    self.wheelpos.prob_calibrated = self.wheelpos.prob_offseter.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT
+    wheelpos_calibrated = self.wheelpos_offsetter.filtered_stat.n >= self.settings._WHEELPOS_FILTER_MIN_COUNT
 
-    if self.wheelpos.prob_calibrated or demo_mode:
-      self.wheel_on_right = self.wheelpos.prob_offseter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD
+    if wheelpos_calibrated or demo_mode:
+      self.wheel_on_right = self.wheelpos_offsetter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD
     else:
       self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished
     # make sure no switching when engaged
@@ -283,42 +251,36 @@ class DriverMonitoring:
       return
 
     self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD
-    self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy)
+    self.pose.pitch, self.pose.yaw = face_orientation_from_model(driver_data.faceOrientation, driver_data.facePosition, cal_rpy)
     steer_d = max(abs(steering_angle_deg) - self.settings._POSE_YAW_MIN_STEER_DEG, 0.)
     self.pose.steer_yaw_offset = radians(steer_d) * -np.sign(steering_angle_deg) * self.settings._POSE_YAW_STEER_FACTOR
     if self.wheel_on_right:
       self.pose.yaw *= -1
       self.pose.steer_yaw_offset *= -1
     self.wheel_on_right_last = self.wheel_on_right
-    self.pose.pitch_std = driver_data.faceOrientationStd[0]
-    self.pose.yaw_std = driver_data.faceOrientationStd[1]
-    model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
-    self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD
+    self.model_std_max = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1])
+    self.pose.low_std = self.model_std_max < self.settings._HI_STD_THRESHOLD
     self.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \
                       * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
     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.distracted_types = self._get_distracted_types()
-    self.driver_distracted = (DistractedType.DISTRACTED_PHONE in self.distracted_types
-                              or DistractedType.DISTRACTED_POSE in self.distracted_types
-                              or DistractedType.DISTRACTED_BLINK in self.distracted_types) \
-                              and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
+    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
     self.driver_distraction_filter.update(self.driver_distracted)
 
-    # update offseter
-    # only update when driver is actively driving the car above a certain speed
+    # only update offsetter when driver is actively driving the car above a certain speed
     if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted):
-      self.pose.pitch_offseter.push_and_update(self.pose.pitch)
-      self.pose.yaw_offseter.push_and_update(self.pose.yaw)
+      self.pose.pitch_offsetter.push_and_update(self.pose.pitch)
+      self.pose.yaw_offsetter.push_and_update(self.pose.yaw)
 
-    self.pose.calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \
-                           self.pose.yaw_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT
+    self.pose.calibrated = self.pose.pitch_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT and \
+                           self.pose.yaw_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT
 
     if self.face_detected and not self.driver_distracted:
-      if model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD:
-        if not standstill:
+      dcam_uncertain = self.model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD
+      if dcam_uncertain and not standstill:
         self.dcam_uncertain_cnt += 1
         self.dcam_reset_cnt = 0
       else:
@@ -326,28 +288,23 @@ class DriverMonitoring:
         if self.dcam_reset_cnt > self.settings._DCAM_UNCERTAIN_RESET_COUNT:
           self.dcam_uncertain_cnt = 0
 
-    self.is_model_uncertain = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME
-    self._set_timers(self.face_detected and not self.is_model_uncertain)
+    self.is_model_uncertain = self.hi_stds >= self.settings._HI_STD_FALLBACK_TIME
+    self._set_policy(MonitoringPolicy.vision if self.face_detected and not self.is_model_uncertain else MonitoringPolicy.wheeltouch)
     if self.face_detected and not self.pose.low_std and not self.driver_distracted:
       self.hi_stds += 1
     elif self.face_detected and self.pose.low_std:
       self.hi_stds = 0
 
-  def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear, car_speed):
-    self._reset_events()
-    # Block engaging until ignition cycle after max number or time of distractions
+  def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear):
+    self.alert_level = AlertLevel.none
+    self.driver_interacting = driver_engaged
+
     if self.terminal_alert_cnt >= self.settings._MAX_TERMINAL_ALERTS or \
        self.terminal_time >= self.settings._MAX_TERMINAL_DURATION:
-      if not self.too_distracted:
-        self.params.put_bool_nonblocking("DriverTooDistracted", True)
       self.too_distracted = True
 
-    # Always-on distraction lockout is temporary
-    if self.too_distracted or (self.always_on and self.awareness <= self.threshold_prompt):
-      self.current_events.add(EventName.tooDistracted)
-
     always_on_valid = self.always_on and not wrong_gear
-    if (driver_engaged and self.awareness > 0 and not self.active_monitoring_mode) or \
+    if (self.driver_interacting and self.awareness > 0 and self.active_policy == MonitoringPolicy.wheeltouch) or \
        (not always_on_valid and not op_engaged) or \
        (always_on_valid and not op_engaged and self.awareness <= 0):
       # always reset on disengage with normal mode; disengage resets only on red if always on
@@ -355,111 +312,118 @@ class DriverMonitoring:
       return
 
     awareness_prev = self.awareness
-    _reaching_pre = self.awareness - self.step_change <= self.threshold_pre
-    _reaching_terminal = self.awareness - self.step_change <= 0
-    standstill_orange_exemption = standstill and _reaching_pre
-    always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal
+    _reaching_alert_1 = self.awareness - self.step_change <= self.threshold_alert_1
+    _reaching_alert_3 = self.awareness - self.step_change <= 0
+    standstill_exemption = standstill and _reaching_alert_1
+    always_on_exemption = always_on_valid and not op_engaged and _reaching_alert_3
 
     if self.awareness > 0 and \
-       ((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or standstill_orange_exemption):
-      if driver_engaged:
+       ((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or standstill_exemption):
+      if self.driver_interacting:
         self._reset_awareness()
         return
       # only restore awareness when paying attention and alert is not red
-      self.awareness = min(self.awareness + ((self.settings._RECOVERY_FACTOR_MAX-self.settings._RECOVERY_FACTOR_MIN)*
-                                             (1.-self.awareness)+self.settings._RECOVERY_FACTOR_MIN)*self.step_change, 1.)
+      self.awareness = min(self.awareness + ((self.settings._TIMEOUT_RECOVERY_FACTOR_MAX-self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)*
+                                             (1.-self.awareness)+self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)*self.step_change, 1.)
       if self.awareness == 1.:
-        self.awareness_passive = min(self.awareness_passive + self.step_change, 1.)
+        self.last_wheeltouch_awareness = min(self.last_wheeltouch_awareness + self.step_change, 1.)
       # don't display alert banner when awareness is recovering and has cleared orange
-      if self.awareness > self.threshold_prompt:
+      if self.awareness > self.threshold_alert_2:
         return
 
     certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected
-    maybe_distracted = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME or not self.face_detected
+    maybe_distracted = self.is_model_uncertain or not self.face_detected
 
     if certainly_distracted or maybe_distracted:
       # should always be counting if distracted unless at standstill and reaching green
       # also will not be reaching 0 if DM is active when not engaged
-      if not (standstill_orange_exemption or always_on_red_exemption):
+      if not (standstill_exemption or always_on_exemption):
         self.awareness = max(self.awareness - self.step_change, -0.1)
 
-    alert = None
     if self.awareness <= 0.:
-      # terminal red alert: disengagement required
-      alert = EventName.driverDistracted3 if self.active_monitoring_mode else EventName.driverUnresponsive3
+      # terminal alert: disengagement required
+      self.alert_level = AlertLevel.three
       self.terminal_time += 1
       if awareness_prev > 0.:
         self.terminal_alert_cnt += 1
-    elif self.awareness <= self.threshold_prompt:
-      # prompt orange alert
-      alert = EventName.driverDistracted2 if self.active_monitoring_mode else EventName.driverUnresponsive2
-    elif self.awareness <= self.threshold_pre:
-      # pre green alert
-      alert = EventName.driverDistracted1 if self.active_monitoring_mode else EventName.driverUnresponsive1
-
-    if alert is not None:
-      self.current_events.add(alert)
-
-    if self.dcam_uncertain_cnt > self.settings._DCAM_UNCERTAIN_ALERT_COUNT and not self.dcam_uncertain_alerted:
-      set_offroad_alert("Offroad_DriverMonitoringUncertain", True)
-      self.dcam_uncertain_alerted = True
-
+    elif self.awareness <= self.threshold_alert_2:
+      self.alert_level = AlertLevel.two
+    elif self.awareness <= self.threshold_alert_1:
+      self.alert_level = AlertLevel.one
 
   def get_state_packet(self, valid=True):
     # build driverMonitoringState packet
     dat = messaging.new_message('driverMonitoringState', valid=valid)
-    dat.driverMonitoringState = {
-      "events": self.current_events.to_msg(),
-      "faceDetected": self.face_detected,
-      "isDistracted": self.driver_distracted,
-      "distractedType": sum(self.distracted_types),
-      "awarenessStatus": self.awareness,
-      "posePitchOffset": self.pose.pitch_offseter.filtered_stat.mean(),
-      "posePitchValidCount": self.pose.pitch_offseter.filtered_stat.n,
-      "poseYawOffset": self.pose.yaw_offseter.filtered_stat.mean(),
-      "poseYawValidCount": self.pose.yaw_offseter.filtered_stat.n,
-      "stepChange": self.step_change,
-      "awarenessActive": self.awareness_active,
-      "awarenessPassive": self.awareness_passive,
-      "isLowStd": self.pose.low_std,
-      "hiStdCount": self.hi_stds,
-      "isActiveMode": self.active_monitoring_mode,
-      "isRHD": self.wheel_on_right,
-      "uncertainCount": self.dcam_uncertain_cnt,
-    }
+    dm = dat.driverMonitoringState
+
+    dm.lockout = self.too_distracted
+    dm.alertCountLockoutPercent = to_percent(self.terminal_alert_cnt / self.settings._MAX_TERMINAL_ALERTS)
+    dm.alertTimeLockoutPercent = to_percent(self.terminal_time / self.settings._MAX_TERMINAL_DURATION)
+    dm.alwaysOn = self.always_on
+    dm.alwaysOnLockout = self.always_on and self.awareness <= self.threshold_alert_2
+    dm.alertLevel = self.alert_level
+    dm.activePolicy = self.active_policy
+    dm.isRHD = self.wheel_on_right
+    dm.rhdCalibration.calibratedPercent = to_percent(self.wheelpos_offsetter.filtered_stat.n / self.settings._WHEELPOS_FILTER_MIN_COUNT)
+    dm.rhdCalibration.offset = self.wheelpos_offsetter.filtered_stat.M
+
+    dm.visionPolicyState.awarenessPercent = to_percent(self.last_vision_awareness if self.active_policy != MonitoringPolicy.vision else self.awareness)
+    dm.visionPolicyState.awarenessStep = self.step_change if self.active_policy == MonitoringPolicy.vision else 0.
+    dm.visionPolicyState.isDistracted = self.driver_distracted
+    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.faceDetected = self.face_detected
+    dm.visionPolicyState.pose.pitch = self.pose.pitch
+    dm.visionPolicyState.pose.yaw = self.pose.yaw
+    dm.visionPolicyState.pose.calibrated = self.pose.calibrated
+    dm.visionPolicyState.pose.pitchCalib.calibratedPercent = to_percent(self.pose.pitch_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT)
+    dm.visionPolicyState.pose.pitchCalib.offset = self.pose.pitch_offsetter.filtered_stat.M
+    dm.visionPolicyState.pose.yawCalib.calibratedPercent = to_percent(self.pose.yaw_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT)
+    dm.visionPolicyState.pose.yawCalib.offset = self.pose.yaw_offsetter.filtered_stat.M
+    dm.visionPolicyState.pose.uncertainty = self.model_std_max
+    dm.visionPolicyState.wheeltouchFallbackPercent = to_percent(self.hi_stds / self.settings._HI_STD_FALLBACK_TIME)
+    dm.visionPolicyState.uncertainOffroadAlertPercent = to_percent(self.dcam_uncertain_cnt / self.settings._DCAM_UNCERTAIN_ALERT_COUNT)
+
+    dm.wheeltouchPolicyState.awarenessPercent = to_percent(self.last_wheeltouch_awareness if self.active_policy == MonitoringPolicy.vision else self.awareness)
+    dm.wheeltouchPolicyState.awarenessStep = 0. if self.active_policy == MonitoringPolicy.vision else self.step_change
+    dm.wheeltouchPolicyState.driverInteracting = self.driver_interacting
     return dat
 
   def run_step(self, sm, demo=False):
     if demo:
-      highway_speed = 30
+      car_speed = 30
       enabled = True
       wrong_gear = False
       standstill = False
       driver_engaged = False
       brake_disengage_prob = 1.0
+      steering_angle_deg = 0.0
       rpyCalib = [0., 0., 0.]
     else:
-      highway_speed = sm['carState'].vEgo
+      car_speed = sm['carState'].vEgo
       enabled = sm['selfdriveState'].enabled or sm['carControl'].latActive
       wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low)
       standstill = sm['carState'].standstill
       driver_engaged = sm['carState'].steeringPressed or (sm['selfdriveState'].enabled and sm['carState'].gasPressed)
       brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
+      steering_angle_deg = sm['carState'].steeringAngleDeg
       rpyCalib = sm['liveCalibration'].rpyCalib
-    self._set_policy(
+
+    self._set_pose_strictness(
       brake_disengage_prob=brake_disengage_prob,
-      car_speed=highway_speed,
+      car_speed=car_speed,
     )
 
     # Parse data from dmonitoringmodeld
     self._update_states(
       driver_state=sm['driverStateV2'],
       cal_rpy=rpyCalib,
-      car_speed=highway_speed,
+      car_speed=car_speed,
       op_engaged=enabled,
       standstill=standstill,
       demo_mode=demo,
-      steering_angle_deg=sm['carState'].steeringAngleDeg,
+      steering_angle_deg=steering_angle_deg,
     )
 
     # Update distraction events
@@ -468,5 +432,4 @@ class DriverMonitoring:
       op_engaged=enabled,
       standstill=standstill,
       wrong_gear=wrong_gear,
-      car_speed=highway_speed
     )

selfdrive/monitoring/test_monitoring.py

@@ -3,17 +3,16 @@ import pytest
 
 from cereal import log, car
 from openpilot.common.realtime import DT_DMON
-from openpilot.selfdrive.monitoring.helpers import DriverMonitoring, DRIVER_MONITOR_SETTINGS
-from openpilot.system.hardware import HARDWARE
+from openpilot.selfdrive.monitoring.policy import DriverMonitoring, DRIVER_MONITOR_SETTINGS
 
 EventName = log.OnroadEvent.EventName
-dm_settings = DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type())
+dm_settings = DRIVER_MONITOR_SETTINGS()
 
 TEST_TIMESPAN = 120  # seconds
-DISTRACTED_SECONDS_TO_ORANGE = dm_settings._DISTRACTED_TIME - dm_settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1
-DISTRACTED_SECONDS_TO_RED = dm_settings._DISTRACTED_TIME + 1
-INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1
-INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 1
+DISTRACTED_SECONDS_TO_ORANGE = dm_settings._VISION_POLICY_ALERT_2_TIMEOUT + 1
+DISTRACTED_SECONDS_TO_RED = dm_settings._VISION_POLICY_ALERT_3_TIMEOUT + 1
+INVISIBLE_SECONDS_TO_ORANGE = dm_settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + 1
+INVISIBLE_SECONDS_TO_RED = dm_settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + 1
 
 def make_msg(face_detected, distracted=False, model_uncertain=False):
   ds = log.DriverStateV2.new_message()
@@ -37,7 +36,7 @@ msg_ATTENTIVE = make_msg(True)
 msg_DISTRACTED = make_msg(True, distracted=True)
 msg_ATTENTIVE_UNCERTAIN = make_msg(True, model_uncertain=True)
 msg_DISTRACTED_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=True)
-msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._POSESTD_THRESHOLD*1.5)
+msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._HI_STD_THRESHOLD*1.5)
 
 # driver interaction with car
 car_interaction_DETECTED = True
@@ -53,49 +52,49 @@ always_false = [False] * int(TEST_TIMESPAN / DT_DMON)
 class TestMonitoring:
   def _run_seq(self, msgs, interaction, engaged, standstill):
     DM = DriverMonitoring()
-    events = []
+    alert_lvls = []
     for idx in range(len(msgs)):
       DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], standstill[idx])
       # cal_rpy and car_speed don't matter here
 
       # evaluate events at 10Hz for tests
-      DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0, 0)
-      events.append(DM.current_events)
-    assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs"
-    return events, DM
+      DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0)
+      alert_lvls.append(DM.alert_level)
+    assert len(alert_lvls) == len(msgs), f"got {len(alert_lvls)} for {len(msgs)} driverState input msgs"
+    return alert_lvls, DM
 
-  def _assert_no_events(self, events):
-    assert all(not len(e) for e in events)
 
   # engaged, driver is attentive all the time
   def test_fully_aware_driver(self):
-    events, _ = self._run_seq(always_attentive, always_false, always_true, always_false)
-    self._assert_no_events(events)
+    alert_lvls, d_status = self._run_seq(always_attentive, always_false, always_true, always_false)
+    assert all(a == 0 for a in alert_lvls)
+    assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.vision
 
   # engaged, driver is distracted and does nothing
   def test_fully_distracted_driver(self):
-    events, d_status = self._run_seq(always_distracted, always_false, always_true, always_false)
-    assert len(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0
-    assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL + \
-                    ((d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \
-                    EventName.driverDistracted1
-    assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + \
-                    ((d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.driverDistracted2
-    assert events[int((d_status.settings._DISTRACTED_TIME + \
-                    ((TEST_TIMESPAN-10-d_status.settings._DISTRACTED_TIME)/2))/DT_DMON)].names[0] == EventName.driverDistracted3
+    alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false)
+    s = d_status.settings
+    assert alert_lvls[int(s._VISION_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0
+    assert alert_lvls[int((s._VISION_POLICY_ALERT_1_TIMEOUT + \
+                    (s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1
+    assert alert_lvls[int((s._VISION_POLICY_ALERT_2_TIMEOUT + \
+                    (s._VISION_POLICY_ALERT_3_TIMEOUT - s._VISION_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2
+    assert alert_lvls[int((s._VISION_POLICY_ALERT_3_TIMEOUT + \
+                    (TEST_TIMESPAN - 10 - s._VISION_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3
     assert isinstance(d_status.awareness, float)
 
   # engaged, no face detected the whole time, no action
   def test_fully_invisible_driver(self):
-    events, d_status = self._run_seq(always_no_face, always_false, always_true, always_false)
-    assert len(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0
-    assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL + \
-                      ((d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \
-                      EventName.driverUnresponsive1
-    assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + \
-                      ((d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive2
-    assert events[int((d_status.settings._AWARENESS_TIME + \
-                      ((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive3
+    alert_lvls, d_status = self._run_seq(always_no_face, always_false, always_true, always_false)
+    s = d_status.settings
+    assert alert_lvls[int(s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0
+    assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT + \
+                    (s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1
+    assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + \
+                    (s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2
+    assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + \
+                    (TEST_TIMESPAN - 10 - s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3
+    assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.wheeltouch
 
   # engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel
   #  - should have short orange recovery time and no green afterwards; wheel touch only recovers when paying attention
@@ -106,13 +105,13 @@ class TestMonitoring:
                 [msg_ATTENTIVE] * (int(TEST_TIMESPAN/DT_DMON)-int((DISTRACTED_SECONDS_TO_ORANGE*3+2)/DT_DMON))
     interaction_vector = [car_interaction_NOT_DETECTED] * int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \
                          [car_interaction_DETECTED] * (int(TEST_TIMESPAN/DT_DMON)-int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON))
-    events, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
-    assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0
-    assert events[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.driverDistracted2
-    assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)]) == 0
-    assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)].names[0] == EventName.driverDistracted2
-    assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)].names[0] == EventName.driverDistracted2
-    assert len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)]) == 0
+    alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
+    assert alert_lvls[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0
+    assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
+    assert alert_lvls[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)] == 0
+    assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)] == 2
+    assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)] == 2
+    assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)] == 0
 
   # engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \
   #                          driver dodges, and then touches wheel to no avail, disengages and reengages
@@ -130,11 +129,11 @@ class TestMonitoring:
                                                         = [True] * int(1/DT_DMON)
     op_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] \
                                                         = [False] * int(0.5/DT_DMON)
-    events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false)
-    assert events[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted2
-    assert events[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted3
-    assert events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0] == EventName.driverDistracted3
-    assert len(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)]) == 0
+    alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false)
+    assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)] == 2
+    assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)] == 3
+    assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] == 3
+    assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)] == 0
 
   # engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears
   #  - both actions should clear the alert, but momentary appearance should not
@@ -145,16 +144,16 @@ class TestMonitoring:
     ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \
                                                                                              [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
     interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON)
-    events, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
-    assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0
-    assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive2
-    assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0
+    alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
+    assert alert_lvls[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)] == 0
     if _visible_time == 0.5:
-      assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive2
-      assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive1
+      assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2
+      assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 1
     elif _visible_time == 10:
-      assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive2
-      assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0
+      assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2
+      assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 0
 
   # engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
   #  - only disengage will clear the alert
@@ -166,19 +165,19 @@ class TestMonitoring:
     ds_vector[int(INVISIBLE_SECONDS_TO_RED/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
     interaction_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON)
     op_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON)
-    events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false)
-    assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0
-    assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive2
-    assert events[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive3
-    assert events[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive3
-    assert events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0] == EventName.driverUnresponsive3
-    assert len(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)]) == 0
+    alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false)
+    assert alert_lvls[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)] == 3
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)] == 3
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] == 3
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)] == 0
 
   # disengaged, always distracted driver
   #  - dm should stay quiet when not engaged
   def test_pure_dashcam_user(self):
-    events, _ = self._run_seq(always_distracted, always_false, always_false, always_false)
-    assert sum(len(event) for event in events) == 0
+    alert_lvls, _ = self._run_seq(always_distracted, always_false, always_false, always_false)
+    assert all(a == 0 for a in alert_lvls)
 
   # engaged, car stops at traffic light, down to orange, no action, then car starts moving
   #  - should only reach green when stopped, but continues counting down on launch
@@ -186,11 +185,12 @@ class TestMonitoring:
     _redlight_time = 60  # seconds
     standstill_vector = always_true[:]
     standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON)
-    events, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector)
-    assert len(events[int((_redlight_time-0.1)/DT_DMON)]) == 0
-    _pre_to_prompt = d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL - d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL
-    assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.driverDistracted1
-    assert events[int((_redlight_time+_pre_to_prompt+0.5)/DT_DMON)].names[0] == EventName.driverDistracted2
+    alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector)
+    s = d_status.settings
+    assert alert_lvls[int((_redlight_time-0.1)/DT_DMON)] == 0
+    _alert_1_to_2 = s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT
+    assert alert_lvls[int((_redlight_time+0.5)/DT_DMON)] == 1
+    assert alert_lvls[int((_redlight_time+_alert_1_to_2+0.5)/DT_DMON)] == 2
 
   # engaged, distracted while moving, then car stops after reaching orange
   #  - should reset timer to pre green at standstill
@@ -198,23 +198,21 @@ class TestMonitoring:
     _stop_time = DISTRACTED_SECONDS_TO_ORANGE + 1  # stop 1 second after reaching orange
     standstill_vector = always_false[:]
     standstill_vector[int(_stop_time/DT_DMON):] = [True] * int((TEST_TIMESPAN-_stop_time)/DT_DMON)
-    events, _ = self._run_seq(always_distracted, always_false, always_true, standstill_vector)
+    alert_lvls, _ = self._run_seq(always_distracted, always_false, always_true, standstill_vector)
     # just before and briefly after stopping: orange alert; goes away quickly after stopped
-    assert events[int((_stop_time+0.1)/DT_DMON)].names[0] == EventName.driverDistracted2
-    assert len(events[int((_stop_time+0.5)/DT_DMON)]) == 0
+    assert alert_lvls[int((_stop_time+0.1)/DT_DMON)] == 2
+    assert alert_lvls[int((_stop_time+0.5)/DT_DMON)] == 0
 
   # engaged, model is somehow uncertain and driver is distracted
   #  - should fall back to wheel touch after uncertain alert
   def test_somehow_indecisive_model(self):
     ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(TEST_TIMESPAN/DT_DMON)
     interaction_vector = always_false[:]
-    events, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
-    assert EventName.driverUnresponsive1 in \
-                              events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names
-    assert EventName.driverUnresponsive2 in \
-                              events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names
-    assert EventName.driverUnresponsive3 in \
-                              events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names
+    alert_lvls, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
+    s = d_status.settings
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)] == 1
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 2
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 3
 
 
 def _build_sm(selfdrive_enabled, lat_active, steering_pressed, gas_pressed):
@@ -253,10 +251,10 @@ def test_run_step_engagement(selfdrive_enabled, lat_active, steering, gas,
   captured = {}
   orig = dm._update_events
 
-  def spy(driver_engaged, op_engaged, standstill, wrong_gear, car_speed):
+  def spy(driver_engaged, op_engaged, standstill, wrong_gear):
     captured['driver_engaged'] = driver_engaged
     captured['op_engaged'] = op_engaged
-    return orig(driver_engaged, op_engaged, standstill, wrong_gear, car_speed)
+    return orig(driver_engaged, op_engaged, standstill, wrong_gear)
 
   dm._update_events = spy
   dm.run_step(sm, demo=False)

selfdrive/pandad/pandad.cc

@@ -1,6 +1,7 @@
 #include "selfdrive/pandad/pandad.h"
 
 #include <array>
+#include <atomic>
 #include <bitset>
 #include <cassert>
 #include <cerrno>
@@ -23,6 +24,14 @@
 
 ExitHandler do_exit;
 
+struct HwmonState {
+  std::atomic<uint32_t> voltage{0};
+  std::atomic<uint32_t> current{0};
+  std::atomic<bool> initialized{false};
+};
+
+HwmonState hwmon_state;
+
 bool check_connected(Panda *panda) {
   if (!panda->connected()) {
     do_exit = true;
@@ -117,6 +126,26 @@ void can_recv(Panda *panda, PubMaster *pm) {
   }
 }
 
+void hwmon_thread() {
+  util::set_thread_name("pandad_hwmon");
+
+  while (!do_exit) {
+    double read_time = millis_since_boot();
+    uint32_t voltage = Hardware::get_voltage();
+    uint32_t current = Hardware::get_current();
+    read_time = millis_since_boot() - read_time;
+    if (read_time > 50) {
+      LOGW("reading hwmon took %lfms", read_time);
+    }
+
+    hwmon_state.voltage.store(voltage);
+    hwmon_state.current.store(current);
+    hwmon_state.initialized.store(true);
+
+    util::sleep_for(500);
+  }
+}
+
 void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::PandaType hw_type, const health_t &health) {
   ps.setVoltage(health.voltage_pkt);
   ps.setCurrent(health.current_pkt);
@@ -249,6 +278,10 @@ std::optional<bool> send_panda_states(PubMaster *pm, Panda *panda, bool is_onroa
 }
 
 void send_peripheral_state(Panda *panda, PubMaster *pm) {
+  if (!hwmon_state.initialized.load()) {
+    return;
+  }
+
   // build msg
   MessageBuilder msg;
   auto evt = msg.initEvent();
@@ -257,13 +290,8 @@ void send_peripheral_state(Panda *panda, PubMaster *pm) {
   auto ps = evt.initPeripheralState();
   ps.setPandaType(panda->hw_type);
 
-  double read_time = millis_since_boot();
-  ps.setVoltage(Hardware::get_voltage());
-  ps.setCurrent(Hardware::get_current());
-  read_time = millis_since_boot() - read_time;
-  if (read_time > 50) {
-    LOGW("reading hwmon took %lfms", read_time);
-  }
+  ps.setVoltage(hwmon_state.voltage.load());
+  ps.setCurrent(hwmon_state.current.load());
 
   // fall back to panda's voltage and current measurement
   if (ps.getVoltage() == 0 && ps.getCurrent() == 0) {
@@ -385,12 +413,12 @@ void pandad_run(Panda *panda) {
   const bool spoofing_started = getenv("STARTED") != nullptr;
   const bool fake_send = getenv("FAKESEND") != nullptr;
 
-  // Start the CAN send thread
+  // Start helper threads for event-driven sendcan and slow non-Panda reads.
   std::thread send_thread(can_send_thread, panda, fake_send);
+  std::thread hardware_thread(hwmon_thread);
 
-  Params params;
   RateKeeper rk("pandad", 100);
-  SubMaster sm({"selfdriveState", "selfdriveStateSP"});
+  SubMaster sm({"selfdriveState", "deviceState", "selfdriveStateSP"});
   PubMaster pm({"can", "pandaStates", "peripheralState"});
   PandaSafety panda_safety(panda);
   bool engaged = false;
@@ -398,7 +426,7 @@ void pandad_run(Panda *panda) {
   bool is_onroad = false;
   bool always_offroad = false;
 
-  // Main loop: receive CAN data and process states
+  // Main loop: receive CAN first, then process lower priority panda and peripheral state.
   while (!do_exit && check_connected(panda)) {
     can_recv(panda, &pm);
 
@@ -411,8 +439,10 @@ void pandad_run(Panda *panda) {
     if (rk.frame() % 10 == 0) {
       sm.update(0);
       engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled();
+      if (sm.updated("deviceState")) {
+        is_onroad = sm["deviceState"].getDeviceState().getStarted();
+      }
       engaged_mads = process_mads_heartbeat(&sm);
-      is_onroad = params.getBool("IsOnroad");
       always_offroad = panda_safety.getOffroadMode();
       process_panda_state(panda, &pm, engaged, engaged_mads, is_onroad, spoofing_started, always_offroad);
       panda_safety.configureSafetyMode(is_onroad);
@@ -445,6 +475,7 @@ void pandad_run(Panda *panda) {
   }
 
   send_thread.join();
+  hardware_thread.join();
 }
 
 void pandad_main_thread(std::string serial) {

selfdrive/pandad/pandad.py

@@ -16,25 +16,17 @@ from openpilot.sunnypilot.selfdrive.pandad.rivian_long_flasher import flash_rivi
 
 
 def get_expected_signature() -> bytes:
-  try:
   fn = os.path.join(FW_PATH, McuType.H7.config.app_fn)
   return Panda.get_signature_from_firmware(fn)
-  except Exception:
-    cloudlog.exception("Error computing expected signature")
-    return b""
 
-def flash_panda(panda_serial: str) -> Panda:
-  try:
+def flash_panda(panda_serial: str):
   panda = Panda(panda_serial)
-  except PandaProtocolMismatch:
-    cloudlog.warning("detected protocol mismatch, reflashing panda")
-    HARDWARE.recover_internal_panda()
-    raise
 
   # skip flashing if the detected panda is not supported
   if panda.get_type() not in Panda.SUPPORTED_DEVICES:
     cloudlog.warning(f"Panda {panda_serial} is not supported (hw_type: {panda.get_type()}), skipping flash...")
-    return panda
+    panda.close()
+    return
 
   fw_signature = get_expected_signature()
   internal_panda = panda.is_internal()
@@ -65,7 +57,7 @@ def flash_panda(panda_serial: str) -> Panda:
     cloudlog.info("Version mismatch after flashing, exiting")
     raise AssertionError
 
-  return panda
+  panda.close()
 
 
 def check_panda_support(panda_serials: list[str]) -> list[str]:
@@ -97,97 +89,56 @@ def main() -> None:
   do_exit = False
   signal.signal(signal.SIGINT, signal_handler)
 
-  count = 0
-  first_run = True
-  params = Params()
-  no_internal_panda_count = 0
+  # check health for lost heartbeat
+  try:
+    for s in Panda.list():
+      with Panda(s) as p:
+        health = p.health()
+        if p.is_internal() and health["heartbeat_lost"]:
+          Params().put_bool("PandaHeartbeatLost", True, block=True)
+          cloudlog.event("heartbeat lost", deviceState=health)
+  except Exception:
+    cloudlog.exception("pandad.uncaught_exception")
 
+  count = 0
   while not do_exit:
     try:
-      count += 1
       cloudlog.event("pandad.flash_and_connect", count=count)
-      params.remove("PandaSignatures")
-
-      # Handle missing internal panda
-      if no_internal_panda_count > 0:
-        if no_internal_panda_count == 3:
-          cloudlog.info("No pandas found, putting internal panda into DFU")
-          HARDWARE.recover_internal_panda()
-        else:
-          cloudlog.info("No pandas found, resetting internal panda")
+      if (count % 2) == 0:
         HARDWARE.reset_internal_panda()
-        time.sleep(3)  # wait to come back up
+      else:
+        HARDWARE.recover_internal_panda()
+      count += 1
 
       # Flash all Pandas in DFU mode
-      dfu_serials = PandaDFU.list()
-      if len(dfu_serials) > 0:
-        for serial in dfu_serials:
+      for serial in PandaDFU.list():
         cloudlog.info(f"Panda in DFU mode found, flashing recovery {serial}")
         PandaDFU(serial).recover()
         time.sleep(1)
 
       panda_serials = Panda.list()
-      if len(panda_serials) == 0:
-        no_internal_panda_count += 1
-        continue
-
-      cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}")
-
+      if len(panda_serials):
         # custom flasher for xnor's Rivian Longitudinal Upgrade Kit
         flash_rivian_long(panda_serials)
-
         # find the internal supported panda (e.g. skip external Black Panda)
         panda_serials = check_panda_support(panda_serials)
-      if len(panda_serials) == 0:
-        continue
 
-      # Flash the first panda
-      panda_serial = panda_serials[0]
-      panda = flash_panda(panda_serial)
+        assert len(panda_serials) == 1
+        cloudlog.info(f"{len(panda_serials)} panda found, connecting - {panda_serials}")
+        flash_panda(panda_serials[0])
 
-      # Ensure internal panda is present if expected
-      if HARDWARE.has_internal_panda() and not panda.is_internal():
-        cloudlog.error("Internal panda is missing, trying again")
-        no_internal_panda_count += 1
-        continue
-      no_internal_panda_count = 0
-
-      # log panda fw version
-      params.put("PandaSignatures", panda.get_signature())
-
-      # check health for lost heartbeat
-      health = panda.health()
-      if health["heartbeat_lost"]:
-        params.put_bool("PandaHeartbeatLost", True)
-        cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial())
-      if health["som_reset_triggered"]:
-        params.put_bool("PandaSomResetTriggered", True)
-        cloudlog.event("panda.som_reset_triggered", health=health, serial=panda.get_usb_serial())
-
-      if first_run:
-        # reset panda to ensure we're in a good state
-        cloudlog.info(f"Resetting panda {panda.get_usb_serial()}")
-        panda.reset(reconnect=True)
-
-      panda.close()
+        # run real pandad
+        os.environ['MANAGER_DAEMON'] = 'pandad'
+        process = subprocess.Popen(["./pandad"], cwd=os.path.join(BASEDIR, "selfdrive/pandad"))
+        process.wait()
     # TODO: wrap all panda exceptions in a base panda exception
     except (usb1.USBErrorNoDevice, usb1.USBErrorPipe):
       # a panda was disconnected while setting everything up. let's try again
       cloudlog.exception("Panda USB exception while setting up")
-      continue
     except PandaProtocolMismatch:
       cloudlog.exception("pandad.protocol_mismatch")
-      continue
     except Exception:
       cloudlog.exception("pandad.uncaught_exception")
-      continue
-
-    first_run = False
-
-    # run pandad with all connected serials as arguments
-    os.environ['MANAGER_DAEMON'] = 'pandad'
-    process = subprocess.Popen(["./pandad", panda_serial], cwd=os.path.join(BASEDIR, "selfdrive/pandad"))
-    process.wait()
 
 
 if __name__ == "__main__":

selfdrive/pandad/tests/test_pandad.py

@@ -15,12 +15,6 @@ HERE = os.path.dirname(os.path.realpath(__file__))
 
 @pytest.mark.tici
 class TestPandad:
-
-  def setup_method(self):
-    # ensure panda is up
-    if len(Panda.list()) == 0:
-      self._run_test(60)
-
   def teardown_method(self):
     managed_processes['pandad'].stop()
 
@@ -30,7 +24,7 @@ class TestPandad:
 
     managed_processes['pandad'].start()
     while (time.monotonic() - st) < timeout:
-      sm.update(100)
+      sm.update(10)
       if len(sm['pandaStates']) and sm['pandaStates'][0].pandaType != log.PandaState.PandaType.unknown:
         break
     dt = time.monotonic() - st
@@ -45,10 +39,6 @@ class TestPandad:
     HARDWARE.recover_internal_panda()
     assert Panda.wait_for_dfu(None, 10)
 
-  def _assert_no_panda(self):
-    assert not Panda.wait_for_dfu(None, 3)
-    assert not Panda.wait_for_panda(None, 3)
-
   def _flash_bootstub(self, fn):
     self._go_to_dfu()
     pd = PandaDFU(None)
@@ -56,12 +46,11 @@ class TestPandad:
       fn = os.path.join(HERE, pd.get_mcu_type().config.bootstub_fn)
     with open(fn, "rb") as f:
       pd.program_bootstub(f.read())
-    pd.reset()
     HARDWARE.reset_internal_panda()
 
   def test_in_dfu(self):
     HARDWARE.recover_internal_panda()
-    self._run_test(60)
+    self._run_test()
 
   def test_in_bootstub(self):
     with Panda() as p:
@@ -69,30 +58,24 @@ class TestPandad:
       assert p.bootstub
     self._run_test()
 
-  def test_internal_panda_reset(self):
+  def test_in_reset(self):
     gpio_init(GPIO.STM_RST_N, True)
     gpio_set(GPIO.STM_RST_N, 1)
-    time.sleep(0.5)
-    assert all(not Panda(s).is_internal() for s in Panda.list())
+    assert not Panda.list()
     self._run_test()
 
-    assert any(Panda(s).is_internal() for s in Panda.list())
-
-  def test_old_spi_protocol(self):
-    # flash firmware with old SPI protocol
-    self._flash_bootstub(os.path.join(HERE, "bootstub.panda_h7_spiv0.bin"))
-    self._run_test(45)
-
   def test_release_to_devel_bootstub(self):
+    st = time.monotonic()
     self._flash_bootstub(None)
-    self._run_test(45)
+    print("flash done", time.monotonic() - st)
+    self._run_test()
 
   def test_recover_from_bad_bootstub(self):
     self._go_to_dfu()
     with PandaDFU(None) as pd:
-      pd.program_bootstub(b"\x00"*1024)
-      pd.reset()
+      pd._handle.program(pd.get_mcu_type().config.bootstub_address, b"\x00"*100)
     HARDWARE.reset_internal_panda()
-    self._assert_no_panda()
+    assert not Panda.list()
+    assert not PandaDFU.list()
 
-    self._run_test(60)
+    self._run_test()

selfdrive/pandad/tests/test_pandad_loopback.py

@@ -16,17 +16,24 @@ from openpilot.selfdrive.pandad import can_list_to_can_capnp
 from openpilot.selfdrive.test.helpers import with_processes
 
 
+def publish_device_state(pm, started):
+  msg = messaging.new_message('deviceState')
+  msg.deviceState.started = started
+  pm.send('deviceState', msg)
+
 @retry(attempts=3)
 def setup_pandad():
   params = Params()
   params.clear_all()
-  params.put_bool("IsOnroad", False)
 
+  pm = messaging.PubMaster(['deviceState'])
   sm = messaging.SubMaster(['pandaStates'])
+  publish_device_state(pm, False)
   with Timeout(90, "pandad didn't start"):
     while sm.recv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \
         any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']):
-      sm.update(1000)
+      publish_device_state(pm, False)
+      sm.update(100)
 
   # pandad safety setting relies on these params
   cp = car.CarParams.new_message()
@@ -35,14 +42,15 @@ def setup_pandad():
   safety_config.safetyModel = car.CarParams.SafetyModel.allOutput
   cp.safetyConfigs = [safety_config]
 
-  params.put_bool("IsOnroad", True)
-  params.put_bool("FirmwareQueryDone", True)
-  params.put_bool("ControlsReady", True)
-  params.put("CarParams", cp.to_bytes())
+  params.put_bool("FirmwareQueryDone", True, block=True)
+  params.put_bool("ControlsReady", True, block=True)
+  params.put("CarParams", cp.to_bytes(), block=True)
 
+  publish_device_state(pm, True)
   with Timeout(90, "pandad didn't set safety mode"):
     while any(ps.safetyModel != car.CarParams.SafetyModel.allOutput for ps in sm['pandaStates']):
-      sm.update(1000)
+      publish_device_state(pm, True)
+      sm.update(100)
 
 def send_random_can_messages(sendcan, count):
   sent_msgs = defaultdict(set)

selfdrive/selfdrived/alertmanager.py

@@ -18,7 +18,7 @@ def set_offroad_alert(alert: str, show_alert: bool, extra_text: str | None = Non
   if show_alert:
     a = copy.copy(OFFROAD_ALERTS[alert])
     a['extra'] = extra_text or ''
-    Params().put(alert, a)
+    Params().put(alert, a, block=True)
   else:
     Params().remove(alert)
 

selfdrive/selfdrived/selfdrived.py

@@ -45,6 +45,8 @@ LaneChangeDirection = log.LaneChangeDirection
 EventName = log.OnroadEvent.EventName
 ButtonType = car.CarState.ButtonEvent.Type
 SafetyModel = car.CarParams.SafetyModel
+AlertLevel = log.DriverMonitoringState.AlertLevel
+MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
 TurnDirection = custom.ModelDataV2SP.TurnDirection
 
 IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
@@ -140,6 +142,8 @@ class SelfdriveD(CruiseHelper):
       self.params
     )
     self.recalibrating_seen = False
+    self.dm_lockout_set = False
+    self.dm_uncertain_alerted = False
     self.state_machine = StateMachine()
     self.rk = Ratekeeper(100, print_delay_threshold=None)
 
@@ -216,8 +220,27 @@ class SelfdriveD(CruiseHelper):
     if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed:
       self.events.add(EventName.resumeBlocked)
 
+    # Handle DM
     if not self.CP.notCar:
-      self.events.add_from_msg(self.sm['driverMonitoringState'].events)
+      # Block engaging until ignition cycle after max number or time of distractions
+      if self.sm['driverMonitoringState'].lockout and not self.dm_lockout_set:
+        self.params.put_bool("DriverTooDistracted", True)
+        self.dm_lockout_set = True
+      # No entry conditions
+      if self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout:
+        self.events.add(EventName.tooDistracted)
+      # Alerts
+      vision_dm = self.sm['driverMonitoringState'].activePolicy == MonitoringPolicy.vision
+      if self.sm['driverMonitoringState'].alertLevel == AlertLevel.one:
+        self.events.add(EventName.driverDistracted1 if vision_dm else EventName.driverUnresponsive1)
+      elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.two:
+        self.events.add(EventName.driverDistracted2 if vision_dm else EventName.driverUnresponsive2)
+      elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.three:
+        self.events.add(EventName.driverDistracted3 if vision_dm else EventName.driverUnresponsive3)
+      # Warn consistent DM uncertainty
+      if self.sm['driverMonitoringState'].visionPolicyState.uncertainOffroadAlertPercent >= 100 and not self.dm_uncertain_alerted:
+        set_offroad_alert("Offroad_DriverMonitoringUncertain", True)
+        self.dm_uncertain_alerted = True
       self.events_sp.add_from_msg(self.sm['longitudinalPlanSP'].events)
 
     # Add car events, ignore if CAN isn't valid
@@ -241,7 +264,7 @@ class SelfdriveD(CruiseHelper):
         self.events.add(EventName.pedalPressed)
 
     # Create events for temperature, disk space, and memory
-    if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
+    if self.sm['deviceState'].thermalStatus >= ThermalStatus.overheated:
       self.events.add(EventName.overheat)
     if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
       self.events.add(EventName.outOfSpace)
@@ -440,7 +463,7 @@ class SelfdriveD(CruiseHelper):
 
     # TODO: fix simulator
     if not SIMULATION or REPLAY:
-      if self.sm['modelV2'].frameDropPerc > 20:
+      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
@@ -454,7 +477,7 @@ class SelfdriveD(CruiseHelper):
       if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
         if not self.experimental_mode_switched:
           self.personality = (self.personality - 1) % 3
-          self.params.put_nonblocking('LongitudinalPersonality', self.personality)
+          self.params.put('LongitudinalPersonality', self.personality)
           self.events.add(EventName.personalityChanged)
         self.experimental_mode_switched = False
 

selfdrive/selfdrived/tests/test_alerts.py

@@ -63,7 +63,7 @@ class TestAlerts:
 
     for alert in ALERTS:
       if not isinstance(alert, Alert):
-        alert = alert(self.CP, self.CS, self.sm, metric=False, soft_disable_time=100, personality=log.LongitudinalPersonality.standard)
+        alert = alert(self.CP, self.CS, self.sm, False, 100, log.LongitudinalPersonality.standard)
 
       # for full size alerts, both text fields wrap the text,
       # so it's unlikely that they  would go past the max width

selfdrive/test/helpers.py

@@ -18,15 +18,15 @@ def set_params_enabled():
   os.environ['LOGPRINT'] = "debug"
 
   params = Params()
-  params.put("HasAcceptedTerms", terms_version)
-  params.put("CompletedTrainingVersion", training_version)
-  params.put_bool("OpenpilotEnabledToggle", True)
+  params.put("HasAcceptedTerms", terms_version, block=True)
+  params.put("CompletedTrainingVersion", training_version, block=True)
+  params.put_bool("OpenpilotEnabledToggle", True, block=True)
 
   # valid calib
   msg = messaging.new_message('liveCalibration')
   msg.liveCalibration.validBlocks = 20
   msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0]
-  params.put("CalibrationParams", msg.to_bytes())
+  params.put("CalibrationParams", msg.to_bytes(), block=True)
 
 def release_only(f):
   @wraps(f)

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -144,6 +144,8 @@ class Plant:
           'gpsLocation': gps_data.gpsLocation}
     self.planner.update(sm)
     self.acceleration = self.planner.output_a_target
+    if self.planner.output_should_stop:
+      self.acceleration = min(-0.5, self.acceleration)
     self.speed = self.speed + self.acceleration * self.ts
     self.should_stop = self.planner.output_should_stop
     fcw = self.planner.fcw