torquey

.github/ISSUE_TEMPLATE/enhancement.md

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

cereal/log.capnp

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

cereal/services.py

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

common/model.h

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

docs/ext/glossary.py

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

launch_env.sh

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

selfdrive/car/tests/test_cruise_speed.py

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

selfdrive/controls/controlsd.py

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

selfdrive/controls/lib/latcontrol_torque.py

@@ -22,15 +22,19 @@ from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext import La
 # Additionally, there is friction in the steering wheel that needs
 # to be overcome to move it at all, this is compensated for too.
 
-KP = 0.8
-KI = 0.15
-
 INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
-KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP]
+KP_INTERP = [150.0, 100.0, 30.0, 6.0, 2.0, 1.5, 1.2, 0.8, 0.5]
+KI_INTERP = [0.06, 0.06, 0.06, 0.08, 0.08, 0.10, 0.12, 0.14, 0.16]
+JERK_INTERP = [0.25, 0.25, 0.25, 0.23, 0.20, 0.18, 0.16, 0.15, 0.15]
+MEAS_FILTER_TAU_INTERP = [0.07, 0.07, 0.07, 0.07, 0.07, 0.06, 0.06, 0.05, 0.01]
+INTEGRATOR_DECAY_INTERP = [0.990, 0.990, 0.990, 0.992, 0.993, 0.995, 0.996, 0.998, 0.999]
+INTEGRATOR_DECAY_FRAMES = 20
+STRAIGHT_DAMP_THRESHOLD_INTERP = [0.03, 0.03, 0.03, 0.03, 0.03, 0.05, 0.05, 0.2, 0.2]
+STRAIGHT_DAMP_MIN_INTERP = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
+STRAIGHT_DAMP_TAU = 0.5
 
 LP_FILTER_CUTOFF_HZ = 1.2
-JERK_LOOKAHEAD_SECONDS = 0.19
-JERK_GAIN = 0.3
+JERK_LOOKAHEAD_SECONDS = 0.30
 LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
 VERSION = 1
 
@@ -40,13 +44,16 @@ class LatControlTorque(LatControl):
     self.torque_params = CP.lateralTuning.torque.as_builder()
     self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
     self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
-    self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1/self.dt)
+    self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], [INTERP_SPEEDS, KI_INTERP], rate=1/self.dt)
     self.update_limits()
     self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
     self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
     self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
     self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt)
     self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
+    self.measurement_filter = FirstOrderFilter(0.0, 0.04, self.dt)
+    self.integrator_decay_counter = 0
+    self.straight_damp_filter = FirstOrderFilter(1.0, STRAIGHT_DAMP_TAU, self.dt)
 
     self.extension = LatControlTorqueExt(self, CP, CP_SP, CI)
 
@@ -68,7 +75,9 @@ class LatControlTorque(LatControl):
     pid_log = log.ControlsState.LateralTorqueState.new_message()
     pid_log.version = VERSION
     measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
-    measurement = measured_curvature * CS.vEgo ** 2
+    meas_tau = float(np.interp(CS.vEgo, INTERP_SPEEDS, MEAS_FILTER_TAU_INTERP))
+    self.measurement_filter.update_alpha(meas_tau)
+    measurement = self.measurement_filter.update(measured_curvature * CS.vEgo ** 2)
     future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
     self.lat_accel_request_buffer.append(future_desired_lateral_accel)
 
@@ -79,7 +88,12 @@ class LatControlTorque(LatControl):
     delay_frames = int(np.clip(lat_delay / self.dt + 1, 1, self.lat_accel_request_buffer_len))
     expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
     setpoint = expected_lateral_accel
-    error = setpoint - measurement
+    raw_error = setpoint - measurement
+    damp_min = float(np.interp(CS.vEgo, INTERP_SPEEDS, STRAIGHT_DAMP_MIN_INTERP))
+    damp_threshold = float(np.interp(CS.vEgo, INTERP_SPEEDS, STRAIGHT_DAMP_THRESHOLD_INTERP))
+    damp_target = max(damp_min, min(1.0, abs(setpoint) / damp_threshold))
+    damp_factor = self.straight_damp_filter.update(damp_target)
+    error = raw_error * damp_factor
 
     lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2))
     raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx+1] - self.lat_accel_request_buffer[lookahead_idx-1]) / (2 * self.dt)
@@ -88,7 +102,8 @@ class LatControlTorque(LatControl):
     ff = gravity_adjusted_future_lateral_accel
     # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
     ff -= self.torque_params.latAccelOffset
-    ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+    jerk_gain = float(np.interp(CS.vEgo, INTERP_SPEEDS, JERK_INTERP))
+    ff += get_friction(error + jerk_gain * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
 
     if not active:
       output_torque = 0.0
@@ -99,6 +114,15 @@ class LatControlTorque(LatControl):
 
       freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
       output_lataccel = self.pid.update(pid_log.error, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
+
+      error_opposes_integrator = (self.pid.i > 0 and pid_log.error < 0) or (self.pid.i < 0 and pid_log.error > 0)
+      if error_opposes_integrator:
+        self.integrator_decay_counter = min(self.integrator_decay_counter + 1, INTEGRATOR_DECAY_FRAMES + 10)
+      else:
+        self.integrator_decay_counter = 0
+      if self.integrator_decay_counter >= INTEGRATOR_DECAY_FRAMES:
+        self.pid.i *= float(np.interp(CS.vEgo, INTERP_SPEEDS, INTEGRATOR_DECAY_INTERP))
+
       output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
 
       # Lateral acceleration torque controller extension updates

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -32,12 +32,12 @@ COST_E_DIM = 5
 COST_DIM = COST_E_DIM + 1
 CONSTR_DIM = 4
 
-X_EGO_OBSTACLE_COST = 3.
+X_EGO_OBSTACLE_COST = 2.
 X_EGO_COST = 0.
 V_EGO_COST = 0.
 A_EGO_COST = 0.
 J_EGO_COST = 5.
-A_CHANGE_COST = 200.
+A_CHANGE_COST = 350.
 DANGER_ZONE_COST = 100.
 CRASH_DISTANCE = .25
 LEAD_DANGER_FACTOR = 0.75
@@ -53,7 +53,7 @@ T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1
 T_IDXS = np.array(T_IDXS_LST)
 FCW_IDXS = T_IDXS < 5.0
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
-COMFORT_BRAKE = 2.5
+COMFORT_BRAKE = 2.
 STOP_DISTANCE = 6.0
 CRUISE_MIN_ACCEL = -1.2
 CRUISE_MAX_ACCEL = 1.6

selfdrive/modeld/dmonitoringmodeld.py

@@ -1,8 +1,12 @@
 #!/usr/bin/env python3
 import os
-from openpilot.selfdrive.modeld.helpers import MODELS_DIR, CompileConfig, set_tinygrad_backend_from_compiled_flags
+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 tinygrad.tensor import Tensor
 import time
 import pickle
@@ -28,7 +32,7 @@ class ModelState:
   inputs: dict[str, np.ndarray]
   output: np.ndarray
 
-  def __init__(self, cam_w: int, cam_h: int):
+  def __init__(self):
     with open(METADATA_PATH, 'rb') as f:
       model_metadata = pickle.load(f)
       self.input_shapes = model_metadata['input_shapes']
@@ -40,18 +44,22 @@ 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 = get_nv12_info(cam_w, cam_h)
+    self.frame_buf_params = None
     self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
     self._blob_cache : dict[int, Tensor] = {}
+    self.image_warp = None
     self.model_run = pickle.loads(read_file_chunked(str(MODEL_PKL_PATH)))
-    with open(CompileConfig(cam_w, cam_h, prefix='dm_', prepare_only=True).pkl_path, "rb") as f:
-      self.image_warp = pickle.load(f)
 
   def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]:
     self.numpy_inputs['calib'][0,:] = calib
 
     t1 = time.perf_counter()
 
+    if self.image_warp is None:
+      self.frame_buf_params = get_nv12_info(buf.width, buf.height)
+      warp_path = MODELS_DIR / f'dm_warp_{buf.width}x{buf.height}_tinygrad.pkl'
+      with open(warp_path, "rb") as f:
+        self.image_warp = pickle.load(f)
     ptr = buf.data.ctypes.data
     # There is a ringbuffer of imgs, just cache tensors pointing to all of them
     if ptr not in self._blob_cache:
@@ -105,6 +113,9 @@ 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):
@@ -112,9 +123,6 @@ 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/monitoring/README.md

