sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8

openpilot/selfdrive/monitoring/dmonitoringd.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+import openpilot.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
+
+
+def dmonitoringd_thread():
+  config_realtime_process([0, 1, 2, 3], 5)
+
+  params = Params()
+  pm = messaging.PubMaster(['driverMonitoringState'])
+  sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2',
+                            'carControl'], poll='driverStateV2')
+
+  DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM"))
+  demo_mode=False
+
+  # 20Hz <- dmonitoringmodeld
+  while True:
+    sm.update()
+    if not sm.updated['driverStateV2']:
+      # iterate when model has new output
+      continue
+
+    valid = sm.all_checks()
+    if demo_mode and sm.valid['driverStateV2']:
+      DM.run_step(sm, demo=True)
+    elif valid:
+      DM.run_step(sm, demo=demo_mode)
+
+    # publish
+    dat = DM.get_state_packet(valid=valid)
+    pm.send('driverMonitoringState', dat)
+
+    # load live always-on toggle
+    if sm['driverStateV2'].frameId % 40 == 1:
+      DM.always_on = params.get_bool("AlwaysOnDM")
+      demo_mode = params.get_bool("IsDriverViewEnabled")
+
+    # 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)):
+      params.put_bool("IsRhdDetected", DM.wheel_on_right)
+
+def main():
+  dmonitoringd_thread()
+
+
+if __name__ == '__main__':
+  main()

openpilot/selfdrive/monitoring/policy.py

