try this one

This reverts commit d1e069210f.

.github/ISSUE_TEMPLATE/enhancement.md

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

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

@@ -172,8 +172,8 @@ jobs:
             output_file="${{ env.MODELS_DIR }}/${base_name}_tinygrad.pkl"
 
             echo "Compiling: $onnx_file -> $output_file"
-            QCOM=1 python3 "${{ env.TINYGRAD_PATH }}/examples/openpilot/compile3.py" "$onnx_file" "$output_file"
+            DEV=QCOM FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1 IMAGE=2 python3 "${{ env.TINYGRAD_PATH }}/examples/openpilot/compile3.py" "$onnx_file" "$output_file"
-            DEV=QCOM FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 python3 "${{ env.MODELS_DIR }}/../get_model_metadata.py" "$onnx_file" || true
+            DEV=QCOM FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1 python3 "${{ env.MODELS_DIR }}/../get_model_metadata.py" "$onnx_file" || true
           done
 
       - name: Validate Model Outputs

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,7 +292,10 @@ struct SensorEventData {
 
   struct SensorVec {
     v @0 :List(Float32);
-    status @1 :Int8;
+
+    deprecated :group {
+      status @1 :Int8;
+    }
   }
 
   enum SensorSource {
@@ -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;
+    ok @0;
-    yellow @1;
+    warmDEPRECATED @1;
-    red @2;
+    overheated @2;
-    danger @3;
+    critical @3;
   }
 
   enum NetworkType {
@@ -2076,7 +2079,7 @@ struct DriverStateV2 {
   }
 }
 
-struct DriverMonitoringState @0xb83cda094a1da284 {
+struct DriverMonitoringStateDEPRECATED @0xb83cda094a1da284 {
   events @18 :List(OnroadEvent);
   faceDetected @1 :Bool;
   isDistracted @2 :Bool;
@@ -2104,6 +2107,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);
@@ -2377,7 +2449,6 @@ struct Event {
     boot @60 :Boot;
 
     # ********** openpilot daemon msgs **********
-    gpsNMEA @3 :GPSNMEAData;
     can @5 :List(CanData);
     controlsState @7 :ControlsState;
     selfdriveState @130 :SelfdriveState;
@@ -2402,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;
@@ -2410,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;
@@ -2436,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;
@@ -2446,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;
 
@@ -2553,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/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/model.h

@@ -1 +1 @@
-#define DEFAULT_MODEL "POP model (Default)"
+#define DEFAULT_MODEL "CD210 (Default)"

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

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.1"
 fi
 
 export STAGING_ROOT="/data/safe_staging"

selfdrive/assets/icons_mici/body.png

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

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/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/modeld/SConscript

@@ -1,10 +1,23 @@
 import glob
 import json
 import os
+from itertools import product
 from SCons.Script import Value
 from openpilot.common.file_chunker import chunk_file, get_chunk_paths
+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 CompileConfig
 from tinygrad import Device
 
+CAMERA_CONFIGS = [
+  (_ar_ox_fisheye.width, _ar_ox_fisheye.height),  # tici: 1928x1208
+  (_os_fisheye.width, _os_fisheye.height),        # mici: 1344x760
+]
+MODELD_CONFIGS = [CompileConfig(cam_w, cam_h, prepare_only, 'driving_')
+                  for (cam_w, cam_h), prepare_only in product(CAMERA_CONFIGS, [True, False])]
+DM_WARP_CONFIGS = [CompileConfig(cam_w, cam_h, True, 'dm_') for cam_w, cam_h in CAMERA_CONFIGS]
+
 Import('env', 'arch')
 chunker_file = File("#common/file_chunker.py")
 lenv = env.Clone()
@@ -16,18 +29,17 @@ 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 'CUDA' in available:
-if False:  # '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} 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_backend = 'CPU' if arch == 'Darwin' else 'CPU:LLVM'
+  # THREADS=0 is need to prevent bug: https://github.com/tinygrad/tinygrad/issues/14689
   tg_flags = f'DEV={tg_backend} THREADS=0'
 
 def write_tg_compiled_flags(target, source, env):
@@ -54,14 +66,35 @@ for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']:
 image_flag = {
      'larch64': 'IMAGE=2',
 }.get(arch, 'IMAGE=0')
-script_files = [File(Dir("#selfdrive/modeld").File("compile_warp.py").abspath)]
+modeld_dir = Dir("#selfdrive/modeld").abspath
-compile_warp_cmd = f'{tg_flags} {mac_brew_string} python3 {Dir("#selfdrive/modeld").abspath}/compile_warp.py '
+compile_modeld_script = [File(f"{modeld_dir}/compile_modeld.py")]
-from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
+compile_dm_warp_script = [File(f"{modeld_dir}/compile_dm_warp.py")]
-warp_targets = []
+driving_onnx_deps = [File(f"models/{m}.onnx").abspath for m in ['driving_vision', 'driving_policy']]
-for cam in [_ar_ox_fisheye, _os_fisheye]:
+driving_metadata_deps = [File(f"models/{m}_metadata.pkl").abspath for m in ['driving_vision', 'driving_policy']]
-  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]
+model_w, model_h = MEDMODEL_INPUT_SIZE
-lenv.Command(warp_targets, tinygrad_files + script_files + [compiled_flags_node], compile_warp_cmd)
+frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
+for cfg in MODELD_CONFIGS:
+  cmd = (f'{tg_flags} {mac_brew_string} {image_flag} python3 {modeld_dir}/compile_modeld.py '
+         f'--model-size {model_w}x{model_h} '
+         f'--nv12 {",".join(str(x) for x in cfg.nv12)} '
+         f'--vision-onnx {File("models/driving_vision.onnx").abspath} '
+         f'--policy-onnx {File("models/driving_policy.onnx").abspath} '
+         f'--output {cfg.pkl_path} --frame-skip {frame_skip}'
+         + (' --prepare-only' if cfg.prepare_only else ''))
+  node = lenv.Command(cfg.pkl_path, tinygrad_files + compile_modeld_script + driving_onnx_deps + driving_metadata_deps + [chunker_file, compiled_flags_node], cmd)
+  onnx_sizes_sum = sum(os.path.getsize(f) for f in driving_onnx_deps)
+  chunk_targets = get_chunk_paths(cfg.pkl_path, estimate_pickle_max_size(onnx_sizes_sum))
+  def do_chunk(target, source, env, pkl=cfg.pkl_path, chunks=chunk_targets):
+    chunk_file(pkl, chunks)
+  lenv.Command(chunk_targets, node, do_chunk)
+
+dm_w, dm_h = DM_INPUT_SIZE
+for cfg in DM_WARP_CONFIGS:
+  cmd = (f'{tg_flags} {mac_brew_string} {image_flag} python3 {modeld_dir}/compile_dm_warp.py '
+         f'--nv12 {",".join(str(x) for x in cfg.nv12)} --warp-to {dm_w}x{dm_h} '
+         f'--output {cfg.pkl_path}')
+  lenv.Command(cfg.pkl_path, tinygrad_files + compile_dm_warp_script + compile_modeld_script + [compiled_flags_node], cmd)
 
 def tg_compile(flags, model_name):
   pythonpath_string = 'PYTHONPATH="${PYTHONPATH}:' + env.Dir("#tinygrad_repo").abspath + '"'
@@ -82,7 +115,4 @@ def tg_compile(flags, model_name):
     do_chunk,
   )
 
-# Compile small models
+tg_compile(tg_flags, 'dmonitoring_model')
-for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']:
-  tg_compile(tg_flags, model_name)
-

selfdrive/modeld/compile_dm_warp.py

@@ -0,0 +1,54 @@
+#!/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.selfdrive.modeld.compile_modeld import NV12Frame, warp_perspective_tinygrad, _parse_size, _parse_nv12
+
+
+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).reshape(-1, dm_h * dm_w)
+  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('--nv12', type=_parse_nv12, required=True,
+                 help=f'NV12 frame layout: {",".join(NV12Frame._fields)}')
+  p.add_argument('--warp-to', type=_parse_size, required=True, help='DM input WxH')
+  p.add_argument('--output', required=True)
+  args = p.parse_args()
+
+  dm_w, dm_h = args.warp_to
+  compile_dm_warp(args.nv12, dm_w, dm_h, args.output)

selfdrive/modeld/compile_modeld.py

@@ -0,0 +1,253 @@
+#!/usr/bin/env python3
+import argparse
+import pickle
+import time
+from functools import partial
+from collections import namedtuple
+
+import numpy as np
+from tinygrad.tensor import Tensor
+from tinygrad.helpers import Context
+from tinygrad.device import Device
+from tinygrad.engine.jit import TinyJit
+from tinygrad.nn.onnx import OnnxRunner
+
+# https://github.com/tinygrad/tinygrad/issues/15682
+from tinygrad.uop.ops import UOp, Ops
+_orig = UOp.__reduce__
+UOp.__reduce__ = lambda self: (UOp.unique, ()) if self.op is Ops.UNIQUE else _orig(self)
+
+
+NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size'])
+
+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)
+
+
+def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad):
+  w_dst, h_dst = dst_shape
+  h_src, w_src = src_shape
+
+  x = Tensor.arange(w_dst).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1)
+  y = Tensor.arange(h_dst).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_nn_clipped = Tensor.round(src_x).clip(0, w_src - 1).cast('int')
+  y_nn_clipped = Tensor.round(src_y).clip(0, h_src - 1).cast('int')
+  idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped
+
+  return src_flat[idx]
+
+
+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]])
+    # 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):
+  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)
+
+  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),
+  }
+  input_queues = {
+    'img_q': Tensor.zeros(img_buf_shape, dtype='uint8').contiguous().realize(),
+    'big_img_q': Tensor.zeros(img_buf_shape, dtype='uint8').contiguous().realize(),
+    'feat_q': Tensor.zeros(frame_skip * (fb[1] - 1) + 1, fb[0], fb[2]).contiguous().realize(),
+    'desire_q': Tensor.zeros(frame_skip * dp[1], dp[0], dp[2]).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_run_policy(vision_runner, policy_runner, nv12: NV12Frame, model_w, model_h,
+                    vision_features_slice, frame_skip, prepare_only=False):
+  frame_prepare = make_frame_prepare(nv12, model_w, model_h)
+  sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
+  sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
+
+  def run_policy(img_q, big_img_q, feat_q, desire_q, desire, traffic_convention, tfm, big_tfm, frame, big_frame):
+    tfm = tfm.to(Device.DEFAULT)
+    big_tfm = big_tfm.to(Device.DEFAULT)
+    desire = desire.to(Device.DEFAULT)
+    traffic_convention = traffic_convention.to(Device.DEFAULT)
+    Tensor.realize(tfm, big_tfm, desire, traffic_convention)
+
+    img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn)
+    big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn)
+
+    if prepare_only:
+      return img, big_img
+
+    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)
+    desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
+
+    inputs = {'features_buffer': feat_buf, 'desire_pulse': desire_buf, 'traffic_convention': traffic_convention}
+    policy_out = next(iter(policy_runner(inputs).values())).cast('float32')
+
+    return vision_out, policy_out
+  return run_policy
+
+
+def compile_modeld(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip,
+                   vision_onnx, policy_onnx, pkl_path):
+  from get_model_metadata import metadata_path_for
+
+  print(f"Compiling combined policy JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
+
+  vision_runner = OnnxRunner(vision_onnx)
+  policy_runner = OnnxRunner(policy_onnx)
+
+  with open(metadata_path_for(vision_onnx), 'rb') as f:
+    vision_metadata = pickle.load(f)
+    vision_features_slice = vision_metadata['output_slices']['hidden_state']
+    vision_input_shapes = vision_metadata['input_shapes']
+  with open(metadata_path_for(policy_onnx), 'rb') as f:
+    policy_input_shapes = pickle.load(f)['input_shapes']
+
+  _run = make_run_policy(vision_runner, policy_runner, nv12, model_w, model_h,
+                         vision_features_slice, frame_skip, prepare_only)
+  run_policy_jit = TinyJit(_run, prune=True)
+
+  N_RUNS = 3
+  SEED = 42
+
+  def random_inputs_run_fn(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)
+    np.random.seed(seed)
+    Tensor.manual_seed(seed)
+
+    for i in range(N_RUNS):
+      frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
+      big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
+      for v in npy.values():
+        v[:] = np.random.randn(*v.shape).astype(v.dtype)
+      Device.default.synchronize()
+      st = time.perf_counter()
+      outs = fn(**input_queues, frame=frame, big_frame=big_frame)
+      mt = time.perf_counter()
+      for o in outs:
+        # .realize() not needed once jitted, but needed for unjitted fn
+        o.realize()
+      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")
+
+    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 fn, val, buffers
+
+  print('run unjitted')
+  _, test_val, test_buffers = random_inputs_run_fn(_run, seed=SEED)
+  print('capture + replay')
+  run_policy_jit, _, _ = random_inputs_run_fn(run_policy_jit, SEED, test_val, test_buffers)
+
+  print('pickle round trip')
+  with open(pkl_path, "wb") as f:
+    pickle.dump(run_policy_jit, f)
+    print(f"  Saved to {pkl_path}")
+  with open(pkl_path, "rb") as f:
+    run_policy_jit = pickle.load(f)
+  random_inputs_run_fn(run_policy_jit, SEED, test_val, test_buffers, expect_match=True)
+  random_inputs_run_fn(run_policy_jit, SEED+1, test_val, test_buffers, expect_match=False)
+
+
+def _parse_size(s):
+  w, h = s.lower().split('x')
+  return int(w), int(h)
+
+
+def _parse_nv12(s):
+  parts = s.split(',')
+  assert len(parts) == len(NV12Frame._fields), \
+    f"--nv12 expects {','.join(NV12Frame._fields)} (got {s!r})"
+  return NV12Frame(*(int(x) for x in parts))
+
+
+if __name__ == "__main__":
+  p = argparse.ArgumentParser()
+  p.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
+  p.add_argument('--nv12', type=_parse_nv12, required=True,
+                 help=f'NV12 frame layout: {",".join(NV12Frame._fields)}')
+  p.add_argument('--vision-onnx', required=True)
+  p.add_argument('--policy-onnx', required=True)
+  p.add_argument('--output', required=True)
+  p.add_argument('--prepare-only', action='store_true')
+  p.add_argument('--frame-skip', type=int, required=True)
+  args = p.parse_args()
+
+  model_w, model_h = args.model_size
+  compile_modeld(args.nv12, model_w, model_h, args.prepare_only, args.frame_skip,
+                 args.vision_onnx, args.policy_onnx, args.output)

selfdrive/modeld/compile_warp.py

@@ -1,201 +0,0 @@
-#!/usr/bin/env python3
-import time
-import pickle
-import numpy as np
-from pathlib import Path
-from tinygrad.tensor import Tensor
-from tinygrad.helpers import Context
-from tinygrad.device import Device
-from tinygrad.engine.jit import TinyJit
-
-from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
-from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE, DM_INPUT_SIZE
-from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
-
-MODELS_DIR = Path(__file__).parent / 'models'
-
-CAMERA_CONFIGS = [
-  (_ar_ox_fisheye.width, _ar_ox_fisheye.height),  # tici: 1928x1208
-  (_os_fisheye.width, _os_fisheye.height),        # mici: 1344x760
-]
-
-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)
-
-IMG_BUFFER_SHAPE = (30, MEDMODEL_INPUT_SIZE[1] // 2, MEDMODEL_INPUT_SIZE[0] // 2)
-
-
-def warp_pkl_path(w, h):
-  return MODELS_DIR / f'warp_{w}x{h}_tinygrad.pkl'
-
-
-def dm_warp_pkl_path(w, h):
-  return MODELS_DIR / f'dm_warp_{w}x{h}_tinygrad.pkl'
-
-
-def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad):
-  w_dst, h_dst = dst_shape
-  h_src, w_src = src_shape
-
-  x = Tensor.arange(w_dst).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1)
-  y = Tensor.arange(h_dst).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_nn_clipped = Tensor.round(src_x).clip(0, w_src - 1).cast('int')
-  y_nn_clipped = Tensor.round(src_y).clip(0, h_src - 1).cast('int')
-  idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped
-
-  return src_flat[idx]
-
-
-def frames_to_tensor(frames, model_w, model_h):
-  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(cam_w, cam_h, model_w, model_h):
-  stride, y_height, uv_height, _ = get_nv12_info(cam_w, cam_h)
-  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]])
-    # 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, model_w, model_h)
-    return tensor
-  return frame_prepare_tinygrad
-
-
-def make_update_img_input(frame_prepare, model_w, model_h):
-  def update_img_input_tinygrad(tensor, frame, M_inv):
-    M_inv = M_inv.to(Device.DEFAULT)
-    new_img = frame_prepare(frame, M_inv)
-    tensor.assign(tensor[6:].cat(new_img, dim=0).contiguous())
-    return Tensor.cat(tensor[:6], tensor[-6:], dim=0).contiguous().reshape(1, 12, model_h//2, model_w//2)
-  return update_img_input_tinygrad
-
-
-def make_update_both_imgs(frame_prepare, model_w, model_h):
-  update_img = make_update_img_input(frame_prepare, model_w, model_h)
-
-  def update_both_imgs_tinygrad(calib_img_buffer, new_img, M_inv,
-                                calib_big_img_buffer, new_big_img, M_inv_big):
-    calib_img_pair = update_img(calib_img_buffer, new_img, M_inv)
-    calib_big_img_pair = update_img(calib_big_img_buffer, new_big_img, M_inv_big)
-    return calib_img_pair, calib_big_img_pair
-  return update_both_imgs_tinygrad
-
-
-def make_warp_dm(cam_w, cam_h, dm_w, dm_h):
-  stride, y_height, _, _ = get_nv12_info(cam_w, cam_h)
-  stride_pad = stride - cam_w
-
-  def warp_dm(input_frame, M_inv):
-    M_inv = M_inv.to(Device.DEFAULT)
-    result = warp_perspective_tinygrad(input_frame[:cam_h*stride], M_inv, (dm_w, dm_h), (cam_h, cam_w), stride_pad).reshape(-1, dm_h * dm_w)
-    return result
-  return warp_dm
-
-
-def compile_modeld_warp(cam_w, cam_h):
-  model_w, model_h = MEDMODEL_INPUT_SIZE
-  _, _, _, yuv_size = get_nv12_info(cam_w, cam_h)
-
-  print(f"Compiling modeld warp for {cam_w}x{cam_h}...")
-
-  frame_prepare = make_frame_prepare(cam_w, cam_h, model_w, model_h)
-  update_both_imgs = make_update_both_imgs(frame_prepare, model_w, model_h)
-  update_img_jit = TinyJit(update_both_imgs, prune=True)
-
-  full_buffer = Tensor.zeros(IMG_BUFFER_SHAPE, dtype='uint8').contiguous().realize()
-  big_full_buffer = Tensor.zeros(IMG_BUFFER_SHAPE, dtype='uint8').contiguous().realize()
-  new_frame_np = np.random.randint(0, 256, yuv_size, dtype=np.uint8)
-  new_big_frame_np = np.random.randint(0, 256, yuv_size, dtype=np.uint8)
-  for i in range(10):
-    img_inputs = [full_buffer,
-                  Tensor.from_blob(new_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(),
-                  Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')]
-    big_img_inputs = [big_full_buffer,
-                      Tensor.from_blob(new_big_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(),
-                      Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')]
-    inputs = img_inputs + big_img_inputs
-    Device.default.synchronize()
-
-    st = time.perf_counter()
-    _ = update_img_jit(*inputs)
-    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")
-
-  pkl_path = warp_pkl_path(cam_w, cam_h)
-  with open(pkl_path, "wb") as f:
-    pickle.dump(update_img_jit, f)
-  print(f"  Saved to {pkl_path}")
-
-  jit = pickle.load(open(pkl_path, "rb"))
-  jit(*inputs)
-
-
-def compile_dm_warp(cam_w, cam_h):
-  dm_w, dm_h = DM_INPUT_SIZE
-  _, _, _, yuv_size = get_nv12_info(cam_w, cam_h)
-
-  print(f"Compiling DM warp for {cam_w}x{cam_h}...")
-
-  warp_dm = make_warp_dm(cam_w, cam_h, dm_w, dm_h)
-  warp_dm_jit = TinyJit(warp_dm, prune=True)
-
-  new_frame_np = np.random.randint(0, 256, yuv_size, dtype=np.uint8)
-  for i in range(10):
-    inputs = [Tensor.from_blob(new_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(),
-              Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')]
-    Device.default.synchronize()
-    st = time.perf_counter()
-    warp_dm_jit(*inputs)
-    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")
-
-  pkl_path = dm_warp_pkl_path(cam_w, cam_h)
-  with open(pkl_path, "wb") as f:
-    pickle.dump(warp_dm_jit, f)
-  print(f"  Saved to {pkl_path}")
-
-
-def run_and_save_pickle():
-  for cam_w, cam_h in CAMERA_CONFIGS:
-    compile_modeld_warp(cam_w, cam_h)
-    compile_dm_warp(cam_w, cam_h)
-
-
-if __name__ == "__main__":
-  run_and_save_pickle()

selfdrive/modeld/dmonitoringmodeld.py

@@ -1,12 +1,8 @@
 #!/usr/bin/env python3
 import os
-from openpilot.selfdrive.modeld.tinygrad_helpers import MODELS_DIR, set_tinygrad_backend_from_compiled_flags
+from openpilot.selfdrive.modeld.helpers import MODELS_DIR, CompileConfig, 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 tinygrad.tensor import Tensor
 import time
 import pickle
@@ -32,7 +28,7 @@ class ModelState:
   inputs: dict[str, np.ndarray]
   output: np.ndarray
 
-  def __init__(self):
+  def __init__(self, cam_w: int, cam_h: int):
     with open(METADATA_PATH, 'rb') as f:
       model_metadata = pickle.load(f)
       self.input_shapes = model_metadata['input_shapes']
@@ -44,22 +40,18 @@ 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(CompileConfig(cam_w, cam_h, prefix='dm_', prepare_only=True).pkl_path, "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
     # There is a ringbuffer of imgs, just cache tensors pointing to all of them
     if ptr not in self._blob_cache:
@@ -113,9 +105,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):
@@ -123,6 +112,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

@@ -7,6 +7,10 @@ from typing import Any
 
 from tinygrad.nn.onnx import OnnxPBParser
 
+def metadata_path_for(onnx_path) -> pathlib.Path:
+  p = pathlib.Path(onnx_path)
+  return p.parent / (p.stem + '_metadata.pkl')
+
 
 class MetadataOnnxPBParser(OnnxPBParser):
   def _parse_ModelProto(self) -> dict:
@@ -48,7 +52,7 @@ if __name__ == "__main__":
     'output_shapes': dict(get_name_and_shape(x) for x in model["graph"]["output"]),
   }
 
-  metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl')
+  metadata_path = metadata_path_for(model_path)
   with open(metadata_path, 'wb') as f:
     pickle.dump(metadata, f)
 

selfdrive/modeld/helpers.py

@@ -0,0 +1,31 @@
+import json
+import os
+from dataclasses import dataclass
+from pathlib import Path
+
+from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
+
+MODELS_DIR = Path(__file__).resolve().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'])
+
+
+@dataclass
+class CompileConfig:
+  cam_w: int
+  cam_h: int
+  prepare_only: bool
+  prefix: str
+
+  @property
+  def pkl_path(self):
+    return str(MODELS_DIR / f'{self.prefix}{"warp_" if self.prepare_only else ""}{self.cam_w}x{self.cam_h}_tinygrad.pkl')
+
+  @property
+  def nv12(self):
+    return (self.cam_w, self.cam_h, *get_nv12_info(self.cam_w, self.cam_h))

selfdrive/modeld/modeld.py

@@ -1,12 +1,8 @@
 #!/usr/bin/env python3
 import os
-from openpilot.selfdrive.modeld.tinygrad_helpers import MODELS_DIR, set_tinygrad_backend_from_compiled_flags
+from openpilot.selfdrive.modeld.helpers import MODELS_DIR, CompileConfig, 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'
@@ -30,6 +26,7 @@ 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
 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.selfdrive.modeld.constants import ModelConstants, Plan
@@ -41,17 +38,13 @@ 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,
@@ -86,108 +79,39 @@ 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):
     ModelStateBase.__init__(self)
     self.LAT_SMOOTH_SECONDS = LAT_SMOOTH_SECONDS
+
     with open(VISION_METADATA_PATH, 'rb') as f:
       vision_metadata = pickle.load(f)
       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)
       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.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
-    self.numpy_inputs = {k: np.zeros(self.policy_input_shapes[k], dtype=np.float32) for k in self.policy_input_shapes}
+    self.input_queues, self.npy = make_input_queues(self.vision_input_shapes, self.policy_input_shapes, self.frame_skip)
-    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.parser = Parser()
-    self.frame_buf_params : dict[str, tuple[int, int, int, int]] = {}
+    self.frame_buf_params = {k: get_nv12_info(cam_w, cam_h) for k in ('img', 'big_img')}
-    self.update_imgs = None
+    self.run_policy = pickle.loads(read_file_chunked(CompileConfig(cam_w, cam_h, prefix='driving_', prepare_only=False).pkl_path))
-    self.vision_run = pickle.loads(read_file_chunked(str(VISION_PKL_PATH)))
+    self.warp_enqueue = pickle.loads(read_file_chunked(CompileConfig(cam_w, cam_h, prefix='driving_', prepare_only=True).pkl_path))
-    self.policy_run = pickle.loads(read_file_chunked(str(POLICY_PKL_PATH)))
+    self.warp_enqueue(
+      **self.input_queues,
+      frame=Tensor.zeros(self.frame_buf_params['img'][3], dtype='uint8').contiguous().realize(),
+      big_frame=Tensor.zeros(self.frame_buf_params['big_img'][3], dtype='uint8').contiguous().realize())
 
   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,18 +119,6 @@ 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
       yuv_size = self.frame_buf_params[key][3]
@@ -215,30 +127,31 @@ class ModelState(ModelStateBase):
       if cache_key not in self._blob_cache:
         self._blob_cache[cache_key] = Tensor.from_blob(ptr, (yuv_size,), dtype='uint8')
       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'],
+    # Model decides when action is completed, so desire input is just a pulse triggered on rising edge
-                           self.img_queues['big_img'], self.full_frames['big_img'], self.transforms['big_img'])
+    inputs['desire_pulse'][0] = 0
-    vision_inputs = {'img': out[0], 'big_img': out[1]}
+    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['tfm'][:,:] = transforms['img'][:,:]
+    self.npy['big_tfm'][:,:] = transforms['big_img'][:,:]
 
     if prepare_only:
+      self.warp_enqueue(**self.input_queues, frame=self.full_frames['img'], big_frame=self.full_frames['big_img'])
       return None
 
-    self.vision_output = self.vision_run(**vision_inputs).contiguous().realize().uop.base.buffer.numpy().flatten()
+    vision_output, policy_output = self.run_policy(
-    vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices))
+      **self.input_queues, frame=self.full_frames['img'], big_frame=self.full_frames['big_img']
+    )
 
-    self.full_input_queues.enqueue({'features_buffer': vision_outputs_dict['hidden_state'], 'desire_pulse': new_desire})
+    vision_output = vision_output.numpy().flatten()
-    for k in ['desire_pulse', 'features_buffer']:
+    policy_output = policy_output.numpy().flatten()
-      self.numpy_inputs[k][:] = self.full_input_queues.get(k)[k]
+    vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(vision_output, self.vision_output_slices))
-    self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
+    policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(policy_output, self.policy_output_slices))
-
-    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))
     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(), policy_output.copy()])
     return combined_outputs_dict
 
 
