mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-09 19:39:00 +08:00
Code Issues Packages Projects Releases Wiki Activity

DM: fewer alerts during maneuvers (#37751)

Browse Source 
* 2in1

* clip

* drop aodm lowspeed

* cleanup

* add lower bd

* that was random
This commit is contained in:
ZwX1616
2026-04-01 00:40:13 -07:00
committed by GitHub
parent bf43c7e8c7
commit 5dcaf3bef8
2 changed files with 24 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/monitoring/helpers.py
+20 -9
View File
@@ -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
selfdrive/monitoring/test_monitoring.py
+4 -4
View File
@@ -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