@@ -0,0 +1,15 @@
+# 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.policy import DriverMonitoring
+from openpilot.selfdrive.monitoring.helpers 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=True)
+      DM.run_step(sm, demo=demo_mode)
     elif valid:
       DM.run_step(sm, demo=demo_mode)
 
@@ -40,8 +40,8 @@ def dmonitoringd_thread():
 
     # save rhd virtual toggle every 5 mins
     if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and
-     DM.wheelpos_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)):
+     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)
 
 def main():

selfdrive/monitoring/helpers.py

@@ -0,0 +1,463 @@
+from math import atan2, radians
+import numpy as np
+
+from cereal import car, log
+import cereal.messaging as messaging
+from openpilot.selfdrive.selfdrived.events import Events
+from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
+from openpilot.common.realtime import DT_DMON
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.params import Params
+from openpilot.common.stat_live import RunningStatFilter
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
+from openpilot.system.hardware import HARDWARE
+
+EventName = log.OnroadEvent.EventName
+
+# ******************************************************************************************
+#  NOTE: To fork maintainers.
+#  Disabling or nerfing safety features will get you and your users banned from our servers.
+#  We recommend that you do not change these numbers from the defaults.
+# ******************************************************************************************
+
+class DRIVER_MONITOR_SETTINGS:
+  def __init__(self, device_type):
+    self._DT_DMON = DT_DMON
+    # ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2
+    self._AWARENESS_TIME = 30. # passive wheeltouch total timeout
+    self._AWARENESS_PRE_TIME_TILL_TERMINAL = 15.
+    self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6.
+    self._DISTRACTED_TIME = 11. # active monitoring total timeout
+    self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 8.
+    self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
+
+    self._FACE_THRESHOLD = 0.7
+    self._EYE_THRESHOLD = 0.5
+    self._BLINK_THRESHOLD = 0.5
+    self._PHONE_THRESH = 0.5
+
+    self._POSE_PITCH_THRESHOLD = 0.3133
+    self._POSE_PITCH_THRESHOLD_SLACK = 0.3237
+    self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD
+    self._POSE_YAW_THRESHOLD = 0.4020
+    self._POSE_YAW_THRESHOLD_SLACK = 0.5042
+    self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD
+    self._POSE_YAW_MIN_STEER_DEG = 30
+    self._POSE_YAW_STEER_FACTOR = 0.15
+    self._POSE_YAW_STEER_MAX_OFFSET = 0.3927
+    self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned
+    self._PITCH_NATURAL_THRESHOLD = 0.449
+    self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned
+    self._PITCH_NATURAL_VAR = 3*0.01
+    self._YAW_NATURAL_VAR = 3*0.05
+    self._PITCH_MAX_OFFSET = 0.124
+    self._PITCH_MIN_OFFSET = -0.0881
+    self._YAW_MAX_OFFSET = 0.289
+    self._YAW_MIN_OFFSET = -0.0246
+
+    self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1
+    self._DCAM_UNCERTAIN_ALERT_COUNT = int(60  / self._DT_DMON)
+    self._DCAM_UNCERTAIN_RESET_COUNT = int(20  / self._DT_DMON)
+    self._POSESTD_THRESHOLD = 0.3
+    self._HI_STD_FALLBACK_TIME = int(10  / self._DT_DMON)  # fall back to wheel touch if model is uncertain for 10s
+    self._DISTRACTED_FILTER_TS = 0.25  # 0.6Hz
+
+    self._POSE_CALIB_MIN_SPEED = 13  # 30 mph
+    self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON)  # valid data counts before calibration completes, 1min cumulative
+    self._POSE_OFFSET_MAX_COUNT = int(360 / self._DT_DMON)  # stop deweighting new data after 6 min, aka "short term memory"
+
+    self._WHEELPOS_CALIB_MIN_SPEED = 11
+    self._WHEELPOS_THRESHOLD = 0.5
+    self._WHEELPOS_FILTER_MIN_COUNT = int(15 / self._DT_DMON) # allow 15 seconds to converge wheel side
+    self._WHEELPOS_DATA_AVG = 0.03
+    self._WHEELPOS_DATA_VAR = 3*5.5e-5
+    self._WHEELPOS_MAX_COUNT = -1
+
+    self._RECOVERY_FACTOR_MAX = 5.  # relative to minus step change
+    self._RECOVERY_FACTOR_MIN = 1.25  # relative to minus step change
+
+    self._MAX_TERMINAL_ALERTS = 3  # not allowed to engage after 3 terminal alerts
+    self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON)  # not allowed to engage after 30s of terminal alerts
+
+class DistractedType:
+
+  NOT_DISTRACTED = 0
+  DISTRACTED_POSE = 1 << 0
+  DISTRACTED_BLINK = 1 << 1
+  DISTRACTED_PHONE = 1 << 2
+
+class DriverPose:
+  def __init__(self, settings):
+    pitch_filter_raw_priors = (settings._PITCH_NATURAL_OFFSET, settings._PITCH_NATURAL_VAR, 2)
+    yaw_filter_raw_priors = (settings._YAW_NATURAL_OFFSET, settings._YAW_NATURAL_VAR, 2)
+    self.yaw = 0.
+    self.pitch = 0.
+    self.roll = 0.
+    self.yaw_std = 0.
+    self.pitch_std = 0.
+    self.roll_std = 0.
+    self.pitch_offseter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
+    self.yaw_offseter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
+    self.calibrated = False
+    self.low_std = True
+    self.cfactor_pitch = 1.
+    self.cfactor_yaw = 1.
+    self.steer_yaw_offset = 0.
+
+class DriverProb:
+  def __init__(self, raw_priors, max_trackable):
+    self.prob = 0.
+    self.prob_offseter = RunningStatFilter(raw_priors=raw_priors, max_trackable=max_trackable)
+    self.prob_calibrated = False
+
+
+# model output refers to center of undistorted+leveled image
+EFL = 598.0 # focal length in K
+cam = DEVICE_CAMERAS[("tici", "ar0231")] # corrected image has same size as raw
+W, H = (cam.dcam.width, cam.dcam.height)  # corrected image has same size as raw
+
+def face_orientation_from_net(angles_desc, pos_desc, rpy_calib):
+  # the output of these angles are in device frame
+  # so from driver's perspective, pitch is up and yaw is right
+
+  pitch_net, yaw_net, roll_net = angles_desc
+
+  face_pixel_position = ((pos_desc[0]+0.5)*W, (pos_desc[1]+0.5)*H)
+  yaw_focal_angle = atan2(face_pixel_position[0] - W//2, EFL)
+  pitch_focal_angle = atan2(face_pixel_position[1] - H//2, EFL)
+
+  pitch = pitch_net + pitch_focal_angle
+  yaw = -yaw_net + yaw_focal_angle
+
+  # no calib for roll
+  pitch -= rpy_calib[1]
+  yaw -= rpy_calib[2]
+  return roll_net, pitch, yaw
+
+
+class DriverMonitoring:
+  def __init__(self, rhd_saved=False, settings=None, always_on=False):
+    # init policy settings
+    self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type())
+
+    # init driver status
+    wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2)
+    self.wheelpos = DriverProb(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT)
+    self.pose = DriverPose(settings=self.settings)
+    self.blink_prob = 0.
+    self.phone_prob = 0.
+
+    self.always_on = always_on
+    self.distracted_types = []
+    self.driver_distracted = False
+    self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON)
+    self.wheel_on_right = False
+    self.wheel_on_right_last = None
+    self.wheel_on_right_default = rhd_saved
+    self.face_detected = False
+    self.terminal_alert_cnt = 0
+    self.terminal_time = 0
+    self.step_change = 0.
+    self.active_monitoring_mode = True
+    self.is_model_uncertain = False
+    self.hi_stds = 0
+    self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
+    self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
+    self.dcam_uncertain_cnt = 0
+    self.dcam_uncertain_alerted = False # once per drive
+    self.dcam_reset_cnt = 0
+
+    self.params = Params()
+    self.too_distracted = self.params.get_bool("DriverTooDistracted")
+
+    self._reset_awareness()
+    self._set_timers(active_monitoring=True)
+    self._reset_events()
+
+  def _reset_awareness(self):
+    self.awareness = 1.
+    self.awareness_active = 1.
+    self.awareness_passive = 1.
+
+  def _reset_events(self):
+    self.current_events = Events()
+
+  def _set_timers(self, active_monitoring):
+    if self.active_monitoring_mode and self.awareness <= self.threshold_prompt:
+      if active_monitoring:
+        self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME
+      else:
+        self.step_change = 0.
+      return  # no exploit after orange alert
+    elif self.awareness <= 0.:
+      return
+
+    if active_monitoring:
+      # when falling back from passive mode to active mode, reset awareness to avoid false alert
+      if not self.active_monitoring_mode:
+        self.awareness_passive = self.awareness
+        self.awareness = self.awareness_active
+
+      self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
+      self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
+      self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME
+      self.active_monitoring_mode = True
+    else:
+      if self.active_monitoring_mode:
+        self.awareness_active = self.awareness
+        self.awareness = self.awareness_passive
+
+      self.threshold_pre = self.settings._AWARENESS_PRE_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME
+      self.threshold_prompt = self.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME
+      self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME
+      self.active_monitoring_mode = False
+
+  def _set_policy(self, brake_disengage_prob, car_speed):
+    bp = brake_disengage_prob
+    k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
+    bp_normal = max(min(bp / k1, 0.5),0)
+    self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
+                                           [self.settings._POSE_PITCH_THRESHOLD_SLACK,
+                                            self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
+    self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
+                                           [self.settings._POSE_YAW_THRESHOLD_SLACK,
+                                            self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
+
+  def _get_distracted_types(self):
+    distracted_types = []
+
+    if not self.pose.calibrated:
+      pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET
+      yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET
+    else:
+      pitch_error = self.pose.pitch - min(max(self.pose.pitch_offseter.filtered_stat.mean(),
+                                                       self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET)
+      yaw_error = self.pose.yaw - min(max(self.pose.yaw_offseter.filtered_stat.mean(),
+                                                    self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET)
+    pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit
+
+    if yaw_error * self.pose.steer_yaw_offset > 0: # unidirectional
+      yaw_error = max(abs(yaw_error) - min(abs(self.pose.steer_yaw_offset), self.settings._POSE_YAW_STEER_MAX_OFFSET), 0.)
+    else:
+      yaw_error = abs(yaw_error)
+
+    pitch_threshold = self.settings._POSE_PITCH_THRESHOLD * self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD
+    yaw_threshold = self.settings._POSE_YAW_THRESHOLD * self.pose.cfactor_yaw
+
+    if pitch_error > pitch_threshold or yaw_error > yaw_threshold:
+      distracted_types.append(DistractedType.DISTRACTED_POSE)
+
+    if self.blink_prob > self.settings._BLINK_THRESHOLD:
+      distracted_types.append(DistractedType.DISTRACTED_BLINK)
+
+    if self.phone_prob > self.settings._PHONE_THRESH:
+      distracted_types.append(DistractedType.DISTRACTED_PHONE)
+
+    return distracted_types
+
+  def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False, steering_angle_deg=0.):
+    rhd_pred = driver_state.wheelOnRightProb
+    # calibrates only when there's movement and either face detected
+    if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or
+                                          driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD):
+      self.wheelpos.prob_offseter.push_and_update(rhd_pred)
+
+    self.wheelpos.prob_calibrated = self.wheelpos.prob_offseter.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT
+
+    if self.wheelpos.prob_calibrated or demo_mode:
+      self.wheel_on_right = self.wheelpos.prob_offseter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD
+    else:
+      self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished
+    # make sure no switching when engaged
+    if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode:
+      self.wheel_on_right = self.wheel_on_right_last
+    driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData
+    if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition,
+                                    driver_data.faceOrientationStd, driver_data.facePositionStd)):
+      return
+
+    self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD
+    self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy)
+    steer_d = max(abs(steering_angle_deg) - self.settings._POSE_YAW_MIN_STEER_DEG, 0.)
+    self.pose.steer_yaw_offset = radians(steer_d) * -np.sign(steering_angle_deg) * self.settings._POSE_YAW_STEER_FACTOR
+    if self.wheel_on_right:
+      self.pose.yaw *= -1
+      self.pose.steer_yaw_offset *= -1
+    self.wheel_on_right_last = self.wheel_on_right
+    self.pose.pitch_std = driver_data.faceOrientationStd[0]
+    self.pose.yaw_std = driver_data.faceOrientationStd[1]
+    model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
+    self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD
+    self.blink_prob = driver_data.eyesClosedProb * (driver_data.eyesVisibleProb > self.settings._EYE_THRESHOLD)
+    self.phone_prob = driver_data.phoneProb
+
+    self.distracted_types = self._get_distracted_types()
+    self.driver_distracted = (DistractedType.DISTRACTED_PHONE in self.distracted_types
+                              or DistractedType.DISTRACTED_POSE in self.distracted_types
+                              or DistractedType.DISTRACTED_BLINK in self.distracted_types) \
+                              and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
+    self.driver_distraction_filter.update(self.driver_distracted)
+
+    # update offseter
+    # only update when driver is actively driving the car above a certain speed
+    if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted):
+      self.pose.pitch_offseter.push_and_update(self.pose.pitch)
+      self.pose.yaw_offseter.push_and_update(self.pose.yaw)
+
+    self.pose.calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \
+                           self.pose.yaw_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT
+
+    if self.face_detected and not self.driver_distracted:
+      if model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD:
+        if not standstill:
+          self.dcam_uncertain_cnt += 1
+          self.dcam_reset_cnt = 0
+      else:
+        self.dcam_reset_cnt += 1
+        if self.dcam_reset_cnt > self.settings._DCAM_UNCERTAIN_RESET_COUNT:
+          self.dcam_uncertain_cnt = 0
+
+    self.is_model_uncertain = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME
+    self._set_timers(self.face_detected and not self.is_model_uncertain)
+    if self.face_detected and not self.pose.low_std and not self.driver_distracted:
+      self.hi_stds += 1
+    elif self.face_detected and self.pose.low_std:
+      self.hi_stds = 0
+
+  def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear, car_speed):
+    self._reset_events()
+    # Block engaging until ignition cycle after max number or time of distractions
+    if self.terminal_alert_cnt >= self.settings._MAX_TERMINAL_ALERTS or \
+       self.terminal_time >= self.settings._MAX_TERMINAL_DURATION:
+      if not self.too_distracted:
+        self.params.put_bool_nonblocking("DriverTooDistracted", True)
+      self.too_distracted = True
+
+    # Always-on distraction lockout is temporary
+    if self.too_distracted or (self.always_on and self.awareness <= self.threshold_prompt):
+      self.current_events.add(EventName.tooDistracted)
+
+    always_on_valid = self.always_on and not wrong_gear
+    if (driver_engaged and self.awareness > 0 and not self.active_monitoring_mode) or \
+       (not always_on_valid and not op_engaged) or \
+       (always_on_valid and not op_engaged and self.awareness <= 0):
+      # always reset on disengage with normal mode; disengage resets only on red if always on
+      self._reset_awareness()
+      return
+
+    awareness_prev = self.awareness
+    _reaching_pre = self.awareness - self.step_change <= self.threshold_pre
+    _reaching_terminal = self.awareness - self.step_change <= 0
+    standstill_orange_exemption = standstill and _reaching_pre
+    always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal
+
+    if self.awareness > 0 and \
+       ((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or standstill_orange_exemption):
+      if driver_engaged:
+        self._reset_awareness()
+        return
+      # only restore awareness when paying attention and alert is not red
+      self.awareness = min(self.awareness + ((self.settings._RECOVERY_FACTOR_MAX-self.settings._RECOVERY_FACTOR_MIN)*
+                                             (1.-self.awareness)+self.settings._RECOVERY_FACTOR_MIN)*self.step_change, 1.)
+      if self.awareness == 1.:
+        self.awareness_passive = min(self.awareness_passive + self.step_change, 1.)
+      # don't display alert banner when awareness is recovering and has cleared orange
+      if self.awareness > self.threshold_prompt:
+        return
+
+    certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected
+    maybe_distracted = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME or not self.face_detected
+
+    if certainly_distracted or maybe_distracted:
+      # should always be counting if distracted unless at standstill and reaching green
+      # also will not be reaching 0 if DM is active when not engaged
+      if not (standstill_orange_exemption or always_on_red_exemption):
+        self.awareness = max(self.awareness - self.step_change, -0.1)
+
+    alert = None
+    if self.awareness <= 0.:
+      # terminal red alert: disengagement required
+      alert = EventName.driverDistracted3 if self.active_monitoring_mode else EventName.driverUnresponsive3
+      self.terminal_time += 1
+      if awareness_prev > 0.:
+        self.terminal_alert_cnt += 1
+    elif self.awareness <= self.threshold_prompt:
+      # prompt orange alert
+      alert = EventName.driverDistracted2 if self.active_monitoring_mode else EventName.driverUnresponsive2
+    elif self.awareness <= self.threshold_pre:
+      # pre green alert
+      alert = EventName.driverDistracted1 if self.active_monitoring_mode else EventName.driverUnresponsive1
+
+    if alert is not None:
+      self.current_events.add(alert)
+
+    if self.dcam_uncertain_cnt > self.settings._DCAM_UNCERTAIN_ALERT_COUNT and not self.dcam_uncertain_alerted:
+      set_offroad_alert("Offroad_DriverMonitoringUncertain", True)
+      self.dcam_uncertain_alerted = True
+
+
+  def get_state_packet(self, valid=True):
+    # build driverMonitoringState packet
+    dat = messaging.new_message('driverMonitoringState', valid=valid)
+    dat.driverMonitoringState = {
+      "events": self.current_events.to_msg(),
+      "faceDetected": self.face_detected,
+      "isDistracted": self.driver_distracted,
+      "distractedType": sum(self.distracted_types),
+      "awarenessStatus": self.awareness,
+      "posePitchOffset": self.pose.pitch_offseter.filtered_stat.mean(),
+      "posePitchValidCount": self.pose.pitch_offseter.filtered_stat.n,
+      "poseYawOffset": self.pose.yaw_offseter.filtered_stat.mean(),
+      "poseYawValidCount": self.pose.yaw_offseter.filtered_stat.n,
+      "stepChange": self.step_change,
+      "awarenessActive": self.awareness_active,
+      "awarenessPassive": self.awareness_passive,
+      "isLowStd": self.pose.low_std,
+      "hiStdCount": self.hi_stds,
+      "isActiveMode": self.active_monitoring_mode,
+      "isRHD": self.wheel_on_right,
+      "uncertainCount": self.dcam_uncertain_cnt,
+    }
+    return dat
+
+  def run_step(self, sm, demo=False):
+    if demo:
+      highway_speed = 30
+      enabled = True
+      wrong_gear = False
+      standstill = False
+      driver_engaged = False
+      brake_disengage_prob = 1.0
+      rpyCalib = [0., 0., 0.]
+    else:
+      highway_speed = sm['carState'].vEgo
+      enabled = sm['selfdriveState'].enabled or sm['carControl'].latActive
+      wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low)
+      standstill = sm['carState'].standstill
+      driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed
+      brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
+      rpyCalib = sm['liveCalibration'].rpyCalib
+    self._set_policy(
+      brake_disengage_prob=brake_disengage_prob,
+      car_speed=highway_speed,
+    )
+
+    # Parse data from dmonitoringmodeld
+    self._update_states(
+      driver_state=sm['driverStateV2'],
+      cal_rpy=rpyCalib,
+      car_speed=highway_speed,
+      op_engaged=enabled,
+      standstill=standstill,
+      demo_mode=demo,
+      steering_angle_deg=sm['carState'].steeringAngleDeg,
+    )
+
+    # Update distraction events
+    self._update_events(
+      driver_engaged=driver_engaged,
+      op_engaged=enabled,
+      standstill=standstill,
+      wrong_gear=wrong_gear,
+      car_speed=highway_speed
+    )

