diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 0da7f3396..fe51af1c6 100644 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -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() diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index b9c0d33c0..70945f8cd 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -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 diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 6ea9b8028..46fe72b12 100644 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -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 -