diff --git a/cereal b/cereal index 8669b82f5f..7fc82bbc06 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 8669b82f5fbe0b21a51e018ead0d6d20cb090e0a +Subproject commit 7fc82bbc06900ff723992af4d7add10dc3afc0f5 diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index b5c86c3ff8..5d13d1897c 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -79,6 +79,7 @@ def dmonitoringd_thread(sm=None, pm=None): "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, "isActiveMode": driver_status.active_monitoring_mode, + "isRHD": driver_status.wheel_on_right, } pm.send('driverMonitoringState', dat) diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index d48aa5a6c3..93166caa7b 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -59,6 +59,9 @@ class DRIVER_MONITOR_SETTINGS(): self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative self._POSE_OFFSET_MAX_COUNT = int(360 / self._DT_DMON) # stop deweighting new data after 6 min, aka "short term memory" + self._WHEELPOS_THRESHOLD = 0.5 + self._WHEELPOS_FILTER_MIN_COUNT = int(5 / self._DT_DMON) + self._RECOVERY_FACTOR_MAX = 5. # relative to minus step change self._RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change @@ -120,6 +123,7 @@ class DriverStatus(): self.settings = settings # init driver status + self.wheelpos_learner = RunningStatFilter() self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT) self.pose_calibrated = False self.blink = DriverBlink() @@ -136,6 +140,7 @@ class DriverStatus(): self.distracted_types = [] self.driver_distracted = False self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON) + self.wheel_on_right = False self.face_detected = False self.terminal_alert_cnt = 0 self.terminal_time = 0 @@ -229,7 +234,14 @@ class DriverStatus(): self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD def update_states(self, driver_state, cal_rpy, car_speed, op_engaged): - driver_data = driver_state.driverDataLH # TODO: use wheelOnRight + rhd_pred = driver_state.wheelOnRight + if car_speed > 0.01: + self.wheelpos_learner.push_and_update(rhd_pred) + if self.wheelpos_learner.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT: + self.wheel_on_right = self.wheelpos_learner.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD + else: + self.wheel_on_right = rhd_pred > self.settings._WHEELPOS_THRESHOLD + driver_data = driver_state.driverDataRH if self.wheel_on_right else driver_state.driverDataLH if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition, driver_data.faceOrientationStd, driver_data.facePositionStd, driver_data.readyProb, driver_data.notReadyProb)):