rhd learner

cereal

@@ -1 +1 @@
-Subproject commit 8669b82f5fbe0b21a51e018ead0d6d20cb090e0a
+Subproject commit 7fc82bbc06900ff723992af4d7add10dc3afc0f5

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)
 

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)):