selfdrive/monitoring/policy.py

@@ -1,426 +0,0 @@
-from collections import defaultdict
-from math import atan2, radians
-import numpy as np
-
-from cereal import car, log
-import cereal.messaging as messaging
-from openpilot.common.realtime import DT_DMON
-from openpilot.common.filter_simple import FirstOrderFilter
-from openpilot.common.params import Params
-from openpilot.common.stat_live import RunningStatFilter
-from openpilot.common.transformations.camera import DEVICE_CAMERAS
-
-AlertLevel = log.DriverMonitoringState.AlertLevel
-MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
-
-def to_percent(v):
-  return int(min(max(v * 100., 0.), 100.))
-
-# ******************************************************************************************
-#  NOTE: To fork maintainers.
-#  Disabling or nerfing safety features will get you and your users banned from our servers.
-#  We recommend that you do not change these numbers from the defaults.
-# ******************************************************************************************
-
-class DRIVER_MONITOR_SETTINGS:
-  def __init__(self):
-    # https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2
-    self._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT = 15.
-    self._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT = 24.
-    self._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT = 30.
-    # https://cdn.euroncap.com/cars/assets/euro_ncap_protocol_safe_driving_driver_engagement_v11_a30e874152.pdf
-    self._VISION_POLICY_ALERT_1_TIMEOUT = 3.
-    self._VISION_POLICY_ALERT_2_TIMEOUT = 5.
-    self._VISION_POLICY_ALERT_3_TIMEOUT = 11.
-
-    self._TIMEOUT_RECOVERY_FACTOR_MAX = 5.
-    self._TIMEOUT_RECOVERY_FACTOR_MIN = 1.25
-
-    self._MAX_TERMINAL_ALERTS = 3  # not allowed to engage after 3 terminal alerts
-    self._MAX_TERMINAL_DURATION = int(30 / DT_DMON)  # not allowed to engage after 30s of terminal alerts
-
-    self._FACE_THRESHOLD = 0.7
-    self._EYE_THRESHOLD = 0.5
-    self._BLINK_THRESHOLD = 0.5
-    self._PHONE_THRESH = 0.5
-    self._POSE_PITCH_THRESHOLD = 0.3133
-    self._POSE_PITCH_THRESHOLD_SLACK = 0.3237
-    self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD
-    self._POSE_YAW_THRESHOLD = 0.4020
-    self._POSE_YAW_THRESHOLD_SLACK = 0.5042
-    self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD
-    self._POSE_YAW_MIN_STEER_DEG = 30
-    self._POSE_YAW_STEER_FACTOR = 0.15
-    self._POSE_YAW_STEER_MAX_OFFSET = 0.3927
-    self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned
-    self._PITCH_NATURAL_THRESHOLD = 0.449
-    self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned
-    self._PITCH_NATURAL_VAR = 3*0.01
-    self._YAW_NATURAL_VAR = 3*0.05
-    self._PITCH_MAX_OFFSET = 0.124
-    self._PITCH_MIN_OFFSET = -0.0881
-    self._YAW_MAX_OFFSET = 0.289
-    self._YAW_MIN_OFFSET = -0.0246
-
-    self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1
-    self._DCAM_UNCERTAIN_ALERT_COUNT = int(60  / DT_DMON)
-    self._DCAM_UNCERTAIN_RESET_COUNT = int(20  / DT_DMON)
-    self._HI_STD_THRESHOLD = 0.3
-    self._HI_STD_FALLBACK_TIME = int(10  / DT_DMON)  # fall back to wheel touch if model is uncertain for 10s
-    self._DISTRACTED_FILTER_TS = 0.25  # 0.6Hz
-
-    self._POSE_CALIB_MIN_SPEED = 13  # 30 mph
-    self._POSE_OFFSET_MIN_COUNT = int(60 / DT_DMON)  # valid data counts before calibration completes, 1min cumulative
-    self._POSE_OFFSET_MAX_COUNT = int(360 / DT_DMON)  # stop deweighting new data after 6 min, aka "short term memory"
-    self._WHEELPOS_CALIB_MIN_SPEED = 11
-    self._WHEELPOS_THRESHOLD = 0.5
-    self._WHEELPOS_FILTER_MIN_COUNT = int(15 / DT_DMON) # allow 15 seconds to converge wheel side
-    self._WHEELPOS_DATA_AVG = 0.03
-    self._WHEELPOS_DATA_VAR = 3*5.5e-5
-    self._WHEELPOS_MAX_COUNT = -1
-
-class DriverPose:
-  def __init__(self, settings):
-    pitch_filter_raw_priors = (settings._PITCH_NATURAL_OFFSET, settings._PITCH_NATURAL_VAR, 2)
-    yaw_filter_raw_priors = (settings._YAW_NATURAL_OFFSET, settings._YAW_NATURAL_VAR, 2)
-    self.yaw = 0.
-    self.pitch = 0.
-    self.pitch_offsetter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
-    self.yaw_offsetter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
-    self.calibrated = False
-    self.low_std = True
-    self.cfactor_pitch = 1.
-    self.cfactor_yaw = 1.
-    self.steer_yaw_offset = 0.
-
-# model output refers to center of undistorted+leveled image
-ref_undistorted_cam = DEVICE_CAMERAS[("tici", "ar0231")].dcam
-dcam_undistorted_FL = 598.0
-dcam_undistorted_W, dcam_undistorted_H = (ref_undistorted_cam.width, ref_undistorted_cam.height)
-
-def face_orientation_from_model(orient_model, pos_model, rpy_calib):
-  pitch_model = orient_model[0]
-  yaw_model = orient_model[1]
-
-  face_pixel_position = ((pos_model[0]+0.5)*dcam_undistorted_W, (pos_model[1]+0.5)*dcam_undistorted_H)
-  yaw_focal_angle = atan2(face_pixel_position[0] - dcam_undistorted_W//2, dcam_undistorted_FL)
-  pitch_focal_angle = atan2(face_pixel_position[1] - dcam_undistorted_H//2, dcam_undistorted_FL)
-
-  pitch = pitch_model + pitch_focal_angle
-  yaw = -yaw_model + yaw_focal_angle
-
-  pitch -= rpy_calib[1]
-  yaw -= rpy_calib[2]
-  return pitch, yaw
-
-
-class DriverMonitoring:
-  def __init__(self, rhd_saved=False, settings=None, always_on=False):
-    # init policy settings
-    self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS()
-
-    # init driver status
-    wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2)
-    self.wheelpos_offsetter = RunningStatFilter(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT)
-    self.pose = DriverPose(settings=self.settings)
-    self.blink_prob = 0.
-    self.phone_prob = 0.
-
-    self.alert_level = AlertLevel.none
-    self.always_on = always_on
-    self.distracted_types = defaultdict(bool)
-    self.driver_distracted = False
-    self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, DT_DMON)
-    self.wheel_on_right = False
-    self.wheel_on_right_last = None
-    self.wheel_on_right_default = rhd_saved
-    self.face_detected = False
-    self.terminal_alert_cnt = 0
-    self.terminal_time = 0
-    self.step_change = 0.
-    self.active_policy = MonitoringPolicy.vision
-    self.driver_interacting = False
-    self.is_model_uncertain = False
-    self.hi_stds = 0
-    self.model_std_max = 0.
-    self.threshold_alert_1 = 0.
-    self.threshold_alert_2 = 0.
-    self.dcam_uncertain_cnt = 0
-    self.dcam_reset_cnt = 0
-    self.too_distracted = Params().get_bool("DriverTooDistracted")
-
-    self._reset_awareness()
-    self._set_policy(MonitoringPolicy.vision)
-
-  def _reset_awareness(self):
-    self.awareness = 1.
-    self.last_vision_awareness = 1.
-    self.last_wheeltouch_awareness = 1.
-
-  def _set_policy(self, target_policy):
-    if self.active_policy == MonitoringPolicy.vision and self.awareness <= self.threshold_alert_2:
-      if target_policy == MonitoringPolicy.vision:
-        self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
-      else:
-        self.step_change = 0.
-      return  # no exploit after orange alert
-    elif self.awareness <= 0.:
-      return
-
-    if target_policy == MonitoringPolicy.vision:
-      # when falling back from passive mode to active mode, reset awareness to avoid false alert
-      if self.active_policy != MonitoringPolicy.vision:
-        self.last_wheeltouch_awareness = self.awareness
-        self.awareness = self.last_vision_awareness
-
-      self.threshold_alert_1 = 1. - self.settings._VISION_POLICY_ALERT_1_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
-      self.threshold_alert_2 = 1. - self.settings._VISION_POLICY_ALERT_2_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
-      self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
-      self.active_policy = MonitoringPolicy.vision
-    else:
-      if self.active_policy == MonitoringPolicy.vision:
-        self.last_vision_awareness = self.awareness
-        self.awareness = self.last_wheeltouch_awareness
-
-      self.threshold_alert_1 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
-      self.threshold_alert_2 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
-      self.step_change = DT_DMON / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
-      self.active_policy = MonitoringPolicy.wheeltouch
-
-  def _set_pose_strictness(self, brake_disengage_prob, car_speed):
-    bp = brake_disengage_prob
-    k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
-    bp_normal = max(min(bp / k1, 0.5),0)
-    self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
-                                           [self.settings._POSE_PITCH_THRESHOLD_SLACK,
-                                            self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
-    self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
-                                           [self.settings._POSE_YAW_THRESHOLD_SLACK,
-                                            self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
-
-  def _get_distracted_types(self):
-    self.distracted_types = defaultdict(bool)
-
-    if not self.pose.calibrated:
-      pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET
-      yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET
-    else:
-      pitch_error = self.pose.pitch - min(max(self.pose.pitch_offsetter.filtered_stat.mean(),
-                                                       self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET)
-      yaw_error = self.pose.yaw - min(max(self.pose.yaw_offsetter.filtered_stat.mean(),
-                                                    self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET)
-    pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit
-
-    if yaw_error * self.pose.steer_yaw_offset > 0: # unidirectional
-      yaw_error = max(abs(yaw_error) - min(abs(self.pose.steer_yaw_offset), self.settings._POSE_YAW_STEER_MAX_OFFSET), 0.)
-    else:
-      yaw_error = abs(yaw_error)
-
-    pitch_threshold = self.settings._POSE_PITCH_THRESHOLD * self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD
-    yaw_threshold = self.settings._POSE_YAW_THRESHOLD * self.pose.cfactor_yaw
-
-    self.distracted_types['pose'] = bool((pitch_error > pitch_threshold) or (yaw_error > yaw_threshold))
-    self.distracted_types['eye'] = bool(self.blink_prob > self.settings._BLINK_THRESHOLD)
-    self.distracted_types['phone'] = bool(self.phone_prob > self.settings._PHONE_THRESH)
-
-  def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False, steering_angle_deg=0.):
-    rhd_pred = driver_state.wheelOnRightProb
-    # calibrates only when there's movement and either face detected
-    if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or
-                                          driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD):
-      self.wheelpos_offsetter.push_and_update(rhd_pred)
-
-    wheelpos_calibrated = self.wheelpos_offsetter.filtered_stat.n >= self.settings._WHEELPOS_FILTER_MIN_COUNT
-
-    if wheelpos_calibrated or demo_mode:
-      self.wheel_on_right = self.wheelpos_offsetter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD
-    else:
-      self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished
-    # make sure no switching when engaged
-    if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode:
-      self.wheel_on_right = self.wheel_on_right_last
-    driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData
-    if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition,
-                                    driver_data.faceOrientationStd, driver_data.facePositionStd)):
-      return
-
-    self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD
-    self.pose.pitch, self.pose.yaw = face_orientation_from_model(driver_data.faceOrientation, driver_data.facePosition, cal_rpy)
-    steer_d = max(abs(steering_angle_deg) - self.settings._POSE_YAW_MIN_STEER_DEG, 0.)
-    self.pose.steer_yaw_offset = radians(steer_d) * -np.sign(steering_angle_deg) * self.settings._POSE_YAW_STEER_FACTOR
-    if self.wheel_on_right:
-      self.pose.yaw *= -1
-      self.pose.steer_yaw_offset *= -1
-    self.wheel_on_right_last = self.wheel_on_right
-    self.model_std_max = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1])
-    self.pose.low_std = self.model_std_max < self.settings._HI_STD_THRESHOLD
-    self.blink_prob = driver_data.eyesClosedProb * (driver_data.eyesVisibleProb > self.settings._EYE_THRESHOLD)
-    self.phone_prob = driver_data.phoneProb
-
-    self._get_distracted_types()
-    self.driver_distracted = any(self.distracted_types.values()) and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
-    self.driver_distraction_filter.update(self.driver_distracted)
-
-    # only update offsetter when driver is actively driving the car above a certain speed
-    if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted):
-      self.pose.pitch_offsetter.push_and_update(self.pose.pitch)
-      self.pose.yaw_offsetter.push_and_update(self.pose.yaw)
-
-    self.pose.calibrated = self.pose.pitch_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT and \
-                           self.pose.yaw_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT
-
-    if self.face_detected and not self.driver_distracted:
-      dcam_uncertain = self.model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD
-      if dcam_uncertain and not standstill:
-        self.dcam_uncertain_cnt += 1
-        self.dcam_reset_cnt = 0
-      else:
-        self.dcam_reset_cnt += 1
-        if self.dcam_reset_cnt > self.settings._DCAM_UNCERTAIN_RESET_COUNT:
-          self.dcam_uncertain_cnt = 0
-
-    self.is_model_uncertain = self.hi_stds >= self.settings._HI_STD_FALLBACK_TIME
-    self._set_policy(MonitoringPolicy.vision if self.face_detected and not self.is_model_uncertain else MonitoringPolicy.wheeltouch)
-    if self.face_detected and not self.pose.low_std and not self.driver_distracted:
-      self.hi_stds += 1
-    elif self.face_detected and self.pose.low_std:
-      self.hi_stds = 0
-
-  def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear):
-    self.alert_level = AlertLevel.none
-    self.driver_interacting = driver_engaged
-
-    if self.terminal_alert_cnt >= self.settings._MAX_TERMINAL_ALERTS or \
-       self.terminal_time >= self.settings._MAX_TERMINAL_DURATION:
-      self.too_distracted = True
-
-    always_on_valid = self.always_on and not wrong_gear
-    if (self.driver_interacting and self.awareness > 0 and self.active_policy == MonitoringPolicy.wheeltouch) or \
-       (not always_on_valid and not op_engaged) or \
-       (always_on_valid and not op_engaged and self.awareness <= 0):
-      # always reset on disengage with normal mode; disengage resets only on red if always on
-      self._reset_awareness()
-      return
-
-    awareness_prev = self.awareness
-    _reaching_alert_1 = self.awareness - self.step_change <= self.threshold_alert_1
-    _reaching_alert_3 = self.awareness - self.step_change <= 0
-    standstill_exemption = standstill and _reaching_alert_1
-    always_on_exemption = always_on_valid and not op_engaged and _reaching_alert_3
-
-    if self.awareness > 0 and \
-       ((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or standstill_exemption):
-      if self.driver_interacting:
-        self._reset_awareness()
-        return
-      # only restore awareness when paying attention and alert is not red
-      self.awareness = min(self.awareness + ((self.settings._TIMEOUT_RECOVERY_FACTOR_MAX-self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)*
-                                             (1.-self.awareness)+self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)*self.step_change, 1.)
-      if self.awareness == 1.:
-        self.last_wheeltouch_awareness = min(self.last_wheeltouch_awareness + self.step_change, 1.)
-      # don't display alert banner when awareness is recovering and has cleared orange
-      if self.awareness > self.threshold_alert_2:
-        return
-
-    certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected
-    maybe_distracted = self.is_model_uncertain or not self.face_detected
-
-    if certainly_distracted or maybe_distracted:
-      # should always be counting if distracted unless at standstill and reaching green
-      # also will not be reaching 0 if DM is active when not engaged
-      if not (standstill_exemption or always_on_exemption):
-        self.awareness = max(self.awareness - self.step_change, -0.1)
-
-    if self.awareness <= 0.:
-      # terminal alert: disengagement required
-      self.alert_level = AlertLevel.three
-      self.terminal_time += 1
-      if awareness_prev > 0.:
-        self.terminal_alert_cnt += 1
-    elif self.awareness <= self.threshold_alert_2:
-      self.alert_level = AlertLevel.two
-    elif self.awareness <= self.threshold_alert_1:
-      self.alert_level = AlertLevel.one
-
-  def get_state_packet(self, valid=True):
-    # build driverMonitoringState packet
-    dat = messaging.new_message('driverMonitoringState', valid=valid)
-    dm = dat.driverMonitoringState
-
-    dm.lockout = self.too_distracted
-    dm.alertCountLockoutPercent = to_percent(self.terminal_alert_cnt / self.settings._MAX_TERMINAL_ALERTS)
-    dm.alertTimeLockoutPercent = to_percent(self.terminal_time / self.settings._MAX_TERMINAL_DURATION)
-    dm.alwaysOn = self.always_on
-    dm.alwaysOnLockout = self.always_on and self.awareness <= self.threshold_alert_2
-    dm.alertLevel = self.alert_level
-    dm.activePolicy = self.active_policy
-    dm.isRHD = self.wheel_on_right
-    dm.rhdCalibration.calibratedPercent = to_percent(self.wheelpos_offsetter.filtered_stat.n / self.settings._WHEELPOS_FILTER_MIN_COUNT)
-    dm.rhdCalibration.offset = self.wheelpos_offsetter.filtered_stat.M
-
-    dm.visionPolicyState.awarenessPercent = to_percent(self.last_vision_awareness if self.active_policy != MonitoringPolicy.vision else self.awareness)
-    dm.visionPolicyState.awarenessStep = self.step_change if self.active_policy == MonitoringPolicy.vision else 0.
-    dm.visionPolicyState.isDistracted = self.driver_distracted
-    dm.visionPolicyState.distractedTypes.pose = self.distracted_types['pose']
-    dm.visionPolicyState.distractedTypes.eye = self.distracted_types['eye']
-    dm.visionPolicyState.distractedTypes.phone = self.distracted_types['phone']
-    dm.visionPolicyState.faceDetected = self.face_detected
-    dm.visionPolicyState.pose.pitch = self.pose.pitch
-    dm.visionPolicyState.pose.yaw = self.pose.yaw
-    dm.visionPolicyState.pose.calibrated = self.pose.calibrated
-    dm.visionPolicyState.pose.pitchCalib.calibratedPercent = to_percent(self.pose.pitch_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT)
-    dm.visionPolicyState.pose.pitchCalib.offset = self.pose.pitch_offsetter.filtered_stat.M
-    dm.visionPolicyState.pose.yawCalib.calibratedPercent = to_percent(self.pose.yaw_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT)
-    dm.visionPolicyState.pose.yawCalib.offset = self.pose.yaw_offsetter.filtered_stat.M
-    dm.visionPolicyState.pose.uncertainty = self.model_std_max
-    dm.visionPolicyState.wheeltouchFallbackPercent = to_percent(self.hi_stds / self.settings._HI_STD_FALLBACK_TIME)
-    dm.visionPolicyState.uncertainOffroadAlertPercent = to_percent(self.dcam_uncertain_cnt / self.settings._DCAM_UNCERTAIN_ALERT_COUNT)
-
-    dm.wheeltouchPolicyState.awarenessPercent = to_percent(self.last_wheeltouch_awareness if self.active_policy == MonitoringPolicy.vision else self.awareness)
-    dm.wheeltouchPolicyState.awarenessStep = 0. if self.active_policy == MonitoringPolicy.vision else self.step_change
-    dm.wheeltouchPolicyState.driverInteracting = self.driver_interacting
-    return dat
-
-  def run_step(self, sm, demo=False):
-    if demo:
-      car_speed = 30
-      enabled = True
-      wrong_gear = False
-      standstill = False
-      driver_engaged = False
-      brake_disengage_prob = 1.0
-      steering_angle_deg = 0.0
-      rpyCalib = [0., 0., 0.]
-    else:
-      car_speed = sm['carState'].vEgo
-      enabled = sm['selfdriveState'].enabled or sm['carControl'].latActive
-      wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low)
-      standstill = sm['carState'].standstill
-      driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed
-      brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
-      steering_angle_deg = sm['carState'].steeringAngleDeg
-      rpyCalib = sm['liveCalibration'].rpyCalib
-
-    self._set_pose_strictness(
-      brake_disengage_prob=brake_disengage_prob,
-      car_speed=car_speed,
-    )
-
-    # Parse data from dmonitoringmodeld
-    self._update_states(
-      driver_state=sm['driverStateV2'],
-      cal_rpy=rpyCalib,
-      car_speed=car_speed,
-      op_engaged=enabled,
-      standstill=standstill,
-      demo_mode=demo,
-      steering_angle_deg=steering_angle_deg,
-    )
-
-    # Update distraction events
-    self._update_events(
-      driver_engaged=driver_engaged,
-      op_engaged=enabled,
-      standstill=standstill,
-      wrong_gear=wrong_gear,
-    )

