IQ.Pilot Release Commit @ 24db8ae

iqpilot/selfdrive/monitoring/dmonitoringd.py

@@ -13,7 +13,8 @@ def dmonitoringd_thread():
   sm = messaging.SubMaster(['driverStateV2', 'extrinsicsCalibration', 'carState', 'selfdriveState', 'modelV2',
                             'carControl'], poll='driverStateV2')
 
-  DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM"))
+  DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM"),
+                        force_rhd=params.get_bool("ForceRHDForBSM"))
   demo_mode=False
 
   # 20Hz <- dmonitoringmodeld
@@ -36,10 +37,11 @@ def dmonitoringd_thread():
     # load live always-on toggle
     if sm['driverStateV2'].frameId % 40 == 1:
       DM.always_on = params.get_bool("AlwaysOnDM")
+      DM.force_rhd = params.get_bool("ForceRHDForBSM")
       demo_mode = params.get_bool("IsDriverViewEnabled")
 
     # save rhd virtual toggle every 5 mins
-    if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and
+    if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and not DM.force_rhd and
      DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and
      DM.wheel_on_right == (DM.wheelpos.prob_offseter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)):
       params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right)

iqpilot/selfdrive/monitoring/helpers.py

@@ -145,7 +145,7 @@ def face_orientation_from_net(angles_desc, pos_desc, rpy_calib):
 
 
 class DriverMonitoring:
-  def __init__(self, rhd_saved=False, settings=None, always_on=False):
+  def __init__(self, rhd_saved=False, settings=None, always_on=False, force_rhd=False):
     # init policy settings
     self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type())
 
@@ -158,6 +158,7 @@ class DriverMonitoring:
     self.blink = DriverBlink()
 
     self.always_on = always_on
+    self.force_rhd = force_rhd
     self.distracted_types = []
     self.driver_distracted = False
     self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON)
@@ -268,18 +269,20 @@ class DriverMonitoring:
   def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False):
     rhd_pred = driver_state.wheelOnRightProb
     # calibrates only when there's movement and either face detected
-    if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or
+    if not self.force_rhd and car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or
                                           driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD):
       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
 
-    if self.wheelpos.prob_calibrated or demo_mode:
+    if self.force_rhd:
+      self.wheel_on_right = True
+    elif self.wheelpos.prob_calibrated or demo_mode:
       self.wheel_on_right = self.wheelpos.prob_offseter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD
     else:
       self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished
     # make sure no switching when engaged
-    if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode:
+    if not self.force_rhd and op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode:
       self.wheel_on_right = self.wheel_on_right_last
     driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData
     if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition,

iqpilot/selfdrive/monitoring/test_monitoring.py

@@ -247,6 +247,22 @@ class TestMonitoring:
     dm._update_states(ds, [0., 0., 0.], 20.0, True, False)
     assert dm.wheel_on_right
 
+  def test_state_update_forces_rhd(self):
+    dm = DriverMonitoring(force_rhd=True)
+    dm.wheel_on_right_last = False
+    dm.wheelpos.prob_offseter.filtered_stat.M = 0.0
+    dm.wheelpos.prob_offseter.filtered_stat.n = dm.settings._WHEELPOS_FILTER_MIN_COUNT + 1
+    ds = log.DriverStateV2.new_message()
+    ds.wheelOnRightProb = 0.0
+    set_driver_data(ds.rightDriverData)
+
+    dm._update_states(ds, [0., 0., 0.], 20.0, True, False)
+
+    assert dm.wheel_on_right
+    assert dm.face_detected
+    assert dm.wheelpos.prob_offseter.filtered_stat.n == dm.settings._WHEELPOS_FILTER_MIN_COUNT + 1
+    assert dm.get_state_packet().driverMonitoringState.isRHD
+
   def test_state_update_rejects_incomplete_driver_data(self):
     dm = DriverMonitoring()
     dm._update_states(log.DriverStateV2.new_message(), [0., 0., 0.], 0.0, False, False)