fix wasted cycles in locationd and dmonitoringd

cereal

@@ -1 +1 @@
-Subproject commit 1538183eaa3c9a22a3e675182d6e1cc8e23ad574
+Subproject commit 0a13413f41d54295f618f497e5434fa64ee53313

selfdrive/locationd/locationd.py

@@ -289,7 +289,7 @@ def locationd_thread(sm, pm, disabled_logs=None):
   localizer = Localizer(disabled_logs=disabled_logs)
 
   while True:
-    sm.update()
+    sm.update(wait_for="cameraOdometry")
 
     for sock, updated in sm.updated.items():
       if updated and sm.valid[sock]:

selfdrive/monitoring/dmonitoringd.py

@@ -40,7 +40,7 @@ def dmonitoringd_thread(sm=None, pm=None):
 
   # 10Hz <- dmonitoringmodeld
   while True:
-    sm.update()
+    sm.update(wait_for="driverState")
 
     # Get interaction
     if sm.updated['carState']:
@@ -57,37 +57,36 @@ def dmonitoringd_thread(sm=None, pm=None):
       driver_status.set_policy(sm['model'])
 
     # Get data from dmonitoringmodeld
-    if sm.updated['driverState']:
+    events = Events()
-      events = Events()
+    driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['carState'].cruiseState.enabled)
-      driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['carState'].cruiseState.enabled)
+
-
+    # Block engaging after max number of distrations
-      # Block engaging after max number of distrations
+    if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
-      if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
+      events.add(car.CarEvent.EventName.tooDistracted)
-        events.add(car.CarEvent.EventName.tooDistracted)
+
-
+    # Update events from driver state
-      # Update events from driver state
+    driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill)
-      driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill)
+
-
+    # build dMonitoringState packet
-      # build dMonitoringState packet
+    dat = messaging.new_message('dMonitoringState')
-      dat = messaging.new_message('dMonitoringState')
+    dat.dMonitoringState = {
-      dat.dMonitoringState = {
+      "events": events.to_msg(),
-        "events": events.to_msg(),
+      "faceDetected": driver_status.face_detected,
-        "faceDetected": driver_status.face_detected,
+      "isDistracted": driver_status.driver_distracted,
-        "isDistracted": driver_status.driver_distracted,
+      "awarenessStatus": driver_status.awareness,
-        "awarenessStatus": driver_status.awareness,
+      "isRHD": driver_status.is_rhd_region,
-        "isRHD": driver_status.is_rhd_region,
+      "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(),
-        "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(),
+      "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n,
-        "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n,
+      "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(),
-        "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(),
+      "poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n,
-        "poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n,
+      "stepChange": driver_status.step_change,
-        "stepChange": driver_status.step_change,
+      "awarenessActive": driver_status.awareness_active,
-        "awarenessActive": driver_status.awareness_active,
+      "awarenessPassive": driver_status.awareness_passive,
-        "awarenessPassive": driver_status.awareness_passive,
+      "isLowStd": driver_status.pose.low_std,
-        "isLowStd": driver_status.pose.low_std,
+      "hiStdCount": driver_status.hi_stds,
-        "hiStdCount": driver_status.hi_stds,
+      "isPreview": offroad,
-        "isPreview": offroad,
+    }
-      }
+    pm.send('dMonitoringState', dat)
-      pm.send('dMonitoringState', dat)
 
 def main(sm=None, pm=None):
   dmonitoringd_thread(sm, pm)