selfdrive/monitoring/test_monitoring.py

@@ -3,16 +3,17 @@ import pytest
 
 from cereal import log, car
 from openpilot.common.realtime import DT_DMON
-from openpilot.selfdrive.monitoring.policy import DriverMonitoring, DRIVER_MONITOR_SETTINGS
+from openpilot.selfdrive.monitoring.helpers import DriverMonitoring, DRIVER_MONITOR_SETTINGS
+from openpilot.system.hardware import HARDWARE
 
 EventName = log.OnroadEvent.EventName
-dm_settings = DRIVER_MONITOR_SETTINGS()
+dm_settings = DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type())
 
 TEST_TIMESPAN = 120  # seconds
-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
+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
 
 def make_msg(face_detected, distracted=False, model_uncertain=False):
   ds = log.DriverStateV2.new_message()
@@ -34,7 +35,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._HI_STD_THRESHOLD*1.5)
+msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._POSESTD_THRESHOLD*1.5)
 
 # driver interaction with car
 car_interaction_DETECTED = True
@@ -50,49 +51,49 @@ always_false = [False] * int(TEST_TIMESPAN / DT_DMON)
 class TestMonitoring:
   def _run_seq(self, msgs, interaction, engaged, standstill):
     DM = DriverMonitoring()
-    alert_lvls = []
+    events = []
     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)
