From 5dcaf3bef877a5ddc9fa7f89f185624b81a82ab7 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Wed, 1 Apr 2026 00:40:13 -0700 Subject: [PATCH] DM: fewer alerts during maneuvers (#37751) * 2in1 * clip * drop aodm lowspeed * cleanup * add lower bd * that was random --- selfdrive/monitoring/helpers.py | 29 +++++++++++++++++-------- selfdrive/monitoring/test_monitoring.py | 8 +++---- 2 files changed, 24 insertions(+), 13 deletions(-) diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index 0b54504b6..19d169da4 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -1,4 +1,4 @@ -from math import atan2 +from math import atan2, radians import numpy as np from cereal import car, log @@ -43,6 +43,9 @@ class DRIVER_MONITOR_SETTINGS: 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 @@ -59,7 +62,6 @@ class DRIVER_MONITOR_SETTINGS: 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 = 11 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 @@ -101,6 +103,7 @@ class DriverPose: 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): @@ -238,7 +241,11 @@ class DriverMonitoring: 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 - yaw_error = abs(yaw_error) + + 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 @@ -254,7 +261,7 @@ class DriverMonitoring: return distracted_types - def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False): + 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 @@ -277,8 +284,11 @@ class DriverMonitoring: 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] @@ -360,19 +370,19 @@ class DriverMonitoring: if self.awareness > self.threshold_prompt: return + _reaching_pre = self.awareness - self.step_change <= self.threshold_pre _reaching_audible = self.awareness - self.step_change <= self.threshold_prompt _reaching_terminal = self.awareness - self.step_change <= 0 - standstill_orange_exemption = standstill and _reaching_audible + standstill_exemption = standstill and _reaching_pre always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal - always_on_lowspeed_exemption = always_on_valid and not op_engaged and car_speed < self.settings._ALWAYS_ON_ALERT_MIN_SPEED 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 (lowspeed for always-on) and reaching orange + # 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 or (always_on_lowspeed_exemption and _reaching_audible)): + if not (standstill_exemption or always_on_red_exemption): self.awareness = max(self.awareness - self.step_change, -0.1) alert = None @@ -385,7 +395,7 @@ class DriverMonitoring: elif self.awareness <= self.threshold_prompt: # prompt orange alert alert = EventName.promptDriverDistracted if self.active_monitoring_mode else EventName.promptDriverUnresponsive - elif self.awareness <= self.threshold_pre and not always_on_lowspeed_exemption: + elif self.awareness <= self.threshold_pre: # pre green alert alert = EventName.preDriverDistracted if self.active_monitoring_mode else EventName.preDriverUnresponsive @@ -451,6 +461,7 @@ class DriverMonitoring: op_engaged=enabled, standstill=standstill, demo_mode=demo, + steering_angle_deg=sm['carState'].steeringAngleDeg, ) # Update distraction events diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 6ea9b8028..15eb2c860 100644 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -186,10 +186,10 @@ class TestMonitoring: standstill_vector = always_true[:] standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON) events, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector) - assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)].names[0] == \ - EventName.preDriverDistracted - assert events[int((_redlight_time-0.1)/DT_DMON)].names[0] == EventName.preDriverDistracted - assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.promptDriverDistracted + 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.preDriverDistracted + assert events[int((_redlight_time+_pre_to_prompt+0.5)/DT_DMON)].names[0] == EventName.promptDriverDistracted # engaged, model is somehow uncertain and driver is distracted # - should fall back to wheel touch after uncertain alert