@@ -0,0 +1,460 @@
+from collections import defaultdict
+from math import atan2, radians
+import numpy as np
+
+from openpilot.cereal import log
+from opendbc.car.structs import car
+import openpilot.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=OJ:L_202501899
+    self._ALERT_MIN_SPEED = 2.8  # 10 km/h
+
+    self._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT = 5.
+    self._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT = 15.
+    self._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT = 25.
+    self._VISION_POLICY_ALERT_1_TIMEOUT = 5.
+    self._VISION_POLICY_ALERT_2_TIMEOUT = 8.
+    self._VISION_POLICY_ALERT_3_TIMEOUT = 13.
+
+    # no response = alert_3 sustained for certain amount of time
+    self._NO_RESPONSE_TIMEOUT = 5.
+
+    # lockout specs
+    self._MAX_ALERT_3 = 2
+    self._MAX_NO_RESPONSE = 1
+    self._LOCKOUT_TIME = int(1800 / DT_DMON)
+
+    self._TIMEOUT_RECOVERY_FACTOR_MAX = 5.
+    self._TIMEOUT_RECOVERY_FACTOR_MIN = 1.25
+
+    self._FACE_THRESHOLD = 0.7
+    self._EYE_THRESHOLD = 0.65
+    self._SG_THRESHOLD = 0.9
+    self._BLINK_THRESHOLD = 0.865
+    self._PHONE_THRESH = 0.5
+    self._POSE_PITCH_THRESHOLD = 0.3133
+    self._POSE_PITCH_THRESHOLD_SLACK = 0.3237
+    self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD
+    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(2  / DT_DMON)
+    self._HI_STD_THRESHOLD = 0.3
+    self._HI_STD_FALLBACK_TIME = int(10  / DT_DMON)  # fall back to wheel touch if model is uncertain for 10s
+    self._DISTRACTED_FILTER_TS = 0.25  # 0.6Hz
+
+    self._POSE_CALIB_MIN_SPEED = 13  # 30 mph
+    self._POSE_OFFSET_MIN_COUNT = int(60 / 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.
+
+class DriverBlink:
+  def __init__(self):
+    self.left = 0.
+    self.right = 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 = DriverBlink()
+    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.alert_3_cnt = 0
+    self.cnt_since_alert_3 = 0
+    self.no_response_timeout = int(self.settings._NO_RESPONSE_TIMEOUT / DT_DMON)
+    self.no_response_cnt = 0
+    self.lockout_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.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD)
+    self.distracted_types['phone'] = bool(self.phone_prob > self.settings._PHONE_THRESH)
+
+  def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, lowspeed, 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.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \
+                      * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
+    self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \
+                      * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
+    self.phone_prob = driver_data.phoneProb
+
+    self._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 lowspeed:
+        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, lowspeed, wrong_gear):
+    self.alert_level = AlertLevel.none
+    self.driver_interacting = driver_engaged
+
+    if self.alert_3_cnt >= self.settings._MAX_ALERT_3 or self.no_response_cnt >= self.settings._MAX_NO_RESPONSE:
+      self.too_distracted = True
+
+    if self.too_distracted:
+      self.lockout_time += 1
+      if self.lockout_time > self.settings._LOCKOUT_TIME:
+        self.too_distracted = False
+        self.alert_3_cnt = 0
+        self.cnt_since_alert_3 = 0
+        self.no_response_cnt = 0
+        self.lockout_time = 0
+
+    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
+    lowspeed_exemption = lowspeed 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 lowspeed_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 low speed and reaching green
+      # also will not be reaching 0 if DM is active when not engaged
+      if not (lowspeed_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
+      if awareness_prev > 0.:
+        self.alert_3_cnt += 1
+        self.cnt_since_alert_3 = 0
+      else:
+        self.cnt_since_alert_3 += 1
+      if self.cnt_since_alert_3 == self.no_response_timeout:
+        self.no_response_cnt += 1
+    else:
+      if 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.lockoutRecoveryPercent = to_percent(self.lockout_time / self.settings._LOCKOUT_TIME)
+    dm.alert3Count = self.alert_3_cnt
+    dm.noResponseCount = self.no_response_cnt
+    dm.noResponseForceDecel = self.alert_level == AlertLevel.three and self.cnt_since_alert_3 >= self.no_response_timeout
+    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
+      lowspeed = 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)
+      lowspeed = car_speed < self.settings._ALERT_MIN_SPEED
+      driver_engaged = sm['carState'].steeringPressed or (sm['selfdriveState'].enabled and sm['carState'].gasPressed)
+      brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
+      steering_angle_deg = sm['carState'].steeringAngleDeg
+      rpyCalib = sm['liveCalibration'].rpyCalib
+
+    self._set_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,
+      lowspeed=lowspeed,
+      demo_mode=demo,
+      steering_angle_deg=steering_angle_deg,
+    )
+
+    # Update distraction events
+    self._update_events(
+      driver_engaged=driver_engaged,
+      op_engaged=enabled,
+      lowspeed=lowspeed,
+      wrong_gear=wrong_gear,
+    )

openpilot/selfdrive/monitoring/test_monitoring.py