@@ -250,11 +163,6 @@ def main(demo=False):
     # 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)
+  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"])

selfdrive/modeld/models/driving_policy.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:853c6634746ff439a848349d00e4d5581cd941f13f7c1862c31b72a31cc24858
+oid sha256:78477124cbf3ffe30fa951ebada8410b43c4242c6054584d656f1d329b067e15
-size 14061595
+size 14060847

selfdrive/modeld/models/driving_vision.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:940e9006a25f27f0b6e85da798e6a8fd1f6dd492dd7d0b9ff1a9436460f46129
+oid sha256:ee29ee5bce84d1ce23e9ff381280de9b4e4d96d2934cd751740354884e112c66
-size 46887794
+size 46877473

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,8 +40,8 @@ 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.wheelpos_offsetter.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)):
+     DM.wheel_on_right == (DM.wheelpos_offsetter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)):
       params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right)
 
 def main():

selfdrive/monitoring/helpers.py

@@ -1,463 +0,0 @@
-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
-
-# ******************************************************************************************
-#  NOTE: To fork maintainers.
-#  Disabling or nerfing safety features will get you and your users banned from our servers.
-#  We recommend that you do not change these numbers from the defaults.
-# ******************************************************************************************
-
-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.
-
-    self._FACE_THRESHOLD = 0.7
-    self._EYE_THRESHOLD = 0.5
-    self._BLINK_THRESHOLD = 0.5
-    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
-    self._POSE_YAW_THRESHOLD = 0.4020
-    self._POSE_YAW_THRESHOLD_SLACK = 0.5042
-    self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD
-    self._POSE_YAW_MIN_STEER_DEG = 30
-    self._POSE_YAW_STEER_FACTOR = 0.15
-    self._POSE_YAW_STEER_MAX_OFFSET = 0.3927
-    self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned
-    self._PITCH_NATURAL_THRESHOLD = 0.449
-    self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned
-    self._PITCH_NATURAL_VAR = 3*0.01
-    self._YAW_NATURAL_VAR = 3*0.05
-    self._PITCH_MAX_OFFSET = 0.124
-    self._PITCH_MIN_OFFSET = -0.0881
-    self._YAW_MAX_OFFSET = 0.289
-    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._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._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_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.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
-
-
-# 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
-
-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
-
-  pitch_net, yaw_net, roll_net = angles_desc
-
-  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_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
-
-
-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())
-
-    # 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.pose = DriverPose(settings=self.settings)
-    self.blink_prob = 0.
-    self.phone_prob = 0.
-
-    self.always_on = always_on
-    self.distracted_types = []
-    self.driver_distracted = False
-    self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON)
-    self.wheel_on_right = False
-    self.wheel_on_right_last = None
-    self.wheel_on_right_default = rhd_saved
-    self.face_detected = False
-    self.terminal_alert_cnt = 0
-    self.terminal_time = 0
-    self.step_change = 0.
-    self.active_monitoring_mode = True
-    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.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._reset_awareness()
-    self._set_timers(active_monitoring=True)
-    self._reset_events()
-
-  def _reset_awareness(self):
-    self.awareness = 1.
-    self.awareness_active = 1.
-    self.awareness_passive = 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
-      else:
-        self.step_change = 0.
-      return  # no exploit after orange alert
-    elif self.awareness <= 0.:
-      return
-
-    if active_monitoring:
-      # 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
-
-      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
-    else:
-      if self.active_monitoring_mode:
-        self.awareness_active = self.awareness
-        self.awareness = self.awareness_passive
-
-      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
-
-  def _set_policy(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)
-    self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
-                                           [self.settings._POSE_PITCH_THRESHOLD_SLACK,
-                                            self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
-    self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
-                                           [self.settings._POSE_YAW_THRESHOLD_SLACK,
-                                            self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
-
-  def _get_distracted_types(self):
-    distracted_types = []
-
-    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(),
-                                                       self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET)
-      yaw_error = self.pose.yaw - min(max(self.pose.yaw_offseter.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
-
-    if yaw_error * self.pose.steer_yaw_offset > 0: # unidirectional
-      yaw_error = max(abs(yaw_error) - min(abs(self.pose.steer_yaw_offset), self.settings._POSE_YAW_STEER_MAX_OFFSET), 0.)
-    else:
-      yaw_error = abs(yaw_error)
-
-    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_prob > 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
-
-  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.prob_calibrated = self.wheelpos.prob_offseter.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
-    else:
-      self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished
-    # make sure no switching when engaged
-    if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode:
-      self.wheel_on_right = self.wheel_on_right_last
-    driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData
-    if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition,
-                                    driver_data.faceOrientationStd, driver_data.facePositionStd)):
-      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)
-    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.blink_prob = driver_data.eyesClosedProb * (driver_data.eyesVisibleProb > self.settings._EYE_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.driver_distraction_filter.update(self.driver_distracted)
-
-    # update offseter
-    # only update 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.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
-
-    if self.face_detected and not self.driver_distracted:
-      if model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD:
-        if not standstill:
-          self.dcam_uncertain_cnt += 1
-          self.dcam_reset_cnt = 0
-      else:
-        self.dcam_reset_cnt += 1
-        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)
-    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
-    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 \
-       (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
-      self._reset_awareness()
-      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
-
-    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._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.)
-      if self.awareness == 1.:
-        self.awareness_passive = min(self.awareness_passive + self.step_change, 1.)
-      # don't display alert banner when awareness is recovering and has cleared orange
-      if self.awareness > self.threshold_prompt:
-        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
-
-    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):
-        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
-      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
-
-
-  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,
-    }
-    return dat
-
-  def run_step(self, sm, demo=False):
-    if demo:
-      highway_speed = 30
-      enabled = True
-      wrong_gear = False
-      standstill = False
-      driver_engaged = False
-      brake_disengage_prob = 1.0
-      rpyCalib = [0., 0., 0.]
-    else:
-      highway_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['carState'].gasPressed
-      brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
-      rpyCalib = sm['liveCalibration'].rpyCalib
-    self._set_policy(
-      brake_disengage_prob=brake_disengage_prob,
-      car_speed=highway_speed,
-    )
-
-    # Parse data from dmonitoringmodeld
-    self._update_states(
-      driver_state=sm['driverStateV2'],
-      cal_rpy=rpyCalib,
-      car_speed=highway_speed,
-      op_engaged=enabled,
-      standstill=standstill,
-      demo_mode=demo,
-      steering_angle_deg=sm['carState'].steeringAngleDeg,
-    )
-
-    # Update distraction events
-    self._update_events(
-      driver_engaged=driver_engaged,
-      op_engaged=enabled,
-      standstill=standstill,
-      wrong_gear=wrong_gear,
-      car_speed=highway_speed
-    )

selfdrive/monitoring/policy.py

@@ -0,0 +1,426 @@
+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.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
+
+AlertLevel = log.DriverMonitoringState.AlertLevel
+MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
+
+def to_percent(v):
+  return int(min(max(v * 100., 0.), 100.))
+
+# ******************************************************************************************
+#  NOTE: To fork maintainers.
+#  Disabling or nerfing safety features will get you and your users banned from our servers.
+#  We recommend that you do not change these numbers from the defaults.
+# ******************************************************************************************
+
+class DRIVER_MONITOR_SETTINGS:
+  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.5
+    self._BLINK_THRESHOLD = 0.5
+    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
+    self._POSE_YAW_THRESHOLD = 0.4020
+    self._POSE_YAW_THRESHOLD_SLACK = 0.5042
+    self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD
+    self._POSE_YAW_MIN_STEER_DEG = 30
+    self._POSE_YAW_STEER_FACTOR = 0.15
+    self._POSE_YAW_STEER_MAX_OFFSET = 0.3927
+    self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned
+    self._PITCH_NATURAL_THRESHOLD = 0.449
+    self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned
+    self._PITCH_NATURAL_VAR = 3*0.01
+    self._YAW_NATURAL_VAR = 3*0.05
+    self._PITCH_MAX_OFFSET = 0.124
+    self._PITCH_MIN_OFFSET = -0.0881
+    self._YAW_MAX_OFFSET = 0.289
+    self._YAW_MIN_OFFSET = -0.0246
+
+    self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1
+    self._DCAM_UNCERTAIN_ALERT_COUNT = int(60  / DT_DMON)
+    self._DCAM_UNCERTAIN_RESET_COUNT = int(20  / 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 / 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 / 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
+
+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.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.
+
+# model output refers to center of undistorted+leveled image
+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_model(orient_model, pos_model, rpy_calib):
+  pitch_model = orient_model[0]
+  yaw_model = orient_model[1]
+
+  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)
+
+  pitch = pitch_model + pitch_focal_angle
+  yaw = -yaw_model + yaw_focal_angle
+
+  pitch -= rpy_calib[1]
+  yaw -= rpy_calib[2]
+  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()
+
+    # init driver status
+    wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2)
+    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_prob = 0.
+    self.phone_prob = 0.
+
+    self.alert_level = AlertLevel.none
+    self.always_on = always_on
+    self.distracted_types = defaultdict(bool)
+    self.driver_distracted = False
+    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
+    self.face_detected = False
+    self.terminal_alert_cnt = 0
+    self.terminal_time = 0
+    self.step_change = 0.
+    self.active_policy = MonitoringPolicy.vision
+    self.driver_interacting = False
+    self.is_model_uncertain = False
+    self.hi_stds = 0
+    self.model_std_max = 0.
+    self.threshold_alert_1 = 0.
+    self.threshold_alert_2 = 0.
+    self.dcam_uncertain_cnt = 0
+    self.dcam_reset_cnt = 0
+    self.too_distracted = Params().get_bool("DriverTooDistracted")
+
+    self._reset_awareness()
+    self._set_policy(MonitoringPolicy.vision)
+
+  def _reset_awareness(self):
+    self.awareness = 1.
+    self.last_vision_awareness = 1.
+    self.last_wheeltouch_awareness = 1.
+
+  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 target_policy == MonitoringPolicy.vision:
+      # when falling back from passive mode to active mode, reset awareness to avoid false alert
+      if self.active_policy != MonitoringPolicy.vision:
+        self.last_wheeltouch_awareness = self.awareness
+        self.awareness = self.last_vision_awareness
+
+      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_policy == MonitoringPolicy.vision:
+        self.last_vision_awareness = self.awareness
+        self.awareness = self.last_wheeltouch_awareness
+
+      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_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)
+    self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
+                                           [self.settings._POSE_PITCH_THRESHOLD_SLACK,
+                                            self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
+    self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
+                                           [self.settings._POSE_YAW_THRESHOLD_SLACK,
+                                            self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
+
+  def _get_distracted_types(self):
+    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_offsetter.filtered_stat.mean(),
+                                                       self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET)
+      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
+
+    if yaw_error * self.pose.steer_yaw_offset > 0: # unidirectional
+      yaw_error = max(abs(yaw_error) - min(abs(self.pose.steer_yaw_offset), self.settings._POSE_YAW_STEER_MAX_OFFSET), 0.)
+    else:
+      yaw_error = abs(yaw_error)
+
+    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
+
+    self.distracted_types['pose'] = bool((pitch_error > pitch_threshold) or (yaw_error > yaw_threshold))
+    self.distracted_types['eye'] = bool(self.blink_prob > 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_offsetter.push_and_update(rhd_pred)
+
+    wheelpos_calibrated = self.wheelpos_offsetter.filtered_stat.n >= self.settings._WHEELPOS_FILTER_MIN_COUNT
+
+    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
+    if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode:
+      self.wheel_on_right = self.wheel_on_right_last
+    driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData
+    if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition,
+                                    driver_data.faceOrientationStd, driver_data.facePositionStd)):
+      return
+
+    self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD
+    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.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_prob = driver_data.eyesClosedProb * (driver_data.eyesVisibleProb > self.settings._EYE_THRESHOLD)
+    self.phone_prob = driver_data.phoneProb
+
+    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)
+
+    # 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_offsetter.push_and_update(self.pose.pitch)
+      self.pose.yaw_offsetter.push_and_update(self.pose.yaw)
+
+    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:
+      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:
+        self.dcam_reset_cnt += 1
+        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_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):
+    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:
+      self.too_distracted = True
+
+    always_on_valid = self.always_on and not wrong_gear
+    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
+      self._reset_awareness()
+      return
+
+    awareness_prev = self.awareness
+    _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_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._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.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_alert_2:
+        return
+
+    certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and 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_exemption or always_on_exemption):
+        self.awareness = max(self.awareness - self.step_change, -0.1)
+
+    if self.awareness <= 0.:
+      # 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_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)
+    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:
+      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:
+      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['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_pose_strictness(
+      brake_disengage_prob=brake_disengage_prob,
+      car_speed=car_speed,
+    )
+
+    # Parse data from dmonitoringmodeld
+    self._update_states(
+      driver_state=sm['driverStateV2'],
+      cal_rpy=rpyCalib,
+      car_speed=car_speed,
+      op_engaged=enabled,
+      standstill=standstill,
+      demo_mode=demo,
+      steering_angle_deg=steering_angle_deg,
+    )
+
+    # Update distraction events
+    self._update_events(
+      driver_engaged=driver_engaged,
+      op_engaged=enabled,
+      standstill=standstill,
+      wrong_gear=wrong_gear,
+    )

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.selfdrive.monitoring.policy import DriverMonitoring, DRIVER_MONITOR_SETTINGS
-from openpilot.system.hardware import HARDWARE
 
 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_ORANGE = dm_settings._VISION_POLICY_ALERT_2_TIMEOUT + 1
