diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 8c7e14edb2..e7e6d46612 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -20,7 +20,6 @@ static inline T *get_buffer(std::vector &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); diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index 874722cd93..ae2bf05394 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/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 net_input_buf; float calib[CALIB_LEN]; diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index f2d0524422..48d4303aa5 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/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)):