From 0fad46457c2063387c66052040754e30d2e2c02f Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Sun, 30 Aug 2020 18:41:02 -0700
Subject: [PATCH] fix wasted cycles in locationd and dmonitoringd

---
 cereal                               |  2 +-
 selfdrive/locationd/locationd.py     |  2 +-
 selfdrive/monitoring/dmonitoringd.py | 63 ++++++++++++++--------------
 3 files changed, 33 insertions(+), 34 deletions(-)

diff --git a/cereal b/cereal
index 1538183eaa..0a13413f41 160000
--- a/cereal
+++ b/cereal
@@ -1 +1 @@
-Subproject commit 1538183eaa3c9a22a3e675182d6e1cc8e23ad574
+Subproject commit 0a13413f41d54295f618f497e5434fa64ee53313
diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py
index be78176844..26c39f6426 100755
--- a/selfdrive/locationd/locationd.py
+++ b/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]:
diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py
index e93964a3a5..3c54657a12 100755
--- a/selfdrive/monitoring/dmonitoringd.py
+++ b/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()
-      driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['carState'].cruiseState.enabled)
-
-      # Block engaging after max number of distrations
-      if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
-        events.add(car.CarEvent.EventName.tooDistracted)
-
-      # Update events from driver state
-      driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill)
-
-      # build dMonitoringState packet
-      dat = messaging.new_message('dMonitoringState')
-      dat.dMonitoringState = {
-        "events": events.to_msg(),
-        "faceDetected": driver_status.face_detected,
-        "isDistracted": driver_status.driver_distracted,
-        "awarenessStatus": driver_status.awareness,
-        "isRHD": driver_status.is_rhd_region,
-        "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(),
-        "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n,
-        "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(),
-        "poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n,
-        "stepChange": driver_status.step_change,
-        "awarenessActive": driver_status.awareness_active,
-        "awarenessPassive": driver_status.awareness_passive,
-        "isLowStd": driver_status.pose.low_std,
-        "hiStdCount": driver_status.hi_stds,
-        "isPreview": offroad,
-      }
-      pm.send('dMonitoringState', dat)
+    events = Events()
+    driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['carState'].cruiseState.enabled)
+
+    # Block engaging after max number of distrations
+    if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
+      events.add(car.CarEvent.EventName.tooDistracted)
+
+    # Update events from driver state
+    driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill)
+
+    # build dMonitoringState packet
+    dat = messaging.new_message('dMonitoringState')
+    dat.dMonitoringState = {
+      "events": events.to_msg(),
+      "faceDetected": driver_status.face_detected,
+      "isDistracted": driver_status.driver_distracted,
+      "awarenessStatus": driver_status.awareness,
+      "isRHD": driver_status.is_rhd_region,
+      "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(),
+      "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n,
+      "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(),
+      "poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n,
+      "stepChange": driver_status.step_change,
+      "awarenessActive": driver_status.awareness_active,
+      "awarenessPassive": driver_status.awareness_passive,
+      "isLowStd": driver_status.pose.low_std,
+      "hiStdCount": driver_status.hi_stds,
+      "isPreview": offroad,
+    }
+    pm.send('dMonitoringState', dat)
 
 def main(sm=None, pm=None):
   dmonitoringd_thread(sm, pm)