Reduce plannerd and dmonitoringd CPU usage (#2108)

* fix wasted cycles in locationd and dmonitoringd

* bump cereal

* calibrationd

* Revert "calibrationd"

This reverts commit 580051a33bc96e1e83300629e9c04817928d0851.

* this is cleaner

* calibrationd

* paramsd is driven by liveLocationKalman

* only log params once a minute

* stricter CPU usage test

* revert locationd for now

* revert calibrationd too

* revert thresholds

cereal

@@ -1 +1 @@
-Subproject commit 1538183eaa3c9a22a3e675182d6e1cc8e23ad574
+Subproject commit 626679d1a56c689cbad7bf99a08c5ecbf9d073fb

selfdrive/controls/plannerd.py

@@ -27,7 +27,8 @@ def plannerd_thread(sm=None, pm=None):
   VM = VehicleModel(CP)
 
   if sm is None:
-    sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'])
+    sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'],
+                             poll=['radarState', 'model'])
 
   if pm is None:
     pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc'])

selfdrive/monitoring/dmonitoringd.py

@@ -18,7 +18,7 @@ def dmonitoringd_thread(sm=None, pm=None):
     pm = messaging.PubMaster(['dMonitoringState'])
 
   if sm is None:
-    sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'model'])
+    sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'model'], poll=['driverState'])
 
   driver_status = DriverStatus()
   driver_status.is_rhd_region = Params().get("IsRHD") == b"1"
@@ -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)

selfdrive/test/test_cpu_usage.py

@@ -16,21 +16,21 @@ def cputime_total(ct):
 def print_cpu_usage(first_proc, last_proc):
   procs = [
     ("selfdrive.controls.controlsd", 66.15),
-    ("selfdrive.locationd.locationd", 34.38),
     ("./loggerd", 33.90),
-    ("selfdrive.controls.plannerd", 19.77),
-    ("./_modeld", 12.74),
+    ("selfdrive.locationd.locationd", 29.5),
+    ("selfdrive.controls.plannerd", 11.84),
     ("selfdrive.locationd.paramsd", 11.53),
+    ("./_modeld", 7.12),
     ("selfdrive.controls.radard", 9.54),
     ("./_ui", 9.54),
     ("./camerad", 7.07),
-    ("selfdrive.locationd.calibrationd", 6.81),
     ("./_sensord", 6.17),
-    ("selfdrive.monitoring.dmonitoringd", 5.48),
+    ("selfdrive.locationd.calibrationd", 6.0),
     ("./boardd", 3.63),
     ("./_dmonitoringmodeld", 2.67),
     ("selfdrive.logmessaged", 2.71),
     ("selfdrive.thermald.thermald", 2.41),
+    ("selfdrive.monitoring.dmonitoringd", 1.90),
     ("./proclogd", 1.54),
     ("./_gpsd", 0.09),
     ("./clocksd", 0.02),
@@ -51,7 +51,7 @@ def print_cpu_usage(first_proc, last_proc):
       if cpu_usage > max(normal_cpu_usage * 1.1, normal_cpu_usage + 5.0):
         result += f"Warning {proc_name} using more CPU than normal\n"
         r = False
-      elif cpu_usage < min(normal_cpu_usage * 0.3, max(normal_cpu_usage - 1.0, 0.0)):
+      elif cpu_usage < min(normal_cpu_usage * 0.65, max(normal_cpu_usage - 1.0, 0.0)):
         result += f"Warning {proc_name} using less CPU than normal\n"
         r = False
       result += f"{proc_name.ljust(35)}  {cpu_usage:.2f}%\n"