-      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
+      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
 
+  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):
-    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
+    events, _ = self._run_seq(always_attentive, always_false, always_true, always_false)
+    self._assert_no_events(events)
 
   # engaged, driver is distracted and does nothing
   def test_fully_distracted_driver(self):
-    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
+    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
     assert isinstance(d_status.awareness, float)
 
   # engaged, no face detected the whole time, no action
   def test_fully_invisible_driver(self):
-    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
+    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
 
   # 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
@@ -103,13 +104,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))
-    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
+    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
 
   # 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
@@ -127,11 +128,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)
-    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
+    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
 
   # 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
@@ -142,16 +143,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)
-    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
+    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
     if _visible_time == 0.5:
-      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
+      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
     elif _visible_time == 10:
-      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
+      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
 
   # engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
   #  - only disengage will clear the alert
@@ -163,19 +164,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)
-    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
+    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
 
   # disengaged, always distracted driver
   #  - dm should stay quiet when not engaged
   def test_pure_dashcam_user(self):
-    alert_lvls, _ = self._run_seq(always_distracted, always_false, always_false, always_false)
-    assert all(a == 0 for a in alert_lvls)
+    events, _ = self._run_seq(always_distracted, always_false, always_false, always_false)
+    assert sum(len(event) for event in events) == 0
 
   # 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
