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

selfdrive/assets/assets.qrc

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

selfdrive/assets/fonts/process.py

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

selfdrive/assets/icons_mici/body.png

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

selfdrive/assets/icons_mici/egpu.png

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

selfdrive/assets/icons_mici/egpu_gray.png

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

selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_cone.png

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:26b3660dbd1e60b0ba98914afa7cb3a67151bb6990d218f55c901f243e38ff3e
-size 3631
+oid sha256:5f5d4b7bd406114945e53f4d56089102ee83ba7d0a4590a371f7acab90c7ad53
+size 1862

selfdrive/assets/prep-svg.sh

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

selfdrive/car/car_specific.py

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

selfdrive/car/card.py

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

selfdrive/car/tests/test_cruise_speed.py

@@ -94,7 +94,7 @@ class TestVCruiseHelper:
         self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
 
       # Expected diff on enabling. Speed should not change on falling edge of pressed
-      assert not pressed == self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last
+      assert (not pressed) == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
 
   def test_resume_in_standstill(self):
     """

selfdrive/car/tests/test_models.py

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

selfdrive/controls/controlsd.py

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

selfdrive/controls/lib/drive_helpers.py

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

selfdrive/controls/lib/lateral_mpc_lib/SConscript

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

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

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

selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

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

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

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

selfdrive/controls/radard.py

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

selfdrive/debug/car/ecu_addrs.py

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

selfdrive/debug/car/fw_versions.py

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

selfdrive/debug/set_car_params.py

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

selfdrive/debug/uiview.py

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

selfdrive/locationd/calibrationd.py

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

selfdrive/locationd/lagd.py

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

selfdrive/locationd/paramsd.py

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

selfdrive/locationd/test/test_calibrationd.py

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

selfdrive/locationd/test/test_lagd.py

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

selfdrive/locationd/test/test_paramsd.py

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

selfdrive/locationd/torqued.py

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

selfdrive/modeld/SConscript

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

selfdrive/modeld/compile_dm_warp.py

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

selfdrive/modeld/compile_modeld.py

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

selfdrive/modeld/constants.py

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

selfdrive/modeld/dmonitoringmodeld.py

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

selfdrive/modeld/get_model_metadata.py

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

selfdrive/modeld/helpers.py

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

selfdrive/modeld/modeld.py

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

selfdrive/modeld/models/big_driving_on_policy.onnx

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

selfdrive/modeld/models/big_driving_policy.onnx

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

selfdrive/modeld/models/big_driving_vision.onnx

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

selfdrive/modeld/models/big_driving_vision.onnx

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

selfdrive/modeld/models/dmonitoring_model.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:7aff7ff1dc08bbaf562a8f77380ab5e5914f8557dba2f760d87e4d953f5604b0
-size 7307246
+oid sha256:3e7b31dfbc0a5234f1baf196513b77fc6af12204b8a8ffe8ee0417e48352f316
+size 7494962

selfdrive/modeld/models/driving_on_policy.onnx

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

selfdrive/modeld/models/driving_policy.onnx

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

selfdrive/modeld/models/driving_vision.onnx

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

selfdrive/modeld/parse_model_outputs.py

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

selfdrive/modeld/tinygrad_helpers.py

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

selfdrive/monitoring/README.md

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

selfdrive/monitoring/dmonitoringd.py

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

selfdrive/monitoring/policy.py

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

selfdrive/monitoring/test_monitoring.py

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

selfdrive/pandad/pandad.cc

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

selfdrive/pandad/pandad.py

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

selfdrive/pandad/tests/test_pandad.py

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

selfdrive/pandad/tests/test_pandad_loopback.py

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

selfdrive/selfdrived/alertmanager.py

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

selfdrive/selfdrived/selfdrived.py

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

selfdrive/selfdrived/tests/test_alerts.py

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

selfdrive/test/helpers.py

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

selfdrive/test/longitudinal_maneuvers/plant.py

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

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -170,7 +170,7 @@ def create_maneuvers(kwargs):
       initial_speed=0.,
       lead_relevancy=True,
       initial_distance_lead=STOP_DISTANCE,
-      speed_lead_values=[0., 0., 2.],
+      speed_lead_values=[0., 0., 7.],
       breakpoints=[1., 10., 15.],
       ensure_start=True,
       **kwargs,

selfdrive/test/process_replay/migration.py

@@ -14,7 +14,7 @@ from opendbc.car.gm.values import GMSafetyFlags
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.modeld.fill_model_msg import fill_xyz_poly, fill_lane_line_meta
 from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_encode_index
-from openpilot.selfdrive.controls.lib.longitudinal_planner import get_accel_from_plan, CONTROL_N_T_IDX
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan
 from openpilot.system.manager.process_config import managed_processes
 from openpilot.tools.lib.logreader import LogIterable
 
@@ -89,6 +89,10 @@ def migrate(lr: LogIterable, migration_funcs: list[MigrationFunc]):
   return lr
 
 
+def as_reader(builder) -> capnp.lib.capnp._DynamicStructReader:
+  return log.Event.from_bytes(builder.to_bytes()).__enter__()  # round-trip through bytes, 2x faster and 30x less memory than builder.as_reader()
+
+
 def migration(inputs: list[str], product: str|None=None):
   def decorator(func):
     @functools.wraps(func)
@@ -124,9 +128,9 @@ def migrate_longitudinalPlan(msgs):
     if msg.which() != 'longitudinalPlan':
       continue
     new_msg = msg.as_builder()
-    a_target, should_stop = get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels, CONTROL_N_T_IDX)
+    a_target, should_stop = get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels, ModelConstants.T_IDXS[:CONTROL_N])
     new_msg.longitudinalPlan.aTarget, new_msg.longitudinalPlan.shouldStop = float(a_target), bool(should_stop)
-    ops.append((index, new_msg.as_reader()))
+    ops.append((index, as_reader(new_msg)))
   return ops, [], []
 
 
@@ -135,7 +139,7 @@ def migrate_driverAssistance(msgs):
   add_ops = []
   for _, msg in msgs:
     new_msg = messaging.new_message('driverAssistance', valid=True, logMonoTime=msg.logMonoTime)
-    add_ops.append(new_msg.as_reader())
+    add_ops.append(as_reader(new_msg))
   return [], add_ops, []
 
 
@@ -152,7 +156,7 @@ def migrate_drivingModelData(msgs):
       fill_lane_line_meta(dmd.drivingModelData.laneLineMeta, msg.modelV2.laneLines, msg.modelV2.laneLineProbs)
     if all(len(a) for a in [msg.modelV2.position.x, msg.modelV2.position.y, msg.modelV2.position.z]):
       fill_xyz_poly(dmd.drivingModelData.path, ModelConstants.POLY_PATH_DEGREE, msg.modelV2.position.x, msg.modelV2.position.y, msg.modelV2.position.z)
-    add_ops.append( dmd.as_reader())
+    add_ops.append(as_reader(dmd))
   return [], add_ops, []
 
 
@@ -177,7 +181,7 @@ def migrate_liveTracks(msgs):
       pts.append(pt)
 
     new_msg.liveTracks.points = pts
-    ops.append((index, new_msg.as_reader()))
+    ops.append((index, as_reader(new_msg)))
   return ops, [], []
 
 
@@ -197,7 +201,7 @@ def migrate_liveLocationKalman(msgs):
       lp_field.valid = llk_field.valid
     for flag in ["inputsOK", "posenetOK", "sensorsOK"]:
       setattr(m.livePose, flag, getattr(msg.liveLocationKalmanDEPRECATED, flag))
-    ops.append((index, m.as_reader()))
+    ops.append((index, as_reader(m)))
   return ops, [], []
 
 
@@ -212,7 +216,7 @@ def migrate_livePose(msgs):
     if msg.which() == "livePose":
       new_msg = msg.as_builder()
       new_msg.livePose.timestamp = msg.logMonoTime
-      ops.append((index, new_msg.as_reader()))
+      ops.append((index, as_reader(new_msg)))
   return ops, [], []
 
 
@@ -228,7 +232,7 @@ def migrate_controlsState(msgs):
                   "alertStatus", "alertSize", "alertType", "experimentalMode",
                   "personality"):
       setattr(ss, field, getattr(msg.controlsState.deprecated, field))
-    add_ops.append(m.as_reader())
+    add_ops.append(as_reader(m))
   return [], add_ops, []
 
 
@@ -244,7 +248,7 @@ def migrate_carState(msgs):
         msg = msg.as_builder()
         msg.carState.vCruise = last_cs.controlsState.deprecated.vCruise
         msg.carState.vCruiseCluster = last_cs.controlsState.deprecated.vCruiseCluster
-        ops.append((index, msg.as_reader()))
+        ops.append((index, as_reader(msg)))
   return ops, [], []
 
 
@@ -254,7 +258,7 @@ def migrate_managerState(msgs):
   for index, msg in msgs:
     new_msg = msg.as_builder()
     new_msg.managerState.processes = [{'name': name, 'running': True} for name in managed_processes]
-    ops.append((index, new_msg.as_reader()))
+    ops.append((index, as_reader(new_msg)))
   return ops, [], []
 
 
@@ -267,7 +271,7 @@ def migrate_gpsLocation(msgs):
     # hasFix is a newer field
     if not g.hasFix and g.flags == 1:
       g.hasFix = True
-    ops.append((index, new_msg.as_reader()))
+    ops.append((index, as_reader(new_msg)))
   return ops, [], []
 
 
@@ -283,7 +287,7 @@ def migrate_deviceState(msgs):
     if msg.which() == 'deviceState':
       n = msg.as_builder()
       n.deviceState.deviceType = init_data.deviceType
-      ops.append((i, n.as_reader()))
+      ops.append((i, as_reader(n)))
   return ops, [], []
 
 
@@ -295,7 +299,7 @@ def migrate_carOutput(msgs):
     co.valid = msg.valid
     co.logMonoTime = msg.logMonoTime
     co.carOutput.actuatorsOutput = msg.carControl.actuatorsOutputDEPRECATED
-    add_ops.append(co.as_reader())
+    add_ops.append(as_reader(co))
   return [], add_ops, []
 
 
@@ -333,13 +337,13 @@ def migrate_pandaStates(msgs):
       new_msg.logMonoTime = msg.logMonoTime
       new_msg.pandaStates[0] = msg.pandaStateDEPRECATED
       new_msg.pandaStates[0].safetyParam = safety_param
-      ops.append((index, new_msg.as_reader()))
+      ops.append((index, as_reader(new_msg)))
     elif msg.which() == 'pandaStates':
       new_msg = msg.as_builder()
       new_msg.pandaStates[-1].safetyParam = safety_param
       # Clear DISABLE_DISENGAGE_ON_GAS bit to fix controls mismatch
       new_msg.pandaStates[-1].alternativeExperience &= ~1
-      ops.append((index, new_msg.as_reader()))
+      ops.append((index, as_reader(new_msg)))
   return ops, [], []
 
 
@@ -354,7 +358,7 @@ def migrate_peripheralState(msgs):
     new_msg = messaging.new_message("peripheralState")
     new_msg.valid = msg.valid
     new_msg.logMonoTime = msg.logMonoTime
-    add_ops.append(new_msg.as_reader())
+    add_ops.append(as_reader(new_msg))
   return [], add_ops, []
 
 
@@ -409,7 +413,7 @@ def migrate_cameraStates(msgs):
     new_msg.valid = msg.valid
 
     del_ops.append(index)
-    add_ops.append(new_msg.as_reader())
+    add_ops.append(as_reader(new_msg))
   return [], add_ops, del_ops
 
 
@@ -421,7 +425,7 @@ def migrate_carParams(msgs):
     CP.carParams.carFingerprint = MIGRATION.get(CP.carParams.carFingerprint, CP.carParams.carFingerprint)
     for car_fw in CP.carParams.carFw:
       car_fw.brand = CP.carParams.brand
-    ops.append((index, CP.as_reader()))
+    ops.append((index, as_reader(CP)))
   return ops, [], []
 
 
@@ -449,14 +453,11 @@ 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()))
 
-      add_ops.append(m.as_reader())
+      add_ops.append(as_reader(m))
     del_ops.append(index)
   return [], add_ops, del_ops
 
@@ -479,27 +480,51 @@ def migrate_onroadEvents(msgs):
     new_msg.valid = msg.valid
     new_msg.logMonoTime = msg.logMonoTime
     new_msg.onroadEvents = onroadEvents
-    ops.append((index, new_msg.as_reader()))
+    ops.append((index, as_reader(new_msg)))
 
   return ops, [], []
 
 
-@migration(inputs=["driverMonitoringState"])
+@migration(inputs=["driverMonitoringStateDEPRECATED"])
 def migrate_driverMonitoringState(msgs):
   ops = []
   for index, msg in msgs:
-    msg = msg.as_builder()
-    events = []
-    for event in msg.driverMonitoringState.deprecated.events:
-      try:
-        if not str(event.name).endswith('DEPRECATED'):
-          migrated_event = migrate_onroad_event(event)
-          if migrated_event is not None:
-            events.append(migrated_event)
-      except RuntimeError:  # Member was null
-        traceback.print_exc()
+    old = msg.driverMonitoringStateDEPRECATED
+    new_msg = messaging.new_message('driverMonitoringState', valid=msg.valid, logMonoTime=msg.logMonoTime)
+    dm = new_msg.driverMonitoringState
+    dm.isRHD = old.isRHD
+    dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision if old.isActiveMode else \
+                          log.DriverMonitoringState.MonitoringPolicy.wheeltouch
 
-    msg.driverMonitoringState.events = events
-    ops.append((index, msg.as_reader()))
+    AlertLevel = log.DriverMonitoringState.AlertLevel
+    event_to_alert_level = {
+      'driverDistracted1': AlertLevel.one, 'driverUnresponsive1': AlertLevel.one,
+      'driverDistracted2': AlertLevel.two, 'driverUnresponsive2': AlertLevel.two,
+      'driverDistracted3': AlertLevel.three, 'driverUnresponsive3': 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.lockout = any(str(event.name) == 'tooDistracted' for event in old.events)
+
+    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.distractedTypes.pose = bool(old.distractedType & 1)
+    dm.visionPolicyState.distractedTypes.eye = bool(old.distractedType & 2)
+    dm.visionPolicyState.distractedTypes.phone = bool(old.distractedType & 4)
+    dm.visionPolicyState.faceDetected = old.faceDetected
+    dm.visionPolicyState.pose.pitchCalib.offset = old.posePitchOffset
+    dm.visionPolicyState.pose.pitchCalib.calibratedPercent = int(min(100, old.posePitchValidCount / 1200 * 100))
+    dm.visionPolicyState.pose.yawCalib.offset = old.poseYawOffset
+    dm.visionPolicyState.pose.yawCalib.calibratedPercent = int(min(100, old.poseYawValidCount / 1200 * 100))
+    dm.visionPolicyState.pose.calibrated = old.posePitchValidCount >= 1200 and old.poseYawValidCount >= 1200
+    dm.visionPolicyState.wheeltouchFallbackPercent = int(min(100, old.hiStdCount / 200 * 100))
+    dm.visionPolicyState.uncertainOffroadAlertPercent = int(min(100, old.uncertainCount / 1200 * 100))
+    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, as_reader(new_msg)))
 
   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/test/process_replay/process_replay.py

@@ -192,9 +192,9 @@ class ProcessContainer:
     params = Params()
     for k, v in params_config.items():
       if isinstance(v, bool):
-        params.put_bool(k, v)
+        params.put_bool(k, v, block=True)
       else:
-        params.put(k, v)
+        params.put(k, v, block=True)
 
     self.environ_config = environ_config
 
@@ -375,8 +375,8 @@ def get_car_params_callback(rc, pm, msgs, fingerprint):
     _CI = get_car(can_recv, lambda _msgs: None, lambda obd: None, params.get_bool("AlphaLongitudinalEnabled"), False, cached_params=cached_params)
     CP, CP_SP = _CI.CP, _CI.CP_SP
 
-  params.put("CarParams", CP.to_bytes())
-  params.put("CarParamsSP", convert_to_capnp(CP_SP).to_bytes())
+  params.put("CarParams", CP.to_bytes(), block=True)
+  params.put("CarParamsSP", convert_to_capnp(CP_SP).to_bytes(), block=True)
 
 
 def card_rcv_callback(msg, cfg, frame):

selfdrive/test/scons_build_test.sh

@@ -13,7 +13,7 @@ cd $BASEDIR
 
 cd $BASEDIR/opendbc_repo/
 scons --clean
-scons --no-cache --random -j$(nproc)
+scons --no-cache --random
 if ! scons -q; then
   echo "FAILED: all build products not up to date after first pass."
   exit 1

selfdrive/test/setup_device_ci.sh

@@ -48,13 +48,6 @@ while true; do
   if ! sudo systemctl is-active -q ssh; then
     sudo systemctl start ssh
   fi
-
-  #if ! pgrep -f 'ciui.py' > /dev/null 2>&1; then
-  #  echo 'starting UI'
-  #  cp $SOURCE_DIR/selfdrive/test/ciui.py /data/
-  #  /data/ciui.py &
-  #fi
-
   sleep 5s
 done
 

selfdrive/test/setup_xvfb.sh

@@ -9,7 +9,7 @@ else
 fi
 export DISPLAY=:$DISP_ID
 
-sudo Xvfb $DISPLAY -screen 0 2160x1080x24 2>/dev/null &
+Xvfb $DISPLAY -screen 0 2160x1080x24 2>/dev/null &
 
 # check for x11 socket for the specified display ID
 while [ ! -S /tmp/.X11-unix/X$DISP_ID ]

selfdrive/test/test_onroad.py

@@ -68,6 +68,7 @@ PROCS = {
   "system.loggerd.deleter": 1.0,
   "./pandad": 19.0,
   "system.qcomgpsd.qcomgpsd": 1.0,
+  "system.hardware.tici.modem": 10.0,
 }
 
 TIMINGS = {
@@ -118,7 +119,7 @@ class TestOnroad:
     # setup env
     params = Params()
     params.remove("CurrentRoute")
-    params.put_bool("RecordFront", True)
+    params.put_bool("RecordFront", True, block=True)
     set_params_enabled()
     os.environ['REPLAY'] = '1'
     os.environ['MSGQ_PREALLOC'] = '1'
@@ -209,7 +210,7 @@ class TestOnroad:
 
     # other processes preempt ui while starting up
     offset = int(20 * LOG_OFFSET)
-    ts = self.ts['uiDebug']['drawTimeMillis'][offset:]
+    ts = self.ts['uiDebug']['cpuTimeMillis'][offset:]
     result += f"min  {min(ts):.2f}ms\n"
     result += f"max  {max(ts):.2f}ms\n"
     result += f"std  {np.std(ts):.2f}ms\n"
@@ -370,23 +371,6 @@ class TestOnroad:
           assert enc_sof == cam_sof, f"SOF mismatch: frameId={fid}, enc_sof={enc_sof}, cam_sof={cam_sof}"
           assert enc_eof == cam_eof, f"EOF mismatch: frameId={fid}, enc_eof={enc_eof}, cam_eof={cam_eof}"
 
-  def test_mpc_execution_timings(self):
-    result = "\n"
-    result += "------------------------------------------------\n"
-    result += "-----------------  MPC Timing ------------------\n"
-    result += "------------------------------------------------\n"
-
-    cfgs = [("longitudinalPlan", 0.05, 0.05),]
-    for (s, instant_max, avg_max) in cfgs:
-      ts = [getattr(m, s).solverExecutionTime for m in self.msgs[s]]
-      assert max(ts) < instant_max, f"high '{s}' execution time: {max(ts)}"
-      assert np.mean(ts) < avg_max, f"high avg '{s}' execution time: {np.mean(ts)}"
-      result += f"'{s}' execution time: min  {min(ts):.5f}s\n"
-      result += f"'{s}' execution time: max  {max(ts):.5f}s\n"
-      result += f"'{s}' execution time: mean {np.mean(ts):.5f}s\n"
-    result += "------------------------------------------------\n"
-    print(result)
-
   def test_model_execution_timings(self, subtests):
     result = "\n"
     result += "------------------------------------------------\n"
@@ -401,9 +385,7 @@ class TestOnroad:
       ("driverStateV2", 0.3, 0.05),
     ]
     for (s, instant_max, avg_max) in cfgs:
-      ts = [getattr(m, s).modelExecutionTime for m in self.msgs[s]]
-      # TODO some init can happen in first iteration
-      ts = ts[1:]
+      ts = [getattr(m, s).modelExecutionTime for m in self.msgs[s] if (m.logMonoTime*1e-9 - self.ts[s]['t'][0]) > LOG_OFFSET]
       result += f"'{s}' execution time: min  {min(ts):.5f}s\n"
       result += f"'{s}' execution time: max {max(ts):.5f}s\n"
       result += f"'{s}' execution time: mean {np.mean(ts):.5f}s\n"

selfdrive/test/test_updated.py

@@ -50,7 +50,7 @@ class TestUpdated:
       f"git clone {BASEDIR} {self.git_remote_dir}",
       f"git clone {self.git_remote_dir} {self.basedir}",
       f"cd {self.basedir} && git submodule init && git submodule update",
-      f"cd {self.basedir} && scons -j{os.cpu_count()} cereal/ common/"
+      f"cd {self.basedir} && scons cereal/ common/"
     ])
 
     self.params = Params(os.path.join(self.basedir, "persist/params"))
@@ -92,7 +92,7 @@ class TestUpdated:
     return subprocess.Popen(updated_path, env=os.environ)
 
   def _start_updater(self, offroad=True, nosleep=False):
-    self.params.put_bool("IsOffroad", offroad)
+    self.params.put_bool("IsOffroad", offroad, block=True)
     self.updated_proc = self._get_updated_proc()
     if not nosleep:
       time.sleep(1)

selfdrive/ui/SConscript

@@ -1,4 +1,6 @@
 from pathlib import Path
+import importlib.util
+
 Import('env', 'arch', 'common')
 
 # build the fonts
@@ -18,15 +20,13 @@ env.Command(
 
 if GetOption('extras') and arch == "larch64":
   # build installers
+  raylib_dir = Path(importlib.util.find_spec("raylib").submodule_search_locations[0]) / "install"
   raylib_env = env.Clone()
-  raylib_env['LIBPATH'] += [f'#third_party/raylib/{arch}/']
+  raylib_env['CPPPATH'] += [str(raylib_dir / "include")]
+  raylib_env['LIBPATH'] += [str(raylib_dir / "lib")]
   raylib_env['LINKFLAGS'].append('-Wl,-strip-debug')
 
-  raylib_libs = common + ["raylib"]
-  if arch == "larch64":
-    raylib_libs += ["GLESv2", "EGL", "gbm", "drm"]
-  else:
-    raylib_libs += ["GL"]
+  raylib_libs = common + ["raylib_comma", "GLESv2", "EGL", "gbm", "drm"]
 
   release = "release3"
   installers = [

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/installer/installer.cc

@@ -6,7 +6,7 @@
 #include "common/swaglog.h"
 #include "common/util.h"
 #include "system/hardware/hw.h"
-#include "third_party/raylib/include/raylib.h"
+#include "raylib.h"
 
 int freshClone();
 int cachedFetch(const std::string &cache);

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/onboarding.py

@@ -69,7 +69,7 @@ class TrainingGuide(Widget):
       if self._step == DM_RECORD_STEP:
         yes = rl.check_collision_point_rec(mouse_pos, DM_RECORD_YES_RECT)
         print(f"putting RecordFront to {yes}")
-        ui_state.params.put_bool("RecordFront", yes)
+        ui_state.params.put_bool("RecordFront", yes, block=True)
 
       # Restart training?
       elif self._step == len(self._image_paths) - 1:
@@ -156,7 +156,7 @@ class DeclinePage(Widget):
                                  click_callback=self._on_uninstall_clicked)
 
   def _on_uninstall_clicked(self):
-    ui_state.params.put_bool("DoUninstall", True)
+    ui_state.params.put_bool("DoUninstall", True, block=True)
     gui_app.request_close()
 
   def _render(self, _):
@@ -209,8 +209,8 @@ class OnboardingWindow(Widget):
     self._state = OnboardingState.TERMS
 
   def _on_terms_accepted(self):
-    ui_state.params.put("HasAcceptedTerms", terms_version)
-    ui_state.params.put("HasAcceptedTermsSP", terms_version_sp)
+    ui_state.params.put("HasAcceptedTerms", terms_version, block=True)
+    ui_state.params.put("HasAcceptedTermsSP", terms_version_sp, block=True)
     if not self._sunnylink.completed:
       self._state = OnboardingState.SUNNYLINK_CONSENT
     elif not self._training_done:
@@ -219,7 +219,7 @@ class OnboardingWindow(Widget):
       gui_app.pop_widget()
 
   def _on_completed_training(self):
-    ui_state.params.put("CompletedTrainingVersion", training_version)
+    ui_state.params.put("CompletedTrainingVersion", training_version, block=True)
 
   def _render(self, _):
     if self._training_guide is None:

selfdrive/ui/layouts/settings/common.py

@@ -1,5 +1,5 @@
 from openpilot.selfdrive.ui.ui_state import ui_state
 
 
-def restart_needed_callback(_):
+def restart_needed_callback(_=None):
   ui_state.params.put_bool("OnroadCycleRequested", True)

selfdrive/ui/layouts/settings/developer.py

@@ -23,7 +23,7 @@ DESCRIPTIONS = {
     "other than your own. A comma employee will NEVER ask you to add their GitHub username."
   ),
   'alpha_longitudinal': tr_noop(
-    "<b>WARNING: sunnypilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).</b><br><br>" +
+    "<b>WARNING: sunnypilot longitudinal control is in alpha for this car and may disable Automatic Emergency Braking (AEB).</b><br><br>" +
     "On this car, sunnypilot defaults to the car's built-in ACC instead of sunnypilot's longitudinal control. " +
     "Enable this to switch to sunnypilot longitudinal control. " +
     "Enabling Experimental mode is recommended when enabling sunnypilot longitudinal control alpha. " +
@@ -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)
@@ -160,45 +154,45 @@ class DeveloperLayout(Widget):
       item.action_item.set_state(self._params.get_bool(key))
 
   def _on_enable_ui_debug(self, state: bool):
-    self._params.put_bool("ShowDebugInfo", state)
+    self._params.put_bool("ShowDebugInfo", state, block=True)
     gui_app.set_show_touches(state)
     gui_app.set_show_fps(state)
     gui_app.set_show_mouse_coords(state)
 
   def _on_enable_adb(self, state: bool):
-    self._params.put_bool("AdbEnabled", state)
+    self._params.put_bool("AdbEnabled", state, block=True)
 
   def _on_enable_ssh(self, state: bool):
-    self._params.put_bool("SshEnabled", state)
+    self._params.put_bool("SshEnabled", state, block=True)
 
   def _on_joystick_debug_mode(self, state: bool):
-    self._params.put_bool("JoystickDebugMode", state)
-    self._params.put_bool("LongitudinalManeuverMode", False)
+    self._params.put_bool("JoystickDebugMode", state, block=True)
+    self._params.put_bool("LongitudinalManeuverMode", False, block=True)
     self._long_maneuver_toggle.action_item.set_state(False)
-    self._params.put_bool("LateralManeuverMode", False)
+    self._params.put_bool("LateralManeuverMode", False, block=True)
     self._lat_maneuver_toggle.action_item.set_state(False)
 
   def _on_long_maneuver_mode(self, state: bool):
-    self._params.put_bool("LongitudinalManeuverMode", state)
-    self._params.put_bool("JoystickDebugMode", False)
+    self._params.put_bool("LongitudinalManeuverMode", state, block=True)
+    self._params.put_bool("JoystickDebugMode", False, block=True)
     self._joystick_toggle.action_item.set_state(False)
-    self._params.put_bool("LateralManeuverMode", False)
+    self._params.put_bool("LateralManeuverMode", False, block=True)
     self._lat_maneuver_toggle.action_item.set_state(False)
 
   def _on_lat_maneuver_mode(self, state: bool):
-    self._params.put_bool("LateralManeuverMode", state)
-    self._params.put_bool("ExperimentalMode", False)
-    self._params.put_bool("JoystickDebugMode", False)
+    self._params.put_bool("LateralManeuverMode", state, block=True)
+    self._params.put_bool("ExperimentalMode", False, block=True)
+    self._params.put_bool("JoystickDebugMode", False, block=True)
     self._joystick_toggle.action_item.set_state(False)
-    self._params.put_bool("LongitudinalManeuverMode", False)
+    self._params.put_bool("LongitudinalManeuverMode", False, block=True)
     self._long_maneuver_toggle.action_item.set_state(False)
 
   def _on_alpha_long_enabled(self, state: bool):
     if state:
       def confirm_callback(result: DialogResult):
         if result == DialogResult.CONFIRM:
-          self._params.put_bool("AlphaLongitudinalEnabled", True)
-          self._params.put_bool("OnroadCycleRequested", True)
+          self._params.put_bool("AlphaLongitudinalEnabled", True, block=True)
+          self._params.put_bool("OnroadCycleRequested", True, block=True)
           self._update_toggles()
         else:
           self._alpha_long_toggle.action_item.set_state(False)
@@ -211,6 +205,6 @@ class DeveloperLayout(Widget):
       gui_app.push_widget(dlg)
 
     else:
-      self._params.put_bool("AlphaLongitudinalEnabled", False)
-      self._params.put_bool("OnroadCycleRequested", True)
+      self._params.put_bool("AlphaLongitudinalEnabled", False, block=True)
+      self._params.put_bool("OnroadCycleRequested", True, block=True)
       self._update_toggles()

selfdrive/ui/layouts/settings/device.py

@@ -108,7 +108,7 @@ class DeviceLayout(Widget):
       self._params.remove("LiveParameters")
       self._params.remove("LiveParametersV2")
       self._params.remove("LiveDelay")
-      self._params.put_bool("OnroadCycleRequested", True)
+      self._params.put_bool("OnroadCycleRequested", True, block=True)
       self._update_calib_description()
 
     dialog = ConfirmDialog(tr("Are you sure you want to reset calibration?"), tr("Reset"), callback=reset_calibration)
@@ -169,7 +169,7 @@ class DeviceLayout(Widget):
 
     def perform_reboot(result: DialogResult):
       if not ui_state.engaged and result == DialogResult.CONFIRM:
-        self._params.put_bool_nonblocking("DoReboot", True)
+        self._params.put_bool("DoReboot", True)
 
     dialog = ConfirmDialog(tr("Are you sure you want to reboot?"), tr("Reboot"), callback=perform_reboot)
     gui_app.push_widget(dialog)
@@ -181,7 +181,7 @@ class DeviceLayout(Widget):
 
     def perform_power_off(result: DialogResult):
       if not ui_state.engaged and result == DialogResult.CONFIRM:
-        self._params.put_bool_nonblocking("DoShutdown", True)
+        self._params.put_bool("DoShutdown", True)
 
     dialog = ConfirmDialog(tr("Are you sure you want to power off?"), tr("Power Off"), callback=perform_power_off)
     gui_app.push_widget(dialog)

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:
-      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)
-      y = self._draw_wrapped_text(x, y, w, contrib_text, gui_app.font(FontWeight.BOLD), 52, rl.WHITE)
-      y += 20 + 20
+    #if self._segment_count > 0:
+    #  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)
+    #  y = self._draw_wrapped_text(x, y, w, contrib_text, gui_app.font(FontWeight.BOLD), 52, rl.WHITE)
+    #  y += 20 + 20
 
     # Separator
     rl.draw_rectangle(x, y, w, 2, self.GRAY)

selfdrive/ui/layouts/settings/software.py

@@ -170,7 +170,7 @@ class SoftwareLayout(Widget):
   def _on_uninstall(self):
     def handle_uninstall_confirmation(result: DialogResult):
       if result == DialogResult.CONFIRM:
-        ui_state.params.put_bool("DoUninstall", True)
+        ui_state.params.put_bool("DoUninstall", True, block=True)
 
     dialog = ConfirmDialog(tr("Are you sure you want to uninstall?"), tr("Uninstall"), callback=handle_uninstall_confirmation)
     gui_app.push_widget(dialog)
@@ -178,7 +178,7 @@ class SoftwareLayout(Widget):
   def _on_install_update(self):
     # Trigger reboot to install update
     self._install_btn.action_item.set_enabled(False)
-    ui_state.params.put_bool("DoReboot", True)
+    ui_state.params.put_bool("DoReboot", True, block=True)
 
   def _on_select_branch(self):
     # Get available branches and order
@@ -197,7 +197,7 @@ class SoftwareLayout(Widget):
       # Confirmed selection
       if result == DialogResult.CONFIRM and self._branch_dialog is not None and self._branch_dialog.selection:
         selection = self._branch_dialog.selection
-        ui_state.params.put("UpdaterTargetBranch", selection)
+        ui_state.params.put("UpdaterTargetBranch", selection, block=True)
         self._branch_btn.action_item.set_value(selection)
         os.system("pkill -SIGUSR1 -f system.updated.updated")
       self._branch_dialog = None

selfdrive/ui/layouts/settings/toggles.py

@@ -221,8 +221,8 @@ class TogglesLayout(Widget):
     if state and not confirmed:
       def confirm_callback(result: DialogResult):
         if result == DialogResult.CONFIRM:
-          self._params.put_bool("ExperimentalMode", True)
-          self._params.put_bool("ExperimentalModeConfirmed", True)
+          self._params.put_bool("ExperimentalMode", True, block=True)
+          self._params.put_bool("ExperimentalModeConfirmed", True, block=True)
         else:
           self._toggles["ExperimentalMode"].action_item.set_state(False)
         self._update_experimental_mode_icon()
@@ -234,16 +234,16 @@ class TogglesLayout(Widget):
       gui_app.push_widget(dlg)
     else:
       self._update_experimental_mode_icon()
-      self._params.put_bool("ExperimentalMode", state)
+      self._params.put_bool("ExperimentalMode", state, block=True)
 
   def _toggle_callback(self, state: bool, param: str):
     if param == "ExperimentalMode":
       self._handle_experimental_mode_toggle(state)
       return
 
-    self._params.put_bool(param, state)
+    self._params.put_bool(param, state, block=True)
     if self._toggle_defs[param][3]:
-      self._params.put_bool("OnroadCycleRequested", True)
+      self._params.put_bool("OnroadCycleRequested", True, block=True)
 
   def _set_longitudinal_personality(self, button_index: int):
-    self._params.put("LongitudinalPersonality", button_index)
+    self._params.put("LongitudinalPersonality", button_index, block=True)

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/lib/prime_state.py

@@ -6,6 +6,7 @@ import time
 
 from openpilot.common.api import api_get
 from openpilot.common.params import Params
+from openpilot.common.realtime import drop_realtime
 from openpilot.common.swaglog import cloudlog
 from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID
 from openpilot.selfdrive.ui.lib.api_helpers import get_token
@@ -69,6 +70,7 @@ class PrimeState:
         cloudlog.info(f"Prime type updated to {prime_type}")
 
   def _worker_thread(self) -> None:
+    drop_realtime()
     from openpilot.selfdrive.ui.ui_state import ui_state, device
     while self._running:
       if not ui_state.started and device._awake:

selfdrive/ui/mici/layouts/home.py

@@ -14,7 +14,6 @@ from openpilot.system.version import RELEASE_BRANCHES
 
 HEAD_BUTTON_FONT_SIZE = 40
 HOME_PADDING = 8
-SETTINGS_ZONE_WIDTH = 280
 ALERTS_ZONE_WIDTH = 180
 
 NetworkType = log.DeviceState.NetworkType
@@ -39,11 +38,16 @@ class AlertsPill(Widget):
     self.set_rect(rl.Rectangle(0, 0, 104, 52))
 
     self._pill_bg_txt = gui_app.texture("icons_mici/alerts_pill.png", 104, 52)
-    self._warning_txt = gui_app.texture("icons_mici/offroad_alerts/red_warning.png", 36, 36)
+    self._icon_red = gui_app.texture("icons_mici/offroad_alerts/red_warning.png", 36, 36)
+    self._icon_orange = gui_app.texture("icons_mici/offroad_alerts/orange_warning.png", 36, 36)
+    self._icon_green = gui_app.texture("icons_mici/offroad_alerts/green_wheel.png", 36, 36)
     self._alert_count_callback: Callable[[], int] | None = None
+    self._max_severity_callback: Callable[[], int | None] | None = None
 
-  def set_alert_count_callback(self, callback: Callable[[], int] | None):
+  def set_alert_count_callback(self, callback: Callable[[], int] | None,
+                               severity_callback: Callable[[], int | None] | None = None):
     self._alert_count_callback = callback
+    self._max_severity_callback = severity_callback
 
   def _render(self, _):
     alert_count = self._alert_count_callback() if self._alert_count_callback else 0
@@ -51,9 +55,17 @@ class AlertsPill(Widget):
       pill_w, pill_h = self._pill_bg_txt.width, self._pill_bg_txt.height
       rl.draw_texture_ex(self._pill_bg_txt, rl.Vector2(self.rect.x, self.rect.y), 0.0, 1.0, rl.WHITE)
 
+      severity = self._max_severity_callback() if self._max_severity_callback else None
+      if severity == -1:
+        warning_txt = self._icon_green
+      elif severity is not None and severity > 0:
+        warning_txt = self._icon_red
+      else:
+        warning_txt = self._icon_orange
+
       warn_x = self.rect.x + self.ICON_OFFSET
-      warn_y = self.rect.y + (pill_h - self._warning_txt.height) / 2
-      rl.draw_texture_ex(self._warning_txt, rl.Vector2(warn_x, warn_y), 0.0, 1.0, rl.WHITE)
+      warn_y = self.rect.y + (pill_h - warning_txt.height) / 2
+      rl.draw_texture_ex(warning_txt, rl.Vector2(warn_x, warn_y), 0.0, 1.0, rl.WHITE)
 
       count_rect = rl.Rectangle(self.rect.x + self.COUNT_OFFSET, self.rect.y, pill_w - self.COUNT_OFFSET, pill_h)
       gui_label(count_rect, str(alert_count), font_size=36,
@@ -120,16 +132,17 @@ class MiciHomeLayout(Widget):
     self._on_alerts_click: Callable | None = None
     self._alert_count_callback: Callable[[], int] | None = None
 
-    self._last_refresh = 0
     self._mouse_down_t: None | float = None
     self._did_long_press = False
     self._is_pressed_prev = False
 
-    self._version_text = None
-    self._experimental_mode = False
+    self._version_text = self._get_version_text()
 
     self._experimental_icon = IconWidget("icons_mici/experimental_mode.png", (48, 48))
+    self._egpu_icon = IconWidget("icons_mici/egpu.png", (50, 37))
+    self._egpu_icon_gray = IconWidget("icons_mici/egpu_gray.png", (50, 37))
     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 +150,9 @@ class MiciHomeLayout(Widget):
       IconWidget("icons_mici/settings.png", (48, 48), opacity=0.9),
       NetworkIcon(),
       self._experimental_icon,
+      self._egpu_icon,
+      self._egpu_icon_gray,
+      self._body_icon,
       self._mic_icon,
     ], spacing=18)
 
@@ -147,14 +163,6 @@ class MiciHomeLayout(Widget):
     self._branch_label = UnifiedLabel("", font_size=36, text_color=rl.GRAY, font_weight=FontWeight.ROMAN, scroll=True)
     self._version_commit_label = UnifiedLabel("", font_size=36, text_color=rl.GRAY, font_weight=FontWeight.ROMAN, max_width=480, wrap_text=False)
 
-  def show_event(self):
-    super().show_event()
-    self._version_text = self._get_version_text()
-    self._update_params()
-
-  def _update_params(self):
-    self._experimental_mode = ui_state.params.get_bool("ExperimentalMode")
-
   def _update_state(self):
     if self.is_pressed and not self._is_pressed_prev:
       self._mouse_down_t = time.monotonic()
@@ -167,34 +175,28 @@ class MiciHomeLayout(Widget):
       if time.monotonic() - self._mouse_down_t > 0.5:
         # long gating for experimental mode - only allow toggle if longitudinal control is available
         if ui_state.has_longitudinal_control:
-          self._experimental_mode = not self._experimental_mode
-          ui_state.params.put("ExperimentalMode", self._experimental_mode)
+          ui_state.experimental_mode = not ui_state.experimental_mode
+          ui_state.params.put("ExperimentalMode", ui_state.experimental_mode, block=True)
         self._mouse_down_t = None
         self._did_long_press = True
 
-    if rl.get_time() - self._last_refresh > 5.0:
-      # Update version text
-      self._version_text = self._get_version_text()
-      self._last_refresh = rl.get_time()
-      self._update_params()
-
   def set_callbacks(self, on_settings: Callable | None = None, on_alerts: Callable | None = None,
-                    alert_count_callback: Callable[[], int] | None = None):
+                    alert_count_callback: Callable[[], int] | None = None,
+                    max_severity_callback: Callable[[], int | None] | None = None):
     self._on_settings_click = on_settings
     self._on_alerts_click = on_alerts
     self._alert_count_callback = alert_count_callback
-    self._alerts_pill.set_alert_count_callback(alert_count_callback)
+    self._alerts_pill.set_alert_count_callback(alert_count_callback, max_severity_callback)
 
   def _handle_mouse_release(self, mouse_pos: MousePos):
     if not self._did_long_press:
       relative_x = mouse_pos.x - self.rect.x
       has_alerts = self._alert_count_callback and self._alert_count_callback() > 0
-      if relative_x < SETTINGS_ZONE_WIDTH:
-        if self._on_settings_click:
-          self._on_settings_click()
-      elif has_alerts and relative_x > self.rect.width - ALERTS_ZONE_WIDTH:
+      if has_alerts and relative_x > self.rect.width - ALERTS_ZONE_WIDTH:
         if self._on_alerts_click:
           self._on_alerts_click()
+      elif self._on_settings_click:
+        self._on_settings_click()
     self._did_long_press = False
 
   def _get_version_text(self) -> tuple[str, str, str, str] | None:
@@ -245,8 +247,11 @@ class MiciHomeLayout(Widget):
         self._version_commit_label.render()
 
     # ***** Center-aligned bottom section icons *****
-    self._experimental_icon.set_visible(self._experimental_mode)
+    self._experimental_icon.set_visible(ui_state.experimental_mode)
+    self._egpu_icon.set_visible(ui_state.usbgpu and ui_state.usbgpu_compiled)
+    self._egpu_icon_gray.set_visible(ui_state.usbgpu and not ui_state.usbgpu_compiled)
     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,23 @@ 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,
+      max_severity_callback=self._alerts_layout.max_severity,
     )
-    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 +145,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/offroad_alerts.py

@@ -1,9 +1,11 @@
 import pyray as rl
 import re
+import threading
 import time
 from dataclasses import dataclass
 from enum import IntEnum
 from openpilot.common.params import Params
+from openpilot.common.realtime import drop_realtime
 from openpilot.selfdrive.selfdrived.alertmanager import OFFROAD_ALERTS
 from openpilot.system.hardware import HARDWARE
 from openpilot.system.ui.widgets import Widget
@@ -195,7 +197,6 @@ class MiciOffroadAlerts(Scroller):
     self.params = Params()
     self.sorted_alerts: list[AlertData] = []
     self.alert_items: list[AlertItem] = []
-    self._last_refresh = 0.0
 
     # Create empty state label
     self._empty_label = UnifiedLabel(tr("no alerts"), 65, FontWeight.DISPLAY, rl.WHITE,
@@ -205,9 +206,17 @@ class MiciOffroadAlerts(Scroller):
     # Build initial alert list
     self._build_alerts()
 
+    # Start param thread
+    self._pending_params: dict | None = None
+    self._params_thread = threading.Thread(target=self._params_worker, daemon=True)
+    self._params_thread.start()
+
   def active_alerts(self) -> int:
     return sum(alert.visible for alert in self.sorted_alerts)
 
+  def max_severity(self) -> int | None:
+    return max((alert.severity for alert in self.sorted_alerts if alert.visible), default=None)
+
   def scrolling(self):
     return self._scroller.scroll_panel.is_touch_valid()
 
@@ -234,12 +243,19 @@ class MiciOffroadAlerts(Scroller):
       self.alert_items.append(alert_item)
       self._scroller.add_widget(alert_item)
 
-  def refresh(self) -> int:
+  def _params_worker(self):
+    drop_realtime()
+    while True:
+      self._pending_params = ({"UpdaterNewDescription": self.params.get("UpdaterNewDescription")} |
+                              {alert_data.key: self.params.get(alert_data.key) for alert_data in self.sorted_alerts})
+      time.sleep(REFRESH_INTERVAL)
+
+  def _refresh(self) -> int:
     """Refresh alerts from params and return active count."""
     active_count = 0
 
     # Handle UpdateAvailable alert specially
-    update_available = self.params.get_bool("UpdateAvailable")
+    update_available = self._pending_params["UpdateAvailable"]
     update_alert_data = next((alert_data for alert_data in self.sorted_alerts if alert_data.key == "UpdateAvailable"), None)
 
     if update_alert_data:
@@ -247,7 +263,7 @@ class MiciOffroadAlerts(Scroller):
         version_string = ""
 
         # Get new version description and parse version and date
-        new_desc = self.params.get("UpdaterNewDescription") or ""
+        new_desc = self._pending_params["UpdaterNewDescription"] or ""
         if new_desc:
           # format: "version / branch / commit / date"
           parts = new_desc.split(" / ")
@@ -268,11 +284,16 @@ class MiciOffroadAlerts(Scroller):
         continue  # Skip, already handled above
 
       text = ""
-      alert_json = self.params.get(alert_data.key)
+      alert_json = self._pending_params[alert_data.key]
 
       if alert_json:
         text = alert_json.get("text", "").replace("%1", alert_json.get("extra", ""))
 
+      if text and not alert_data.visible:
+        # Bump newly visible alerts to the top, severity sort keeps it at the top of its category
+        widget = next(w for w in self._scroller.items if w.alert_data is alert_data)
+        self._scroller.items.remove(widget)
+        self._scroller.items.insert(0, widget)
       alert_data.text = text
       alert_data.visible = bool(text)
 
@@ -283,21 +304,16 @@ class MiciOffroadAlerts(Scroller):
     for alert_item in self.alert_items:
       alert_item.update_alert_data(alert_item.alert_data)
 
-    return active_count
+    self._scroller.items.sort(key=lambda w: -w.alert_data.severity)
 
-  def show_event(self):
-    """Reset scroll position when shown and refresh alerts."""
-    super().show_event()
-    self._last_refresh = time.monotonic()
-    self.refresh()
+    return active_count
 
   def _update_state(self):
     """Periodically refresh alerts."""
-    # Refresh alerts periodically, not every frame
-    current_time = time.monotonic()
-    if current_time - self._last_refresh >= REFRESH_INTERVAL:
-      self.refresh()
-      self._last_refresh = current_time
+    # Refresh alerts when thread updates params
+    if self._pending_params is not None:
+      self._refresh()
+      self._pending_params = None
 
   def _render(self, rect: rl.Rectangle):
     """Render the alerts scroller or empty state."""

selfdrive/ui/mici/layouts/onboarding.py

@@ -71,7 +71,7 @@ class TrainingGuidePreDMTutorial(NavScroller):
   def show_event(self):
     super().show_event()
     # Get driver monitoring model ready for next step
-    ui_state.params.put_bool_nonblocking("IsDriverViewEnabled", True)
+    ui_state.params.put_bool("IsDriverViewEnabled", True)
 
 
 class DMBadFaceDetected(NavScroller):
@@ -123,7 +123,7 @@ class TrainingGuideDMTutorial(NavWidget):
   def _update_state(self):
     super()._update_state()
     if device.awake and not ui_state.params.get_bool("IsDriverViewEnabled"):
-      ui_state.params.put_bool_nonblocking("IsDriverViewEnabled", True)
+      ui_state.params.put_bool("IsDriverViewEnabled", True)
 
     sm = ui_state.sm
     if sm.recv_frame.get("driverMonitoringState", 0) == 0:
@@ -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)
@@ -219,11 +219,11 @@ class TrainingGuideRecordFront(NavScroller):
     super().__init__()
 
     def on_accept():
-      ui_state.params.put_bool_nonblocking("RecordFront", True)
+      ui_state.params.put_bool("RecordFront", True)
       continue_callback()
 
     def on_decline():
-      ui_state.params.put_bool_nonblocking("RecordFront", False)
+      ui_state.params.put_bool("RecordFront", False)
       continue_callback()
 
     self._accept_button = BigConfirmationCircleButton("allow data uploading", gui_app.texture("icons_mici/setup/driver_monitoring/dm_check.png", 64, 64),
@@ -367,7 +367,7 @@ class OnboardingWindow(Widget):
     self._needs_initial_push = False
 
   def _on_uninstall(self):
-    ui_state.params.put_bool("DoUninstall", True)
+    ui_state.params.put_bool("DoUninstall", True, block=True)
 
   def show_event(self):
     super().show_event()
@@ -386,12 +386,12 @@ class OnboardingWindow(Widget):
     return self._accepted_terms and self._sunnylink_consent_done and self._training_done
 
   def close(self):
-    ui_state.params.put_bool_nonblocking("IsDriverViewEnabled", False)
+    ui_state.params.put_bool("IsDriverViewEnabled", False)
     self._completed_callback()
 
   def _on_terms_accepted(self):
-    ui_state.params.put("HasAcceptedTerms", terms_version)
-    ui_state.params.put("HasAcceptedTermsSP", terms_version_sp)
+    ui_state.params.put("HasAcceptedTerms", terms_version, block=True)
+    ui_state.params.put("HasAcceptedTermsSP", terms_version_sp, block=True)
     self._accepted_terms = True
     if not self._sunnylink_consent_done:
       gui_app.push_widget(self._sunnylink_consent)
@@ -401,7 +401,7 @@ class OnboardingWindow(Widget):
       self.close()
 
   def _on_sunnylink_accepted(self):
-    ui_state.params.put("CompletedSunnylinkConsentVersion", sunnylink_consent_version)
+    ui_state.params.put("CompletedSunnylinkConsentVersion", sunnylink_consent_version, block=True)
     ui_state.params.put_bool("SunnylinkEnabled", True)
     self._sunnylink_consent_done = True
     if not self._training_done:
@@ -410,7 +410,7 @@ class OnboardingWindow(Widget):
       self.close()
 
   def _on_sunnylink_declined(self):
-    ui_state.params.put("CompletedSunnylinkConsentVersion", sunnylink_consent_declined)
+    ui_state.params.put("CompletedSunnylinkConsentVersion", sunnylink_consent_declined, block=True)
     ui_state.params.put_bool("SunnylinkEnabled", False)
     self._sunnylink_consent_done = True
     if not self._training_done:
@@ -419,7 +419,7 @@ class OnboardingWindow(Widget):
       self.close()
 
   def _on_completed_training(self):
-    ui_state.params.put("CompletedTrainingVersion", training_version)
+    ui_state.params.put("CompletedTrainingVersion", training_version, block=True)
     self._training_done = True
     self.close()
 

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

@@ -1,13 +1,34 @@
+from collections.abc import Callable
 from openpilot.common.time_helpers import system_time_valid
 from openpilot.system.ui.widgets.scroller import NavScroller
-from openpilot.selfdrive.ui.mici.widgets.button import BigButton, BigToggle, BigParamControl, BigCircleParamControl
-from openpilot.selfdrive.ui.mici.widgets.dialog import BigDialog, BigInputDialog
+from openpilot.selfdrive.ui.mici.widgets.button import BigButton, BigToggle, BigParamControl, BigCircleParamControl, GreyBigButton
+from openpilot.selfdrive.ui.mici.widgets.dialog import BigDialog, BigInputDialog, BigConfirmationCircleButton
 from openpilot.system.ui.lib.application import gui_app
 from openpilot.selfdrive.ui.layouts.settings.common import restart_needed_callback
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.selfdrive.ui.widgets.ssh_key import SshKeyFetcher
 
 
+class AlphaLongConfirmPage(NavScroller):
+  def __init__(self, on_confirm: Callable[[], None]):
+    super().__init__()
+
+    accept = BigConfirmationCircleButton("enable alpha\nlongitudinal",
+                                         gui_app.texture("icons_mici/setup/driver_monitoring/dm_check.png", 64, 64),
+                                         lambda: self.dismiss(on_confirm))
+
+    self._scroller.add_widgets([
+      GreyBigButton("enabling alpha longitudinal", "scroll to continue",
+                    gui_app.texture("icons_mici/setup/warning.png", 64, 64)),
+      GreyBigButton("", "WARNING: alpha longitudinal control may disable Automatic Emergency Braking (AEB)"),
+      GreyBigButton("", "On this car, openpilot defaults to the stock system's built-in ACC."),
+      GreyBigButton("", "Enabling this will switch to openpilot longitudinal control."),
+      GreyBigButton("", "Using Experimental mode is recommended with openpilot longitudinal control alpha."),
+      GreyBigButton("", "Changing this setting will restart openpilot if the car is powered on."),
+      accept,
+    ])
+
+
 class DeveloperLayoutMici(NavScroller):
   def __init__(self):
     super().__init__()
@@ -131,12 +152,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)
@@ -147,31 +162,38 @@ class DeveloperLayoutMici(NavScroller):
       item.set_checked(ui_state.params.get_bool(key))
 
   def _on_joystick_debug_mode(self, state: bool):
-    ui_state.params.put_bool("JoystickDebugMode", state)
-    ui_state.params.put_bool("LongitudinalManeuverMode", False)
+    ui_state.params.put_bool("JoystickDebugMode", state, block=True)
+    ui_state.params.put_bool("LongitudinalManeuverMode", False, block=True)
     self._long_maneuver_toggle.set_checked(False)
-    ui_state.params.put_bool("LateralManeuverMode", False)
+    ui_state.params.put_bool("LateralManeuverMode", False, block=True)
     self._lat_maneuver_toggle.set_checked(False)
 
   def _on_long_maneuver_mode(self, state: bool):
-    ui_state.params.put_bool("LongitudinalManeuverMode", state)
-    ui_state.params.put_bool("JoystickDebugMode", False)
+    ui_state.params.put_bool("LongitudinalManeuverMode", state, block=True)
+    ui_state.params.put_bool("JoystickDebugMode", False, block=True)
     self._joystick_toggle.set_checked(False)
-    ui_state.params.put_bool("LateralManeuverMode", False)
+    ui_state.params.put_bool("LateralManeuverMode", False, block=True)
     self._lat_maneuver_toggle.set_checked(False)
-    restart_needed_callback(state)
+    restart_needed_callback()
 
   def _on_lat_maneuver_mode(self, state: bool):
-    ui_state.params.put_bool("LateralManeuverMode", state)
-    ui_state.params.put_bool("ExperimentalMode", False)
-    ui_state.params.put_bool("JoystickDebugMode", False)
+    ui_state.params.put_bool("LateralManeuverMode", state, block=True)
+    ui_state.params.put_bool("ExperimentalMode", False, block=True)
+    ui_state.params.put_bool("JoystickDebugMode", False, block=True)
     self._joystick_toggle.set_checked(False)
-    ui_state.params.put_bool("LongitudinalManeuverMode", False)
+    ui_state.params.put_bool("LongitudinalManeuverMode", False, block=True)
     self._long_maneuver_toggle.set_checked(False)
-    restart_needed_callback(state)
+    restart_needed_callback()
 
   def _on_alpha_long_enabled(self, state: bool):
-    # TODO: show confirmation dialog before enabling
-    ui_state.params.put_bool("AlphaLongitudinalEnabled", state)
-    restart_needed_callback(state)
-    self._update_toggles()
+    def do_toggle(_state: bool):
+      ui_state.params.put_bool("AlphaLongitudinalEnabled", _state, block=True)
+      restart_needed_callback()
+      self._update_toggles()
+
+    if state:
+      # Don't show enabled state until confirm
+      self._alpha_long_toggle.set_checked(False)
+      gui_app.push_widget(AlphaLongConfirmPage(lambda: do_toggle(True)))
+    else:
+      do_toggle(False)

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

@@ -40,7 +40,7 @@ class ReviewTrainingGuide(TrainingGuide):
   def hide_event(self):
     super().hide_event()
     device.set_override_interactive_timeout(None)
-    ui_state.params.put_bool_nonblocking("IsDriverViewEnabled", False)
+    ui_state.params.put_bool("IsDriverViewEnabled", False)
 
 
 class MiciFccModal(NavRawScrollPanel):
@@ -200,7 +200,7 @@ class UpdateOpenpilotBigButton(BigButton):
       if self.get_value() == "download update":
         os.system("pkill -SIGHUP -f system.updated.updated")
       elif self.get_value() == "update now":
-        ui_state.params.put_bool("DoReboot", True)
+        ui_state.params.put_bool("DoReboot", True, block=True)
       else:
         os.system("pkill -SIGUSR1 -f system.updated.updated")
 
@@ -258,6 +258,7 @@ class UpdateOpenpilotBigButton(BigButton):
     elif self._state == UpdaterState.IDLE:
       self.set_rotate_icon(False)
       if failed:
+        self.set_enabled(True)  # allow retry when failure came from updater param
         if self.get_value() != "failed to update":
           self.set_value("failed to update")
 
@@ -292,10 +293,10 @@ class DeviceLayoutMici(NavScroller):
     self._fcc_dialog: HtmlModal | None = None
 
     def power_off_callback():
-      ui_state.params.put_bool("DoShutdown", True)
+      ui_state.params.put_bool("DoShutdown", True, block=True)
 
     def reboot_callback():
-      ui_state.params.put_bool("DoReboot", True)
+      ui_state.params.put_bool("DoReboot", True, block=True)
 
     def reset_calibration_callback():
       params = ui_state.params
@@ -304,10 +305,10 @@ class DeviceLayoutMici(NavScroller):
       params.remove("LiveParameters")
       params.remove("LiveParametersV2")
       params.remove("LiveDelay")
-      params.put_bool("OnroadCycleRequested", True)
+      params.put_bool("OnroadCycleRequested", True, block=True)
 
     def uninstall_openpilot_callback():
-      ui_state.params.put_bool("DoUninstall", True)
+      ui_state.params.put_bool("DoUninstall", True, block=True)
 
     reset_calibration_btn = EngagedConfirmationButton("reset calibration", "reset", gui_app.texture("icons_mici/settings/device/lkas.png", 122, 64),
                                                       reset_calibration_callback)

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

@@ -5,6 +5,7 @@ import pyray as rl
 
 from openpilot.common.api import api_get
 from openpilot.common.params import Params
+from openpilot.common.realtime import drop_realtime
 from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.ui.lib.api_helpers import get_token
 from openpilot.selfdrive.ui.ui_state import ui_state, device
@@ -212,6 +213,7 @@ class FirehoseLayoutBase(Widget):
       cloudlog.error(f"Failed to fetch firehose stats: {e}")
 
   def _update_loop(self):
+    drop_realtime()
     while self._running:
       if not ui_state.started and device._awake:
         self._fetch_firehose_stats()

selfdrive/ui/mici/layouts/settings/network/network_layout.py

@@ -66,14 +66,14 @@ class NetworkLayoutMici(NavScroller):
 
     # ******** Advanced settings ********
     # ******** Roaming toggle ********
-    self._roaming_btn = BigParamControl("enable roaming", "GsmRoaming", toggle_callback=self._toggle_roaming)
+    self._roaming_btn = BigParamControl("enable roaming", "GsmRoaming")
 
     # ******** APN settings ********
     self._apn_btn = BigButton("apn settings", "edit")
     self._apn_btn.set_click_callback(self._edit_apn)
 
     # ******** Cellular metered toggle ********
-    self._cellular_metered_btn = BigParamControl("cellular metered", "GsmMetered", toggle_callback=self._toggle_cellular_metered)
+    self._cellular_metered_btn = BigParamControl("cellular metered", "GsmMetered")
 
     # Main scroller ----------------------------------
     self._scroller.add_widgets([
@@ -88,11 +88,6 @@ class NetworkLayoutMici(NavScroller):
       # */
     ])
 
-    # Set initial config
-    roaming_enabled = ui_state.params.get_bool("GsmRoaming")
-    metered = ui_state.params.get_bool("GsmMetered")
-    self._wifi_manager.update_gsm_settings(roaming_enabled, ui_state.params.get("GsmApn") or "", metered)
-
   def _update_state(self):
     super()._update_state()
 
@@ -116,9 +111,6 @@ class NetworkLayoutMici(NavScroller):
 
     gui_app.remove_nav_stack_tick(self._wifi_manager.process_callbacks)
 
-  def _toggle_roaming(self, checked: bool):
-    self._wifi_manager.update_gsm_settings(checked, ui_state.params.get("GsmApn") or "", ui_state.params.get_bool("GsmMetered"))
-
   def _edit_apn(self):
     def update_apn(apn: str):
       apn = apn.strip()
@@ -127,15 +119,10 @@ class NetworkLayoutMici(NavScroller):
       else:
         ui_state.params.put("GsmApn", apn)
 
-      self._wifi_manager.update_gsm_settings(ui_state.params.get_bool("GsmRoaming"), apn, ui_state.params.get_bool("GsmMetered"))
-
     current_apn = ui_state.params.get("GsmApn") or ""
     dlg = BigInputDialog("enter APN...", current_apn, minimum_length=0, confirm_callback=update_apn)
     gui_app.push_widget(dlg)
 
-  def _toggle_cellular_metered(self, checked: bool):
-    self._wifi_manager.update_gsm_settings(ui_state.params.get_bool("GsmRoaming"), ui_state.params.get("GsmApn") or "", checked)
-
   def _on_network_updated(self, networks: list[Network]):
     # Update tethering state
     tethering_active = self._wifi_manager.is_tethering_active()

selfdrive/ui/mici/layouts/settings/network/wifi_ui.py

@@ -383,7 +383,7 @@ class WifiUIMici(NavScroller):
 
       if scroll:
         # Scroll to the new position of the network
-        self._scroller.scroll_to(self._scroller.scroll_panel.get_offset(), smooth=True)
+        self._scroller.scroll_to(self._scroller.scroll_panel.get_offset(), smooth=True, block_widget_interaction=True)
 
   def _update_state(self):
     super()._update_state()

selfdrive/ui/mici/onroad/augmented_road_view.py

@@ -1,7 +1,6 @@
-import time
 import numpy as np
 import pyray as rl
-from cereal import messaging, car, log
+from cereal import car, log
 from msgq.visionipc import VisionStreamType
 from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
 from openpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH
@@ -143,9 +142,7 @@ class AugmentedRoadView(CameraView):
     self.view_from_calib = view_frame_from_device_frame.copy()
     self.view_from_wide_calib = view_frame_from_device_frame.copy()
 
-    self._last_calib_time: float = 0
-    self._last_rect_dims = (0.0, 0.0)
-    self._last_stream_type = stream_type
+    self._matrix_cache_key: tuple | None = None
     self._cached_matrix: np.ndarray | None = None
     self._content_rect = rl.Rectangle()
     self._last_click_time = 0.0
@@ -165,9 +162,6 @@ class AugmentedRoadView(CameraView):
 
     self._fade_texture = gui_app.texture("icons_mici/onroad/onroad_fade.png")
 
-    # debug
-    self._pm = messaging.PubMaster(['uiDebug'])
-
   def is_swiping_left(self) -> bool:
     """Check if currently swiping left (for scroller to disable)."""
     return self._bookmark_icon.is_swiping_left()
@@ -189,7 +183,12 @@ class AugmentedRoadView(CameraView):
       super()._handle_mouse_release(mouse_pos)
 
   def _render(self, _):
-    start_draw = time.monotonic()
+    # Draw text if not onroad
+    if not ui_state.started:
+      rl.draw_rectangle_rec(self.rect, rl.BLACK)
+      self._offroad_label.render(self._rect)
+      return
+
     self._switch_stream_if_needed(ui_state.sm)
 
     # Update calibration before rendering
@@ -224,7 +223,7 @@ class AugmentedRoadView(CameraView):
     alert_to_render, not_animating_out = self._alert_renderer.will_render()
 
     # Hide DMoji when disengaged unless AlwaysOnDM is enabled
-    should_draw_dmoji = (not self._hud_renderer.drawing_top_icons() and ui_state.is_onroad() and
+    should_draw_dmoji = (not self._hud_renderer.drawing_top_icons() and
                          (ui_state.status != UIStatus.DISENGAGED or ui_state.always_on_dm))
     self._driver_state_renderer.set_should_draw(should_draw_dmoji)
     self._driver_state_renderer.set_position(self._rect.x + 16, self._rect.y + 10)
@@ -233,9 +232,7 @@ class AugmentedRoadView(CameraView):
     self._hud_renderer.set_can_draw_top_icons(alert_to_render is None)
     self._hud_renderer.set_wheel_critical_icon(alert_to_render is not None and not not_animating_out and
                                                alert_to_render.visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired)
-    # TODO: have alert renderer draw offroad mici label below
-    if ui_state.started:
-      self._alert_renderer.render(self._content_rect)
+    self._alert_renderer.render(self._content_rect)
     self._hud_renderer.render(self._content_rect)
 
     # Draw fake rounded border
@@ -250,16 +247,6 @@ class AugmentedRoadView(CameraView):
 
     self._bookmark_icon.render(self.rect)
 
-    # Draw darkened background and text if not onroad
-    if not ui_state.started:
-      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))
-      self._offroad_label.render(self._rect)
-
-    # publish uiDebug
-    msg = messaging.new_message('uiDebug')
-    msg.uiDebug.drawTimeMillis = (time.monotonic() - start_draw) * 1000
-    self._pm.send('uiDebug', msg)
-
   def _switch_stream_if_needed(self, sm):
     if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams:
       v_ego = sm['carState'].vEgo
@@ -300,10 +287,18 @@ class AugmentedRoadView(CameraView):
       self.view_from_wide_calib = view_frame_from_device_frame @ wide_from_device @ device_from_calib
 
   def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray:
+    cache_key = (
+      ui_state.sm.recv_frame['liveCalibration'],
+      int(self._content_rect.width),
+      int(self._content_rect.height),
+      self.stream_type,
+      round(ui_state.sm['carState'].vEgo, 1),
+    )
+
+    if cache_key == self._matrix_cache_key and self._cached_matrix is not None:
+      return self._cached_matrix
+
     # Get camera configuration
-    # TODO: cache with vEgo?
-    calib_time = ui_state.sm.recv_frame['liveCalibration']
-    current_dims = (self._content_rect.width, self._content_rect.height)
     device_camera = self.device_camera or DEFAULT_DEVICE_CAMERA
     is_wide_camera = self.stream_type == WIDE_CAM
     intrinsic = device_camera.ecam.intrinsics if is_wide_camera else device_camera.fcam.intrinsics
@@ -319,7 +314,6 @@ class AugmentedRoadView(CameraView):
     kep = calib_transform @ inf_point
 
     # Calculate center points and dimensions
-    x, y = self._content_rect.x, self._content_rect.y
     w, h = self._content_rect.width, self._content_rect.height
     cx, cy = intrinsic[0, 2], intrinsic[1, 2]
 
@@ -339,18 +333,17 @@ class AugmentedRoadView(CameraView):
       x_offset, y_offset = 0, 0
 
     # Cache the computed transformation matrix to avoid recalculations
-    self._last_calib_time = calib_time
-    self._last_rect_dims = current_dims
-    self._last_stream_type = self.stream_type
+    self._matrix_cache_key = cache_key
     self._cached_matrix = np.array([
       [zoom * 2 * cx / w, 0, -x_offset / w * 2],
       [0, zoom * 2 * cy / h, -y_offset / h * 2],
       [0, 0, 1.0]
     ])
 
+    # built without rect.x/y so cache stays hot during scroll. ModelRenderer adds offset at draw time
     video_transform = np.array([
-      [zoom, 0.0, (w / 2 + x - x_offset) - (cx * zoom)],
-      [0.0, zoom, (h / 2 + y - y_offset) - (cy * zoom)],
+      [zoom, 0.0, (w / 2 - x_offset) - (cx * zoom)],
+      [0.0, zoom, (h / 2 - y_offset) - (cy * zoom)],
       [0.0, 0.0, 1.0]
     ])
     self._model_renderer.set_transform(video_transform @ calib_transform)

selfdrive/ui/mici/onroad/cameraview.py

@@ -150,15 +150,11 @@ class CameraView(Widget):
     ui_state.add_offroad_transition_callback(self._offroad_transition)
 
   def _offroad_transition(self):
-    # Reconnect if not first time going onroad
-    if ui_state.is_onroad() and self.frame is not None:
-      # Prevent old frames from showing when going onroad. Qt has a separate thread
-      # which drains the VisionIpcClient SubSocket for us. Re-connecting is not enough
-      # and only clears internal buffers, not the message queue.
-      self.available_streams.clear()
-      if self.client:
-        del self.client
-      self.client = VisionIpcClient(self._name, self._stream_type, conflate=True)
+    # Drain queued SubSocket messages to prevent old frames from showing when going
+    # onroad. Qt had a separate thread which drains the VisionIpcClient SubSocket for us.
+    if self.client and self.client.is_connected():
+      while self.client.recv(timeout_ms=0) is not None:
+        pass
     self.frame = None
 
   def _set_placeholder_color(self, color: rl.Color):
@@ -312,8 +308,8 @@ class CameraView(Widget):
       y_data = self.frame.data[: self.frame.uv_offset]
       uv_data = self.frame.data[self.frame.uv_offset:]
 
-      rl.update_texture(self.texture_y, rl.ffi.cast("void *", y_data.ctypes.data))
-      rl.update_texture(self.texture_uv, rl.ffi.cast("void *", uv_data.ctypes.data))
+      rl.update_texture(self.texture_y, rl.ffi.cast("void *", rl.ffi.from_buffer(y_data)))
+      rl.update_texture(self.texture_uv, rl.ffi.cast("void *", rl.ffi.from_buffer(uv_data)))
       self._texture_needs_update = False
 
     # Render with shader

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):
@@ -109,11 +104,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):
-      event_name = EVENT_TO_INT[dm_state.events[0].name]
-      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
+    if dm_state is not None:
+      msg.selfdriveState.alertSound = ALERT_SOUNDS.get(str(dm_state.alertLevel), AudibleAlert.none)
     self._pm.send('selfdriveState', msg)
 
   def _render_dm_alerts(self, rect: rl.Rectangle):
@@ -121,29 +119,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)
-    event_name_str = str(dm_state.events[0].name).split('.')[-1]
+    # Show alert level
+    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)))
@@ -160,7 +160,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
 
@@ -16,11 +16,15 @@ DEBUG = False
 LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6)
 LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3)
 
+CONE_COLOR_GREEN = (0, 255, 64)
+CONE_COLOR_ORANGE = (255, 115, 0)
+
 
 class DriverStateRenderer(Widget):
   BASE_SIZE = 60
   LINES_ANGLE_INCREMENT = 5
   LINES_STALE_ANGLES = 3.0  # seconds
+  AWARENESS_UNFULL_PERCENT = 95  # ~0.5s
 
   def __init__(self, lines: bool = False, inset: bool = False):
     super().__init__()
@@ -35,11 +39,15 @@ 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
+    self._awareness_unfull = False
 
     self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps)
+    self._color_fade_filter = FirstOrderFilter(1.0, 0.05, 1 / gui_app.target_fps) # 1.0 = full green, 0.0 = full orange
     self._pitch_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False)
     self._yaw_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False)
     self._rotation_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False)
@@ -97,6 +105,7 @@ class DriverStateRenderer(Widget):
                                   self._rect.y + (self._rect.height - self._dm_person.height) / 2), 0.0, 1.0,
                        rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x)))
 
+    green_amount = self._color_fade_filter.update(0.0 if self._awareness_unfull else 1.0)
     if self.effective_active:
       source_rect = rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height)
       dest_rect = rl.Rectangle(
@@ -107,13 +116,16 @@ class DriverStateRenderer(Widget):
       )
 
       if not self._lines:
+        r = int(round(CONE_COLOR_GREEN[0] * green_amount + CONE_COLOR_ORANGE[0] * (1 - green_amount)))
+        g = int(round(CONE_COLOR_GREEN[1] * green_amount + CONE_COLOR_ORANGE[1] * (1 - green_amount)))
+        b = int(round(CONE_COLOR_GREEN[2] * green_amount + CONE_COLOR_ORANGE[2] * (1 - green_amount)))
         rl.draw_texture_pro(
           self._dm_cone,
           source_rect,
           dest_rect,
           rl.Vector2(dest_rect.width / 2, dest_rect.height / 2),
           self._rotation_filter.x - 90,
-          rl.Color(255, 255, 255, int(255 * self._fade_filter.x)),
+          rl.Color(r, g, b, int(255 * self._fade_filter.x)),
         )
 
       else:
@@ -153,9 +165,12 @@ 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._awareness_unfull = self.effective_active and dm_state.visionPolicyState.awarenessPercent < self.AWARENESS_UNFULL_PERCENT
+    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 +178,9 @@ class DriverStateRenderer(Widget):
 
   def _update_state(self):
     # Get monitoring state
-    driver_data = self.get_driver_data()
-    driver_orient = driver_data.faceOrientation
-    driver_position = driver_data.facePosition
-
-    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)
+    _ = self.get_driver_data()
+    pitch = self._pitch_filter.update(self._face_pitch)
+    yaw = self._yaw_filter.update(self._face_yaw)
 
     # hysteresis on looking center
     if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER:

selfdrive/ui/mici/onroad/hud_renderer.py

@@ -235,7 +235,7 @@ class HudRenderer(Widget):
 
     # draw drop shadow
     circle_radius = 162 // 2
-    rl.draw_circle_gradient(int(x + circle_radius), int(y + circle_radius), circle_radius,
+    rl.draw_circle_gradient(rl.Vector2(x + circle_radius, y + circle_radius), circle_radius,
                             rl.Color(0, 0, 0, int(255 / 2 * alpha)), rl.BLANK)
 
     set_speed_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha))