DM: track RHD predictions (#24947)

* driverview

* auto choose

* useless

* remove

* modeld not use toggle

* remove from params

* Revert "remove from params"

This reverts commit a08df0b492.

* Revert "modeld not use toggle"

This reverts commit 2730bf8f57.

* Revert "remove"

This reverts commit 21f7cfaaee.

* Revert "driverview"

This reverts commit 222d129711.

* semi revert

selfdrive/modeld/models/dmonitoring.cc

@@ -20,7 +20,6 @@ static inline T *get_buffer(std::vector<T> &buf, const size_t size) {
 }
 
 void dmonitoring_init(DMonitoringModelState* s) {
-  s->is_rhd = Params().getBool("IsRHD");
 
 #ifdef USE_ONNX_MODEL
   s->m = new ONNXModel("models/dmonitoring_model.onnx", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME, false, true);

selfdrive/modeld/models/dmonitoring.h

@@ -38,7 +38,6 @@ typedef struct DMonitoringModelResult {
 
 typedef struct DMonitoringModelState {
   RunModel *m;
-  bool is_rhd;
   float output[OUTPUT_SIZE];
   std::vector<uint8_t> net_input_buf;
   float calib[CALIB_LEN];

selfdrive/monitoring/driver_monitor.py

@@ -120,7 +120,7 @@ class DriverStatus():
     self.settings = settings
 
     # init driver status
-    # self.wheelpos_learner = RunningStatFilter()
+    self.wheelpos_learner = RunningStatFilter()
     self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT)
     self.pose_calibrated = False
     self.blink = DriverBlink()
@@ -137,7 +137,8 @@ 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 = rhd
+    self.wheel_on_right = False
+    self.rhd_toggled = rhd
     self.face_detected = False
     self.terminal_alert_cnt = 0
     self.terminal_time = 0
@@ -225,14 +226,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):
-    # rhd_pred = driver_state.wheelOnRightProb
-    # 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.rightDriverData if self.wheel_on_right else driver_state.leftDriverData
+    rhd_pred = driver_state.wheelOnRightProb
+    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.rightDriverData if self.rhd_toggled else driver_state.leftDriverData
     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)):