@@ -183,12 +184,11 @@ 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)
-    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
+    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
 
   # engaged, distracted while moving, then car stops after reaching orange
   #  - should reset timer to pre green at standstill
@@ -196,21 +196,23 @@ 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)
-    alert_lvls, _ = self._run_seq(always_distracted, always_false, always_true, standstill_vector)
+    events, _ = 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 alert_lvls[int((_stop_time+0.1)/DT_DMON)] == 2
-    assert alert_lvls[int((_stop_time+0.5)/DT_DMON)] == 0
+    assert events[int((_stop_time+0.1)/DT_DMON)].names[0] == EventName.driverDistracted2
+    assert len(events[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[:]
-    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
+    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
 
 
 @pytest.mark.parametrize("enabled_state, lat_active_state, expected", [
@@ -247,12 +249,12 @@ def test_enabled_states(enabled_state, lat_active_state, expected):
   ds = make_msg(False)
 
   sm = {
-    'carState': cs,
-    'selfdriveState': ss,
-    'carControl': cc,
-    'modelV2': mv2,
-    'liveCalibration': lc,
-    'driverStateV2': ds
+      'carState': cs,
+      'selfdriveState': ss,
+      'carControl': cc,
+      'modelV2': mv2,
+      'liveCalibration': lc,
+      'driverStateV2': ds
   }
 
   driver_monitoring = DriverMonitoring()
@@ -261,9 +263,9 @@ def test_enabled_states(enabled_state, lat_active_state, expected):
   captured_args = []
   original_update_events = driver_monitoring._update_events
 
-  def spy_update_events(driver_engaged, op_engaged, standstill, wrong_gear):
-    captured_args.append(op_engaged)
-    return original_update_events(driver_engaged, op_engaged, standstill, wrong_gear)
+  def spy_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed):
+      captured_args.append(op_engaged)
+      return original_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed)
 
   driver_monitoring._update_events = spy_update_events
 

selfdrive/selfdrived/selfdrived.py

@@ -45,8 +45,6 @@ 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)
@@ -142,8 +140,6 @@ 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)
 