-DISTRACTED_SECONDS_TO_RED = dm_settings._DISTRACTED_TIME + 1
+DISTRACTED_SECONDS_TO_RED = dm_settings._VISION_POLICY_ALERT_3_TIMEOUT + 1
-INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1
+INVISIBLE_SECONDS_TO_ORANGE = dm_settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + 1
-INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 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()
@@ -35,7 +34,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
@@ -51,49 +50,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)
+      DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0)
-      events.append(DM.current_events)
+      alert_lvls.append(DM.alert_level)
-    assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs"
+    assert len(alert_lvls) == len(msgs), f"got {len(alert_lvls)} for {len(msgs)} driverState input msgs"
-    return events, DM
+    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)
+    alert_lvls, d_status = self._run_seq(always_attentive, always_false, always_true, always_false)
-    self._assert_no_events(events)
+    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)
+    alert_lvls, 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
+    s = d_status.settings
-    assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL + \
+    assert alert_lvls[int(s._VISION_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0
-                    ((d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \
+    assert alert_lvls[int((s._VISION_POLICY_ALERT_1_TIMEOUT + \
-                    EventName.driverDistracted1
+                    (s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1
-    assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + \
+    assert alert_lvls[int((s._VISION_POLICY_ALERT_2_TIMEOUT + \
-                    ((d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.driverDistracted2
+                    (s._VISION_POLICY_ALERT_3_TIMEOUT - s._VISION_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2
-    assert events[int((d_status.settings._DISTRACTED_TIME + \
+    assert alert_lvls[int((s._VISION_POLICY_ALERT_3_TIMEOUT + \
-                    ((TEST_TIMESPAN-10-d_status.settings._DISTRACTED_TIME)/2))/DT_DMON)].names[0] == EventName.driverDistracted3
+                    (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)
+    alert_lvls, 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
+    s = d_status.settings
-    assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL + \
+    assert alert_lvls[int(s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0
-                      ((d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \
+    assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT + \
-                      EventName.driverUnresponsive1
+                    (s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1
-    assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + \
+    assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + \
-                      ((d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive2
+                    (s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2
-    assert events[int((d_status.settings._AWARENESS_TIME + \
+    assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + \
-                      ((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive3
+                    (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
@@ -104,13 +103,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)
+    alert_lvls, _ = 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 alert_lvls[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 alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
-    assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)]) == 0
+    assert alert_lvls[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 alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)] == 2
-    assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)].names[0] == EventName.driverDistracted2
+    assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)] == 2
-    assert len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)]) == 0
+    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
@@ -128,11 +127,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)
+    alert_lvls, _ = 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 alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)] == 2
-    assert events[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted3
+    assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)] == 3
-    assert events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0] == EventName.driverDistracted3
+    assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] == 3
-    assert len(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)]) == 0
+    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
@@ -143,16 +142,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)
+    alert_lvls, _ = 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 alert_lvls[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 alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
-    assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0
+    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 alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2
-      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+_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 alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2
-      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+_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
@@ -164,19 +163,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)
+    alert_lvls, _ = 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 alert_lvls[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 alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
-    assert events[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive3
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)] == 3
-    assert events[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive3
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)] == 3
-    assert events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0] == EventName.driverUnresponsive3
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] == 3
-    assert len(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)]) == 0
+    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)
+    alert_lvls, _ = self._run_seq(always_distracted, always_false, always_false, always_false)
-    assert sum(len(event) for event in events) == 0
+    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
@@ -184,11 +183,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)
+    alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector)
-    assert len(events[int((_redlight_time-0.1)/DT_DMON)]) == 0
+    s = d_status.settings
-    _pre_to_prompt = d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL - d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL
+    assert alert_lvls[int((_redlight_time-0.1)/DT_DMON)] == 0
-    assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.driverDistracted1
+    _alert_1_to_2 = s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT
-    assert events[int((_redlight_time+_pre_to_prompt+0.5)/DT_DMON)].names[0] == EventName.driverDistracted2
+    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
@@ -196,23 +196,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 alert_lvls[int((_stop_time+0.1)/DT_DMON)] == 2
-    assert len(events[int((_stop_time+0.5)/DT_DMON)]) == 0
+    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)
+    alert_lvls, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
-    assert EventName.driverUnresponsive1 in \
+    s = d_status.settings
-                              events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)] == 1
-    assert EventName.driverUnresponsive2 in \
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 2
-                              events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 3
-    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
 
 
 @pytest.mark.parametrize("enabled_state, lat_active_state, expected", [
@@ -249,12 +247,12 @@ def test_enabled_states(enabled_state, lat_active_state, expected):
   ds = make_msg(False)
 
   sm = {
-      'carState': cs,
+    'carState': cs,
-      'selfdriveState': ss,
+    'selfdriveState': ss,
-      'carControl': cc,
+    'carControl': cc,
-      'modelV2': mv2,
+    'modelV2': mv2,
-      'liveCalibration': lc,
+    'liveCalibration': lc,
-      'driverStateV2': ds
+    'driverStateV2': ds
   }
 
   driver_monitoring = DriverMonitoring()
@@ -263,9 +261,9 @@ def test_enabled_states(enabled_state, lat_active_state, expected):
   captured_args = []
   original_update_events = driver_monitoring._update_events
 
-  def spy_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed):
+  def spy_update_events(driver_engaged, op_engaged, standstill, wrong_gear):
-      captured_args.append(op_engaged)
+    captured_args.append(op_engaged)
-      return original_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed)
+    return original_update_events(driver_engaged, op_engaged, standstill, wrong_gear)
 
   driver_monitoring._update_events = spy_update_events
 

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_nonblocking("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)

selfdrive/test/process_replay/migration.py

@@ -449,9 +449,6 @@ def migrate_sensorEvents(msgs):
       m.logMonoTime = msg.logMonoTime
 
       m_dat = getattr(m, sensor_service)
-      m_dat.version = evt.version
-      m_dat.sensor = evt.sensor
-      m_dat.type = evt.type
       m_dat.source = evt.source
       m_dat.timestamp = evt.timestamp
       setattr(m_dat, evt.which(), getattr(evt, evt.which()))
@@ -484,22 +481,41 @@ def migrate_onroadEvents(msgs):
   return ops, [], []
 
 
-@migration(inputs=["driverMonitoringState"])
+@migration(inputs=["driverMonitoringStateDEPRECATED"])
 def migrate_driverMonitoringState(msgs):
   ops = []
   for index, msg in msgs:
-    msg = msg.as_builder()
+    old = msg.driverMonitoringStateDEPRECATED
-    events = []
+    new_msg = messaging.new_message('driverMonitoringState', valid=msg.valid, logMonoTime=msg.logMonoTime)
-    for event in msg.driverMonitoringState.deprecated.events:
+    dm = new_msg.driverMonitoringState
-      try:
+    dm.isRHD = old.isRHD
-        if not str(event.name).endswith('DEPRECATED'):
+    dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision if old.isActiveMode else \
-          migrated_event = migrate_onroad_event(event)
+                          log.DriverMonitoringState.MonitoringPolicy.wheeltouch
-          if migrated_event is not None:
-            events.append(migrated_event)
-      except RuntimeError:  # Member was null
-        traceback.print_exc()
 
-    msg.driverMonitoringState.events = events
+    AlertLevel = log.DriverMonitoringState.AlertLevel
-    ops.append((index, msg.as_reader()))
+    event_to_alert_level = {
+      'driverDistracted1': AlertLevel.one, 'driverUnresponsive1': AlertLevel.one,
+      'driverDistracted2': AlertLevel.two, 'driverUnresponsive2': AlertLevel.two,
+      'driverDistracted3': AlertLevel.three, 'driverUnresponsive3': AlertLevel.three,
+      'tooDistracted': AlertLevel.three,
+    }
+    for event in old.events:
+      level = event_to_alert_level.get(str(event.name))
+      if level is not None:
+        dm.alertLevel = level
+        break
+
+    dm.visionPolicyState.awarenessPercent = int(max(0, min(100, (old.awarenessStatus if old.isActiveMode else old.awarenessActive) * 100)))
+    dm.visionPolicyState.awarenessStep = old.stepChange if old.isActiveMode else 0.
+    dm.visionPolicyState.isDistracted = old.isDistracted
+    dm.visionPolicyState.faceDetected = old.faceDetected
+    dm.visionPolicyState.pose.pitchCalib.offset = old.posePitchOffset
+    dm.visionPolicyState.pose.pitchCalib.calibratedPercent = int(min(100, old.posePitchValidCount / 600 * 100))
+    dm.visionPolicyState.pose.yawCalib.offset = old.poseYawOffset
+    dm.visionPolicyState.pose.yawCalib.calibratedPercent = int(min(100, old.poseYawValidCount / 600 * 100))
+    dm.visionPolicyState.pose.calibrated = old.posePitchValidCount >= 600 and old.poseYawValidCount >= 600
+    dm.wheeltouchPolicyState.awarenessPercent = int(max(0, min(100, (old.awarenessPassive if old.isActiveMode else old.awarenessStatus) * 100)))
+    dm.wheeltouchPolicyState.awarenessStep = 0. if old.isActiveMode else old.stepChange
+    ops.append((index, new_msg.as_reader()))
 
   return ops, [], []

selfdrive/test/process_replay/model_replay.py

@@ -35,7 +35,7 @@ GITHUB = GithubUtils(API_TOKEN, DATA_TOKEN)
 EXEC_TIMINGS = [
   # model, instant max, average max
   ("modelV2", 0.05, 0.028),
-  ("driverStateV2", 0.05, 0.016),
+  ("driverStateV2", 0.05, 0.018),
 ]
 
 def get_log_fn(test_route, ref="master"):

selfdrive/ui/body/animations.py

@@ -0,0 +1,278 @@
+from dataclasses import dataclass
+from enum import Enum
+import time
+
+
+class AnimationMode(Enum):
+  ONCE_FORWARD = 1
+  ONCE_FORWARD_BACKWARD = 2
+  REPEAT_FORWARD = 3
+  REPEAT_FORWARD_BACKWARD = 4
+
+
+@dataclass
+class Animation:
+  frames: list[list[tuple[int, int]]]
+  starting_frames: list[list[tuple[int, int]]] | None = None  # played once before the main loop
+  frame_duration: float = 0.15       # seconds each frame is shown
+  mode: AnimationMode = AnimationMode.REPEAT_FORWARD_BACKWARD
+  repeat_interval: float = 5.0      # seconds between animation restarts (only for REPEAT modes)
+  hold_end: float = 0.0             # seconds to hold the last frame before playing backward (only for *_BACKWARD modes)
+  left_turn_remove: list[tuple[int, int]] | None = None   # dots to remove from frame when turning left
+  right_turn_remove: list[tuple[int, int]] | None = None   # dots to remove from frame when turning right
+
+
+# --- Animation Helper Functions ---
+
+def _mirror(dots: list[tuple[int, int]]) -> list[tuple[int, int]]:
+  """Mirror a component from the left side of the face to the right"""
+  return [(r, 15 - c) for r, c in dots]
+
+
+def _mirror_no_flip(dots: list[tuple[int, int]]) -> list[tuple[int, int]]:
+  """Move a component to the mirrored position on the right half without flipping its shape."""
+  min_c = min(c for _, c in dots)
+  max_c = max(c for _, c in dots)
+  return [(r, 15 - max_c - min_c + c) for r, c in dots]
+
+
+def _shift(dots: list[tuple[int, int]], rc: tuple[int, int]) -> list[tuple[int, int]]:
+  dr, dc = rc
+  return [(r + dr, c + dc) for r, c in dots]
+
+
+def _make_frame(left_eye: list[tuple[int, int]], right_eye: list[tuple[int, int]],
+                left_brow: list[tuple[int, int]], right_brow: list[tuple[int, int]],
+                mouth: list[tuple[int, int]]) -> list[tuple[int, int]]:
+  return left_eye + left_brow + right_eye + right_brow + mouth
+
+
+# --- Animation Helper Components ---
+
+# Eyes (left side)
+EYE_OPEN = [
+        (2, 2), (2, 3),
+(3, 1), (3, 2), (3, 3), (3, 4),
+(4, 1), (4, 2), (4, 3), (4, 4),
+        (5, 2), (5, 3)
+]
+EYE_HALF = [
+(4, 1), (4, 2), (4, 3), (4, 4),
+        (5, 2), (5, 3)
+]
+EYE_CLOSED = [
+(4, 1),                 (4, 4),
+        (5, 2), (5, 3),
+]
+EYE_LEFT_LOOK = [
+        (2, 2), (2, 3),
+(3, 1), (3, 2),
+(4, 1), (4, 2),
+        (5, 2), (5, 3),
+]
+EYE_RIGHT_LOOK = [
+        (2, 2), (2, 3),
+                (3, 3), (3, 4),
+                (4, 3), (4, 4),
+        (5, 2), (5, 3),
+]
+
+# Eyebrows (left side)
+BROW_HIGH = [
+        (0, 1), (0, 2),
+(1, 0),
+]
+BROW_LOWERED = [
+        (1, 1), (1, 2),
+(2, 0)
+]
+BROW_STRAIGHT = [(1, 0), (1, 1), (1, 2)]
+BROW_DOWN = [
+(0, 1), (0, 2),
+                (1, 3)
+]
+
+# Mouths (centered, not mirrored)
+MOUTH_SMILE = [
+(6, 6),                 (6, 9),
+        (7, 7), (7, 8),
+]
+MOUTH_NORMAL = [(7, 7), (7, 8)]
+MOUTH_SAD = [
+        (6, 7), (6, 8),
+(7, 6),                 (7, 9)
+]
+
+# --- Animations ---
+
+NORMAL = Animation(
+  frames=[
+    _make_frame(EYE_OPEN, _mirror(EYE_OPEN), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+    _make_frame(EYE_HALF, _mirror(EYE_HALF), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+    _make_frame(EYE_CLOSED, _mirror(EYE_CLOSED), BROW_LOWERED, _mirror(BROW_LOWERED), MOUTH_SMILE),
+  ],
+  left_turn_remove=[
+    (3, 3), (3, 4),
+    (4, 3), (4, 4),
+  ] + _mirror_no_flip([
+    (3, 1), (3, 2),
+    (4, 1), (4, 2),
+  ]),
+  right_turn_remove=[
+    (3, 1), (3, 2),
+    (4, 1), (4, 2),
+  ] + _mirror_no_flip([
+    (3, 3), (3, 4),
+    (4, 3), (4, 4),
+  ])
+)
+
+ASLEEP = Animation(
+  frames=[
+    _make_frame(EYE_CLOSED, _mirror(EYE_CLOSED), [], [], MOUTH_NORMAL),
+  ],
+)
+
+SLEEPY = Animation(
+  frames=[
+    _make_frame(EYE_CLOSED, _mirror(EYE_CLOSED), _shift(BROW_STRAIGHT, (1, 0)), [], MOUTH_NORMAL),
+    _make_frame(EYE_HALF, _mirror(EYE_CLOSED), BROW_LOWERED, [], MOUTH_NORMAL),
+    _make_frame(EYE_OPEN, _mirror(EYE_CLOSED), BROW_HIGH, [], MOUTH_NORMAL)
+  ],
+  frame_duration=0.25,
+  mode=AnimationMode.ONCE_FORWARD_BACKWARD,
+  repeat_interval=10,
+  hold_end=1.5,
+)
+
+INQUISITIVE = Animation(
+  frames=[
+    _make_frame(EYE_OPEN, _mirror(EYE_OPEN), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+
+    _make_frame(EYE_LEFT_LOOK, _mirror(EYE_RIGHT_LOOK), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+    _make_frame(_shift(EYE_LEFT_LOOK, (0, -1)), _shift(_mirror(EYE_RIGHT_LOOK), (0, -1)), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+    _make_frame(_shift(EYE_LEFT_LOOK, (0, -1)), _shift(_mirror(EYE_RIGHT_LOOK), (0, -1)), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+    _make_frame(_shift(EYE_LEFT_LOOK, (0, -1)), _shift(_mirror(EYE_RIGHT_LOOK), (0, -1)), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+    _make_frame(EYE_LEFT_LOOK, _mirror(EYE_RIGHT_LOOK), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+
+    _make_frame(EYE_RIGHT_LOOK, _mirror(EYE_LEFT_LOOK), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+    _make_frame(_shift(EYE_RIGHT_LOOK, (0, 1)), _shift(_mirror(EYE_LEFT_LOOK), (0, 1)), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+    _make_frame(_shift(EYE_RIGHT_LOOK, (0, 1)), _shift(_mirror(EYE_LEFT_LOOK), (0, 1)), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+    _make_frame(_shift(EYE_RIGHT_LOOK, (0, 1)), _shift(_mirror(EYE_LEFT_LOOK), (0, 1)), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+    _make_frame(EYE_RIGHT_LOOK, _mirror(EYE_LEFT_LOOK), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+
+    _make_frame(EYE_OPEN, _mirror(EYE_OPEN), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+  ],
+  mode=AnimationMode.REPEAT_FORWARD,
+  frame_duration=0.15,
+  repeat_interval=10
+)
+
+WINK = Animation(
+  frames=[
+    _make_frame(EYE_OPEN, _mirror(EYE_OPEN), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE),
+    _make_frame(EYE_OPEN, _mirror(EYE_CLOSED), BROW_HIGH, _mirror(_shift(BROW_DOWN, (0, 2))), MOUTH_SMILE),
+  ],
+  mode=AnimationMode.ONCE_FORWARD_BACKWARD,
+  frame_duration=0.75,
+)
+
+
+# --- Face Animator Class ---
+
+class FaceAnimator:
+  def __init__(self, animation: Animation):
+    self._animation = animation
+    self._next: Animation | None = None
+    self._start_time = time.monotonic()
+    self._rewinding = False
+    self._rewind_start: float = 0.0
+    self._rewind_from: int = 0
+    self._seen_nonzero = False
+
+  def set_animation(self, animation: Animation):
+    if animation is not self._animation:
+      self._next = animation
+
+  def get_dots(self) -> list[tuple[int, int]]:
+    now = time.monotonic()
+    elapsed = now - self._start_time
+
+    # Handle rewind for forward-only animations
+    if self._rewinding:
+      rewind_elapsed = now - self._rewind_start
+      frames_back = round(rewind_elapsed / self._animation.frame_duration)
+      frame_index = self._rewind_from - frames_back
+      if frame_index <= 0:
+        return self._switch_to_next(now)
+      return self._animation.frames[frame_index]
+
+    # Play starting frames first (once)
+    starting = self._animation.starting_frames or []
+    starting_duration = len(starting) * self._animation.frame_duration
+    if starting and elapsed < starting_duration:
+      frame_index = min(int(elapsed / self._animation.frame_duration), len(starting) - 1)
+      return starting[frame_index]
+
+    # Main loop
+    loop_elapsed = elapsed - starting_duration if starting else elapsed
+    frame_index = _get_frame_index(self._animation, loop_elapsed, gap_first=bool(starting))
+
+    if frame_index != 0:
+      self._seen_nonzero = True
+
+    if self._next is not None:
+      if frame_index == 0 and (len(self._animation.frames) == 1 or self._seen_nonzero):
+        return self._switch_to_next(now)
+      # No natural return to frame 0 — start rewinding
+      if self._animation.mode in (AnimationMode.ONCE_FORWARD, AnimationMode.REPEAT_FORWARD):
+        self._rewinding = True
+        self._rewind_start = now
+        self._rewind_from = frame_index
+
+    return self._animation.frames[frame_index]
+
+  def _switch_to_next(self, now: float) -> list[tuple[int, int]]:
+    self._animation = self._next
+    self._next = None
+    self._rewinding = False
+    self._seen_nonzero = False
+    self._start_time = now
+    return self._animation.frames[0]
+
+
+def _get_frame_index(animation: Animation, elapsed: float, gap_first: bool = False) -> int:
+  """Get the current frame index given elapsed time and animation mode."""
+  num_frames = len(animation.frames)
+  if num_frames == 1:
+    return 0
+
+  fd = animation.frame_duration
+  has_backward = animation.mode in (AnimationMode.ONCE_FORWARD_BACKWARD, AnimationMode.REPEAT_FORWARD_BACKWARD)
+  repeats = animation.mode in (AnimationMode.REPEAT_FORWARD, AnimationMode.REPEAT_FORWARD_BACKWARD)
+
+  forward_duration = num_frames * fd
+  backward_frames = max(num_frames - 2, 0) if has_backward else 0
+  hold = animation.hold_end if has_backward else 0.0
+  cycle_duration = forward_duration + hold + backward_frames * fd
+
+  if not repeats:
+    t = min(elapsed, cycle_duration)
+  else:
+    t = (elapsed + cycle_duration if gap_first else elapsed) % animation.repeat_interval
+
+  # Forward phase
+  if t < forward_duration:
+    return min(int(t / fd), num_frames - 1)
+  t -= forward_duration
+
+  # Hold at last frame
+  if t < hold:
+    return num_frames - 1
+  t -= hold
+
+  # Backward phase
+  if backward_frames and t < backward_frames * fd:
+    return num_frames - 2 - min(int(t / fd), backward_frames - 1)
+
+  return 0 if has_backward else num_frames - 1

selfdrive/ui/body/layouts/onroad.py

@@ -0,0 +1,93 @@
+import time
+import pyray as rl
+
+from openpilot.system.ui.lib.application import gui_app, FontWeight
+from openpilot.system.ui.widgets import Widget
+from openpilot.system.ui.widgets.label import UnifiedLabel
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.selfdrive.ui.body.animations import FaceAnimator, ASLEEP, INQUISITIVE, NORMAL, SLEEPY
+
+GRID_COLS = 16
+GRID_ROWS = 8
+DOT_RADIUS = 50 if gui_app.big_ui() else 10
+
+IDLE_TIMEOUT = 30.0        # seconds of no joystick input before playing INQUISITIVE
+IDLE_STEER_THRESH = 0.5    # degrees — below this counts as no input
+IDLE_SPEED_THRESH = 0.01   # m/s — below this counts as no input
+
+
+# This class is used both in BIG (tizi) and small (mici) UIs
+class BodyLayout(Widget):
+  def __init__(self):
+    super().__init__()
+    self._animator = FaceAnimator(ASLEEP)
+    self._turning_left = False
+    self._turning_right = False
+    self._last_input_time = time.monotonic()
+    self._was_active = False
+    self._offroad_label = UnifiedLabel("turn on ignition to use", 95 if gui_app.big_ui() else 45, FontWeight.DISPLAY,
+                                       alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
+                                       alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE)
+
+  def draw_dot_grid(self, rect: rl.Rectangle, dots: list[tuple[int, int]], color: rl.Color):
+    spacing = min(rect.height / GRID_ROWS, rect.width / GRID_COLS)
+
+    grid_w = (GRID_COLS - 1) * spacing
+    grid_h = (GRID_ROWS - 1) * spacing
+
+    offset_x = rect.x + (rect.width - grid_w) / 2
+    offset_y = rect.y + (rect.height - grid_h) / 2
+
+    for row, col in dots:
+      x = int(offset_x + col * spacing)
+      y = int(offset_y + row * spacing)
+      rl.draw_circle(x, y, DOT_RADIUS, color)
+
+  def _update_state(self):
+    super()._update_state()
+
+    sm = ui_state.sm
+
+    if ui_state.is_onroad():
+      if not self._was_active:
+        self._last_input_time = time.monotonic()
+        self._was_active = True
+
+      cs = sm['carState']
+      has_input = abs(cs.steeringAngleDeg) > IDLE_STEER_THRESH or abs(cs.vEgo) > IDLE_SPEED_THRESH
+      if has_input:
+        self._last_input_time = time.monotonic()
+
+      if time.monotonic() - self._last_input_time > IDLE_TIMEOUT:
+        self._animator.set_animation(INQUISITIVE)
+      else:
+        self._animator.set_animation(NORMAL)
+    else:
+      self._was_active = False
+      self._animator.set_animation(ASLEEP)
+
+    steer = sm['testJoystick'].axes[1] if len(sm['testJoystick'].axes) > 1 else 0
+    self._turning_left = steer <= -0.05
+    self._turning_right = steer >= 0.05
+
+  # play animation on screen tap
+  def _handle_mouse_release(self, mouse_pos):
+    super()._handle_mouse_release(mouse_pos)
+    if not self._was_active:
+      self._animator.set_animation(SLEEPY)
+
+  def _render(self, rect: rl.Rectangle):
+    dots = self._animator.get_dots()
+    animation = self._animator._animation
+    if self._turning_left and animation.left_turn_remove:
+      remove_set = set(animation.left_turn_remove)
+      dots = [d for d in dots if d not in remove_set]
+    elif self._turning_right and animation.right_turn_remove:
+      remove_set = set(animation.right_turn_remove)
+      dots = [d for d in dots if d not in remove_set]
+    self.draw_dot_grid(rect, dots, rl.WHITE)
+
+    if ui_state.is_offroad():
+      rl.draw_rectangle(int(self.rect.x), int(self.rect.y), int(self.rect.width), int(self.rect.height), rl.Color(0, 0, 0, 175))
+      upper_half = rl.Rectangle(rect.x, rect.y, rect.width, rect.height / 2)
+      self._offroad_label.render(upper_half)

selfdrive/ui/layouts/main.py

@@ -2,13 +2,14 @@ import pyray as rl
 from enum import IntEnum
 import cereal.messaging as messaging
 from openpilot.system.ui.lib.application import gui_app
+from openpilot.system.ui.widgets import Widget
 from openpilot.selfdrive.ui.layouts.sidebar import Sidebar, SIDEBAR_WIDTH
 from openpilot.selfdrive.ui.layouts.home import HomeLayout
 from openpilot.selfdrive.ui.layouts.settings.settings import SettingsLayout, PanelType
 from openpilot.selfdrive.ui.onroad.augmented_road_view import AugmentedRoadView
 from openpilot.selfdrive.ui.ui_state import device, ui_state
-from openpilot.system.ui.widgets import Widget
 from openpilot.selfdrive.ui.layouts.onboarding import OnboardingWindow
+from openpilot.selfdrive.ui.body.layouts.onroad import BodyLayout
 
 if gui_app.sunnypilot_ui():
   from openpilot.selfdrive.ui.sunnypilot.layouts.settings.settings import SettingsLayoutSP as SettingsLayout
@@ -31,7 +32,9 @@ class MainLayout(Widget):
     self._prev_onroad = False
 
     # Initialize layouts
-    self._layouts = {MainState.HOME: HomeLayout(), MainState.SETTINGS: SettingsLayout(), MainState.ONROAD: AugmentedRoadView()}
+    self._home_layout = HomeLayout()
+    self._home_body_layout = BodyLayout()
+    self._layouts = {MainState.HOME: self._home_layout, MainState.SETTINGS: SettingsLayout(), MainState.ONROAD: AugmentedRoadView()}
 
     self._sidebar_rect = rl.Rectangle(0, 0, 0, 0)
     self._content_rect = rl.Rectangle(0, 0, 0, 0)
@@ -57,14 +60,18 @@ class MainLayout(Widget):
     self._layouts[MainState.HOME]._setup_widget.set_open_settings_callback(lambda: self.open_settings(PanelType.FIREHOSE))
     self._layouts[MainState.HOME].set_settings_callback(lambda: self.open_settings(PanelType.TOGGLES))
     self._layouts[MainState.SETTINGS].set_callbacks(on_close=self._set_mode_for_state)
-    self._layouts[MainState.ONROAD].set_click_callback(self._on_onroad_clicked)
+
+    for layout in (self._layouts[MainState.ONROAD], self._home_body_layout):
+      layout.set_click_callback(self._on_onroad_clicked)
+
     device.add_interactive_timeout_callback(self._set_mode_for_state)
+    ui_state.add_on_body_changed_callbacks(self._on_body_changed)
 
   def _update_layout_rects(self):
     self._sidebar_rect = rl.Rectangle(self._rect.x, self._rect.y, SIDEBAR_WIDTH, self._rect.height)
 
     x_offset = SIDEBAR_WIDTH if self._sidebar.is_visible else 0
-    self._content_rect = rl.Rectangle(self._rect.y + x_offset, self._rect.y, self._rect.width - x_offset, self._rect.height)
+    self._content_rect = rl.Rectangle(self._rect.x + x_offset, self._rect.y, self._rect.width - x_offset, self._rect.height)
 
   def _handle_onroad_transition(self):
     if ui_state.started != self._prev_onroad:
@@ -73,6 +80,12 @@ class MainLayout(Widget):
       self._set_mode_for_state()
 
   def _set_mode_for_state(self):
+    # Don't go onroad if body, home is onroad
+    if ui_state.is_body:
+      self._set_current_layout(MainState.HOME)
+      self._sidebar.set_visible(not ui_state.ignition)
+      return
+
     if ui_state.started:
       # Don't hide sidebar from interactive timeout
       if self._current_mode != MainState.ONROAD:
@@ -104,6 +117,10 @@ class MainLayout(Widget):
   def _on_onroad_clicked(self):
     self._sidebar.set_visible(not self._sidebar.is_visible)
 
+  def _on_body_changed(self):
+    self._layouts[MainState.HOME] = self._home_body_layout if ui_state.is_body else self._home_layout
+    self._set_mode_for_state()
+
   def _render_main_content(self):
     # Render sidebar
     if self._sidebar.is_visible:

selfdrive/ui/layouts/settings/developer.py

@@ -135,12 +135,6 @@ class DeveloperLayout(Widget):
 
       long_man_enabled = ui_state.has_longitudinal_control and ui_state.is_offroad()
       self._long_maneuver_toggle.action_item.set_enabled(long_man_enabled)
-      if not long_man_enabled:
-        self._long_maneuver_toggle.action_item.set_state(False)
-        self._params.put_bool("LongitudinalManeuverMode", False)
-
-      lat_man_enabled = ui_state.is_offroad()
-      self._lat_maneuver_toggle.action_item.set_enabled(lat_man_enabled)
     else:
       self._long_maneuver_toggle.action_item.set_enabled(False)
       self._lat_maneuver_toggle.action_item.set_enabled(False)

selfdrive/ui/layouts/settings/firehose.py

@@ -1,7 +1,7 @@
 import pyray as rl
 
 from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE
-from openpilot.system.ui.lib.multilang import tr, trn, tr_noop
+from openpilot.system.ui.lib.multilang import tr, tr_noop
 from openpilot.system.ui.lib.text_measure import measure_text_cached
 from openpilot.system.ui.lib.scroll_panel import GuiScrollPanel
 from openpilot.system.ui.lib.wrap_text import wrap_text
@@ -65,12 +65,13 @@ class FirehoseLayout(FirehoseLayoutBase):
     y = self._draw_wrapped_text(x, y, w, status_text, gui_app.font(FontWeight.BOLD), 60, status_color)
     y += 20 + 20
 
+    # TODO: add back once reliable
     # Contribution count (if available)
-    if self._segment_count > 0:
+    #if self._segment_count > 0:
-      contrib_text = trn("{} segment of your driving is in the training dataset so far.",
+    #  contrib_text = trn("{} segment of your driving is in the training dataset so far.",
-                         "{} segments of your driving is in the training dataset so far.", self._segment_count).format(self._segment_count)
+    #                     "{} segments of your driving is in the training dataset so far.", self._segment_count).format(self._segment_count)
-      y = self._draw_wrapped_text(x, y, w, contrib_text, gui_app.font(FontWeight.BOLD), 52, rl.WHITE)
+    #  y = self._draw_wrapped_text(x, y, w, contrib_text, gui_app.font(FontWeight.BOLD), 52, rl.WHITE)
-      y += 20 + 20
+    #  y += 20 + 20
 
     # Separator
     rl.draw_rectangle(x, y, w, 2, self.GRAY)

selfdrive/ui/layouts/sidebar.py

@@ -125,10 +125,8 @@ class Sidebar(Widget, SidebarSP):
   def _update_temperature_status(self, device_state):
     thermal_status = device_state.thermalStatus
 
-    if thermal_status == ThermalStatus.green:
+    if thermal_status == ThermalStatus.ok:
       self._temp_status.update(tr_noop("TEMP"), tr_noop("GOOD"), Colors.GOOD)
-    elif thermal_status == ThermalStatus.yellow:
-      self._temp_status.update(tr_noop("TEMP"), tr_noop("OK"), Colors.WARNING)
     else:
       self._temp_status.update(tr_noop("TEMP"), tr_noop("HIGH"), Colors.DANGER)
 

selfdrive/ui/mici/layouts/home.py

@@ -130,6 +130,7 @@ class MiciHomeLayout(Widget):
 
     self._experimental_icon = IconWidget("icons_mici/experimental_mode.png", (48, 48))
     self._mic_icon = IconWidget("icons_mici/microphone.png", (32, 46))
+    self._body_icon = IconWidget("icons_mici/body.png", (54, 37))
 
     self._alerts_pill = AlertsPill()
 
@@ -137,6 +138,7 @@ class MiciHomeLayout(Widget):
       IconWidget("icons_mici/settings.png", (48, 48), opacity=0.9),
       NetworkIcon(),
       self._experimental_icon,
+      self._body_icon,
       self._mic_icon,
     ], spacing=18)
 
@@ -247,6 +249,7 @@ class MiciHomeLayout(Widget):
     # ***** Center-aligned bottom section icons *****
     self._experimental_icon.set_visible(self._experimental_mode)
     self._mic_icon.set_visible(ui_state.recording_audio)
+    self._body_icon.set_visible(ui_state.is_body)
 
     footer_rect = rl.Rectangle(self.rect.x + HOME_PADDING, self.rect.y + self.rect.height - 48, self.rect.width - HOME_PADDING, 48)
     self._status_bar_layout.render(footer_rect)

selfdrive/ui/mici/layouts/main.py

@@ -6,6 +6,7 @@ from openpilot.selfdrive.ui.mici.layouts.offroad_alerts import MiciOffroadAlerts
 from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import AugmentedRoadView
 from openpilot.selfdrive.ui.ui_state import device, ui_state
 from openpilot.selfdrive.ui.mici.layouts.onboarding import OnboardingWindow
+from openpilot.selfdrive.ui.body.layouts.onroad import BodyLayout
 from openpilot.system.ui.widgets import Widget
 from openpilot.system.ui.widgets.scroller import Scroller
 from openpilot.system.ui.lib.application import gui_app
@@ -31,22 +32,25 @@ class MiciMainLayout(Scroller):
     self._home_layout = MiciHomeLayout()
     self._alerts_layout = MiciOffroadAlerts()
     self._settings_layout = SettingsLayout()
-    self._onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked)
+    self._car_onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked)
+    self._body_onroad_layout = BodyLayout()
 
     # Initialize widget rects
-    for widget in (self._home_layout, self._settings_layout, self._alerts_layout, self._onroad_layout):
+    for widget in (self._home_layout, self._alerts_layout, self._settings_layout,
+                   self._car_onroad_layout, self._body_onroad_layout):
       # TODO: set parent rect and use it if never passed rect from render (like in Scroller)
       widget.set_rect(rl.Rectangle(0, 0, gui_app.width, gui_app.height))
 
     self._scroller.add_widgets([
       self._alerts_layout,
       self._home_layout,
-      self._onroad_layout,
+      self._car_onroad_layout,
+      self._body_onroad_layout,
     ])
     self._scroller.set_reset_scroll_at_show(False)
 
     # Disable scrolling when onroad is interacting with bookmark
-    self._scroller.set_scrolling_enabled(lambda: not self._onroad_layout.is_swiping_left())
+    self._scroller.set_scrolling_enabled(lambda: not self._car_onroad_layout.is_swiping_left())
 
     # Set callbacks
     self._setup_callbacks()
@@ -59,14 +63,22 @@ class MiciMainLayout(Scroller):
     if not self._onboarding_window.completed:
       gui_app.push_widget(self._onboarding_window)
 
+  @property
+  def _onroad_layout(self) -> Widget:
+    # For scroll_to
+    return self._body_onroad_layout if ui_state.is_body else self._car_onroad_layout
+
   def _setup_callbacks(self):
     self._home_layout.set_callbacks(
       on_settings=lambda: gui_app.push_widget(self._settings_layout),
       on_alerts=lambda: self._scroll_to(self._alerts_layout),
       alert_count_callback=self._alerts_layout.active_alerts,
     )
-    self._onroad_layout.set_click_callback(lambda: self._scroll_to(self._home_layout))
+    for layout in (self._car_onroad_layout, self._body_onroad_layout):
+      layout.set_click_callback(lambda: self._scroll_to(self._home_layout))
+
     device.add_interactive_timeout_callback(self._on_interactive_timeout)
+    ui_state.add_on_body_changed_callbacks(self._on_body_changed)
 
   def _scroll_to(self, layout: Widget):
     layout_x = int(layout.rect.x)
@@ -132,3 +144,7 @@ class MiciMainLayout(Scroller):
     user_bookmark = messaging.new_message('bookmarkButton')
     user_bookmark.valid = True
     self._pm.send('bookmarkButton', user_bookmark)
+
+  def _on_body_changed(self):
+    self._car_onroad_layout.set_visible(not ui_state.is_body)
+    self._body_onroad_layout.set_visible(ui_state.is_body)

selfdrive/ui/mici/layouts/onboarding.py

@@ -140,7 +140,7 @@ class TrainingGuideDMTutorial(NavWidget):
 
     # stay at 100% once reached
     in_bad_face = gui_app.get_active_widget() == self._bad_face_page
-    if ((dm_state.faceDetected and looking_center) or self._progress.x > 0.99) and not in_bad_face:
+    if ((dm_state.visionPolicyState.faceDetected and looking_center) or self._progress.x > 0.99) and not in_bad_face:
       slow = self._progress.x < 0.25
       duration = self.PROGRESS_DURATION * 2 if slow else self.PROGRESS_DURATION
       self._progress.x += 1.0 / (duration * gui_app.target_fps)

selfdrive/ui/mici/layouts/settings/developer.py

@@ -131,12 +131,6 @@ class DeveloperLayoutMici(NavScroller):
 
       long_man_enabled = ui_state.has_longitudinal_control and ui_state.is_offroad()
       self._long_maneuver_toggle.set_enabled(long_man_enabled)
-      if not long_man_enabled:
-        self._long_maneuver_toggle.set_checked(False)
-        ui_state.params.put_bool("LongitudinalManeuverMode", False)
-
-      lat_man_enabled = ui_state.is_offroad()
-      self._lat_maneuver_toggle.set_enabled(lat_man_enabled)
     else:
       self._long_maneuver_toggle.set_enabled(False)
       self._lat_maneuver_toggle.set_enabled(False)

selfdrive/ui/mici/onroad/driver_camera_dialog.py

@@ -1,20 +1,15 @@
 import pyray as rl
-from cereal import log, messaging
+from cereal import car, log, messaging
 from msgq.visionipc import VisionStreamType
 from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
 from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer
 from openpilot.selfdrive.ui.ui_state import ui_state, device
-from openpilot.selfdrive.selfdrived.events import EVENTS, ET
 from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr
 from openpilot.system.ui.widgets import Widget
 from openpilot.system.ui.widgets.nav_widget import NavWidget
 from openpilot.system.ui.widgets.label import gui_label
 
-EventName = log.OnroadEvent.EventName
-
-EVENT_TO_INT = EventName.schema.enumerants
-
 
 class DriverCameraView(CameraView):
   def _calc_frame_matrix(self, rect: rl.Rectangle):
@@ -107,11 +102,14 @@ class BaseDriverCameraDialog(Widget):
     if self._pm is None:
       return
 
+    AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+    ALERT_SOUNDS = {
+      'two': AudibleAlert.promptDistracted,
+      'three': AudibleAlert.warningImmediate,
+    }
     msg = messaging.new_message('selfdriveState')
-    if dm_state is not None and len(dm_state.events):
+    if dm_state is not None:
-      event_name = EVENT_TO_INT[dm_state.events[0].name]
+      msg.selfdriveState.alertSound = ALERT_SOUNDS.get(str(dm_state.alertLevel), AudibleAlert.none)
-      if event_name is not None and event_name in EVENTS and ET.PERMANENT in EVENTS[event_name]:
-        msg.selfdriveState.alertSound = EVENTS[event_name][ET.PERMANENT].audible_alert
     self._pm.send('selfdriveState', msg)
 
   def _render_dm_alerts(self, rect: rl.Rectangle):
@@ -119,29 +117,31 @@ class BaseDriverCameraDialog(Widget):
     dm_state = ui_state.sm["driverMonitoringState"]
     self._publish_alert_sound(dm_state)
 
+    is_vision = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
+    awareness_pct = dm_state.visionPolicyState.awarenessPercent if is_vision else dm_state.wheeltouchPolicyState.awarenessPercent
     gui_label(rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height),
-              f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
+              f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
               alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
               alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
               color=rl.Color(0, 0, 0, 180))
-    gui_label(rect, f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
+    gui_label(rect, f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
               alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
               alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
               color=rl.Color(255, 255, 255, int(255 * 0.9)))
 
-    if not dm_state.events:
+    if dm_state.alertLevel == log.DriverMonitoringState.AlertLevel.none:
       return
 
-    # Show first event (only one should be active at a time)
+    # Show alert level
-    event_name_str = str(dm_state.events[0].name).split('.')[-1]
+    alert_level_str = f"{'Pay Attention' if is_vision else 'Touch Wheel'} - level {dm_state.alertLevel}"
     alignment = rl.GuiTextAlignment.TEXT_ALIGN_RIGHT if self.driver_state_renderer.is_rhd else rl.GuiTextAlignment.TEXT_ALIGN_LEFT
 
     shadow_rect = rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height)
-    gui_label(shadow_rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD,
+    gui_label(shadow_rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD,
               alignment=alignment,
               alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM,
               color=rl.Color(0, 0, 0, 180))
-    gui_label(rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD,
+    gui_label(rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD,
               alignment=alignment,
               alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM,
               color=rl.Color(255, 255, 255, int(255 * 0.9)))
@@ -156,7 +156,7 @@ class BaseDriverCameraDialog(Widget):
   def _draw_face_detection(self, rect: rl.Rectangle):
     dm_state = ui_state.sm["driverMonitoringState"]
     driver_data = self.driver_state_renderer.get_driver_data()
-    if not dm_state.faceDetected:
+    if not dm_state.visionPolicyState.faceDetected:
       return
 
     # Get face position and orientation

selfdrive/ui/mici/onroad/driver_state.py

@@ -6,7 +6,7 @@ from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.system.ui.lib.application import gui_app
 from openpilot.system.ui.widgets import Widget
 from openpilot.selfdrive.ui.ui_state import ui_state
-from openpilot.selfdrive.monitoring.helpers import face_orientation_from_net
+
 
 AlertSize = log.SelfdriveState.AlertSize
 
@@ -35,6 +35,8 @@ class DriverStateRenderer(Widget):
     self._is_active = False
     self._is_rhd = False
     self._face_detected = False
+    self._face_pitch = 0.
+    self._face_yaw = 0.
     self._should_draw = False
     self._force_active = False
     self._looking_center = False
@@ -153,9 +155,11 @@ class DriverStateRenderer(Widget):
     sm = ui_state.sm
 
     dm_state = sm["driverMonitoringState"]
-    self._is_active = dm_state.isActiveMode
+    self._is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
     self._is_rhd = dm_state.isRHD
-    self._face_detected = dm_state.faceDetected
+    self._face_detected = dm_state.visionPolicyState.faceDetected
+    self._face_pitch = dm_state.visionPolicyState.pose.pitch + math.radians(6) # calib or DM pose is not accurate, add a fake upward pitch to bias forward
+    self._face_yaw = -dm_state.visionPolicyState.pose.yaw # undo sign flip in face_orientation_from_model to match UI convention
 
     driverstate = sm["driverStateV2"]
     driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData
@@ -163,26 +167,9 @@ class DriverStateRenderer(Widget):
 
   def _update_state(self):
     # Get monitoring state
-    driver_data = self.get_driver_data()
+    _ = self.get_driver_data()
-    driver_orient = driver_data.faceOrientation
+    pitch = self._pitch_filter.update(self._face_pitch)
-    driver_position = driver_data.facePosition
+    yaw = self._yaw_filter.update(self._face_yaw)
-
-    if len(driver_orient) != 3:
-      return
-
-    # Calibrate orientation so looking straight ahead at road (instead of at device) is (0, 0, 0)
-    sm = ui_state.sm
-    if sm.valid['liveCalibration'] and len(sm['liveCalibration'].rpyCalib) == 3:
-      cal_rpy = sm['liveCalibration'].rpyCalib
-    else:
-      cal_rpy = [0.0, 0.0, 0.0]
-
-    _, pitch, yaw = face_orientation_from_net(driver_orient, driver_position, cal_rpy)
-    pitch += math.radians(6)  # calib or DM pose is not accurate, add a fake upward pitch to bias forward
-    yaw = -yaw  # undo sign flip in face_orientation_from_net to match UI convention
-
-    pitch = self._pitch_filter.update(pitch)
-    yaw = self._yaw_filter.update(yaw)
 
     # hysteresis on looking center
     if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER:

selfdrive/ui/onroad/driver_state.py

@@ -114,7 +114,7 @@ class DriverStateRenderer(Widget):
 
     # Get monitoring state
     dm_state = sm["driverMonitoringState"]
-    self.is_active = dm_state.isActiveMode
+    self.is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
     self.is_rhd = dm_state.isRHD
 
     # Update fade state (smoother transition between active/inactive)

selfdrive/ui/sunnypilot/ui_state.py

@@ -34,7 +34,7 @@ class UIStateSP:
     ]
 
     self.sunnylink_state = SunnylinkState()
-    self.update_params()
+    self.update_params_()
 
     self.onroad_brightness_timer: int = 0
     self.custom_interactive_timeout: int = self.params.get("InteractivityTimeout", return_default=True)
@@ -123,7 +123,7 @@ class UIStateSP:
 
     return "disengaged"
 
-  def update_params(self) -> None:
+  def update_params_(self) -> None:
     CP_SP_bytes = self.params.get("CarParamsSPPersistent")
     if CP_SP_bytes is not None:
       self.CP_SP = messaging.log_from_bytes(CP_SP_bytes, custom.CarParamsSP)

selfdrive/ui/ui_state.py

@@ -15,6 +15,7 @@ from openpilot.system.hardware import HARDWARE, PC
 from openpilot.selfdrive.ui.sunnypilot.ui_state import UIStateSP, DeviceSP
 
 BACKLIGHT_OFFROAD = 65 if HARDWARE.get_device_type() == "mici" else 50
+PARAM_UPDATE_TIME = 5.0
 
 
 class UIStatus(Enum):
@@ -59,6 +60,7 @@ class UIState(UIStateSP):
         "carOutput",
         "carControl",
         "liveParameters",
+        "testJoystick",
         "rawAudioData",
       ] + self.sm_services_ext
     )
@@ -82,15 +84,15 @@ class UIState(UIStateSP):
     self.panda_type: log.PandaState.PandaType = log.PandaState.PandaType.unknown
     self.personality: log.LongitudinalPersonality = log.LongitudinalPersonality.standard
     self.has_longitudinal_control: bool = False
+    self.is_body: bool | None = None
     self.CP: car.CarParams | None = None
     self.light_sensor: float = -1.0
-    self._param_update_time: float = 0.0
+    self._param_update_time: float = -PARAM_UPDATE_TIME
 
     # Callbacks
     self._offroad_transition_callbacks: list[Callable[[], None]] = []
     self._engaged_transition_callbacks: list[Callable[[], None]] = []
-
+    self._on_body_changed_callbacks: list[Callable[[], None]] = []
-    self.update_params()
 
   def add_offroad_transition_callback(self, callback: Callable[[], None]):
     self._offroad_transition_callbacks.append(callback)
@@ -98,6 +100,9 @@ class UIState(UIStateSP):
   def add_engaged_transition_callback(self, callback: Callable[[], None]):
     self._engaged_transition_callbacks.append(callback)
 
+  def add_on_body_changed_callbacks(self, callback: Callable[[], None]):
+    self._on_body_changed_callbacks.append(callback)
+
   @property
   def engaged(self) -> bool:
     return self.started and (self.sm["selfdriveState"].enabled or self.sm["selfdriveStateSP"].mads.enabled)
@@ -113,7 +118,7 @@ class UIState(UIStateSP):
     self.sm.update(0)
     self._update_state()
     self._update_status()
-    if time.monotonic() - self._param_update_time > 5.0:
+    if time.monotonic() - self._param_update_time >= PARAM_UPDATE_TIME:
       self.update_params()
     device.update()
     UIStateSP.update(self)
@@ -188,7 +193,13 @@ class UIState(UIStateSP):
         self.has_longitudinal_control = self.params.get_bool("AlphaLongitudinalEnabled")
       else:
         self.has_longitudinal_control = self.CP.openpilotLongitudinalControl
-    UIStateSP.update_params(self)
+
+      if self.is_body != self.CP.notCar:
+        self.is_body = self.CP.notCar
+        for callback in self._on_body_changed_callbacks:
+          callback()
+
+    UIStateSP.update_params_(self)
     self._param_update_time = time.monotonic()
 
 

sunnypilot/modeld_v2/SConscript

@@ -1,5 +1,6 @@
 import os
 import glob
+from tinygrad import Device
 
 Import('env', 'arch')
 lenv = env.Clone()
@@ -21,10 +22,19 @@ if PC:
   if outputs:
     lenv.Command(outputs, inputs, cmd)
 
-tg_flags = {
+available = set(Device.get_available_devices())
-  'larch64': 'DEV=QCOM FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0',
+if 'CUDA' in available:
-  'Darwin': f'DEV=CPU THREADS=0 HOME={os.path.expanduser("~")}',
+  tg_backend = 'CUDA'
-}.get(arch, 'DEV=CPU CPU_LLVM=1 THREADS=0')
+  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 OPENPILOT_HACKS=1'
+else:
+  tg_backend = 'CPU' if arch == 'Darwin' else 'CPU:LLVM'
+  # THREADS=0 is need to prevent bug: https://github.com/tinygrad/tinygrad/issues/14689
+  tg_flags = f'DEV={tg_backend} THREADS=0'
+
+mac_brew_string = f'HOME={os.path.expanduser("~")}' if arch == 'Darwin' else ''
 
 image_flag = {
   'larch64': 'IMAGE=2',
@@ -38,7 +48,7 @@ def tg_compile(flags, model_name):
   return lenv.Command(
     out,
     [fn + ".onnx"] + tinygrad_files,
-    f'{pythonpath_string} {flags} {image_flag} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {out}'
+    f'{pythonpath_string} {tg_flags} {mac_brew_string} {image_flag} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {out}'
   )
 
 # Compile models
@@ -46,9 +56,9 @@ for model_name in ['supercombo', 'driving_vision', 'driving_off_policy', 'drivin
   if File(f"models/{model_name}.onnx").exists():
     tg_compile(tg_flags, model_name)
 
-script_files = [File("warp.py"), File(Dir("#selfdrive/modeld").File("compile_warp.py").abspath)]
+script_files = [File("warp.py"), File(Dir("#selfdrive/modeld").File("compile_modeld.py").abspath)]
 pythonpath_string = 'PYTHONPATH="${PYTHONPATH}:' + env.Dir("#tinygrad_repo").abspath + ':' + env.Dir("#").abspath + '"'
-compile_warp_cmd = f'{pythonpath_string} {tg_flags} python3 -m sunnypilot.modeld_v2.warp'
+compile_warp_cmd = f'{pythonpath_string} {tg_flags} {mac_brew_string} {image_flag} python3 -m sunnypilot.modeld_v2.warp'
 
 from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
 warp_targets = []

sunnypilot/modeld_v2/tests/test_warp.py

@@ -2,8 +2,11 @@ import os
 os.environ['DEV'] = 'CPU'
 import pytest
 import numpy as np
-from openpilot.selfdrive.modeld.compile_warp import get_nv12_info, CAMERA_CONFIGS
+from openpilot.sunnypilot.modeld_v2.warp import CAMERA_CONFIGS
-from openpilot.sunnypilot.modeld_v2.warp import Warp, MODEL_W, MODEL_H
+from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
+from openpilot.sunnypilot.modeld_v2.warp import Warp
+from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE
+MODEL_W, MODEL_H = MEDMODEL_INPUT_SIZE
 
 VISION_NAME_PAIRS = [  # needed to account for supercombos input_imgs
   ('img', 'big_img'),

sunnypilot/modeld_v2/warp.py

@@ -6,29 +6,61 @@ from tinygrad.tensor import Tensor
 from tinygrad.engine.jit import TinyJit
 from tinygrad.device import Device
 
+# https://github.com/tinygrad/tinygrad/issues/15682
+from tinygrad.uop.ops import UOp, Ops
+_orig = UOp.__reduce__
+UOp.__reduce__ = lambda self: (UOp.unique, ()) if self.op is Ops.UNIQUE else _orig(self)
 from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
-from openpilot.selfdrive.modeld.compile_warp import (
+from openpilot.selfdrive.modeld.compile_modeld import (
-  CAMERA_CONFIGS, MEDMODEL_INPUT_SIZE, make_frame_prepare, make_update_both_imgs,
+  NV12Frame, make_frame_prepare,
-  warp_pkl_path,
 )
+from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
+
+CAMERA_CONFIGS = [
+  (_ar_ox_fisheye.width, _ar_ox_fisheye.height),  # tici: 1928x1208
+  (_os_fisheye.width, _os_fisheye.height),        # mici: 1344x760
+]
+from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE
 
 MODELS_DIR = Path(__file__).parent / 'models'
-MODEL_W, MODEL_H = MEDMODEL_INPUT_SIZE
+
 UPSTREAM_BUFFER_LENGTH = 5
 
+def warp_pkl_path(cam_w, cam_h):
+  return MODELS_DIR / f'warp_{cam_w}x{cam_h}_tinygrad.pkl'
+
+def make_update_img_input(frame_prepare, model_w, model_h):
+  def update_img_input_tinygrad(tensor, frame, M_inv):
+    M_inv = M_inv.to(Device.DEFAULT)
+    new_img = frame_prepare(frame, M_inv)
+    tensor.assign(tensor[6:].cat(new_img, dim=0).contiguous())
+    return Tensor.cat(tensor[:6], tensor[-6:], dim=0).contiguous().reshape(1, 12, model_h//2, model_w//2)
+  return update_img_input_tinygrad
+
+def make_update_both_imgs(frame_prepare, model_w, model_h):
+  update_img = make_update_img_input(frame_prepare, model_w, model_h)
+
+  def update_both_imgs_tinygrad(calib_img_buffer, new_img, M_inv,
+                                calib_big_img_buffer, new_big_img, M_inv_big):
+    calib_img_pair = update_img(calib_img_buffer, new_img, M_inv)
+    calib_big_img_pair = update_img(calib_big_img_buffer, new_big_img, M_inv_big)
+    return calib_img_pair, calib_big_img_pair
+  return update_both_imgs_tinygrad
+
 
 def v2_warp_pkl_path(cam_w, cam_h, buffer_length):
   return MODELS_DIR / f'warp_{cam_w}x{cam_h}_b{buffer_length}_tinygrad.pkl'
 
 
-def compile_v2_warp(cam_w, cam_h, buffer_length):
+def compile_v2_warp(cam_w, cam_h, buffer_length, model_w=MEDMODEL_INPUT_SIZE[0], model_h=MEDMODEL_INPUT_SIZE[1]):
   _, _, _, yuv_size = get_nv12_info(cam_w, cam_h)
-  img_buffer_shape = (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
+  img_buffer_shape = (buffer_length * 6, model_h // 2, model_w // 2)
 
   print(f"Compiling v2 warp for {cam_w}x{cam_h} buffer_length={buffer_length}...")
 
-  frame_prepare = make_frame_prepare(cam_w, cam_h, MODEL_W, MODEL_H)
+  nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-  update_both_imgs = make_update_both_imgs(frame_prepare, MODEL_W, MODEL_H)
+  frame_prepare = make_frame_prepare(nv12, model_w, model_h)
+  update_both_imgs = make_update_both_imgs(frame_prepare, model_w, model_h)
   update_img_jit = TinyJit(update_both_imgs, prune=True)
 
   full_buffer = Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize()
@@ -62,9 +94,11 @@ def compile_v2_warp(cam_w, cam_h, buffer_length):
 
 
 class Warp:
-  def __init__(self, buffer_length=2):
+  def __init__(self, buffer_length=2, model_w=MEDMODEL_INPUT_SIZE[0], model_h=MEDMODEL_INPUT_SIZE[1]):
     self.buffer_length = buffer_length
-    self.img_buffer_shape = (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
+    self.model_w = model_w
+    self.model_h = model_h
+    self.img_buffer_shape = (buffer_length * 6, model_h // 2, model_w // 2)
 
     self.jit_cache = {}
     self.full_buffers = {k: Tensor.zeros(self.img_buffer_shape, dtype='uint8').contiguous().realize() for k in ['img', 'big_img']}
@@ -92,8 +126,9 @@ class Warp:
           with open(upstream_pkl, 'rb') as f:
             self.jit_cache[key] = pickle.load(f)
       if key not in self.jit_cache:
-        frame_prepare = make_frame_prepare(cam_w, cam_h, MODEL_W, MODEL_H)
+        nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-        update_both_imgs = make_update_both_imgs(frame_prepare, MODEL_W, MODEL_H)
+        frame_prepare = make_frame_prepare(nv12, self.model_w, self.model_h)
+        update_both_imgs = make_update_both_imgs(frame_prepare, self.model_w, self.model_h)
         self.jit_cache[key] = TinyJit(update_both_imgs, prune=True)
 
     if key not in self._nv12_cache:

sunnypilot/models/fetcher.py

@@ -116,7 +116,7 @@ class ModelCache:
 
 class ModelFetcher:
   """Handles fetching and caching of model data from remote source"""
-  MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v16.json"
+  MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v17.json"
 
   def __init__(self, params: Params):
     self.params = params

sunnypilot/models/tests/model_hash

@@ -1 +1 @@
-5d4d21f1899de21137f69d74a4602c44cc5a6b04cf4e4aa9d0ec9206f8c30350
+32f57bdc91f910df1f48ddae7c59aaf6e751f9df6756da481a210577dbce8bcf

sunnypilot/selfdrive/locationd/locationd.cc

@@ -255,7 +255,7 @@ void Localizer::handle_sensor(double current_time, const cereal::SensorEventData
   }
 
   // Gyro Uncalibrated
-  if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
+  if (log.which() == cereal::SensorEventData::GYRO_UNCALIBRATED) {
     auto v = log.getGyroUncalibrated().getV();
     auto meas = Vector3d(-v[2], -v[1], -v[0]);
 
@@ -273,7 +273,7 @@ void Localizer::handle_sensor(double current_time, const cereal::SensorEventData
   }
 
   // Accelerometer
-  if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
+  if (log.which() == cereal::SensorEventData::ACCELERATION) {
     auto v = log.getAcceleration().getV();
 
     // TODO: reduce false positives and re-enable this check

sunnypilot/selfdrive/locationd/tests/test_locationd.py

@@ -19,7 +19,7 @@ if platform.system() == 'Darwin':
 
 class TestLocationdProc:
   LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
-              'accelerometer', 'gyroscope', 'magnetometer']
+              'accelerometer', 'gyroscope']
 
   def setup_method(self):
     self.pm = messaging.PubMaster(self.LLD_MSGS)

sunnypilot/system/sensord/sensors/bmx055_accel.cc

@@ -71,9 +71,10 @@ bool BMX055_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
 
   auto event = msg.initEvent().initAccelerometer2();
   event.setSource(cereal::SensorEventData::SensorSource::BMX055);
-  event.setVersion(1);
+  auto deprecated = event.initDeprecated();
-  event.setSensor(SENSOR_ACCELEROMETER);
+  deprecated.setVersion(1);
-  event.setType(SENSOR_TYPE_ACCELEROMETER);
+  deprecated.setSensor(SENSOR_ACCELEROMETER);
+  deprecated.setType(SENSOR_TYPE_ACCELEROMETER);
   event.setTimestamp(start_time);
 
   float xyz[] = {x, y, z};

sunnypilot/system/sensord/sensors/bmx055_gyro.cc

@@ -78,9 +78,10 @@ bool BMX055_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
 
   auto event = msg.initEvent().initGyroscope2();
   event.setSource(cereal::SensorEventData::SensorSource::BMX055);
-  event.setVersion(1);
+  auto deprecated = event.initDeprecated();
-  event.setSensor(SENSOR_GYRO_UNCALIBRATED);
+  deprecated.setVersion(1);
-  event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
+  deprecated.setSensor(SENSOR_GYRO_UNCALIBRATED);
+  deprecated.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
   event.setTimestamp(start_time);
 
   float xyz[] = {x, y, z};

sunnypilot/system/sensord/sensors/bmx055_magn.cc

@@ -229,9 +229,10 @@ bool BMX055_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
 
     auto event = msg.initEvent().initMagnetometer();
     event.setSource(cereal::SensorEventData::SensorSource::BMX055);
-    event.setVersion(2);
+    auto deprecated = event.initDeprecated();
-    event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
+    deprecated.setVersion(2);
-    event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
+    deprecated.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
+    deprecated.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
     event.setTimestamp(start_time);
 
     // Move magnetometer into same reference frame as accel/gryo

sunnypilot/system/sensord/sensors/bmx055_temp.cc

@@ -22,8 +22,9 @@ bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
 
   auto event = msg.initEvent().initTemperatureSensor();
   event.setSource(cereal::SensorEventData::SensorSource::BMX055);
-  event.setVersion(1);
+  auto deprecated = event.initDeprecated();
-  event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
+  deprecated.setVersion(1);
+  deprecated.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
   event.setTimestamp(start_time);
   event.setTemperature(temp);
 

sunnypilot/system/sensord/sensors/lsm6ds3_accel.cc

@@ -236,9 +236,10 @@ bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
 
   auto event = msg.initEvent().initAccelerometer();
   event.setSource(source);
-  event.setVersion(1);
+  auto deprecated = event.initDeprecated();
-  event.setSensor(SENSOR_ACCELEROMETER);
+  deprecated.setVersion(1);
-  event.setType(SENSOR_TYPE_ACCELEROMETER);
+  deprecated.setSensor(SENSOR_ACCELEROMETER);
+  deprecated.setType(SENSOR_TYPE_ACCELEROMETER);
   event.setTimestamp(ts);
 
   float xyz[] = {y, -x, z};

sunnypilot/system/sensord/sensors/lsm6ds3_gyro.cc

@@ -219,9 +219,10 @@ bool LSM6DS3_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
 
   auto event = msg.initEvent().initGyroscope();
   event.setSource(source);
-  event.setVersion(2);
+  auto deprecated = event.initDeprecated();
-  event.setSensor(SENSOR_GYRO_UNCALIBRATED);
+  deprecated.setVersion(2);
-  event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
+  deprecated.setSensor(SENSOR_GYRO_UNCALIBRATED);
+  deprecated.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
   event.setTimestamp(ts);
 
   float xyz[] = {y, -x, z};

sunnypilot/system/sensord/sensors/lsm6ds3_temp.cc

@@ -28,8 +28,9 @@ bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
 
   auto event = msg.initEvent().initTemperatureSensor();
   event.setSource(source);
-  event.setVersion(1);
+  auto deprecated = event.initDeprecated();
-  event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
+  deprecated.setVersion(1);
+  deprecated.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
   event.setTimestamp(start_time);
   event.setTemperature(temp);
 

sunnypilot/system/sensord/sensors/mmc5603nj_magn.cc

@@ -91,9 +91,10 @@ bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
 
   auto event = msg.initEvent().initMagnetometer();
   event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ);
-  event.setVersion(1);
+  auto deprecated = event.initDeprecated();
-  event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
+  deprecated.setVersion(1);
-  event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
+  deprecated.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
+  deprecated.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
   event.setTimestamp(start_time);
 
   float vals[] = {xyz[0], xyz[1], xyz[2], reset_xyz[0], reset_xyz[1], reset_xyz[2]};

system/camerad/cameras/cdm.cc

@@ -1,9 +1,9 @@
 #include "cdm.h"
 #include "stddef.h"
 
-int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel) {
+int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel, uint8_t opcode) {
   struct cdm_dmi_cmd *cmd = (struct cdm_dmi_cmd*)dst;
-  cmd->cmd = CAM_CDM_CMD_DMI_32;
+  cmd->cmd = opcode;
   cmd->length = length - 1;
   cmd->reserved = 0;
   cmd->addr = 0; // gets patched in

system/camerad/cameras/cdm.h

@@ -6,11 +6,6 @@
 #include <vector>
 #include <memory>
 
-// our helpers
-int write_random(uint8_t *dst, const std::vector<uint32_t> &vals);
-int write_cont(uint8_t *dst, uint32_t reg, const std::vector<uint32_t> &vals);
-int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel);
-
 // from drivers/media/platform/msm/camera/cam_cdm/cam_cdm_util.{c,h}
 
 enum cam_cdm_command {
@@ -32,6 +27,11 @@ enum cam_cdm_command {
   CAM_CDM_CMD_PRIVATE_BASE_MAX = 0x7F
 };
 
+// our helpers
+int write_random(uint8_t *dst, const std::vector<uint32_t> &vals);
+int write_cont(uint8_t *dst, uint32_t reg, const std::vector<uint32_t> &vals);
+int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel, uint8_t opcode = CAM_CDM_CMD_DMI_32);
+
 /**
  * struct cdm_regrandom_cmd - Definition for CDM random register command.
  * @count: Number of register writes

system/camerad/cameras/spectra.cc

@@ -466,8 +466,11 @@ void SpectraCamera::config_bps(int idx, int request_id) {
     * BPS = Bayer Processing Segment
   */
 
-  int size = sizeof(struct cam_packet) + sizeof(struct cam_cmd_buf_desc)*2 + sizeof(struct cam_buf_io_cfg)*2;
+  bool needs_downscale = sensor->out_scale > 1;
-  size += sizeof(struct cam_patch_desc)*9;
+  int num_io_cfgs = needs_downscale ? 3 : 2;
+  int num_patches = needs_downscale ? 14 : 12;
+  int size = sizeof(struct cam_packet) + sizeof(struct cam_cmd_buf_desc)*2 + sizeof(struct cam_buf_io_cfg)*num_io_cfgs;
+  size += sizeof(struct cam_patch_desc)*num_patches;
 
   uint32_t cam_packet_handle = 0;
   auto pkt = m->mem_mgr.alloc<struct cam_packet>(size, &cam_packet_handle);
@@ -545,8 +548,15 @@ void SpectraCamera::config_bps(int idx, int request_id) {
     int cdm_len = 0;
 
     if (bps_lin_reg.size() == 0) {
+      // set first knee pt to do BLC
+      uint32_t new_knee[8];
+      new_knee[0] = sensor->black_level << (14 - sensor->bits_per_pixel);
+      for (int i = 0; i < 7; i++) {
+        uint32_t pts = sensor->linearization_pts[i / 2];
+        new_knee[i + 1] = (i % 2 == 0) ? (pts >> 16) : (pts & 0xffff);
+      }
       for (int i = 0; i < 4; i++) {
-        bps_lin_reg.push_back(((sensor->linearization_pts[i] & 0xffff) << 0x10) | (sensor->linearization_pts[i] >> 0x10));
+        bps_lin_reg.push_back((new_knee[2*i + 1] << 16) | new_knee[2*i]);
       }
     }
 
@@ -569,20 +579,24 @@ void SpectraCamera::config_bps(int idx, int request_id) {
       0x00000080,
       0x00800066,
     });
-    // linearization, EN=0
+    // linearization
     cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1868, bps_lin_reg);
     cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1878, bps_lin_reg);
     cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1888, bps_lin_reg);
     cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1898, bps_lin_reg);
-    /*
-    uint8_t *start = (unsigned char *)bps_cdm_program_array.ptr + cdm_len;
     uint64_t addr;
-    cdm_len += write_dmi((unsigned char *)bps_cdm_program_array.ptr + cdm_len, &addr, sensor->linearization_lut.size()*sizeof(uint32_t), 0x1808, 1);
+    cdm_len += write_dmi((unsigned char *)bps_cdm_program_array.ptr + cdm_len, &addr, sensor->linearization_lut.size()*sizeof(uint32_t), 0x1808, 1, CAM_CDM_CMD_DMI);
-    patches.push_back(addr - (uint64_t)start);
+    patches.push_back(addr - (uint64_t)bps_cdm_program_array.ptr);
-    */
+
     // color correction
     cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x2e68, bps_ccm_reg);
 
+    // gamma
+    for (uint8_t ch = 1; ch <= 3; ch++) {
+      cdm_len += write_dmi((unsigned char *)bps_cdm_program_array.ptr + cdm_len, &addr, sensor->gamma_lut_rgb.size()*sizeof(uint32_t), 0x3208, ch, CAM_CDM_CMD_DMI);
+      patches.push_back(addr - (uint64_t)bps_cdm_program_array.ptr);
+    }
+
     cdm_len += build_common_ife_bps((unsigned char *)bps_cdm_program_array.ptr + cdm_len, cc, sensor.get(), patches, false);
 
     pa->length = cdm_len - 1;
@@ -596,7 +610,7 @@ void SpectraCamera::config_bps(int idx, int request_id) {
     tmp.header = CAM_ICP_CMD_GENERIC_BLOB_CLK;
     tmp.header |= (sizeof(cam_icp_clk_bw_request)) << 8;
     tmp.clk.budget_ns = 0x1fca058;
-    tmp.clk.frame_cycles = 2329024; // comes from the striping lib
+    tmp.clk.frame_cycles = sensor->frame_width * sensor->frame_height; // matches striping lib pixelCount
     tmp.clk.rt_flag = 0x0;
     tmp.clk.uncompressed_bw = 0x38512180;
     tmp.clk.compressed_bw = 0x38512180;
@@ -611,7 +625,7 @@ void SpectraCamera::config_bps(int idx, int request_id) {
   }
 
   // *** io config ***
-  pkt->num_io_configs = 2;
+  pkt->num_io_configs = num_io_cfgs;
   pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf;
   struct cam_buf_io_cfg *io_cfg = (struct cam_buf_io_cfg *)((char*)&pkt->payload + pkt->io_configs_offset);
   {
@@ -655,28 +669,69 @@ void SpectraCamera::config_bps(int idx, int request_id) {
 
     io_cfg[1].format = CAM_FORMAT_NV12;  // TODO: why is this 21 in the dump? should be 12
     io_cfg[1].color_space = CAM_COLOR_SPACE_BT601_FULL;
-    io_cfg[1].resource_type = CAM_ICP_BPS_OUTPUT_IMAGE_FULL;
+    io_cfg[1].resource_type = needs_downscale ? CAM_ICP_BPS_OUTPUT_IMAGE_REG1 : CAM_ICP_BPS_OUTPUT_IMAGE_FULL;
     io_cfg[1].fence = sync_objs_bps[idx];
     io_cfg[1].direction = CAM_BUF_OUTPUT;
     io_cfg[1].subsample_pattern = 0x1;
     io_cfg[1].framedrop_pattern = 0x1;
+
+    if (needs_downscale) {
+      // downscaling needs a full res placeholder
+      uint32_t full_stride, full_y_h, full_uv_h, full_yuv_size;
+      std::tie(full_stride, full_y_h, full_uv_h, full_yuv_size) = get_nv12_info(sensor->frame_width, sensor->frame_height);
+      io_cfg[2].mem_handle[0] = bps_fullres_dummy.handle;
+      io_cfg[2].mem_handle[1] = bps_fullres_dummy.handle;
+      io_cfg[2].planes[0] = (struct cam_plane_cfg){
+        .width = sensor->frame_width,
+        .height = sensor->frame_height,
+        .plane_stride = full_stride,
+        .slice_height = full_y_h,
+      };
+      io_cfg[2].planes[1] = (struct cam_plane_cfg){
+        .width = sensor->frame_width,
+        .height = sensor->frame_height / 2,
+        .plane_stride = full_stride,
+        .slice_height = full_uv_h,
+      };
+      io_cfg[2].offsets[1] = ALIGNED_SIZE(full_stride * full_y_h, 0x1000);
+      io_cfg[2].format = CAM_FORMAT_NV12;
+      io_cfg[2].color_space = CAM_COLOR_SPACE_BT601_FULL;
+      io_cfg[2].resource_type = CAM_ICP_BPS_OUTPUT_IMAGE_FULL;
+      io_cfg[2].fence = sync_objs_bps[idx];
+      io_cfg[2].direction = CAM_BUF_OUTPUT;
+      io_cfg[2].subsample_pattern = 0x1;
+      io_cfg[2].framedrop_pattern = 0x1;
+    }
   }
 
   // *** patches ***
   {
-    assert(patches.size() == 0 | patches.size() == 1);
+    assert(patches.size() == 0 || patches.size() == 4);
     pkt->patch_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf + sizeof(struct cam_buf_io_cfg)*pkt->num_io_configs;
 
     if (patches.size() > 0) {
-      add_patch(pkt.get(), bps_cmd.handle, patches[0], bps_linearization_lut.handle, 0);
+      // linearization LUT
+      add_patch(pkt.get(), bps_cdm_program_array.handle, patches[0], bps_linearization_lut.handle, 0);
+      // gamma LUTs
+      for (int i = 0; i < 3; i++) {
+        add_patch(pkt.get(), bps_cdm_program_array.handle, patches[i+1], bps_gamma_lut.handle, 0);
+      }
     }
 
     // input frame
     add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[0].ptr[0]), buf_handle_raw[idx], 0);
 
-    // output frame
+    if (needs_downscale) {
-    add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[0]), buf_handle_yuv[idx], 0);
+      add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[0]), bps_fullres_dummy.handle, 0);
-    add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[1]), buf_handle_yuv[idx], io_cfg[1].offsets[1]);
+      add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[1]), bps_fullres_dummy.handle, io_cfg[2].offsets[1]);
+      // output frame at REG1
+      add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[7].ptr[0]), buf_handle_yuv[idx], 0);
+      add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[7].ptr[1]), buf_handle_yuv[idx], io_cfg[1].offsets[1]);
+    } else {
+      // output frame at FULL
+      add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[0]), buf_handle_yuv[idx], 0);
+      add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[1]), buf_handle_yuv[idx], io_cfg[1].offsets[1]);
+    }
 
     // rest of buffers
     add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, settings_addr), bps_iq.handle, 0);
