From 273a4f5ca9313673caadccf9e6910ab080f7f047 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Wed, 8 May 2024 20:05:03 -0700 Subject: [PATCH] Always-on DM: no audible alert at low speeds / block engagement if alert present (#32379) * min speed * cmments * no entry? * comment --- selfdrive/monitoring/dmonitoringd.py | 9 +++++---- selfdrive/monitoring/driver_monitor.py | 16 +++++++++++----- selfdrive/monitoring/test_monitoring.py | 2 +- 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 84a147bd477d51..c18bbf29b991f3 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -22,7 +22,7 @@ def dmonitoringd_thread(): v_cruise_last = 0 driver_engaged = False - # 10Hz <- dmonitoringmodeld + # 20Hz <- dmonitoringmodeld while True: sm.update() if not sm.updated['driverStateV2']: @@ -46,14 +46,15 @@ def dmonitoringd_thread(): if sm.all_checks() and len(sm['liveCalibration'].rpyCalib): driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) - # Block engaging after max number of distrations + # Block engaging after max number of distrations or when alert active if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \ - driver_status.terminal_time >= driver_status.settings._MAX_TERMINAL_DURATION: + driver_status.terminal_time >= driver_status.settings._MAX_TERMINAL_DURATION or \ + driver_status.always_on and driver_status.awareness <= driver_status.threshold_prompt: events.add(car.CarEvent.EventName.tooDistracted) # Update events from driver state driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, - sm['carState'].standstill, sm['carState'].gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park]) + sm['carState'].standstill, sm['carState'].gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park], sm['carState'].vEgo) # build driverMonitoringState packet dat = messaging.new_message('driverMonitoringState', valid=sm.all_checks()) diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 749931af77f02f..7db0bd6c9f7d53 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -55,6 +55,7 @@ def __init__(self): 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._ALWAYS_ON_ALERT_MIN_SPEED = 7 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 @@ -302,7 +303,7 @@ def update_states(self, driver_state, cal_rpy, car_speed, op_engaged): elif self.face_detected and self.pose.low_std: self.hi_stds = 0 - def update_events(self, events, driver_engaged, ctrl_active, standstill, wrong_gear): + def update_events(self, events, driver_engaged, ctrl_active, standstill, wrong_gear, car_speed): 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 ctrl_active) or \ @@ -327,14 +328,19 @@ def update_events(self, events, driver_engaged, ctrl_active, standstill, wrong_g if self.awareness > self.threshold_prompt: return - standstill_exemption = standstill and self.awareness - self.step_change <= self.threshold_prompt - always_on_red_exemption = always_on_valid and not ctrl_active and self.awareness - self.step_change <= 0 + _reaching_audible = self.awareness - self.step_change <= self.threshold_prompt + _reaching_terminal = self.awareness - self.step_change <= 0 + standstill_exemption = standstill and _reaching_audible + always_on_red_exemption = always_on_valid and not ctrl_active and _reaching_terminal + always_on_lowspeed_exemption = always_on_valid and not ctrl_active and car_speed < self.settings._ALWAYS_ON_ALERT_MIN_SPEED and _reaching_audible + 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 orange + # should always be counting if distracted unless at standstill (lowspeed for always-on) and reaching orange # also will not be reaching 0 if DM is active when not engaged - if not standstill_exemption and not always_on_red_exemption: + if not (standstill_exemption or always_on_red_exemption or always_on_lowspeed_exemption): self.awareness = max(self.awareness - self.step_change, -0.1) alert = None diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 39fef75479a112..50b2746e2dd66f 100755 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -63,7 +63,7 @@ def _run_seq(self, msgs, interaction, engaged, standstill): # cal_rpy and car_speed don't matter here # evaluate events at 10Hz for tests - DS.update_events(e, interaction[idx], engaged[idx], standstill[idx], 0) + DS.update_events(e, interaction[idx], engaged[idx], standstill[idx], 0, 0) events.append(e) assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs" return events, DS