@@ -220,27 +216,8 @@ 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:
-      # Block engaging until ignition cycle after max number or time of distractions
-      if self.sm['driverMonitoringState'].lockout and not self.dm_lockout_set:
-        self.params.put_bool_nonblocking("DriverTooDistracted", True)
-        self.dm_lockout_set = True
-      # No entry conditions
-      if self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout:
-        self.events.add(EventName.tooDistracted)
-      # Alerts
-      vision_dm = self.sm['driverMonitoringState'].activePolicy == MonitoringPolicy.vision
-      if self.sm['driverMonitoringState'].alertLevel == AlertLevel.one:
-        self.events.add(EventName.driverDistracted1 if vision_dm else EventName.driverUnresponsive1)
-      elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.two:
-        self.events.add(EventName.driverDistracted2 if vision_dm else EventName.driverUnresponsive2)
-      elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.three:
-        self.events.add(EventName.driverDistracted3 if vision_dm else EventName.driverUnresponsive3)
-      # Warn consistent DM uncertainty
-      if self.sm['driverMonitoringState'].visionPolicyState.uncertainOffroadAlertPercent >= 100 and not self.dm_uncertain_alerted:
-        set_offroad_alert("Offroad_DriverMonitoringUncertain", True)
-        self.dm_uncertain_alerted = True
+      self.events.add_from_msg(self.sm['driverMonitoringState'].events)
       self.events_sp.add_from_msg(self.sm['longitudinalPlanSP'].events)
 
     # Add car events, ignore if CAN isn't valid
@@ -264,7 +241,7 @@ class SelfdriveD(CruiseHelper):
         self.events.add(EventName.pedalPressed)
 
     # Create events for temperature, disk space, and memory
-    if self.sm['deviceState'].thermalStatus >= ThermalStatus.overheated:
+    if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
       self.events.add(EventName.overheat)
     if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
       self.events.add(EventName.outOfSpace)

selfdrive/test/process_replay/migration.py

@@ -449,6 +449,9 @@ 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()))
@@ -481,41 +484,22 @@ def migrate_onroadEvents(msgs):
   return ops, [], []
 
 
-@migration(inputs=["driverMonitoringStateDEPRECATED"])
+@migration(inputs=["driverMonitoringState"])
 def migrate_driverMonitoringState(msgs):
   ops = []
   for index, msg in msgs:
-    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 = 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()
 
-    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,
-      'tooDistracted': AlertLevel.three,
-    }
-    for event in old.events:
-      level = event_to_alert_level.get(str(event.name))
-      if level is not None:
-        dm.alertLevel = level
-        break
-
-    dm.visionPolicyState.awarenessPercent = int(max(0, min(100, (old.awarenessStatus if old.isActiveMode else old.awarenessActive) * 100)))
-    dm.visionPolicyState.awarenessStep = old.stepChange if old.isActiveMode else 0.
-    dm.visionPolicyState.isDistracted = old.isDistracted
-    dm.visionPolicyState.faceDetected = old.faceDetected
-    dm.visionPolicyState.pose.pitchCalib.offset = old.posePitchOffset
-    dm.visionPolicyState.pose.pitchCalib.calibratedPercent = int(min(100, old.posePitchValidCount / 600 * 100))
-    dm.visionPolicyState.pose.yawCalib.offset = old.poseYawOffset
-    dm.visionPolicyState.pose.yawCalib.calibratedPercent = int(min(100, old.poseYawValidCount / 600 * 100))
-    dm.visionPolicyState.pose.calibrated = old.posePitchValidCount >= 600 and old.poseYawValidCount >= 600
-    dm.wheeltouchPolicyState.awarenessPercent = int(max(0, min(100, (old.awarenessPassive if old.isActiveMode else old.awarenessStatus) * 100)))
-    dm.wheeltouchPolicyState.awarenessStep = 0. if old.isActiveMode else old.stepChange
-    ops.append((index, new_msg.as_reader()))
+    msg.driverMonitoringState.events = events
+    ops.append((index, msg.as_reader()))
 
   return ops, [], []

selfdrive/test/process_replay/model_replay.py

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

selfdrive/ui/body/animations.py

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

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

selfdrive/ui/layouts/main.py

@@ -2,14 +2,13 @@ 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
@@ -32,9 +31,7 @@ class MainLayout(Widget):
     self._prev_onroad = False
 
     # Initialize layouts
-    self._home_layout = HomeLayout()
-    self._home_body_layout = BodyLayout()
-    self._layouts = {MainState.HOME: self._home_layout, MainState.SETTINGS: SettingsLayout(), MainState.ONROAD: AugmentedRoadView()}
+    self._layouts = {MainState.HOME: HomeLayout(), MainState.SETTINGS: SettingsLayout(), MainState.ONROAD: AugmentedRoadView()}
 
     self._sidebar_rect = rl.Rectangle(0, 0, 0, 0)
     self._content_rect = rl.Rectangle(0, 0, 0, 0)