@@ -987,9 +1042,6 @@ bool SpectraCamera::openSensor() {
   LOGD("-- Probing sensor %d", cc.camera_num);
 
   auto init_sensor_lambda = [this](SensorInfo *s) {
-    if (s->image_sensor == cereal::FrameData::ImageSensor::OS04C10 && cc.output_type == ISP_IFE_PROCESSED) {
-      ((OS04C10*)s)->ife_downscale_configure();
-    }
     sensor.reset(s);
     return (sensors_init() == 0);
   };
@@ -1167,13 +1219,39 @@ void SpectraCamera::configICP() {
 
   // used internally by the BPS, we just allocate it.
   // size comes from the BPSStripingLib
-  bps_cdm_striping_bl.init(m, 0xa100, 0x20, true, m->icp_device_iommu);
+  bps_cdm_striping_bl.init(m, 0xcfe0, 0x20, true, m->icp_device_iommu);
+
+  if (sensor->out_scale > 1) {
+    uint32_t full_stride, full_y_h, full_uv_h, full_yuv_size;
+    std::tie(full_stride, full_y_h, full_uv_h, full_yuv_size) = get_nv12_info(sensor->frame_width, sensor->frame_height);
+    bps_fullres_dummy.init(m, full_yuv_size, 0x1000, true, m->icp_device_iommu);
+  }
 
   // LUTs
-  /*
+  assert(sensor->linearization_lut.size() == 36);
   bps_linearization_lut.init(m, sensor->linearization_lut.size()*sizeof(uint32_t), 0x20, true, m->icp_device_iommu);
-  memcpy(bps_linearization_lut.ptr, sensor->linearization_lut.data(), bps_linearization_lut.size);
+
-  */
+  // bit shift linearization_lut to bps specs, also compensate for black level here
+  uint32_t bl = sensor->black_level << (14 - sensor->bits_per_pixel);
+  uint32_t* bps_lut = (uint32_t*)bps_linearization_lut.ptr;
+  for (size_t i = 0; i < sensor->linearization_lut.size(); i++) {
+    size_t seg = i / 4;
+    size_t ch = i % 4;
+    if (seg == 0) {
+      bps_lut[i] = 0;
+      continue;
+    }
+    uint32_t e = sensor->linearization_lut[(seg - 1) * 4 + ch];
+    uint32_t base = e & 0x3fff;
+    uint32_t slope_q11 = (e >> 14) & 0x3fff;
+    uint32_t slope_q12 = std::min<uint32_t>(slope_q11 << 1, 0x3fff);
+    base = (base > bl) ? (base - bl) : 0;
+    bps_lut[i] = base | (slope_q12 << 14);
+  }
+
+  assert(sensor->gamma_lut_rgb.size() == 64);
+  bps_gamma_lut.init(m, sensor->gamma_lut_rgb.size()*sizeof(uint32_t), 0x20, true, m->icp_device_iommu);
+  memcpy(bps_gamma_lut.ptr, sensor->gamma_lut_rgb.data(), bps_gamma_lut.size);
 }
 
 void SpectraCamera::configCSIPHY() {

system/camerad/cameras/spectra.h

@@ -173,6 +173,8 @@ public:
   SpectraBuf bps_iq;
   SpectraBuf bps_striping;
   SpectraBuf bps_linearization_lut;
+  SpectraBuf bps_gamma_lut;
+  SpectraBuf bps_fullres_dummy;
   std::vector<uint32_t> bps_lin_reg;
   std::vector<uint32_t> bps_ccm_reg;
 

system/camerad/sensors/os04c10.cc

@@ -21,27 +21,16 @@ const uint32_t os04c10_analog_gains_reg[] = {
 
 }  // namespace
 
-void OS04C10::ife_downscale_configure() {
-  out_scale = 2;
-
-  pixel_size_mm = 0.002;
-  frame_width = 2688;
-  frame_height = 1520;
-  exposure_time_max = 2352;
-
-  init_reg_array.insert(init_reg_array.end(), std::begin(ife_downscale_override_array_os04c10), std::end(ife_downscale_override_array_os04c10));
-}
-
 OS04C10::OS04C10() {
   image_sensor = cereal::FrameData::ImageSensor::OS04C10;
   bayer_pattern = CAM_ISP_PATTERN_BAYER_BGBGBG;
-  pixel_size_mm = 0.004;
+  pixel_size_mm = 0.002;
   data_word = false;
 
-  // hdr_offset = 64 * 2 + 8; // stagger
+  out_scale = 2;
-  frame_width = 1344;
+  frame_width = 2688;
-  frame_height = 760; //760 * 2 + hdr_offset;
+  frame_height = 1520;
-  frame_stride = (frame_width * 12 / 8); // no alignment
+  frame_stride = frame_width * 12 / 8;
 
   extra_height = 0;
   frame_offset = 0;
@@ -65,7 +54,7 @@ OS04C10::OS04C10() {
   dc_gain_on_grey = 0.9;
   dc_gain_off_grey = 1.0;
   exposure_time_min = 2;
-  exposure_time_max = 1684;
+  exposure_time_max = 2352;
   analog_gain_min_idx = 0x0;
   analog_gain_rec_idx = 0x0;  // 1x
   analog_gain_max_idx = 0x28;

system/camerad/sensors/os04c10_registers.h

@@ -4,7 +4,7 @@ const struct i2c_random_wr_payload start_reg_array_os04c10[] = {{0x100, 1}};
 const struct i2c_random_wr_payload stop_reg_array_os04c10[] = {{0x100, 0}};
 
 const struct i2c_random_wr_payload init_array_os04c10[] = {
-  // baseed on DP_2688X1520_NEWSTG_MIPI0776Mbps_30FPS_10BIT_FOURLANE
+  // OS04C10_AA_00_02_17_wAO_2688x1524_MIPI728Mbps_Linear12bit_20FPS_4Lane_MCLK24MHz
   {0x0103, 0x01}, // software reset
 
   // PLL + clocks
@@ -93,7 +93,7 @@ const struct i2c_random_wr_payload init_array_os04c10[] = {
   {0x388b, 0x00},
   {0x3c80, 0x10},
   {0x3c86, 0x00},
-  {0x3c8c, 0x20},
+  {0x3c8c, 0x40},
   {0x3c9f, 0x01},
   {0x3d85, 0x1b},
   {0x3d8c, 0x71},
@@ -197,7 +197,7 @@ const struct i2c_random_wr_payload init_array_os04c10[] = {
   {0x370b, 0x48},
   {0x370c, 0x01},
   {0x370f, 0x00},
-  {0x3714, 0x28},
+  {0x3714, 0x24},
   {0x3716, 0x04},
   {0x3719, 0x11},
   {0x371a, 0x1e},
@@ -229,7 +229,7 @@ const struct i2c_random_wr_payload init_array_os04c10[] = {
   {0x37bd, 0x01},
   {0x37bf, 0x26},
   {0x37c0, 0x11},
-  {0x37c2, 0x14},
+  {0x37c2, 0x04},
   {0x37cd, 0x19},
   {0x37e0, 0x08},
   {0x37e6, 0x04},
@@ -239,14 +239,14 @@ const struct i2c_random_wr_payload init_array_os04c10[] = {
   {0x37d8, 0x02},
   {0x37e2, 0x10},
   {0x3739, 0x10},
-  {0x3662, 0x08},
+  {0x3662, 0x10},
   {0x37e4, 0x20},
   {0x37e3, 0x08},
-  {0x37d9, 0x04},
+  {0x37d9, 0x08},
   {0x4040, 0x00},
-  {0x4041, 0x03},
+  {0x4041, 0x07},
-  {0x4008, 0x01},
+  {0x4008, 0x02},
-  {0x4009, 0x06},
+  {0x4009, 0x0d},
 
   // FSIN - frame sync
   {0x3002, 0x22},
@@ -267,20 +267,20 @@ const struct i2c_random_wr_payload init_array_os04c10[] = {
   {0x3802, 0x00}, {0x3803, 0x00},
   {0x3804, 0x0a}, {0x3805, 0x8f},
   {0x3806, 0x05}, {0x3807, 0xff},
-  {0x3808, 0x05}, {0x3809, 0x40},
+  {0x3808, 0x0a}, {0x3809, 0x80},
-  {0x380a, 0x02}, {0x380b, 0xf8},
+  {0x380a, 0x05}, {0x380b, 0xf0},
   {0x3811, 0x08},
   {0x3813, 0x08},
-  {0x3814, 0x03},
+  {0x3814, 0x01},
   {0x3815, 0x01},
-  {0x3816, 0x03},
+  {0x3816, 0x01},
   {0x3817, 0x01},
 
-  {0x380c, 0x0b}, {0x380d, 0xac}, // HTS (line length)
+  {0x380c, 0x08}, {0x380d, 0x5c}, // HTS (line length)
-  {0x380e, 0x06}, {0x380f, 0x9c}, // VTS (frame length)
+  {0x380e, 0x09}, {0x380f, 0x38}, // VTS (frame length)
 
-  {0x3820, 0xb3},
+  {0x3820, 0xb0},
-  {0x3821, 0x01},
+  {0x3821, 0x00},
   {0x3880, 0x00},
   {0x3882, 0x20},
   {0x3c91, 0x0b},
@@ -330,23 +330,3 @@ const struct i2c_random_wr_payload init_array_os04c10[] = {
   {0x5104, 0x08}, {0x5105, 0xd6},
   {0x5144, 0x08}, {0x5145, 0xd6},
 };
-
-const struct i2c_random_wr_payload ife_downscale_override_array_os04c10[] = {
-  // based on OS04C10_AA_00_02_17_wAO_2688x1524_MIPI728Mbps_Linear12bit_20FPS_4Lane_MCLK24MHz
-  {0x3c8c, 0x40},
-  {0x3714, 0x24},
-  {0x37c2, 0x04},
-  {0x3662, 0x10},
-  {0x37d9, 0x08},
-  {0x4041, 0x07},
-  {0x4008, 0x02},
-  {0x4009, 0x0d},
-  {0x3808, 0x0a}, {0x3809, 0x80},
-  {0x380a, 0x05}, {0x380b, 0xf0},
-  {0x3814, 0x01},
-  {0x3816, 0x01},
-  {0x380c, 0x08}, {0x380d, 0x5c}, // HTS
-  {0x380e, 0x09}, {0x380f, 0x38}, // VTS
-  {0x3820, 0xb0},
-  {0x3821, 0x00},
-};

system/camerad/sensors/sensor.h

@@ -98,7 +98,6 @@ public:
 class OS04C10 : public SensorInfo {
 public:
   OS04C10();
-  void ife_downscale_configure();
   std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
   float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
   int getSlaveAddress(int port) const override;

system/hardware/base.py

@@ -94,6 +94,10 @@ class LPABase(ABC):
   def process_notifications(self) -> None:
     pass
 
+  @abstractmethod
+  def is_euicc(self) -> bool:
+    pass
+
   def is_comma_profile(self, iccid: str) -> bool:
     return any(iccid.startswith(prefix) for prefix in ('8985235',))
 

system/hardware/esim.py

@@ -28,8 +28,7 @@ if __name__ == '__main__':
     lpa.nickname_profile(args.nickname[0], args.nickname[1])
   else:
     parser.print_help()
-
+    profiles = lpa.list_profiles()
-  profiles = lpa.list_profiles()
+    print(f'\n{len(profiles)} profile{"s" if len(profiles) > 1 else ""}:')
-  print(f'\n{len(profiles)} profile{"s" if len(profiles) > 1 else ""}:')
+    for p in profiles:
-  for p in profiles:
+      print(f'- {p.iccid} (nickname: {p.nickname or "<none provided>"}) (provider: {p.provider}) - {"enabled" if p.enabled else "disabled"}')
-    print(f'- {p.iccid} (nickname: {p.nickname or "<none provided>"}) (provider: {p.provider}) - {"enabled" if p.enabled else "disabled"}')

system/hardware/hardwared.py

@@ -40,15 +40,21 @@ HardwareState = namedtuple("HardwareState", ['network_type', 'network_info', 'ne
 
 # List of thermal bands. We will stay within this region as long as we are within the bounds.
 # When exiting the bounds, we'll jump to the lower or higher band. Bands are ordered in the dict.
-THERMAL_BANDS = OrderedDict({
+if HARDWARE.get_device_type() == "mici":
-  ThermalStatus.green: ThermalBand(None, 80.0),
+  THERMAL_BANDS = OrderedDict({
-  ThermalStatus.yellow: ThermalBand(75.0, 96.0),
+    ThermalStatus.ok: ThermalBand(None, 100.0),
-  ThermalStatus.red: ThermalBand(88.0, 107.),
+    ThermalStatus.overheated: ThermalBand(92.0, 107.),
-  ThermalStatus.danger: ThermalBand(94.0, None),
+    ThermalStatus.critical: ThermalBand(98.0, None),
-})
+  })
+else:
+  THERMAL_BANDS = OrderedDict({
+    ThermalStatus.ok: ThermalBand(None, 96.0),
+    ThermalStatus.overheated: ThermalBand(88.0, 107.),
+    ThermalStatus.critical: ThermalBand(94.0, None),
+  })
 
 # Override to highest thermal band when offroad and above this temp
-OFFROAD_DANGER_TEMP = 75
+OFFROAD_DANGER_TEMP = 85 if HARDWARE.get_device_type() == "mici" else 75
 
 prev_offroad_states: dict[str, tuple[bool, str | None]] = {}
 
@@ -180,7 +186,7 @@ def hardware_thread(end_event, hw_queue) -> None:
   started_ts: float | None = None
   started_seen = False
   startup_blocked_ts: float | None = None
-  thermal_status = ThermalStatus.yellow
+  thermal_status = ThermalStatus.ok
 
   last_hw_state = HardwareState(
     network_type=NetworkType.none,
@@ -288,7 +294,7 @@ def hardware_thread(end_event, hw_queue) -> None:
     if is_offroad_for_5_min and offroad_comp_temp > OFFROAD_DANGER_TEMP:
       # if device is offroad and already hot without the extra onroad load,
       # we want to cool down first before increasing load
-      thermal_status = ThermalStatus.danger
+      thermal_status = ThermalStatus.critical
     else:
       current_band = THERMAL_BANDS[thermal_status]
       band_idx = list(THERMAL_BANDS.keys()).index(thermal_status)
@@ -312,7 +318,7 @@ def hardware_thread(end_event, hw_queue) -> None:
     startup_conditions["not_taking_snapshot"] = not params.get_bool("IsTakingSnapshot")
 
     # must be at an engageable thermal band to go onroad
-    startup_conditions["device_temp_engageable"] = thermal_status < ThermalStatus.red
+    startup_conditions["device_temp_engageable"] = thermal_status < ThermalStatus.overheated
 
     # ensure device is fully booted
     startup_conditions["device_booted"] = startup_conditions.get("device_booted", False) or HARDWARE.booted()
@@ -333,7 +339,7 @@ def hardware_thread(end_event, hw_queue) -> None:
     set_offroad_alert("Offroad_TiciSupport", is_unsupported_combo, extra_text=build_metadata.channel)
 
     # if the temperature enters the danger zone, go offroad to cool down
-    onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger
+    onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.critical
     extra_text = f"{offroad_comp_temp:.1f}C"
     show_alert = (not onroad_conditions["device_temp_good"] or not startup_conditions["device_temp_engageable"]) and onroad_conditions["ignition"]
     set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", show_alert, extra_text=extra_text)

system/hardware/tici/agnos.json

@@ -56,28 +56,28 @@
   },
   {
     "name": "boot",
-    "url": "https://commadist.azureedge.net/agnosupdate/boot-d726315cf98a43e1090e5b49297404cf3d084cfbd42ad8bb7d8afb68136b9f51.img.xz",
+    "url": "https://commadist.azureedge.net/agnosupdate/boot-3a6285bc114d21cfdab312c42f5a76f81762328b41d404816ee46a2f83523f04.img.xz",
-    "hash": "d726315cf98a43e1090e5b49297404cf3d084cfbd42ad8bb7d8afb68136b9f51",
+    "hash": "3a6285bc114d21cfdab312c42f5a76f81762328b41d404816ee46a2f83523f04",
-    "hash_raw": "d726315cf98a43e1090e5b49297404cf3d084cfbd42ad8bb7d8afb68136b9f51",
+    "hash_raw": "3a6285bc114d21cfdab312c42f5a76f81762328b41d404816ee46a2f83523f04",
     "size": 17500160,
     "sparse": false,
     "full_check": true,
     "has_ab": true,
-    "ondevice_hash": "2454108de1161289bc4a75449ad6421f1772b13b3e5cba68a84fca7530557699"
+    "ondevice_hash": "4e12accbcbb03fd9eebc5c1231a7b79dc1d9e15341cce56dc88930832ee86207"
   },
   {
     "name": "system",
-    "url": "https://commadist.azureedge.net/agnosupdate/system-dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5.img.xz",
+    "url": "https://commadist.azureedge.net/agnosupdate/system-82ffa59e322c5abab18f2ec6fb83dce124f55c9658c5c673e9adbd4655d6e6a2.img.xz",
-    "hash": "5f319030ad05942267b77f1a4686c4ca24cc09b2c2a4688e57342ffc9720fd49",
+    "hash": "bb4ccf77037120ace841cbcd94387c0650b317595cd0eab555b3ba0beb74e840",
-    "hash_raw": "dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5",
+    "hash_raw": "82ffa59e322c5abab18f2ec6fb83dce124f55c9658c5c673e9adbd4655d6e6a2",
     "size": 4718592000,
     "sparse": true,
     "full_check": false,
     "has_ab": true,
-    "ondevice_hash": "c12f1b7d790a418aea17424accf4cd59c575e5745cad82bdc9452f384483648c",
+    "ondevice_hash": "f1505151cde236f0d3ccdb764e3c604761ad2c1beb40f2c340ea9470d0cecc0e",
     "alt": {
-      "hash": "dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5",
+      "hash": "82ffa59e322c5abab18f2ec6fb83dce124f55c9658c5c673e9adbd4655d6e6a2",
-      "url": "https://commadist.azureedge.net/agnosupdate/system-dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5.img",
+      "url": "https://commadist.azureedge.net/agnosupdate/system-82ffa59e322c5abab18f2ec6fb83dce124f55c9658c5c673e9adbd4655d6e6a2.img",
       "size": 4718592000
     }
   }

system/hardware/tici/all-partitions.json

@@ -339,51 +339,51 @@
   },
   {
     "name": "boot",
-    "url": "https://commadist.azureedge.net/agnosupdate/boot-d726315cf98a43e1090e5b49297404cf3d084cfbd42ad8bb7d8afb68136b9f51.img.xz",
+    "url": "https://commadist.azureedge.net/agnosupdate/boot-3a6285bc114d21cfdab312c42f5a76f81762328b41d404816ee46a2f83523f04.img.xz",
-    "hash": "d726315cf98a43e1090e5b49297404cf3d084cfbd42ad8bb7d8afb68136b9f51",
+    "hash": "3a6285bc114d21cfdab312c42f5a76f81762328b41d404816ee46a2f83523f04",
-    "hash_raw": "d726315cf98a43e1090e5b49297404cf3d084cfbd42ad8bb7d8afb68136b9f51",
+    "hash_raw": "3a6285bc114d21cfdab312c42f5a76f81762328b41d404816ee46a2f83523f04",
     "size": 17500160,
     "sparse": false,
     "full_check": true,
     "has_ab": true,
-    "ondevice_hash": "2454108de1161289bc4a75449ad6421f1772b13b3e5cba68a84fca7530557699"
+    "ondevice_hash": "4e12accbcbb03fd9eebc5c1231a7b79dc1d9e15341cce56dc88930832ee86207"
   },
   {
     "name": "system",
-    "url": "https://commadist.azureedge.net/agnosupdate/system-dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5.img.xz",
+    "url": "https://commadist.azureedge.net/agnosupdate/system-82ffa59e322c5abab18f2ec6fb83dce124f55c9658c5c673e9adbd4655d6e6a2.img.xz",
-    "hash": "5f319030ad05942267b77f1a4686c4ca24cc09b2c2a4688e57342ffc9720fd49",
+    "hash": "bb4ccf77037120ace841cbcd94387c0650b317595cd0eab555b3ba0beb74e840",
-    "hash_raw": "dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5",
+    "hash_raw": "82ffa59e322c5abab18f2ec6fb83dce124f55c9658c5c673e9adbd4655d6e6a2",
     "size": 4718592000,
     "sparse": true,
     "full_check": false,
     "has_ab": true,
-    "ondevice_hash": "c12f1b7d790a418aea17424accf4cd59c575e5745cad82bdc9452f384483648c",
+    "ondevice_hash": "f1505151cde236f0d3ccdb764e3c604761ad2c1beb40f2c340ea9470d0cecc0e",
     "alt": {
-      "hash": "dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5",
+      "hash": "82ffa59e322c5abab18f2ec6fb83dce124f55c9658c5c673e9adbd4655d6e6a2",
-      "url": "https://commadist.azureedge.net/agnosupdate/system-dcdea6bd675d0276a63c25151727829620794baf42ada2e5e19a3f77b3f583a5.img",
+      "url": "https://commadist.azureedge.net/agnosupdate/system-82ffa59e322c5abab18f2ec6fb83dce124f55c9658c5c673e9adbd4655d6e6a2.img",
       "size": 4718592000
     }
   },
   {
     "name": "userdata_90",
-    "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-a7b25ea29255f4fd3a2da99e037f40b4ca10bd4afd57dd96563353b8dfb0f634.img.xz",
+    "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-6bf2fb546944f6fc0cdf865f92ade316430a75a7b1680d3fe891b34921c29b64.img.xz",
-    "hash": "7ea9d7d4685ec36bbfdf06afe0b51650d567416c3092fef96bd97158ed322742",
+    "hash": "f3badccff4330301cdae10e072a460a331421715f71253ea3bdcbf708f61d012",
-    "hash_raw": "a7b25ea29255f4fd3a2da99e037f40b4ca10bd4afd57dd96563353b8dfb0f634",
+    "hash_raw": "6bf2fb546944f6fc0cdf865f92ade316430a75a7b1680d3fe891b34921c29b64",
     "size": 96636764160,
     "sparse": true,
     "full_check": true,
     "has_ab": false,
-    "ondevice_hash": "79ed653c1679d84b13ee23083a511b0e668454e4af9b0db99a3279072ed041c1"
+    "ondevice_hash": "82e907612afc989363656c821b9cf2107cddcbdc5030cc9b5608a38855dd1c60"
   },
   {
     "name": "userdata_89",
-    "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-8e428632c967aa609cac184bff938a90240e53ffd3b4fca40bc94c33c81202ba.img.xz",
+    "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-575b01a0e84bff31180a1589b9d62810e2198e4012250e01fd80c2d5cd9ba9e1.img.xz",
-    "hash": "7104cdb0384e4ecb1ebfa6136a2330251bc8aa829b9ec48c4b740f656252d382",
+    "hash": "ebeebbe3b8e111fe807b88b993b2fa50c0ea15e838c531a3885dd3037338b1a4",
-    "hash_raw": "8e428632c967aa609cac184bff938a90240e53ffd3b4fca40bc94c33c81202ba",
+    "hash_raw": "575b01a0e84bff31180a1589b9d62810e2198e4012250e01fd80c2d5cd9ba9e1",
     "size": 95563022336,
     "sparse": true,
     "full_check": true,
     "has_ab": false,
-    "ondevice_hash": "fbede3b0831dbc4a4edd336e5f547f4978902b9421fb1484e86c416192c59165"
+    "ondevice_hash": "2a9d00ad72978d94640e7fb6c4b20b398e1a1bbc7a0c32f7d02fa988f755bd12"
   }
 ]

system/hardware/tici/lpa.py

@@ -716,22 +716,29 @@ class TiciLPA(LPABase):
     atexit.register(self._client.close)
 
   @contextmanager
-  def _acquire_channel(self):
+  def _acquire_lock(self):
     fd = os.open(LOCK_FILE, os.O_CREAT | os.O_RDWR)
     try:
       fcntl.flock(fd, fcntl.LOCK_EX)
-      self._client.open_isdr()
       yield
     finally:
-      if self._client.channel:
-        try:
-          self._client.query(f"AT+CCHC={self._client.channel}")
-        except (RuntimeError, TimeoutError):
-          pass
-        self._client.channel = None
       fcntl.flock(fd, fcntl.LOCK_UN)
       os.close(fd)
 
+  @contextmanager
+  def _acquire_channel(self):
+    with self._acquire_lock():
+      try:
+        self._client.open_isdr()
+        yield
+      finally:
+        if self._client.channel:
+          try:
+            self._client.query(f"AT+CCHC={self._client.channel}")
+          except (RuntimeError, TimeoutError):
+            pass
+          self._client.channel = None
+
   def list_profiles(self) -> list[Profile]:
     with self._acquire_channel():
       return [
@@ -792,3 +799,21 @@ class TiciLPA(LPABase):
     if HARDWARE.get_device_type() == "mici":
       self._client.send_raw(b'AT+CFUN=0\rAT+CFUN=1\r')  # mici has no SIM presence pin; raw because CFUN=0 drops serial
       self._client._ensure_serial(reconnect=True)
+
+  def is_euicc(self) -> bool:
+    # +CCHO:<n> -> eUICC; bare ERROR -> applet absent, non-eUICC; +CME ERROR -> applet
+    # exists but bus busy or modem in transient state, still eUICC.
+    with self._acquire_lock():
+      try:
+        lines = self._client.query(f'AT+CCHO="{ISDR_AID}"')
+      except RuntimeError as e:
+        return "+CME ERROR" in str(e)
+      for line in lines:
+        if line.startswith("+CCHO:") and (ch := line.split(":", 1)[1].strip()):
+          try:
+            self._client.query(f"AT+CCHC={ch}")
+          except (RuntimeError, TimeoutError):
+            pass
+          self._client.channel = None
+          return True
+      return False

system/loggerd/SConscript

@@ -11,8 +11,9 @@ if arch != "larch64":
   libs += ['yuv']
   if arch == "Darwin":
     frameworks += ['VideoToolbox', 'CoreMedia', 'CoreFoundation', 'CoreVideo']
-  else:
+
-    libs += ['va', 'va-drm', 'drm']
+if arch != "Darwin":
+  libs += ['va', 'va-drm', 'drm']
 
 if arch == "Darwin":
   # exclude v4l

system/qcomgpsd/qcomgpsd.py

@@ -5,11 +5,8 @@ import signal
 import itertools
 import math
 import time
-import requests
-import shutil
 from serial import Serial
 import datetime
-from multiprocessing import Process, Event
 from typing import NoReturn
 from struct import unpack_from, calcsize, pack
 
@@ -30,9 +27,6 @@ from openpilot.system.qcomgpsd.structs import (dict_unpacker, position_report, r
                                               LOG_GNSS_OEMDRE_SVPOLY_REPORT)
 
 DEBUG = int(os.getenv("DEBUG", "0"))==1
-ASSIST_DATA_FILE = '/tmp/xtra3grc.bin'
-ASSIST_DATA_FILE_DOWNLOAD = ASSIST_DATA_FILE + '.download'
-ASSISTANCE_URL = 'http://xtrapath3.izatcloud.net/xtra3grc.bin'
 
 LOG_TYPES = [
   LOG_GNSS_GPS_MEASUREMENT_REPORT,
@@ -112,49 +106,8 @@ def at_cmd(cmd: str) -> str:
 def gps_enabled() -> bool:
   return "QGPS: 1" in at_cmd("AT+QGPS?")
 
-def download_assistance():
-  try:
-    response = requests.get(ASSISTANCE_URL, timeout=5, stream=True)
-
-    with open(ASSIST_DATA_FILE_DOWNLOAD, 'wb') as fp:
-      for chunk in response.iter_content(chunk_size=8192):
-        fp.write(chunk)
-        if fp.tell() > 1e5:
-          cloudlog.error("Qcom assistance data larger than expected")
-          return
-
-    os.rename(ASSIST_DATA_FILE_DOWNLOAD, ASSIST_DATA_FILE)
-
-  except requests.exceptions.RequestException:
-    cloudlog.exception("Failed to download assistance file")
-    return
-
-def downloader_loop(event):
-  if os.path.exists(ASSIST_DATA_FILE):
-    os.remove(ASSIST_DATA_FILE)
-
-  alt_path = os.getenv("QCOM_ALT_ASSISTANCE_PATH", None)
-  if alt_path is not None and os.path.exists(alt_path):
-    shutil.copyfile(alt_path, ASSIST_DATA_FILE)
-
-  try:
-    while not os.path.exists(ASSIST_DATA_FILE) and not event.is_set():
-      download_assistance()
-      event.wait(timeout=10)
-  except KeyboardInterrupt:
-    pass
-
-@retry(attempts=5, delay=0.2, ignore_failure=True)
-def inject_assistance():
-  import subprocess
-  cmd = f"mmcli -m any --timeout 30 --location-inject-assistance-data={ASSIST_DATA_FILE}"
-  subprocess.check_output(cmd, stderr=subprocess.PIPE, shell=True)
-  cloudlog.info("successfully loaded assistance data")
-
 @retry(attempts=5, delay=1.0)
-def setup_quectel(diag: ModemDiag) -> bool:
+def setup_quectel(diag: ModemDiag):
-  ret = False
-
   # enable OEMDRE in the NV
   # TODO: it has to reboot for this to take effect
   DIAG_NV_READ_F = 38
@@ -168,26 +121,11 @@ def setup_quectel(diag: ModemDiag) -> bool:
   if gps_enabled():
     at_cmd("AT+QGPSEND")
 
-  if "GPS_COLD_START" in os.environ:
-    # deletes all assistance
-    at_cmd("AT+QGPSDEL=0")
-  else:
-    # allow module to perform hot start
-    at_cmd("AT+QGPSDEL=1")
-
   # disable DPO power savings for more accuracy
   at_cmd("AT+QGPSCFG=\"dpoenable\",0")
   # don't automatically turn on GNSS on powerup
   at_cmd("AT+QGPSCFG=\"autogps\",0")
 
-  # Do internet assistance
-  at_cmd("AT+QGPSXTRA=1")
-  at_cmd("AT+QGPSSUPLURL=\"NULL\"")
-  if os.path.exists(ASSIST_DATA_FILE):
-    ret = True
-    inject_assistance()
-    os.remove(ASSIST_DATA_FILE)
-  #at_cmd("AT+QGPSXTRADATA?")
   if system_time_valid():
     time_str = datetime.datetime.now(datetime.UTC).replace(tzinfo=None).strftime("%Y/%m/%d,%H:%M:%S")
     at_cmd(f"AT+QGPSXTRATIME=0,\"{time_str}\",1,1,1000")
@@ -214,7 +152,6 @@ def setup_quectel(diag: ModemDiag) -> bool:
     0,0
   ))
 
-  return ret
 
 def teardown_quectel(diag):
   at_cmd("AT+QGPSCFG=\"outport\",\"none\"")
@@ -255,9 +192,6 @@ def main() -> NoReturn:
 
   wait_for_modem()
 
-  stop_download_event = Event()
-  assist_fetch_proc = Process(target=downloader_loop, args=(stop_download_event,))
-  assist_fetch_proc.start()
   def cleanup(sig, frame):
     cloudlog.warning("caught sig disabling quectel gps")
 
@@ -268,18 +202,13 @@ def main() -> NoReturn:
     except NameError:
       cloudlog.warning('quectel not yet setup')
 
-    stop_download_event.set()
-    assist_fetch_proc.kill()
-    assist_fetch_proc.join()
-
     sys.exit(0)
   signal.signal(signal.SIGINT, cleanup)
   signal.signal(signal.SIGTERM, cleanup)
 
   # connect to modem
   diag = ModemDiag()
-  r = setup_quectel(diag)
+  setup_quectel(diag)
-  want_assistance = not r
   cloudlog.warning("quectel setup done")
   gpio_init(GPIO.GNSS_PWR_EN, True)
   gpio_set(GPIO.GNSS_PWR_EN, True)
@@ -287,10 +216,6 @@ def main() -> NoReturn:
   pm = messaging.PubMaster(['qcomGnss', 'gpsLocation'])
 
   while 1:
-    if os.path.exists(ASSIST_DATA_FILE) and want_assistance:
-      setup_quectel(diag)
-      want_assistance = False
-
     opcode, payload = diag.recv()
     if opcode != DIAG_LOG_F:
       cloudlog.error(f"Unhandled opcode: {opcode}")
@@ -383,9 +308,6 @@ def main() -> NoReturn:
       gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma]))
       # quectel gps verticalAccuracy is clipped to 500, set invalid if so
       gps.hasFix = gps.verticalAccuracy != 500
-      if gps.hasFix:
-        want_assistance = False
-        stop_download_event.set()
       pm.send('gpsLocation', msg)
 
     elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:

system/qcomgpsd/tests/test_qcomgpsd.py

@@ -1,36 +1,29 @@
 import os
 import pytest
 import time
-import datetime
 
 import cereal.messaging as messaging
 from openpilot.system.qcomgpsd.qcomgpsd import at_cmd, wait_for_modem
 from openpilot.system.manager.process_config import managed_processes
 
-GOOD_SIGNAL = bool(int(os.getenv("GOOD_SIGNAL", '0')))
-
 
 @pytest.mark.tici
-class TestRawgpsd:
+class TestQcomgpsd:
   @classmethod
   def setup_class(cls):
-    os.environ['GPS_COLD_START'] = '1'
-    os.system("sudo systemctl start systemd-resolved")
     os.system("sudo systemctl restart ModemManager lte")
     wait_for_modem()
 
   @classmethod
   def teardown_class(cls):
     managed_processes['qcomgpsd'].stop()
-    os.system("sudo systemctl restart systemd-resolved")
     os.system("sudo systemctl restart ModemManager lte")
 
   def setup_method(self):
-    self.sm = messaging.SubMaster(['qcomGnss', 'gpsLocation', 'gnssMeasurements'])
+    self.sm = messaging.SubMaster(['qcomGnss', 'gpsLocation'])
 
   def teardown_method(self):
     managed_processes['qcomgpsd'].stop()
-    os.system("sudo systemctl restart systemd-resolved")
 
   def _wait_for_output(self, t):
     dt = 0.1
@@ -41,24 +34,10 @@ class TestRawgpsd:
       time.sleep(dt)
     return self.sm.updated['qcomGnss']
 
-  def test_no_crash_double_command(self):
+  def test_startup_time(self):
-    wait_for_modem()
-    at_cmd("AT+QGPSDEL=0")
-    at_cmd("AT+QGPSDEL=0")
-
-  def test_wait_for_modem(self):
-    os.system("sudo systemctl stop ModemManager")
     managed_processes['qcomgpsd'].start()
     assert self._wait_for_output(30)
-
+    managed_processes['qcomgpsd'].stop()
-  def test_startup_time(self, subtests):
-    for internet in (True, False):
-      if not internet:
-        os.system("sudo systemctl stop systemd-resolved")
-      with subtests.test(internet=internet):
-        managed_processes['qcomgpsd'].start()
-        assert self._wait_for_output(30)
-        managed_processes['qcomgpsd'].stop()
 
   def test_turns_off_gnss(self, subtests):
     for s in (0.1, 1, 5):
@@ -70,50 +49,3 @@ class TestRawgpsd:
         wait_for_modem()
         resp = at_cmd("AT+QGPS?")
         assert "+QGPS: 0" in resp
-
-
-  def check_assistance(self, should_be_loaded):
-    # after QGPSDEL: '+QGPSXTRADATA: 0,"1980/01/05,19:00:00"'
-    # after loading: '+QGPSXTRADATA: 10080,"2023/06/24,19:00:00"'
-    wait_for_modem()
-    out = at_cmd("AT+QGPSXTRADATA?")
-    out = out.split("+QGPSXTRADATA:")[1].split("'")[0].strip()
-    valid_duration, injected_time_str = out.split(",", 1)
-    if should_be_loaded:
-      assert valid_duration == "10080"  # should be max time
-      injected_time = datetime.datetime.strptime(injected_time_str.replace("\"", ""), "%Y/%m/%d,%H:%M:%S")
-      assert abs((datetime.datetime.now(datetime.UTC).replace(tzinfo=None) - injected_time).total_seconds()) < 60*60*12
-    else:
-      valid_duration, injected_time_str = out.split(",", 1)
-      injected_time_str = injected_time_str.replace('\"', '').replace('\'', '')
-      assert injected_time_str[:] == '1980/01/05,19:00:00'[:]
-      assert valid_duration == '0'
-
-  @pytest.mark.skip(reason="XTRA injection via QMI needs debugging on AGNOS 17")
-  def test_assistance_loading(self):
-    managed_processes['qcomgpsd'].start()
-    assert self._wait_for_output(30)
-    managed_processes['qcomgpsd'].stop()
-    self.check_assistance(True)
-
-  @pytest.mark.skip(reason="XTRA injection via QMI needs debugging on AGNOS 17")
-  def test_no_assistance_loading(self):
-    os.system("sudo systemctl stop systemd-resolved")
-
-    managed_processes['qcomgpsd'].start()
-    assert self._wait_for_output(30)
-    managed_processes['qcomgpsd'].stop()
-    self.check_assistance(False)
-
-  @pytest.mark.skip(reason="XTRA injection via QMI needs debugging on AGNOS 17")
-  def test_late_assistance_loading(self):
-    os.system("sudo systemctl stop systemd-resolved")
-
-    managed_processes['qcomgpsd'].start()
-    self._wait_for_output(17)
-    assert self.sm.updated['qcomGnss']
-
-    os.system("sudo systemctl restart systemd-resolved")
-    time.sleep(15)
-    managed_processes['qcomgpsd'].stop()
-    self.check_assistance(True)

system/sensord/sensord.py

@@ -11,19 +11,21 @@ from openpilot.common.utils import sudo_write
 from openpilot.common.realtime import config_realtime_process, Ratekeeper
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.gpio import gpiochip_get_ro_value_fd, gpioevent_data
-from openpilot.system.hardware import HARDWARE
 
 from openpilot.system.sensord.sensors.i2c_sensor import Sensor
 from openpilot.system.sensord.sensors.lsm6ds3_accel import LSM6DS3_Accel
 from openpilot.system.sensord.sensors.lsm6ds3_gyro import LSM6DS3_Gyro
 from openpilot.system.sensord.sensors.lsm6ds3_temp import LSM6DS3_Temp
-from openpilot.system.sensord.sensors.mmc5603nj_magn import MMC5603NJ_Magn
 
 I2C_BUS_IMU = 1
 
 def interrupt_loop(sensors: list[tuple[Sensor, str, bool]], event) -> None:
   pm = messaging.PubMaster([service for sensor, service, interrupt in sensors if interrupt])
 
+  # NOTE: the gyro and accelerometer share an IRQ due to the comma three
+  # routing only one GPIO from the LSM to the SOC, but comma 3X and four
+  # have two. if we want better timestamps in the future, we can use both.
+
   # Requesting both edges as the data ready pulse from the lsm6ds sensor is
   # very short (75us) and is mostly detected as falling edge instead of rising.
   # So if it is detected as rising the following falling edge is skipped.
@@ -97,10 +99,6 @@ def main() -> None:
     (LSM6DS3_Gyro(I2C_BUS_IMU), "gyroscope", True),
     (LSM6DS3_Temp(I2C_BUS_IMU), "temperatureSensor", False),
   ]
-  if HARDWARE.get_device_type() == "tizi":
-    sensors_cfg.append(
-      (MMC5603NJ_Magn(I2C_BUS_IMU), "magnetometer", False),
-    )
 
   # Reset sensors
   for sensor, _, _ in sensors_cfg:

system/sensord/sensors/lsm6ds3_accel.py

@@ -77,13 +77,9 @@ class LSM6DS3_Accel(Sensor):
 
     event = log.SensorEventData.new_message()
     event.timestamp = ts
-    event.version = 1
-    event.sensor = 1  # SENSOR_ACCELEROMETER
-    event.type = 1    # SENSOR_TYPE_ACCELEROMETER
     event.source = self.source
     a = event.init('acceleration')
     a.v = [y, -x, z]
-    a.status = 1
     return event
 
   def shutdown(self) -> None:

system/sensord/sensors/lsm6ds3_gyro.py

@@ -73,13 +73,9 @@ class LSM6DS3_Gyro(Sensor):
 
     event = log.SensorEventData.new_message()
     event.timestamp = ts
-    event.version = 2
-    event.sensor = 5  # SENSOR_GYRO_UNCALIBRATED
-    event.type = 16   # SENSOR_TYPE_GYROSCOPE_UNCALIBRATED
     event.source = self.source
     g = event.init('gyroUncalibrated')
     g.v = xyz
-    g.status = 1
     return event
 
   def shutdown(self) -> None:

system/sensord/sensors/lsm6ds3_temp.py

@@ -23,7 +23,6 @@ class LSM6DS3_Temp(Sensor):
 
   def get_event(self, ts: int | None = None) -> log.SensorEventData:
     event = log.SensorEventData.new_message()
-    event.version = 1
     event.timestamp = int(time.monotonic() * 1e9)
     event.source = self.source
     event.temperature = self._read_temperature()

system/sensord/sensors/mmc5603nj_magn.py

@@ -1,76 +0,0 @@
-import time
-
-from cereal import log
-from openpilot.system.sensord.sensors.i2c_sensor import Sensor
-
-# https://www.mouser.com/datasheet/2/821/Memsic_09102019_Datasheet_Rev.B-1635324.pdf
-
-# Register addresses
-REG_ODR = 0x1A
-REG_INTERNAL_0 = 0x1B
-REG_INTERNAL_1 = 0x1C
-
-# Control register settings
-CMM_FREQ_EN = (1 << 7)
-AUTO_SR_EN  = (1 << 5)
-SET         = (1 << 3)
-RESET       = (1 << 4)
-
-class MMC5603NJ_Magn(Sensor):
-  @property
-  def device_address(self) -> int:
-    return 0x30
-
-  def init(self):
-    self.verify_chip_id(0x39, [0x10, ])
-    self.writes((
-      (REG_ODR, 0),
-
-      # Set BW to 0b01 for 1-150 Hz operation
-      (REG_INTERNAL_1, 0b01),
-    ))
-
-  def _read_data(self, cycle) -> list[float]:
-    # start measurement
-    self.write(REG_INTERNAL_0, cycle)
-    self.wait()
-
-    # read out XYZ
-    scale = 1.0 / 16384.0
-    b = self.read(0x00, 9)
-    return [
-      (self.parse_20bit(b[6], b[1], b[0]) * scale) - 32.0,
-      (self.parse_20bit(b[7], b[3], b[2]) * scale) - 32.0,
-      (self.parse_20bit(b[8], b[5], b[4]) * scale) - 32.0,
-    ]
-
-  def get_event(self, ts: int | None = None) -> log.SensorEventData:
-    ts = time.monotonic_ns()
-
-    # SET - RESET cycle
-    xyz = self._read_data(SET)
-    reset_xyz = self._read_data(RESET)
-    vals = [*xyz, *reset_xyz]
-
-    event = log.SensorEventData.new_message()
-    event.timestamp = ts
-    event.version = 1
-    event.sensor = 3 # SENSOR_MAGNETOMETER_UNCALIBRATED
-    event.type = 14  # SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED
-    event.source = log.SensorEventData.SensorSource.mmc5603nj
-
-    m = event.init('magneticUncalibrated')
-    m.v = vals
-    m.status = int(all(int(v) != -32 for v in vals))
-
-    return event
-
-  def shutdown(self) -> None:
-    v = self.read(REG_INTERNAL_0, 1)[0]
-    self.writes((
-      # disable auto-reset of measurements
-      (REG_INTERNAL_0, (v & (~(CMM_FREQ_EN | AUTO_SR_EN)))),
-
-      # disable continuous mode
-      (REG_ODR, 0),
-    ))

system/sensord/tests/test_sensord.py

@@ -5,44 +5,19 @@ import numpy as np
 from collections import namedtuple, defaultdict
 
 import cereal.messaging as messaging
-from cereal import log
 from cereal.services import SERVICE_LIST
 from openpilot.common.gpio import get_irqs_for_action
 from openpilot.common.timeout import Timeout
-from openpilot.system.hardware import HARDWARE
 from openpilot.system.manager.process_config import managed_processes
 
-LSM = {
+SensorConfig = namedtuple('SensorConfig', ['service', 'measurement', 'sanity_min', 'sanity_max', 'std_max'])
-  ('lsm6ds3', 'acceleration'),
-  ('lsm6ds3', 'gyroUncalibrated'),
-  ('lsm6ds3', 'temperature'),
-}
-LSM_C = {(x[0]+'trc', x[1]) for x in LSM}
 
-MMC = {
+SENSOR_CONFIGS = (
-  ('mmc5603nj', 'magneticUncalibrated'),
+  SensorConfig("accelerometer", "acceleration", 5, 15, 5),
-}
+  SensorConfig("gyroscope", "gyroUncalibrated", 0, .15, 0.5),
-
+  SensorConfig("temperatureSensor", "temperature", 10, 40, 0.5),  # set for max range of our office
-SENSOR_CONFIGURATIONS: list[set] = {
+)
-  "mici": [LSM, LSM_C],
+SENSOR_CONFIGS_BY_MEASUREMENT = {config.measurement: config for config in SENSOR_CONFIGS}
-  "tizi": [MMC | LSM, MMC | LSM_C],
-  "tici": [LSM, LSM_C, MMC | LSM, MMC | LSM_C],
-}.get(HARDWARE.get_device_type(), [])
-
-Sensor = log.SensorEventData.SensorSource
-SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max'])
-ALL_SENSORS = {
-  Sensor.lsm6ds3trc: {
-    SensorConfig("acceleration", 5, 15),
-    SensorConfig("gyroUncalibrated", 0, .2),
-    SensorConfig("temperature", 10, 40),  # set for max range of our office
-  },
-
-  Sensor.mmc5603nj: {
-    SensorConfig("magneticUncalibrated", 0, 300),
-  }
-}
-ALL_SENSORS[Sensor.lsm6ds3] = ALL_SENSORS[Sensor.lsm6ds3trc]
 
 def get_irq_count(irq: int):
   with open(f"/sys/kernel/irq/{irq}/per_cpu_count") as f:
@@ -50,12 +25,11 @@ def get_irq_count(irq: int):
     return sum(per_cpu)
 
 def read_sensor_events(duration_sec):
-  sensor_types = ['accelerometer', 'gyroscope', 'magnetometer', 'temperatureSensor',]
   socks = {}
   poller = messaging.Poller()
   events = defaultdict(list)
-  for stype in sensor_types:
+  for config in SENSOR_CONFIGS:
-    socks[stype] = messaging.sub_sock(stype, poller=poller, timeout=100)
+    socks[config.service] = messaging.sub_sock(config.service, poller=poller, timeout=100)
 
   # wait for sensors to come up
   with Timeout(int(os.environ.get("SENSOR_WAIT", "5")), "sensors didn't come up"):
@@ -70,11 +44,15 @@ def read_sensor_events(duration_sec):
     for s in socks:
       events[s] += messaging.drain_sock(socks[s])
     time.sleep(0.1)
-
   assert sum(map(len, events.values())) != 0, "No sensor events collected!"
 
   return {k: v for k, v in events.items() if len(v) > 0}
 
+def iter_measurements(events):
+  for msgs in events.values():
+    for measurement in msgs:
+      yield measurement, getattr(measurement, measurement.which())
+
 @pytest.mark.tici
 class TestSensord:
   @classmethod
@@ -102,31 +80,19 @@ class TestSensord:
   def teardown_method(self):
     managed_processes["sensord"].stop()
 
-  def test_sensors_present(self):
+  def test_all_sensors_present(self):
-    # verify correct sensors configuration
+    missing = [config.service for config in SENSOR_CONFIGS if config.service not in self.events]
-    seen = set()
+    assert len(missing) == 0, f"missing sensors: {missing}"
-    for etype in self.events:
-      for measurement in self.events[etype]:
-        m = getattr(measurement, measurement.which())
-        seen.add((str(m.source), m.which()))
-
-    assert seen in SENSOR_CONFIGURATIONS
 
   def test_lsm6ds3_timing(self, subtests):
     # verify measurements are sampled and published at 104Hz
 
-    sensor_t = {
+    sensor_t = {service: [] for service in ('accelerometer', 'gyroscope')}
-      1: [], # accel
-      5: [], # gyro
-    }
 
-    for measurement in self.events['accelerometer']:
+    for service in sensor_t:
-      m = getattr(measurement, measurement.which())
+      for measurement in self.events.get(service, []):
-      sensor_t[m.sensor].append(m.timestamp)
+        m = getattr(measurement, measurement.which())
-
+        sensor_t[service].append(m.timestamp)
-    for measurement in self.events['gyroscope']:
-      m = getattr(measurement, measurement.which())
-      sensor_t[m.sensor].append(m.timestamp)
 
     for s, vals in sensor_t.items():
       with subtests.test(sensor=s):
@@ -153,19 +119,16 @@ class TestSensord:
   def test_logmonottime_timestamp_diff(self):
     # ensure diff between the message logMonotime and sample timestamp is small
 
-    tdiffs = list()
+    tdiffs = []
-    for etype in self.events:
+    for measurement, m in iter_measurements(self.events):
-      for measurement in self.events[etype]:
+      # check if gyro and accel timestamps are before logMonoTime
-        m = getattr(measurement, measurement.which())
+      if str(m.source).startswith("lsm6ds3") and m.which() != 'temperature':
+        err_msg = f"Timestamp after logMonoTime: {m.timestamp} > {measurement.logMonoTime}"
+        assert m.timestamp < measurement.logMonoTime, err_msg
 
-        # check if gyro and accel timestamps are before logMonoTime
+      # negative values might occur, as non interrupt packages created
-        if str(m.source).startswith("lsm6ds3") and m.which() != 'temperature':
+      # before the sensor is read
-          err_msg = f"Timestamp after logMonoTime: {m.timestamp} > {measurement.logMonoTime}"
+      tdiffs.append(abs(measurement.logMonoTime - m.timestamp) / 1e6)
-          assert m.timestamp < measurement.logMonoTime, err_msg
-
-        # negative values might occur, as non interrupt packages created
-        # before the sensor is read
-        tdiffs.append(abs(measurement.logMonoTime - m.timestamp) / 1e6)
 
     # some sensors have a read procedure that will introduce an expected diff on the order of 20ms
     high_delay_diffs = set(filter(lambda d: d >= 25., tdiffs))
@@ -175,32 +138,29 @@ class TestSensord:
     assert avg_diff < 4, f"Avg packet diff: {avg_diff:.1f}ms"
 
   def test_sensor_values(self):
-    sensor_values = dict()
+    sensor_values = defaultdict(list)
-    for etype in self.events:
+    for _, m in iter_measurements(self.events):
-      for measurement in self.events[etype]:
+      key = (m.source.raw, m.which())
-        m = getattr(measurement, measurement.which())
+      values = getattr(m, m.which())
-        key = (m.source.raw, m.which())
-        values = getattr(m, m.which())
 
-        if hasattr(values, 'v'):
+      if hasattr(values, 'v'):
-          values = values.v
+        values = values.v
-        values = np.atleast_1d(values)
+      sensor_values[key].append(np.atleast_1d(values))
-
-        if key in sensor_values:
-          sensor_values[key].append(values)
-        else:
-          sensor_values[key] = [values]
 
     # Sanity check sensor values
-    for sensor, stype in sensor_values:
+    for (sensor, stype), values in sensor_values.items():
-      for s in ALL_SENSORS[sensor]:
+      config = SENSOR_CONFIGS_BY_MEASUREMENT[stype]
-        if s.type != stype:
-          continue
 
-        key = (sensor, s.type)
+      if config.measurement == 'temperature':
-        mean_norm = np.mean(np.linalg.norm(sensor_values[key], axis=1))
+        measurement_stat = np.mean(values)
-        err_msg = f"Sensor '{sensor} {s.type}' failed sanity checks {mean_norm} is not between {s.sanity_min} and {s.sanity_max}"
+      else:
-        assert s.sanity_min <= mean_norm <= s.sanity_max, err_msg
+        measurement_stat = np.mean(np.linalg.norm(values, axis=1))
+      err_msg = f"Sensor '{sensor} {config.measurement}' failed sanity checks {measurement_stat} is not between {config.sanity_min} and {config.sanity_max}"
+      assert config.sanity_min <= measurement_stat <= config.sanity_max, err_msg
+
+      std_dev = np.std(values, axis=0)
+      err_msg = f"Sensor '{sensor} {config.measurement}' failed std dev test {std_dev} is not under {config.std_max}"
+      assert np.all(std_dev <= config.std_max), err_msg
 
   def test_sensor_verify_no_interrupts_after_stop(self):
     managed_processes["sensord"].start()
@@ -222,4 +182,3 @@ class TestSensord:
     time.sleep(1)
     state_two = get_irq_count(self.sensord_irq)
     assert state_one == state_two, "Interrupts received after sensord stop!"
-

tools/jotpluggler/.gitignore

@@ -3,6 +3,7 @@ jot_*.o
 *.o
 jotpluggler
 car_fingerprint_to_dbc.h
+generated_event_extractors.h
 generated_dbcs/.stamp
 generated_dbcs/*.dbc
 layouts/.jotpluggler_autosave/

tools/jotpluggler/SConscript

@@ -1,4 +1,5 @@
 import os
+import subprocess
 import imgui
 import libusb
 from opendbc import get_generated_dbcs
@@ -77,8 +78,24 @@ def write_car_fingerprint_to_dbc_header(target, source, env):
 
   return None
 
+def generate_event_extractors(target, source, env):
+  subprocess.check_call([
+    "python3",
+    "tools/jotpluggler/generate_event_extractors.py",
+    os.path.realpath(BASEDIR),
+    str(target[0]),
+  ])
+  return None
+
 generated_dbc_stamp = jot_env.Command(f"generated_dbcs/.stamp", [], materialize_generated_dbcs)
 car_fingerprint_to_dbc = jot_env.Command("car_fingerprint_to_dbc.h", [], write_car_fingerprint_to_dbc_header)
+event_extractors = jot_env.Command("generated_event_extractors.h", [
+    "generate_event_extractors.py",
+    jot_env.Glob("#cereal/*.capnp"),
+    jot_env.Glob("#cereal/include/*.capnp"),
+  ],
+  generate_event_extractors,
+)
 
 libs = [replay_lib, common, messaging, visionipc, cereal, File(f"{imgui.LIB_DIR}/libimgui.a"), File(f"{imgui.LIB_DIR}/libglfw3.a"),
         "avformat", "avcodec", "avutil", "x264", "yuv", "z", "bz2", "zstd", "m", "pthread", "usb-1.0"]
@@ -90,3 +107,4 @@ else:
 program = jot_env.Program("jotpluggler", jot_env.Glob("*.cc"), LIBS=libs)
 jot_env.Depends(program, generated_dbc_stamp)
 jot_env.Depends(program, car_fingerprint_to_dbc)
+jot_env.Depends(program, event_extractors)

tools/jotpluggler/app.cc

@@ -324,7 +324,7 @@ void configure_style() {
   plot_style.LegendInnerPadding = ImVec2(6.0f, 3.0f);
   plot_style.LegendSpacing = ImVec2(7.0f, 2.0f);
   plot_style.PlotPadding = ImVec2(4.0f, 8.0f);
-  plot_style.FitPadding = ImVec2(0.02f, 0.4f);
+  plot_style.FitPadding = ImVec2(0.02f, static_cast<float>(PLOT_Y_PADDING_FRACTION));
 
   ImPlot::MapInputDefault();
   ImPlotInputMap &input_map = ImPlot::GetInputMap();

tools/jotpluggler/generate_event_extractors.py

@@ -0,0 +1,327 @@
+import sys
+import capnp
+from pathlib import Path
+
+NO_DISCRIMINANT = 65535
+SCALAR_KINDS = {
+  "bool": "Bool",
+  "int8": "Int",
+  "int16": "Int",
+  "int32": "Int",
+  "int64": "Int",
+  "uint8": "UInt",
+  "uint16": "UInt",
+  "uint32": "UInt",
+  "uint64": "UInt",
+  "float32": "Float",
+  "float64": "Float",
+  "enum": "Enum",
+}
+NESTED_TYPE_KINDS = {"struct", "list"}
+IGNORED_TYPE_KINDS = {"void", "text", "data", "interface", "anyPointer"}
+
+
+def cxx_string(value):
+  return '"' + value.replace("\\", "\\\\").replace('"', '\\"') + '"'
+
+
+def accessor(prefix, name):
+  return prefix + name[:1].upper() + name[1:]
+
+
+def field_type(field):
+  if field.proto.which() == "group":
+    return "struct"
+  return field.proto.slot.type.which()
+
+
+def field_type_proto(field):
+  return field.proto.slot.type if field.proto.which() == "slot" else None
+
+
+def scalar_kind(type_proto):
+  if type_proto is None:
+    return None
+  return SCALAR_KINDS.get(type_proto.which())
+
+
+def enum_names(schema):
+  if schema is None:
+    return []
+  names_by_ordinal = schema.enumerants
+  if not names_by_ordinal:
+    return []
+  max_ordinal = max(names_by_ordinal.values())
+  out = [""] * (max_ordinal + 1)
+  for name, ordinal in names_by_ordinal.items():
+    out[ordinal] = name
+  return out
+
+
+class Generator:
+  def __init__(self, event_schema):
+    self.event_schema = event_schema
+    self.fixed_paths = []
+    self.tmp_index = 0
+    self.lines = []
+    self.emits_memo = {}
+
+  def tmp(self, prefix):
+    self.tmp_index += 1
+    return f"{prefix}_{self.tmp_index}"
+
+  def add_fixed_path(self, path):
+    slot = len(self.fixed_paths)
+    self.fixed_paths.append(path)
+    return slot
+
+  def emit(self, indent, text=""):
+    self.lines.append(" " * indent + text)
+
+  def scalar_double_expr(self, value_expr, kind):
+    if kind == "Bool":
+      return f"({value_expr} ? 1.0 : 0.0)"
+    if kind == "Enum":
+      return f"static_cast<double>(static_cast<uint16_t>({value_expr}))"
+    return f"static_cast<double>({value_expr})"
+
+  def emit_enum_capture(self, indent, path_expr, names):
+    if not names:
+      return
+    names_expr = "{" + ", ".join(cxx_string(name) for name in names) + "}"
+    self.emit(indent, f"capture_static_enum_info({path_expr}, {names_expr}, series);")
+
+  def emit_node(self, indent, type_kind, type_proto, schema, expr, path, path_expr, dynamic_path):
+    if not self.node_emits(type_kind, type_proto, schema):
+      return
+    kind = scalar_kind(type_proto)
+    if kind is not None:
+      double_expr = self.scalar_double_expr(expr, kind)
+      if dynamic_path:
+        if kind == "Enum":
+          self.emit_enum_capture(indent, path_expr, enum_names(schema))
+        self.emit(indent, f"append_dynamic_scalar_point({path_expr}, tm, {double_expr}, series);")
+      else:
+        slot = self.add_fixed_path(path)
+        if kind == "Enum":
+          self.emit_enum_capture(indent, cxx_string(path), enum_names(schema))
+        self.emit(indent, f"append_fixed_scalar_point(&series->fixed_series[{slot}], tm, {double_expr});")
+      return
+
+    if type_kind == "struct":
+      self.emit_struct(indent, schema, expr, path, path_expr, dynamic_path)
+      return
+
+    if type_kind == "list":
+      self.emit_list(indent, type_proto, schema, expr, path, path_expr, dynamic_path)
+
+  def emit_field(self, indent, struct_schema, reader_expr, field_name, base_path, base_path_expr, dynamic_path):
+    field = struct_schema.fields[field_name]
+    proto = field.proto
+    type_kind = field_type(field)
+    type_proto = field_type_proto(field)
+    kind = scalar_kind(type_proto)
+    value_schema = field.schema if kind == "Enum" or type_kind in NESTED_TYPE_KINDS else None
+    if not self.node_emits(type_kind, type_proto, value_schema):
+      return
+
+    field_path = f"{base_path}/{field_name}"
+    field_path_expr = None
+    if dynamic_path:
+      field_path_var = self.tmp("path")
+      self.emit(indent, f"const std::string {field_path_var} = {base_path_expr} + {cxx_string('/' + field_name)};")
+      field_path_expr = field_path_var
+
+    get_call = f"{reader_expr}.{accessor('get', field_name)}()"
+    has_call = f"{reader_expr}.{accessor('has', field_name)}()"
+    conditions = []
+    if proto.discriminantValue != NO_DISCRIMINANT:
+      conditions.append(f"{reader_expr}.which() == static_cast<decltype({reader_expr}.which())>({proto.discriminantValue})")
+    if proto.which() == "slot" and type_kind in NESTED_TYPE_KINDS:
+      conditions.append(has_call)
+
+    if conditions:
+      self.emit(indent, f"if ({' && '.join(conditions)}) {{")
+      indent += 2
+
+    value_var = self.tmp("value")
+    self.emit(indent, f"const auto {value_var} = {get_call};")
+    self.emit_node(indent, type_kind, type_proto, value_schema, value_var, field_path, field_path_expr, dynamic_path)
+
+    if conditions:
+      indent -= 2
+      self.emit(indent, "}")
+
+  def emit_struct(self, indent, schema, reader_expr, path, path_expr, dynamic_path):
+    if schema is None:
+      return
+    for field_name in schema.fieldnames:
+      self.emit_field(indent, schema, reader_expr, field_name, path, path_expr, dynamic_path)
+
+  def emit_list(self, indent, type_proto, schema, list_expr, path, path_expr, dynamic_path):
+    elem_type = type_proto.list.elementType
+    elem_kind = elem_type.which()
+    if elem_kind in IGNORED_TYPE_KINDS:
+      return
+
+    base_path_var = path_expr
+    if base_path_var is None:
+      base_path_var = self.tmp("base_path")
+      self.emit(indent, f"const std::string {base_path_var} = {cxx_string(path)};")
+
+    elem_scalar = scalar_kind(elem_type)
+    if elem_scalar is not None:
+      self.emit(indent, f"if ({list_expr}.size() <= 16) {{")
+      index_var = self.tmp("i")
+      self.emit(indent + 2, f"for (uint {index_var} = 0; {index_var} < {list_expr}.size(); ++{index_var}) {{")
+      item_series = self.tmp("item_series")
+      self.emit(indent + 4, f"RouteSeries *{item_series} = ensure_list_scalar_series({base_path_var}, {index_var}, series);")
+      if elem_scalar == "Enum":
+        self.emit_enum_capture(indent + 4, f"{item_series}->path", enum_names(schema.elementType))
+      self.emit(indent + 4, f"append_fixed_scalar_point({item_series}, tm, {self.scalar_double_expr(f'{list_expr}[{index_var}]', elem_scalar)});")
+      self.emit(indent + 2, "}")
+      self.emit(indent, "}")
+      return
+
+    if elem_kind in {"struct", "list"}:
+      index_var = self.tmp("i")
+      self.emit(indent, f"for (uint {index_var} = 0; {index_var} < {list_expr}.size(); ++{index_var}) {{")
+      item_path = self.tmp("item_path")
+      self.emit(indent + 2, f"const std::string {item_path} = {base_path_var} + \"/\" + std::to_string({index_var});")
+      item = self.tmp("item")
+      self.emit(indent + 2, f"const auto {item} = {list_expr}[{index_var}];")
+      if elem_kind == "struct":
+        self.emit_struct(indent + 2, schema.elementType, item, path, item_path, True)
+      else:
+        self.emit_list(indent + 2, elem_type, schema.elementType, item, path, item_path, True)
+      self.emit(indent, "}")
+
+  def node_emits(self, type_kind, type_proto, schema, seen=frozenset()):
+    if scalar_kind(type_proto) is not None:
+      return True
+    if type_kind == "struct":
+      if schema is None:
+        return False
+      schema_id = int(schema.node.id)
+      if schema_id in seen:
+        return False
+      if schema_id in self.emits_memo:
+        return self.emits_memo[schema_id]
+      next_seen = seen | {schema_id}
+      for field_name in schema.fieldnames:
+        field = schema.fields[field_name]
+        ft = field_type(field)
+        ftp = field_type_proto(field)
+        fkind = scalar_kind(ftp)
+        if ft in IGNORED_TYPE_KINDS:
+          continue
+        fschema = field.schema if fkind == "Enum" or ft in NESTED_TYPE_KINDS else None
+        if self.node_emits(ft, ftp, fschema, next_seen):
+          self.emits_memo[schema_id] = True
+          return True
+      self.emits_memo[schema_id] = False
+      return False
+    if type_kind == "list":
+      if type_proto is None or schema is None:
+        return False
+      elem_type = type_proto.list.elementType
+      elem_kind = elem_type.which()
+      if elem_kind in IGNORED_TYPE_KINDS:
+        return False
+      if scalar_kind(elem_type) is not None:
+        return True
+      if elem_kind == "struct":
+        return self.node_emits("struct", None, schema.elementType, seen)
+      if elem_kind == "list":
+        return self.node_emits("list", elem_type, schema.elementType, seen)
+    return False
+
+  def emit_can_special(self, indent, service_name):
+    service_kind = "CanServiceKind::Can" if service_name == "can" else "CanServiceKind::Sendcan"
+    self.emit(indent, f"const CanServiceKind can_service = {service_kind};")
+    self.emit(indent, f"for (const auto &msg : event.{accessor('get', service_name)}()) {{")
+    self.emit(indent + 2, "append_can_frame(can_service, static_cast<uint8_t>(msg.getSrc()), msg.getAddress(), msg.getDeprecated().getBusTime(), msg.getDat(), tm, series);")  # noqa: E501
+    self.emit(indent + 2, "if (skip_raw_can) {")
+    self.emit(indent + 4, "const auto dat = msg.getDat();")
+    self.emit(indent + 4, f"decode_can_frame(can_dbc, {cxx_string(service_name)}, static_cast<uint8_t>(msg.getSrc()), msg.getAddress(), dat.begin(), dat.size(), tm, series);")  # noqa: E501
+    self.emit(indent + 2, "}")
+    self.emit(indent, "}")
+    self.emit(indent, "if (skip_raw_can) {")
+    self.emit(indent + 2, "return true;")
+    self.emit(indent, "}")
+
+  def emit_event_case(self, field_name):
+    field = self.event_schema.fields[field_name]
+    proto = field.proto
+    type_kind = field_type(field)
+    type_proto = field_type_proto(field)
+    kind = scalar_kind(type_proto)
+    schema = field.schema if kind == "Enum" or type_kind in NESTED_TYPE_KINDS else None
+    self.emit(4, f"case static_cast<cereal::Event::Which>({proto.discriminantValue}): {{")
+    valid_slot = self.add_fixed_path(f"/{field_name}/valid")
+    mono_slot = self.add_fixed_path(f"/{field_name}/logMonoTime")
+    seconds_slot = self.add_fixed_path(f"/{field_name}/t")
+    self.emit(6, f"append_fixed_scalar_point(&series->fixed_series[{valid_slot}], tm, event.getValid() ? 1.0 : 0.0);")
+    self.emit(6, f"append_fixed_scalar_point(&series->fixed_series[{mono_slot}], tm, static_cast<double>(event.getLogMonoTime()));")
+    self.emit(6, f"append_fixed_scalar_point(&series->fixed_series[{seconds_slot}], tm, tm);")
+    if field_name in {"can", "sendcan"}:
+      self.emit_can_special(6, field_name)
+    if self.node_emits(type_kind, type_proto, schema):
+      payload = self.tmp("payload")
+      self.emit(6, f"const auto {payload} = event.{accessor('get', field_name)}();")
+      self.emit_node(6, type_kind, type_proto, schema, payload, f"/{field_name}", None, False)
+    self.emit(6, "return true;")
+    self.emit(4, "}")
+
+  def generate(self):
+    self.lines = []
+    self.emit(0, "// Generated by tools/jotpluggler/generate_event_extractors.py; do not edit.")
+    self.emit(0, "")
+    self.emit(0, "const std::vector<std::string> &static_event_fixed_paths() {")
+    self.emit(2, "static const std::vector<std::string> paths = {")
+    path_insert_at = len(self.lines)
+    self.emit(2, "};")
+    self.emit(2, "return paths;")
+    self.emit(0, "}")
+    self.emit(0, "")
+    self.emit(0, "void capture_static_enum_info(const std::string &path, std::initializer_list<std::string_view> names, SeriesAccumulator *series) {")
+    self.emit(2, "if (series->enum_info.find(path) != series->enum_info.end()) {")
+    self.emit(4, "return;")
+    self.emit(2, "}")
+    self.emit(2, "EnumInfo info;")
+    self.emit(2, "info.names.reserve(names.size());")
+    self.emit(2, "for (std::string_view name : names) {")
+    self.emit(4, "info.names.emplace_back(name);")
+    self.emit(2, "}")
+    self.emit(2, "if (!info.names.empty()) {")
+    self.emit(4, "series->enum_info.emplace(path, std::move(info));")
+    self.emit(2, "}")
+    self.emit(0, "}")
+    self.emit(0, "")
+    self.emit(0, "bool append_event_static_reader(cereal::Event::Which which, const cereal::Event::Reader &event, const dbc::Database *can_dbc, bool skip_raw_can, double time_offset, SeriesAccumulator *series) {")  # noqa: E501
+    self.emit(2, "const double tm = static_cast<double>(event.getLogMonoTime()) / 1.0e9 - time_offset;")
+    self.emit(2, "switch (which) {")
+    for field_name in self.event_schema.union_fields:
+      self.emit_event_case(field_name)
+    self.emit(4, "default:")
+    self.emit(6, "return false;")
+    self.emit(2, "}")
+    self.emit(0, "}")
+
+    path_lines = ["    " + cxx_string(path) + "," for path in self.fixed_paths]
+    self.lines[path_insert_at:path_insert_at] = path_lines
+    return "\n".join(self.lines) + "\n"
+
+
+if __name__ == "__main__":
+  if len(sys.argv) != 3:
+    print(f"usage: {sys.argv[0]} <repo-root> <output>", file=sys.stderr)
+    sys.exit(2)
+
+  repo_root = Path(sys.argv[1]).resolve()
+  output = Path(sys.argv[2])
+  capnp.remove_import_hook()
+  log = capnp.load(str(repo_root / "cereal" / "log.capnp"))
+  generated = Generator(log.Event.schema).generate()
+  output.parent.mkdir(parents=True, exist_ok=True)
+  output.write_text(generated)

tools/jotpluggler/internal.h

@@ -58,6 +58,7 @@ inline constexpr float SIDEBAR_MAX_WIDTH = 520.0f;
 inline constexpr float TIMELINE_BAR_HEIGHT = 14.0f;
 inline constexpr float STATUS_BAR_HEIGHT = 52.0f;
 inline constexpr double MIN_HORIZONTAL_ZOOM_SECONDS = 2.0;
+inline constexpr double PLOT_Y_PADDING_FRACTION = 0.05;
 
 struct UiMetrics {
   float width = 0.0f;

tools/jotpluggler/plot.cc

@@ -7,8 +7,6 @@
 #include <cstdio>
 #include <limits>
 
-constexpr double PLOT_Y_PAD_FRACTION = 0.4;
-
 struct PlotBounds {
   double x_min = 0.0;
   double x_max = 1.0;
@@ -484,7 +482,7 @@ PlotBounds compute_plot_bounds(const Pane &pane,
     min_value = std::min(min_value, 0.0);
     max_value = std::max(max_value, 1.0);
   }
-  ensure_non_degenerate_range(&min_value, &max_value, PLOT_Y_PAD_FRACTION, 0.1);
+  ensure_non_degenerate_range(&min_value, &max_value, PLOT_Y_PADDING_FRACTION, 0.1);
   if (pane.range.has_y_limit_min) {
     min_value = pane.range.y_limit_min;
   }
@@ -848,7 +846,7 @@ void draw_plot(const AppSession &session, Pane *pane, UiState *state) {
     } else {
       for (size_t i = 0; i < prepared_curves.size(); ++i) {
         const PreparedCurve &curve = prepared_curves[i];
-        std::string series_id = curve_legend_label(curve, has_cursor_time, max_legend_label_width) + "##curve" + std::to_string(i);
+        std::string series_id = curve_legend_label(curve, has_cursor_time, max_legend_label_width) + "###curve" + std::to_string(curve.pane_curve_index);
         ImPlotSpec spec;
         spec.LineColor = color_rgb(curve.color);
         spec.LineWeight = curve.line_weight;

tools/jotpluggler/runtime.cc

@@ -71,7 +71,6 @@ bool should_subscribe_stream_service(const std::string &name) {
     "livestreamRoadEncodeIdx",
     "livestreamDriverEncodeIdx",
     "thumbnail",
-    "navThumbnail",
   }};
   if (name == "rawAudioData") return false;
   for (std::string_view skipped : kSkippedServices) {

tools/jotpluggler/sketch_layout.cc

@@ -2,7 +2,6 @@
 #include "tools/jotpluggler/car_fingerprint_to_dbc.h"
 #include "tools/jotpluggler/common.h"
 
-#include <capnp/dynamic.h>
 #include <kj/exception.h>
 
 #include <chrono>
@@ -10,6 +9,7 @@
 #include <cstdlib>
 #include <cstring>
 #include <iostream>
+#include <initializer_list>
 #include <map>
 #include <mutex>
 #include <limits>
@@ -51,47 +51,7 @@ struct SegmentLogs {
   std::string qcamera;
 };
 
-enum class ScalarKind {
-  None,
-  Bool,
-  Int,
-  UInt,
-  Float,
-  Enum,
-};
-
-enum class ResolvedNodeKind {
-  Ignore,
-  Scalar,
-  Struct,
-  List,
-};
-
-struct ResolvedNode {
-  ResolvedNodeKind kind = ResolvedNodeKind::Ignore;
-  ScalarKind scalar_kind = ScalarKind::None;
-  int fixed_slot = -1;
-  bool has_field = false;
-  capnp::StructSchema::Field field;
-  std::string segment;
-  std::string path;
-  bool skip_large_scalar_list = false;
-  std::vector<ResolvedNode> children;
-  std::unique_ptr<ResolvedNode> element;
-};
-
-struct ResolvedService {
-  uint16_t event_which = 0;
-  capnp::StructSchema::Field union_field;
-  std::string service_name;
-  int valid_slot = -1;
-  int log_mono_time_slot = -1;
-  int seconds_slot = -1;
-  ResolvedNode payload;
-};
-
 struct SchemaIndex {
-  std::vector<std::optional<ResolvedService>> by_which;
   size_t fixed_series_count = 0;
   std::vector<std::string> fixed_paths;
 
@@ -112,6 +72,27 @@ struct SeriesAccumulator {
   std::unordered_map<std::string, EnumInfo> enum_info;
 };
 
+void append_fixed_scalar_point(RouteSeries *series, double tm, double value);
+void append_dynamic_scalar_point(const std::string &path, double tm, double value, SeriesAccumulator *series);
+RouteSeries *ensure_list_scalar_series(const std::string &base_path, size_t index, SeriesAccumulator *series);
+void append_can_frame(CanServiceKind service,
+                      uint8_t bus,
+                      uint32_t address,
+                      uint16_t bus_time,
+                      capnp::Data::Reader dat,
+                      double tm,
+                      SeriesAccumulator *series);
+void decode_can_frame(const dbc::Database *can_dbc,
+                      const std::string &service_name,
+                      uint8_t bus,
+                      uint32_t address,
+                      const uint8_t *raw,
+                      size_t data_size,
+                      double tm,
+                      SeriesAccumulator *series);
+
+#include "tools/jotpluggler/generated_event_extractors.h"
+
 struct LoadedRouteArtifacts {
   std::vector<RouteSeries> series;
   std::vector<CanMessageData> can_messages;
@@ -898,125 +879,11 @@ SketchLayout parse_layout(const fs::path &layout_path) {
   return layout;
 }
 
-ScalarKind scalar_kind_for_type(const capnp::Type &type) {
-  if (type.isBool()) return ScalarKind::Bool;
-  if (type.isInt8() || type.isInt16() || type.isInt32() || type.isInt64()) {
-    return ScalarKind::Int;
-  }
-  if (type.isUInt8() || type.isUInt16() || type.isUInt32() || type.isUInt64()) {
-    return ScalarKind::UInt;
-  }
-  if (type.isFloat32() || type.isFloat64()) {
-    return ScalarKind::Float;
-  }
-  if (type.isEnum()) return ScalarKind::Enum;
-  return ScalarKind::None;
-}
-
-ResolvedNode build_resolved_type(const capnp::Type &type,
-                                 bool has_field,
-                                 capnp::StructSchema::Field field,
-                                 std::string segment,
-                                 std::string path,
-                                 size_t *next_fixed_slot,
-                                 std::vector<std::string> *fixed_paths,
-                                 bool dynamic_path = false) {
-  ResolvedNode node;
-  node.has_field = has_field;
-  node.field = field;
-  node.segment = std::move(segment);
-  node.path = std::move(path);
-  node.scalar_kind = scalar_kind_for_type(type);
-  if (node.scalar_kind != ScalarKind::None) {
-    node.kind = ResolvedNodeKind::Scalar;
-    if (!dynamic_path) {
-      node.fixed_slot = static_cast<int>((*next_fixed_slot)++);
-      fixed_paths->push_back(node.path);
-    }
-    return node;
-  }
-
-  if (type.isStruct()) {
-    node.kind = ResolvedNodeKind::Struct;
-    for (auto child : type.asStruct().getFields()) {
-      const std::string child_segment = child.getProto().getName().cStr();
-      node.children.push_back(build_resolved_type(
-        child.getType(),
-        true,
-        child,
-        child_segment,
-        node.path + "/" + child_segment,
-        next_fixed_slot,
-        fixed_paths,
-        dynamic_path));
-    }
-    return node;
-  }
-
-  if (type.isList()) {
-    const capnp::Type element_type = type.asList().getElementType();
-    if (element_type.isText() || element_type.isData() || element_type.isInterface() || element_type.isAnyPointer()) {
-      node.kind = ResolvedNodeKind::Ignore;
-      return node;
-    }
-    node.kind = ResolvedNodeKind::List;
-    node.skip_large_scalar_list = scalar_kind_for_type(element_type) != ScalarKind::None;
-    node.element = std::make_unique<ResolvedNode>(
-      build_resolved_type(element_type,
-                          false,
-                          capnp::StructSchema::Field(),
-                          "",
-                          node.path,
-                          next_fixed_slot,
-                          fixed_paths,
-                          true));
-    return node;
-  }
-
-  node.kind = ResolvedNodeKind::Ignore;
-  return node;
-}
-
-int register_fixed_series_path(const std::string &path,
-                               size_t *next_fixed_slot,
-                               std::vector<std::string> *fixed_paths) {
-  const int slot = static_cast<int>((*next_fixed_slot)++);
-  fixed_paths->push_back(path);
-  return slot;
-}
-
 const SchemaIndex &SchemaIndex::instance() {
   static const SchemaIndex index = [] {
     SchemaIndex out;
-    const auto event_schema = capnp::Schema::from<cereal::Event>().asStruct();
+    out.fixed_paths = static_event_fixed_paths();
-    uint16_t max_discriminant = 0;
+    out.fixed_series_count = out.fixed_paths.size();
-    for (auto union_field : event_schema.getUnionFields()) {
-      max_discriminant = std::max<uint16_t>(max_discriminant, union_field.getProto().getDiscriminantValue());
-    }
-    out.by_which.resize(static_cast<size_t>(max_discriminant) + 1);
-    size_t next_fixed_slot = 0;
-    for (auto union_field : event_schema.getUnionFields()) {
-      ResolvedService service;
-      service.event_which = union_field.getProto().getDiscriminantValue();
-      service.union_field = union_field;
-      service.service_name = union_field.getProto().getName().cStr();
-      service.valid_slot = register_fixed_series_path(
-        "/" + service.service_name + "/valid", &next_fixed_slot, &out.fixed_paths);
-      service.log_mono_time_slot = register_fixed_series_path(
-        "/" + service.service_name + "/logMonoTime", &next_fixed_slot, &out.fixed_paths);
-      service.seconds_slot = register_fixed_series_path(
-        "/" + service.service_name + "/t", &next_fixed_slot, &out.fixed_paths);
-      service.payload = build_resolved_type(
-        union_field.getType(),
-        false,
-        capnp::StructSchema::Field(),
-        service.service_name,
-        "/" + service.service_name,
-        &next_fixed_slot,
-        &out.fixed_paths);
-      out.by_which[service.event_which] = std::move(service);
-    }
-    out.fixed_series_count = next_fixed_slot;
     return out;
   }();
   return index;
@@ -1026,45 +893,6 @@ bool is_absolute_curve(const std::string &name) {
   return !name.empty() && name.front() == '/';
 }
 
-std::optional<double> scalar_value_to_double(const capnp::DynamicValue::Reader &value, ScalarKind kind) {
-  switch (kind) {
-    case ScalarKind::Bool:
-      return value.as<bool>() ? 1.0 : 0.0;
-    case ScalarKind::Int:
-      return static_cast<double>(value.as<int64_t>());
-    case ScalarKind::UInt:
-      return static_cast<double>(value.as<uint64_t>());
-    case ScalarKind::Float:
-      return value.as<double>();
-    case ScalarKind::Enum:
-      return static_cast<double>(value.as<capnp::DynamicEnum>().getRaw());
-    case ScalarKind::None:
-      return std::nullopt;
-  }
-  return std::nullopt;
-}
-
-void capture_enum_info(const std::string &path,
-                       const capnp::DynamicValue::Reader &value,
-                       SeriesAccumulator *series) {
-  if (series->enum_info.find(path) != series->enum_info.end()) {
-    return;
-  }
-
-  const auto dynamic_enum = value.as<capnp::DynamicEnum>();
-  EnumInfo info;
-  for (auto enumerant : dynamic_enum.getSchema().getEnumerants()) {
-    const uint16_t ordinal = enumerant.getOrdinal();
-    if (ordinal >= info.names.size()) {
-      info.names.resize(static_cast<size_t>(ordinal) + 1);
-    }
-    info.names[ordinal] = enumerant.getProto().getName().cStr();
-  }
-  if (!info.names.empty()) {
-    series->enum_info.emplace(path, std::move(info));
-  }
-}
-
 void append_scalar_point(RouteSeries *series,
                          const std::string &path,
                          double tm,
@@ -1195,156 +1023,9 @@ void append_dynamic_scalar_point(const std::string &path, double tm, double valu
   append_scalar_point(ensure_dynamic_series(path, series), path, tm, value);
 }
 
-void append_scalar_value(const ResolvedNode &node,
-                         const std::string *path_override,
-                         const capnp::DynamicValue::Reader &raw_value,
-                         double tm,
-                         double value,
-                         SeriesAccumulator *series) {
-  if (path_override == nullptr && node.fixed_slot >= 0) {
-    if (node.scalar_kind == ScalarKind::Enum) {
-      capture_enum_info(node.path, raw_value, series);
-    }
-    append_fixed_scalar_point(&series->fixed_series[static_cast<size_t>(node.fixed_slot)], tm, value);
-    return;
-  }
-
-  const std::string &path = path_override != nullptr ? *path_override : node.path;
-  if (node.scalar_kind == ScalarKind::Enum) {
-    capture_enum_info(path, raw_value, series);
-  }
-  append_dynamic_scalar_point(path, tm, value, series);
-}
-
-void append_fast_node(const ResolvedNode &node,
-                      const capnp::DynamicValue::Reader &value,
-                      double tm,
-                      SeriesAccumulator *series,
-                      const std::string *path_override = nullptr) {
-  switch (node.kind) {
-    case ResolvedNodeKind::Scalar: {
-      if (std::optional<double> scalar = scalar_value_to_double(value, node.scalar_kind); scalar.has_value()) {
-        append_scalar_value(node, path_override, value, tm, *scalar, series);
-      }
-      return;
-    }
-    case ResolvedNodeKind::Struct: {
-      const capnp::DynamicStruct::Reader reader = value.as<capnp::DynamicStruct>();
-      for (const ResolvedNode &child : node.children) {
-        if (!child.has_field || !reader.has(child.field)) continue;
-        if (path_override == nullptr) {
-          append_fast_node(child, reader.get(child.field), tm, series, nullptr);
-        } else {
-          const std::string child_path = child.segment.empty() ? *path_override : (*path_override + "/" + child.segment);
-          append_fast_node(child, reader.get(child.field), tm, series, &child_path);
-        }
-      }
-      return;
-    }
-    case ResolvedNodeKind::List: {
-      if (!node.element) {
-        return;
-      }
-      const capnp::DynamicList::Reader list = value.as<capnp::DynamicList>();
-      if (list.size() == 0) {
-        return;
-      }
-      if (node.skip_large_scalar_list && list.size() > 16) {
-        return;
-      }
-      const std::string &base_path = path_override != nullptr ? *path_override : node.path;
-      if (node.element->kind == ResolvedNodeKind::Scalar) {
-        for (uint i = 0; i < list.size(); ++i) {
-          if (std::optional<double> scalar = scalar_value_to_double(list[i], node.element->scalar_kind); scalar.has_value()) {
-            RouteSeries *item_series = ensure_list_scalar_series(base_path, i, series);
-            if (node.element->scalar_kind == ScalarKind::Enum && !item_series->path.empty()) {
-              capture_enum_info(item_series->path, list[i], series);
-            }
-            append_fixed_scalar_point(item_series, tm, *scalar);
-          }
-        }
-        return;
-      }
-      for (uint i = 0; i < list.size(); ++i) {
-        const std::string item_path = base_path + "/" + std::to_string(i);
-        append_fast_node(*node.element, list[i], tm, series, &item_path);
-      }
-      return;
-    }
-    case ResolvedNodeKind::Ignore:
-      return;
-  }
-}
-
-void append_event_fast_reader(cereal::Event::Which which,
-                              const cereal::Event::Reader &event,
-                              const SchemaIndex &schema,
-                              const dbc::Database *can_dbc,
-                              bool skip_raw_can,
-                              double time_offset,
-                              SeriesAccumulator *series) {
-  const uint16_t which_index = static_cast<uint16_t>(which);
-  if (which_index >= schema.by_which.size() || !schema.by_which[which_index].has_value()) {
-    return;
-  }
-  const ResolvedService &service = *schema.by_which[which_index];
-  const capnp::DynamicStruct::Reader dynamic_event(event);
-  const capnp::DynamicValue::Reader payload = dynamic_event.get(service.union_field);
-  const double tm = static_cast<double>(event.getLogMonoTime()) / 1.0e9 - time_offset;
-  append_fixed_scalar_point(&series->fixed_series[static_cast<size_t>(service.valid_slot)],
-                            tm,
-                            event.getValid() ? 1.0 : 0.0);
-  append_fixed_scalar_point(&series->fixed_series[static_cast<size_t>(service.log_mono_time_slot)],
-                            tm,
-                            static_cast<double>(event.getLogMonoTime()));
-  append_fixed_scalar_point(&series->fixed_series[static_cast<size_t>(service.seconds_slot)],
-                            tm,
-                            tm);
-  if (service.service_name == "can" || service.service_name == "sendcan") {
-    const CanServiceKind can_service = service.service_name == "can"
-      ? CanServiceKind::Can
-      : CanServiceKind::Sendcan;
-    auto decode_message = [&](uint8_t bus, uint32_t address, const auto &dat_reader) {
-      const auto bytes = dat_reader.begin();
-      decode_can_frame(can_dbc, service.service_name, bus, address, bytes, dat_reader.size(), tm, series);
-    };
-    if (service.service_name == "can") {
-      for (const auto &msg : event.getCan()) {
-        append_can_frame(can_service,
-                         static_cast<uint8_t>(msg.getSrc()),
-                         msg.getAddress(),
-                         msg.getDeprecated().getBusTime(),
-                         msg.getDat(),
-                         tm,
-                         series);
-        if (!skip_raw_can) continue;
-        decode_message(static_cast<uint8_t>(msg.getSrc()), msg.getAddress(), msg.getDat());
-      }
-    } else {
-      for (const auto &msg : event.getSendcan()) {
-        append_can_frame(can_service,
-                         static_cast<uint8_t>(msg.getSrc()),
-                         msg.getAddress(),
-                         msg.getDeprecated().getBusTime(),
-                         msg.getDat(),
-                         tm,
-                         series);
-        if (!skip_raw_can) continue;
-        decode_message(static_cast<uint8_t>(msg.getSrc()), msg.getAddress(), msg.getDat());
-      }
-    }
-    if (skip_raw_can) {
-      return;
-    }
-  }
-
-  append_fast_node(service.payload, payload, tm, series);
-}
-
 void append_event_fast(cereal::Event::Which which,
                        int32_t eidx_segnum,
                        kj::ArrayPtr<const capnp::word> data,
-                       const SchemaIndex &schema,
                        const dbc::Database *can_dbc,
                        bool skip_raw_can,
                        double time_offset,
@@ -1353,14 +1034,13 @@ void append_event_fast(cereal::Event::Which which,
     return;
   }
   with_parseable_event(data, [&](const cereal::Event::Reader &event) {
-    append_event_fast_reader(which, event, schema, can_dbc, skip_raw_can, time_offset, series);
+    append_event_static_reader(which, event, can_dbc, skip_raw_can, time_offset, series);
   });
 }
 
 void append_events_fast_range(const std::vector<Event> &events,
                               size_t begin,
                               size_t end,
-                              const SchemaIndex &schema,
                               const dbc::Database *can_dbc,
                               bool skip_raw_can,
                               SeriesAccumulator *series) {
@@ -1369,7 +1049,6 @@ void append_events_fast_range(const std::vector<Event> &events,
     append_event_fast(event_record.which,
                       event_record.eidx_segnum,
                       event_record.data,
-                      schema,
                       can_dbc,
                       skip_raw_can,
                       0.0,
@@ -1802,7 +1481,7 @@ SeriesAccumulator extract_segment_series(const std::vector<Event> &events,
   const size_t chunk_count = extract_chunk_count(events.size(), worker_budget, segment_workers);
   if (chunk_count <= 1 || events.empty()) {
     SeriesAccumulator series = make_series_accumulator(schema);
-    append_events_fast_range(events, 0, events.size(), schema, can_dbc, skip_raw_can, &series);
+    append_events_fast_range(events, 0, events.size(), can_dbc, skip_raw_can, &series);
     return series;
   }
 
@@ -1819,10 +1498,10 @@ SeriesAccumulator extract_segment_series(const std::vector<Event> &events,
     workers.emplace_back([&, chunk]() {
       const size_t begin = chunk * events_per_chunk;
       const size_t end = std::min(events.size(), begin + events_per_chunk);
-      append_events_fast_range(events, begin, end, schema, can_dbc, skip_raw_can, &chunk_results[chunk]);
+      append_events_fast_range(events, begin, end, can_dbc, skip_raw_can, &chunk_results[chunk]);
     });
   }
-  append_events_fast_range(events, 0, std::min(events.size(), events_per_chunk), schema, can_dbc, skip_raw_can, &chunk_results[0]);
+  append_events_fast_range(events, 0, std::min(events.size(), events_per_chunk), can_dbc, skip_raw_can, &chunk_results[0]);
   for (std::thread &worker : workers) {
     worker.join();
   }
@@ -2044,13 +1723,12 @@ void StreamAccumulator::appendEvent(kj::ArrayPtr<const capnp::word> data) {
       }
     }
 
-    append_event_fast_reader(which,
+    append_event_static_reader(which,
-                             event,
+                               event,
-                             impl_->schema,
+                               impl_->can_dbc ? &*impl_->can_dbc : nullptr,
-                             impl_->can_dbc ? &*impl_->can_dbc : nullptr,
+                               impl_->can_dbc.has_value(),
-                             impl_->can_dbc.has_value(),
+                               *impl_->time_offset,
-                             *impl_->time_offset,
+                               &impl_->series);
-                             &impl_->series);
     append_log_event(which, event, *impl_->time_offset, &impl_->logs, &impl_->last_alert_key);
     if (which == cereal::Event::Which::SELFDRIVE_STATE) {
       const auto sd = event.getSelfdriveState();

tools/lateral_maneuvers/lateral_maneuversd.py

@@ -13,7 +13,7 @@ from openpilot.tools.longitudinal_maneuvers.maneuversd import Action, Maneuver a
 # thresholds for starting maneuvers
 MAX_SPEED_DEV = 0.7 # deviation in m/s
 MAX_CURV = 0.002 # 500 m radius
-MAX_ROLL = 0.08 # 4.56°
+MAX_ROLL = 0.12 # 6.8°
 TIMER = 2.0 # sec stable conditions before starting maneuver
 
 @dataclass

tools/setup_dependencies.sh

@@ -19,7 +19,7 @@ function retry() {
   return 1
 }
 
-function install_ubuntu_deps() {
+function install_linux_deps() {
   SUDO=""
 
   if [[ ! $(id -u) -eq 0 ]]; then
@@ -30,61 +30,49 @@ function install_ubuntu_deps() {
     SUDO="sudo"
   fi
 
-  # Detect OS using /etc/os-release file
+  # normal stuff, this mostly for bare docker images
-  if [ -f "/etc/os-release" ]; then
+  if command -v apt-get > /dev/null 2>&1; then
-    source /etc/os-release
+    $SUDO apt-get update
-    case "$VERSION_CODENAME" in
+    $SUDO apt-get install -y --no-install-recommends ca-certificates build-essential curl libcurl4-openssl-dev locales git xvfb
-      "jammy" | "kinetic" | "noble")
+  elif command -v dnf > /dev/null 2>&1; then
-        ;;
+    $SUDO dnf install -y ca-certificates gcc gcc-c++ make curl libcurl-devel glibc-langpack-en git xorg-x11-server-Xvfb
-      *)
+  elif command -v yum > /dev/null 2>&1; then
-        echo "$ID $VERSION_ID is unsupported. This setup script is written for Ubuntu 24.04."
+    $SUDO yum install -y ca-certificates gcc gcc-c++ make curl libcurl-devel glibc-langpack-en git xorg-x11-server-Xvfb
-        read -p "Would you like to attempt installation anyway? " -n 1 -r
+  elif command -v pacman > /dev/null 2>&1; then
-        echo ""
+    $SUDO pacman -Syu --noconfirm --needed base-devel ca-certificates curl git xorg-server-xvfb
-        if [[ ! $REPLY =~ ^[Yy]$ ]]; then
+  elif command -v zypper > /dev/null 2>&1; then
-          exit 1
+    $SUDO zypper --non-interactive refresh
-        fi
+    $SUDO zypper --non-interactive install ca-certificates gcc gcc-c++ make curl libcurl-devel glibc-locale git xorg-x11-server
-        ;;
+  elif command -v apk > /dev/null 2>&1; then
-    esac
+    $SUDO apk add --no-cache ca-certificates build-base curl curl-dev musl-locales git xvfb
+  elif command -v xbps-install > /dev/null 2>&1; then
+    $SUDO xbps-install -Syu base-devel ca-certificates curl git libcurl-devel glibc-locales xorg-server-xvfb
   else
-    echo "No /etc/os-release in the system. Make sure you're running on Ubuntu, or similar."
+    echo "Unsupported Linux distribution. Supported package managers: apt-get, dnf, yum, pacman, zypper, apk, xbps-install."
     exit 1
   fi
 
-  $SUDO apt-get update
-
-  # normal stuff, mostly for the bare docker image
-  $SUDO apt-get install -y --no-install-recommends \
-    ca-certificates \
-    build-essential \
-    curl \
-    libcurl4-openssl-dev \
-    locales \
-    git \
-    xvfb
-
   if [[ -d "/etc/udev/rules.d/" ]]; then
-    # Setup jungle udev rules
+    $SUDO tee /etc/udev/rules.d/11-openpilot.rules > /dev/null <<-EOF
-    $SUDO tee /etc/udev/rules.d/12-panda_jungle.rules > /dev/null <<EOF
+	# Panda Jungle devices
-SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddcf", MODE="0666"
+	SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddcf", MODE="0666"
-SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddef", MODE="0666"
+	SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddef", MODE="0666"
-SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddcf", MODE="0666"
+	SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddcf", MODE="0666"
-SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddef", MODE="0666"
+	SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddef", MODE="0666"
 
-EOF
+	# Panda devices
+	SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="df11", MODE="0666"
+	SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddcc", MODE="0666"
+	SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddee", MODE="0666"
+	SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddcc", MODE="0666"
+	SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddee", MODE="0666"
 
-    # Setup panda udev rules
+	# comma devices over ADB
-    $SUDO tee /etc/udev/rules.d/11-panda.rules > /dev/null <<EOF
+	SUBSYSTEM=="usb", ATTR{idVendor}=="04d8", ATTR{idProduct}=="1234", ENV{adb_user}="yes"
-SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="df11", MODE="0666"
+	EOF
-SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddcc", MODE="0666"
-SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddee", MODE="0666"
-SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddcc", MODE="0666"
-SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddee", MODE="0666"
-EOF
 
-    # Setup adb udev rules
+    # delete the old ones
-    $SUDO tee /etc/udev/rules.d/50-comma-adb.rules > /dev/null <<EOF
+    $SUDO rm -f /etc/udev/rules.d/11-panda.rules /etc/udev/rules.d/12-panda_jungle.rules /etc/udev/rules.d/50-comma-adb.rules
-SUBSYSTEM=="usb", ATTR{idVendor}=="04d8", ATTR{idProduct}=="1234", ENV{adb_user}="yes"
-EOF
 
     $SUDO udevadm control --reload-rules && $SUDO udevadm trigger || true
   fi
@@ -116,7 +104,7 @@ function install_python_deps() {
 # --- Main ---
 
 if [[ "$OSTYPE" == "linux-gnu"* ]]; then
-  install_ubuntu_deps
+  install_linux_deps
   echo "[ ] installed system dependencies t=$SECONDS"
 elif [[ "$OSTYPE" == "darwin"* ]]; then
   if [[ $SHELL == "/bin/zsh" ]]; then

tools/sim/lib/simulated_sensors.py

@@ -23,17 +23,12 @@ class SimulatedSensors:
   def send_imu_message(self, simulator_state: 'SimulatorState'):
     for _ in range(5):
       dat = messaging.new_message('accelerometer', valid=True)
-      dat.accelerometer.sensor = 4
-      dat.accelerometer.type = 0x10
       dat.accelerometer.timestamp = dat.logMonoTime  # TODO: use the IMU timestamp
       dat.accelerometer.init('acceleration')
       dat.accelerometer.acceleration.v = [simulator_state.imu.accelerometer.x, simulator_state.imu.accelerometer.y, simulator_state.imu.accelerometer.z]
       self.pm.send('accelerometer', dat)
 
-      # copied these numbers from locationd
       dat = messaging.new_message('gyroscope', valid=True)
-      dat.gyroscope.sensor = 5
-      dat.gyroscope.type = 0x10
       dat.gyroscope.timestamp = dat.logMonoTime  # TODO: use the IMU timestamp
       dat.gyroscope.init('gyroUncalibrated')
       dat.gyroscope.gyroUncalibrated.v = [simulator_state.imu.gyroscope.x, simulator_state.imu.gyroscope.y, simulator_state.imu.gyroscope.z]
@@ -92,11 +87,12 @@ class SimulatedSensors:
 
     # dmonitoringd output
     dat = messaging.new_message('driverMonitoringState', valid=True)
-    dat.driverMonitoringState = {
+    dm = dat.driverMonitoringState
-      "faceDetected": True,
+    dm.alertLevel = log.DriverMonitoringState.AlertLevel.none
-      "isDistracted": False,
+    dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision
-      "awarenessStatus": 1.,
+    dm.visionPolicyState.faceDetected = True
-    }
+    dm.visionPolicyState.isDistracted = False
+    dm.visionPolicyState.awarenessPercent = 100
     self.pm.send('driverMonitoringState', dat)
 
   def send_camera_images(self, world: 'World'):