add check driver camera alert (#36577)

* add event

* missing arg

* creation_delay is wrong

* add logging

* set offroad alert

* Update selfdrive/selfdrived/alerts_offroad.json

Co-authored-by: Shane Smiskol <[email protected]>

* rm onard

* add details

* rename to DM

* log rename

* no poss

---------

Co-authored-by: Shane Smiskol <[email protected]>

Author: ZwX1616

cereal/log.capnp

@@ -2224,6 +2224,7 @@ struct DriverMonitoringState @0xb83cda094a1da284 {
   hiStdCount @14 :UInt32;
   isActiveMode @16 :Bool;
   isRHD @4 :Bool;
+  uncertainCount @19 :UInt32;
 
   isPreviewDEPRECATED @15 :Bool;
   rhdCheckedDEPRECATED @5 :Bool;

common/params_keys.h

@@ -97,6 +97,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"Offroad_TemperatureTooHigh", {CLEAR_ON_MANAGER_START, JSON}},
     {"Offroad_UnregisteredHardware", {CLEAR_ON_MANAGER_START, JSON}},
     {"Offroad_UpdateFailed", {CLEAR_ON_MANAGER_START, JSON}},
+    {"Offroad_DriverMonitoringUncertain", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
     {"OnroadCycleRequested", {CLEAR_ON_MANAGER_START, BOOL}},
     {"OpenpilotEnabledToggle", {PERSISTENT, BOOL, "1"}},
     {"PandaHeartbeatLost", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},

selfdrive/monitoring/helpers.py

@@ -4,6 +4,7 @@
 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
@@ -57,6 +58,9 @@ def __init__(self):
     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
@@ -158,6 +162,9 @@ def __init__(self, rhd_saved=False, settings=None, always_on=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")
@@ -245,7 +252,7 @@ def _get_distracted_types(self):
 
     return distracted_types
 
-  def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged):
+  def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill):
     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
@@ -296,6 +303,16 @@ def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged):
                                        self.pose.yaw_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT
     self.ee1_calibrated = self.ee1_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:
@@ -372,6 +389,10 @@ def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear, car
     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
@@ -393,6 +414,7 @@ def get_state_packet(self, valid=True):
       "hiStdCount": self.hi_stds,
       "isActiveMode": self.active_monitoring_mode,
       "isRHD": self.wheel_on_right,
+      "uncertainCount": self.dcam_uncertain_cnt,
     }
     return dat
 
@@ -408,7 +430,8 @@ def run_step(self, sm):
       driver_state=sm['driverStateV2'],
       cal_rpy=sm['liveCalibration'].rpyCalib,
       car_speed=sm['carState'].vEgo,
-      op_engaged=sm['selfdriveState'].enabled
+      op_engaged=sm['selfdriveState'].enabled,
+      standstill=sm['carState'].standstill,
     )
 
     # Update distraction events

selfdrive/monitoring/test_monitoring.py

@@ -53,7 +53,7 @@ def _run_seq(self, msgs, interaction, engaged, standstill):
     DM = DriverMonitoring()
     events = []
     for idx in range(len(msgs)):
-      DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx])
+      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

selfdrive/selfdrived/alerts_offroad.json

@@ -37,6 +37,10 @@
     "text": "openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.",
     "severity": 0
   },
+  "Offroad_DriverMonitoringUncertain": {
+    "text": "openpilot detected poor visibility for driver monitoring. Ensure the device has a clear view of the driver. This can be checked using Settings -> Device -> Driver Camera Preview. Extreme lighting conditions and/or unconventional mounting positions may also trigger this alert.",
+    "severity": 0
+  },
   "Offroad_ExcessiveActuation": {
     "text": "openpilot detected excessive %1 actuation on your last drive. Please contact support at https://comma.ai/support and share your device's Dongle ID for troubleshooting.",
     "severity": 1,