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

Monitoring left hand

Browse Source
This commit is contained in:
firestar5683
2026-03-22 22:17:30 -05:00
parent e4a1c33f35
commit 8258b6db84
3 changed files with 49 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/monitoring/dmonitoringd.py
+10 -10
View File
@@ -48,16 +48,16 @@ def dmonitoringd_thread():
DM.always_on = params.get_bool("AlwaysOnDM")
demo_mode = params.get_bool("IsDriverViewEnabled") and sm["carState"].gearShifter != GearShifter.reverse
# save rhd virtual toggle every 5 mins, but only with clear confidence.
if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and
DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT):
wheelpos_mean = DM.wheelpos.prob_offseter.filtered_stat.M
save_rhd = DM.settings._WHEELPOS_THRESHOLD_ENTER_RHD + DM.settings._WHEELPOS_SAVE_MARGIN
save_lhd = DM.settings._WHEELPOS_THRESHOLD_ENTER_LHD - DM.settings._WHEELPOS_SAVE_MARGIN
if wheelpos_mean >= save_rhd:
params.put_bool_nonblocking("IsRhdDetected", True)
elif wheelpos_mean <= save_lhd:
params.put_bool_nonblocking("IsRhdDetected", False)
# save rhd virtual toggle every 5 mins, but only with clear confidence.
if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and
DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT):
wheelpos_mean = DM.wheelpos.prob_offseter.filtered_stat.M
save_rhd = DM.settings._WHEELPOS_THRESHOLD_ENTER_RHD + DM.settings._WHEELPOS_SAVE_MARGIN
save_lhd = DM.settings._WHEELPOS_THRESHOLD_ENTER_LHD - DM.settings._WHEELPOS_SAVE_MARGIN
if wheelpos_mean >= save_rhd:
params.put_bool_nonblocking("IsRhdDetected", True)
elif wheelpos_mean <= save_lhd:
params.put_bool_nonblocking("IsRhdDetected", False)
def main():
dmonitoringd_thread()
selfdrive/monitoring/helpers.py
+12
View File
@@ -73,6 +73,8 @@ class DRIVER_MONITOR_SETTINGS:
self._WHEELPOS_THRESHOLD_ENTER_RHD = self._WHEELPOS_THRESHOLD
self._WHEELPOS_THRESHOLD_ENTER_LHD = self._WHEELPOS_THRESHOLD
self._WHEELPOS_SAVE_MARGIN = 0.0
self._WHEELPOS_STARTUP_OVERRIDE_RHD = 0.55
self._WHEELPOS_STARTUP_OVERRIDE_LHD = 0.45
# C4 (mici) has shown borderline wheel-side probabilities around 0.5x.
# Use hysteresis and stricter persistence thresholds to avoid false RHD latching.
@@ -272,6 +274,14 @@ class DriverMonitoring:
self.wheelpos.prob_offseter.push_and_update(rhd_pred)
self.wheelpos.prob_calibrated = self.wheelpos.prob_offseter.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT
startup_override = None
if not self.wheelpos.prob_calibrated and not demo_mode and not op_engaged:
left_face_detected = driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD
right_face_detected = driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD
if rhd_pred <= self.settings._WHEELPOS_STARTUP_OVERRIDE_LHD and left_face_detected and not right_face_detected:
startup_override = False
elif rhd_pred >= self.settings._WHEELPOS_STARTUP_OVERRIDE_RHD and right_face_detected and not left_face_detected:
startup_override = True
if self.wheelpos.prob_calibrated or demo_mode:
wheelpos_mean = self.wheelpos.prob_offseter.filtered_stat.M
@@ -290,6 +300,8 @@ class DriverMonitoring:
self.wheel_on_right = wheelpos_mean > enter_lhd
else:
self.wheel_on_right = wheelpos_mean >= enter_rhd
elif startup_override is not None:
self.wheel_on_right = startup_override
else:
self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished
# make sure no switching when engaged
selfdrive/monitoring/test_monitoring.py
+27 -1
View File
@@ -30,6 +30,23 @@ def make_msg(face_detected, distracted=False, model_uncertain=False):
return ds
def make_dual_msg(left_face_prob, right_face_prob, wheel_on_right_prob=0., model_uncertain=False):
ds = log.DriverStateV2.new_message()
ds.wheelOnRightProb = wheel_on_right_prob
for side, face_prob in ((ds.leftDriverData, left_face_prob), (ds.rightDriverData, right_face_prob)):
side.faceOrientation = [0., 0., 0.]
side.facePosition = [0., 0.]
side.faceProb = face_prob
side.leftEyeProb = 1.
side.rightEyeProb = 1.
side.leftBlinkProb = 0.
side.rightBlinkProb = 0.
side.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain]
side.facePositionStd = [1.*model_uncertain, 1.*model_uncertain]
side.phoneProb = 0.
return ds
# driver state from neural net, 10Hz
msg_NO_FACE_DETECTED = make_msg(False)
msg_ATTENTIVE = make_msg(True)
@@ -71,6 +88,16 @@ class TestMonitoring:
events, _ = self._run_seq(always_attentive, always_false, always_true, always_false)
self._assert_no_events(events)
def test_saved_rhd_recovers_to_lhd_with_strong_left_face(self):
settings = DRIVER_MONITOR_SETTINGS(device_type='mici')
DM = DriverMonitoring(rhd_saved=True, settings=settings)
msg = make_dual_msg(left_face_prob=0.95, right_face_prob=0.2, wheel_on_right_prob=0.35)
DM._update_states(msg, [0, 0, 0], 0, False, False)
assert not DM.wheel_on_right
assert DM.face_detected
# engaged, driver is distracted and does nothing
def test_fully_distracted_driver(self):
events, d_status = self._run_seq(always_distracted, always_false, always_true, always_false)
@@ -203,4 +230,3 @@ class TestMonitoring:
events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names
assert EventName.driverUnresponsive in \
events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names