@@ -60,18 +57,14 @@ 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)
-
-    for layout in (self._layouts[MainState.ONROAD], self._home_body_layout):
-      layout.set_click_callback(self._on_onroad_clicked)
-
+    self._layouts[MainState.ONROAD].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.x + x_offset, self._rect.y, self._rect.width - x_offset, self._rect.height)
+    self._content_rect = rl.Rectangle(self._rect.y + 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:
@@ -80,12 +73,6 @@ 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:
@@ -117,10 +104,6 @@ class MainLayout(Widget):
   def _on_onroad_clicked(self):
     self._sidebar.set_visible(not self._sidebar.is_visible)
 
-  def _on_body_changed(self):
-    self._layouts[MainState.HOME] = self._home_body_layout if ui_state.is_body else self._home_layout
-    self._set_mode_for_state()
-
   def _render_main_content(self):
     # Render sidebar
     if self._sidebar.is_visible:

selfdrive/ui/layouts/settings/developer.py

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

selfdrive/ui/layouts/settings/firehose.py

@@ -1,7 +1,7 @@
 import pyray as rl
 
 from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE
-from openpilot.system.ui.lib.multilang import tr, tr_noop
+from openpilot.system.ui.lib.multilang import tr, trn, 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,13 +65,12 @@ 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/sidebar.py

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

selfdrive/ui/mici/layouts/home.py

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

selfdrive/ui/mici/layouts/main.py

@@ -6,7 +6,6 @@ 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
@@ -32,25 +31,22 @@ class MiciMainLayout(Scroller):
     self._home_layout = MiciHomeLayout()
     self._alerts_layout = MiciOffroadAlerts()
     self._settings_layout = SettingsLayout()
-    self._car_onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked)
-    self._body_onroad_layout = BodyLayout()
+    self._onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked)
 
     # Initialize widget rects
-    for widget in (self._home_layout, self._alerts_layout, self._settings_layout,
-                   self._car_onroad_layout, self._body_onroad_layout):
+    for widget in (self._home_layout, self._settings_layout, self._alerts_layout, self._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._car_onroad_layout,
-      self._body_onroad_layout,
+      self._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._car_onroad_layout.is_swiping_left())
+    self._scroller.set_scrolling_enabled(lambda: not self._onroad_layout.is_swiping_left())
 
     # Set callbacks
     self._setup_callbacks()
@@ -63,22 +59,14 @@ 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,
     )
-    for layout in (self._car_onroad_layout, self._body_onroad_layout):
-      layout.set_click_callback(lambda: self._scroll_to(self._home_layout))
-
+    self._onroad_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)
@@ -144,7 +132,3 @@ class MiciMainLayout(Scroller):
     user_bookmark = messaging.new_message('bookmarkButton')
     user_bookmark.valid = True
     self._pm.send('bookmarkButton', user_bookmark)
-
-  def _on_body_changed(self):
-    self._car_onroad_layout.set_visible(not ui_state.is_body)
-    self._body_onroad_layout.set_visible(ui_state.is_body)

selfdrive/ui/mici/layouts/onboarding.py

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

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

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

selfdrive/ui/mici/onroad/driver_camera_dialog.py

@@ -1,15 +1,20 @@
 import pyray as rl
-from cereal import car, log, messaging
+from cereal import 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):
@@ -102,14 +107,11 @@ 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:
-      msg.selfdriveState.alertSound = ALERT_SOUNDS.get(str(dm_state.alertLevel), AudibleAlert.none)
+    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
     self._pm.send('selfdriveState', msg)
 
   def _render_dm_alerts(self, rect: rl.Rectangle):
@@ -117,31 +119,29 @@ 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: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
+              f"Awareness: {dm_state.awarenessStatus * 100:.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: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
+    gui_label(rect, f"Awareness: {dm_state.awarenessStatus * 100:.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 dm_state.alertLevel == log.DriverMonitoringState.AlertLevel.none:
+    if not dm_state.events:
       return
 
-    # Show alert level
-    alert_level_str = f"{'Pay Attention' if is_vision else 'Touch Wheel'} - level {dm_state.alertLevel}"
+    # Show first event (only one should be active at a time)
+    event_name_str = str(dm_state.events[0].name).split('.')[-1]
     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, alert_level_str, font_size=40, font_weight=FontWeight.BOLD,
+    gui_label(shadow_rect, event_name_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, alert_level_str, font_size=40, font_weight=FontWeight.BOLD,
+    gui_label(rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD,
               alignment=alignment,
               alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM,
               color=rl.Color(255, 255, 255, int(255 * 0.9)))
@@ -156,7 +156,7 @@ class BaseDriverCameraDialog(Widget):
   def _draw_face_detection(self, rect: rl.Rectangle):
     dm_state = ui_state.sm["driverMonitoringState"]
     driver_data = self.driver_state_renderer.get_driver_data()
-    if not dm_state.visionPolicyState.faceDetected:
+    if not dm_state.faceDetected:
       return
 
     # Get face position and orientation

selfdrive/ui/mici/onroad/driver_state.py

@@ -6,7 +6,7 @@ from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.system.ui.lib.application import gui_app
 from openpilot.system.ui.widgets import Widget
 from openpilot.selfdrive.ui.ui_state import ui_state
-
+from openpilot.selfdrive.monitoring.helpers import face_orientation_from_net
 
 AlertSize = log.SelfdriveState.AlertSize
 
@@ -35,8 +35,6 @@ 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
@@ -155,11 +153,9 @@ class DriverStateRenderer(Widget):
     sm = ui_state.sm
 
     dm_state = sm["driverMonitoringState"]
-    self._is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
+    self._is_active = dm_state.isActiveMode
     self._is_rhd = dm_state.isRHD
-    self._face_detected = dm_state.visionPolicyState.faceDetected
-    self._face_pitch = dm_state.visionPolicyState.pose.pitch + math.radians(6) # calib or DM pose is not accurate, add a fake upward pitch to bias forward
-    self._face_yaw = -dm_state.visionPolicyState.pose.yaw # undo sign flip in face_orientation_from_model to match UI convention
+    self._face_detected = dm_state.faceDetected
 
     driverstate = sm["driverStateV2"]
     driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData
@@ -167,9 +163,26 @@ class DriverStateRenderer(Widget):
 
   def _update_state(self):
     # Get monitoring state
-    _ = self.get_driver_data()
-    pitch = self._pitch_filter.update(self._face_pitch)
-    yaw = self._yaw_filter.update(self._face_yaw)
+    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)
 
     # hysteresis on looking center
     if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER:

selfdrive/ui/onroad/driver_state.py

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

selfdrive/ui/sunnypilot/ui_state.py

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

selfdrive/ui/ui_state.py

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

sunnypilot/models/tests/model_hash

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

sunnypilot/selfdrive/locationd/locationd.cc

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

sunnypilot/selfdrive/locationd/tests/test_locationd.py

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

sunnypilot/system/sensord/sensors/bmx055_accel.cc

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

sunnypilot/system/sensord/sensors/bmx055_gyro.cc

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

sunnypilot/system/sensord/sensors/bmx055_magn.cc

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

sunnypilot/system/sensord/sensors/bmx055_temp.cc

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

sunnypilot/system/sensord/sensors/lsm6ds3_accel.cc

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

sunnypilot/system/sensord/sensors/lsm6ds3_gyro.cc

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

sunnypilot/system/sensord/sensors/lsm6ds3_temp.cc

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

sunnypilot/system/sensord/sensors/mmc5603nj_magn.cc

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

system/camerad/cameras/cdm.cc

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

system/camerad/cameras/cdm.h

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

system/camerad/cameras/spectra.cc

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

system/camerad/cameras/spectra.h

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

system/camerad/sensors/os04c10.cc

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

system/camerad/sensors/os04c10_registers.h

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

system/camerad/sensors/sensor.h

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

system/hardware/base.py

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

system/hardware/esim.py

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

system/hardware/hardwared.py

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

system/hardware/tici/agnos.json

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

system/hardware/tici/all-partitions.json

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

system/hardware/tici/lpa.py

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

system/loggerd/SConscript

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

system/qcomgpsd/qcomgpsd.py

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

system/qcomgpsd/tests/test_qcomgpsd.py

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

system/sensord/sensord.py

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

system/sensord/sensors/lsm6ds3_accel.py

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

system/sensord/sensors/lsm6ds3_gyro.py

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

system/sensord/sensors/lsm6ds3_temp.py

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

system/sensord/sensors/mmc5603nj_magn.py

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

system/sensord/tests/test_sensord.py

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

tools/jotpluggler/.gitignore

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

tools/jotpluggler/SConscript

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

tools/jotpluggler/app.cc

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

tools/jotpluggler/generate_event_extractors.py

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

tools/jotpluggler/internal.h

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

tools/jotpluggler/plot.cc

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

tools/jotpluggler/runtime.cc

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

tools/jotpluggler/sketch_layout.cc

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

tools/lateral_maneuvers/lateral_maneuversd.py

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

tools/setup_dependencies.sh

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

tools/sim/lib/simulated_sensors.py

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