@@ -0,0 +1,281 @@
+import pytest
+
+from openpilot.cereal import log
+from opendbc.car.structs import car
+from openpilot.common.realtime import DT_DMON
+from openpilot.selfdrive.monitoring.policy import DriverMonitoring, DRIVER_MONITOR_SETTINGS
+
+EventName = log.OnroadEvent.EventName
+dm_settings = DRIVER_MONITOR_SETTINGS()
+
+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
+
+def make_msg(face_detected, distracted=False, model_uncertain=False):
+  ds = log.DriverStateV2.new_message()
+  ds.leftDriverData.faceOrientation = [0., 0., 0.]
+  ds.leftDriverData.facePosition = [0., 0.]
+  ds.leftDriverData.faceProb = 1. * face_detected
+  ds.leftDriverData.leftEyeProb = 1.
+  ds.leftDriverData.rightEyeProb = 1.
+  ds.leftDriverData.leftBlinkProb = 1. * distracted
+  ds.leftDriverData.rightBlinkProb = 1. * distracted
+  ds.leftDriverData.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain]
+  ds.leftDriverData.facePositionStd = [1.*model_uncertain, 1.*model_uncertain]
+  # TODO: test both separately when e2e is used
+  ds.leftDriverData.phoneProb = 0.
+  return ds
+
+
+# driver state from neural net, 10Hz
+msg_NO_FACE_DETECTED = make_msg(False)
+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)
+
+# driver interaction with car
+car_interaction_DETECTED = True
+car_interaction_NOT_DETECTED = False
+
+# some common state vectors
+always_no_face = [msg_NO_FACE_DETECTED] * int(TEST_TIMESPAN / DT_DMON)
+always_attentive = [msg_ATTENTIVE] * int(TEST_TIMESPAN / DT_DMON)
+always_distracted = [msg_DISTRACTED] * int(TEST_TIMESPAN / DT_DMON)
+always_true = [True] * int(TEST_TIMESPAN / DT_DMON)
+always_false = [False] * int(TEST_TIMESPAN / DT_DMON)
+
+class TestMonitoring:
+  def _run_seq(self, msgs, interaction, engaged, lowspeed):
+    DM = DriverMonitoring()
+    alert_lvls = []
+    for idx in range(len(msgs)):
+      DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], lowspeed[idx])
+      # cal_rpy and car_speed don't matter here
+
+      # evaluate events at 10Hz for tests
+      DM._update_events(interaction[idx], engaged[idx], lowspeed[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
+
+
+  # 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
+
+  # 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
+    assert isinstance(d_status.awareness, float)
+
+  # engaged, distracted past red and beyond the no-response window -> unavailability response + lockout
+  def test_distracted_lockout(self):
+    alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false)
+    s = d_status.settings
+    assert alert_lvls[int(DISTRACTED_SECONDS_TO_RED / DT_DMON)] == 3
+    assert d_status.alert_3_cnt == 1
+    assert d_status.no_response_cnt == s._MAX_NO_RESPONSE
+    assert d_status.too_distracted
+    assert d_status.lockout_time > 0
+
+  # no face -> wheeltouch red, sustained past the no-response timeout -> unavailability response + lockout
+  def test_invisible_lockout(self):
+    _, d_status = self._run_seq(always_no_face, always_false, always_true, always_false)
+    s = d_status.settings
+    assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.wheeltouch
+    assert d_status.alert_3_cnt == 1
+    assert d_status.no_response_cnt == s._MAX_NO_RESPONSE
+    assert d_status.too_distracted
+
+  # 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
+
+  # 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
+  def test_normal_driver(self):
+    ds_vector = [msg_DISTRACTED] * int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \
+                [msg_ATTENTIVE] * int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \
+                [msg_DISTRACTED] * int((DISTRACTED_SECONDS_TO_ORANGE+2)/DT_DMON) + \
+                [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
+
+  # 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
+  #  - orange/red alert should remain after disappearance, and only disengaging clears red
+  def test_biggest_comma_fan(self):
+    _invisible_time = 2  # seconds
+    ds_vector = always_distracted[:]
+    interaction_vector = always_false[:]
+    op_vector = always_true[:]
+    ds_vector[int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON):int((DISTRACTED_SECONDS_TO_ORANGE+_invisible_time)/DT_DMON)] \
+                                                        = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON)
+    ds_vector[int((DISTRACTED_SECONDS_TO_RED+_invisible_time)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time)/DT_DMON)] \
+                                                        = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON)
+    interaction_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+0.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] \
+                                                        = [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
+
+  # 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
+  def test_sometimes_transparent_commuter(self):
+    for _visible_time in (0.5, 10):
+      ds_vector = always_no_face[:]*2
+      interaction_vector = always_false[:]*2
+      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(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/DT_DMON)] == 0
+      assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
+      assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)] == 0
+      if _visible_time == 0.5:
+        assert 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)] == 2
+      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
+
+  # engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
+  #  - only disengage will clear the alert
+  def test_last_second_responder(self):
+    _visible_time = 2  # seconds
+    ds_vector = always_no_face[:]
+    interaction_vector = always_false[:]
+    op_vector = always_true[:]
+    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(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/DT_DMON)] == 0
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)] == 3
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)] == 3
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] == 3
+    assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)] == 0
+
+  # disengaged, always distracted driver
+  #  - dm should stay quiet when not engaged
+  def test_pure_dashcam_user(self):
+    alert_lvls, _ = self._run_seq(always_distracted, always_false, always_false, always_false)
+    assert all(a == 0 for a in alert_lvls)
+
+  # engaged, car stops at traffic light, down to orange, no action, then car starts moving
+  #  - should only reach green when stopped, but continues counting down on launch
+  def test_long_traffic_light_victim(self):
+    _redlight_time = 60  # seconds
+    lowspeed_vector = always_true[:]
+    lowspeed_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, lowspeed_vector)
+    s = d_status.settings
+    assert alert_lvls[int((_redlight_time-0.1)/DT_DMON)] == 0
+    _alert_1_to_2 = s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT
+    assert alert_lvls[int((_redlight_time+0.5)/DT_DMON)] == 1
+    assert alert_lvls[int((_redlight_time+_alert_1_to_2+0.5)/DT_DMON)] == 2
+
+  # engaged, distracted while moving, then car stops after reaching orange
+  #  - should reset timer to pre green at low speed
+  def test_distracted_then_stops(self):
+    _stop_time = DISTRACTED_SECONDS_TO_ORANGE + 1  # stop 1 second after reaching orange
+    lowspeed_vector = always_false[:]
+    lowspeed_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, lowspeed_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
+
+  # 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
+
+
+def _build_sm(selfdrive_enabled, lat_active, steering_pressed, gas_pressed):
+  cs = car.CarState.new_message()
+  cs.vEgo = 30.0
+  cs.gearShifter = car.CarState.GearShifter.drive
+  cs.steeringPressed = steering_pressed
+  cs.gasPressed = gas_pressed
+  ss = log.SelfdriveState.new_message()
+  ss.enabled = selfdrive_enabled
+  cc = car.CarControl.new_message()
+  cc.latActive = lat_active
+  mv2 = log.ModelDataV2.new_message()
+  mv2.meta.disengagePredictions.brakeDisengageProbs = [0.0]
+  lc = log.LiveCalibrationData.new_message()
+  lc.rpyCalib = [0.0, 0.0, 0.0]
+  return {
+    'carState': cs, 'selfdriveState': ss, 'carControl': cc,
+    'modelV2': mv2, 'liveCalibration': lc, 'driverStateV2': make_msg(False),
+  }
+
+
+@pytest.mark.parametrize("selfdrive_enabled, lat_active, steering, gas, expected_op_engaged, expected_driver_engaged", [
+  (False, False, False, False, False, False),  # disabled
+  (True,  False, False, False, True,  False),  # OP enabled
+  (False, True,  False, False, True,  False),  # MADS lat-only
+  (True,  True,  False, False, True,  False),  # both active
+  (False, True,  False, True,  True,  False),  # MADS lat-only + gas
+  (True,  True,  False, True,  True,  True),   # full op + gas: override
+  (False, True,  True,  False, True,  True),   # MADS lat-only + wheel touch: override
+])
+def test_run_step_engagement(selfdrive_enabled, lat_active, steering, gas,
+                             expected_op_engaged, expected_driver_engaged):
+  sm = _build_sm(selfdrive_enabled, lat_active, steering, gas)
+  dm = DriverMonitoring()
+  captured = {}
+  orig = dm._update_events
+
+  def spy(driver_engaged, op_engaged, lowspeed, wrong_gear):
+    captured['driver_engaged'] = driver_engaged
+    captured['op_engaged'] = op_engaged
+    return orig(driver_engaged, op_engaged, lowspeed, wrong_gear)
+
+  dm._update_events = spy
+  dm.run_step(sm, demo=False)
+  assert captured['op_engaged'] == expected_op_engaged
+  assert captured['driver_engaged'] == expected_driver_engaged