Reduce paramsd and calibrationd CPU usage (#2119)

* reduce paramsd cpu

* reduce calibrationd cpu usage

* calibration_helpers was mostly unused

* more calibration cleanup

* update refs

* fix thresholds in CPU test
old-commit-hash: e0004d0981

release/files_common

@@ -291,7 +291,6 @@ selfdrive/locationd/models/car_kf.py
 selfdrive/locationd/models/constants.py
 
 selfdrive/locationd/calibrationd.py
-selfdrive/locationd/calibration_helpers.py
 
 selfdrive/logcatd/SConscript
 selfdrive/logcatd/logcatd_android.cc

selfdrive/controls/controlsd.py

@@ -21,7 +21,7 @@ from selfdrive.controls.lib.events import Events, ET
 from selfdrive.controls.lib.alertmanager import AlertManager
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.controls.lib.planner import LON_MPC_STEP
-from selfdrive.locationd.calibration_helpers import Calibration
+from selfdrive.locationd.calibrationd import Calibration
 
 LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
 LANE_DEPARTURE_THRESHOLD = 0.1

selfdrive/controls/lib/events.py

@@ -3,7 +3,7 @@ from functools import total_ordering
 from cereal import log, car
 from common.realtime import DT_CTRL
 from selfdrive.config import Conversions as CV
-from selfdrive.locationd.calibration_helpers import Filter
+from selfdrive.locationd.calibrationd import MIN_SPEED_FILTER
 
 AlertSize = log.ControlsState.AlertSize
 AlertStatus = log.ControlsState.AlertStatus
@@ -184,7 +184,7 @@ def below_steer_speed_alert(CP, sm, metric):
     Priority.MID, VisualAlert.steerRequired, AudibleAlert.none, 0., 0.4, .3)
 
 def calibration_incomplete_alert(CP, sm, metric):
-  speed = int(Filter.MIN_SPEED * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
+  speed = int(MIN_SPEED_FILTER * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
   unit = "km/h" if metric else "mph"
   return Alert(
     "Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc,

selfdrive/locationd/calibration_helpers.py

@@ -1,10 +0,0 @@
-import math
-
-class Filter:
-  MIN_SPEED = 7  # m/s  (~15.5mph)
-  MAX_YAW_RATE = math.radians(3)  # per second
-
-class Calibration:
-  UNCALIBRATED = 0
-  CALIBRATED = 1
-  INVALID = 2

selfdrive/locationd/calibrationd.py

@@ -2,7 +2,7 @@
 '''
 This process finds calibration values. More info on what these calibration values
 are can be found here https://github.com/commaai/openpilot/tree/master/common/transformations
-While the roll calibration is a real value that can be estimated, here we assume it zero,
+While the roll calibration is a real value that can be estimated, here we assume it's zero,
 and the image input into the neural network is not corrected for roll.
 '''
 
@@ -12,7 +12,6 @@ import json
 import numpy as np
 import cereal.messaging as messaging
 from selfdrive.config import Conversions as CV
-from selfdrive.locationd.calibration_helpers import Calibration
 from selfdrive.swaglog import cloudlog
 from common.params import Params, put_nonblocking
 from common.transformations.model import model_height
@@ -34,6 +33,10 @@ PITCH_LIMITS = np.array([-0.09074112085129739, 0.14907572052989657])
 YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235])
 DEBUG = os.getenv("DEBUG") is not None
 
+class Calibration:
+  UNCALIBRATED = 0
+  CALIBRATED = 1
+  INVALID = 2
 
 def is_calibration_valid(rpy):
   return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1])
@@ -47,7 +50,6 @@ def sanity_clip(rpy):
                    np.clip(rpy[2], YAW_LIMITS[0] - .005, YAW_LIMITS[1] + .005)])
 
 
-
 class Calibrator():
   def __init__(self, param_put=False):
     self.param_put = param_put
@@ -60,12 +62,9 @@ class Calibrator():
     self.just_calibrated = False
     self.v_ego = 0
 
-    # Read calibration
+    # Read saved calibration
-    if param_put:
+    calibration_params = Params().get("CalibrationParams")
-      calibration_params = Params().get("CalibrationParams")
+    if param_put and calibration_params:
-    else:
-      calibration_params = None
-    if calibration_params:
       try:
         calibration_params = json.loads(calibration_params)
         self.rpy = calibration_params["calib_radians"]
@@ -85,11 +84,7 @@ class Calibrator():
       self.cal_status = Calibration.UNCALIBRATED
     else:
       self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.rpy) else Calibration.INVALID
-    end_status = self.cal_status
+    self.just_calibrated = start_status == Calibration.UNCALIBRATED and self.cal_status != Calibration.UNCALIBRATED
-
-    self.just_calibrated = False
-    if start_status == Calibration.UNCALIBRATED and end_status != Calibration.UNCALIBRATED:
-      self.just_calibrated = True
 
   def handle_v_ego(self, v_ego):
     self.v_ego = v_ego
@@ -115,6 +110,7 @@ class Calibrator():
         self.rpy = np.mean(self.rpys[:self.valid_blocks], axis=0)
       self.update_status()
 
+      # TODO: this should use the liveCalibration struct from cereal
       if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):
         cal_params = {"calib_radians": list(self.rpy),
                       "valid_blocks": self.valid_blocks}
@@ -145,37 +141,29 @@ class Calibrator():
 
 def calibrationd_thread(sm=None, pm=None):
   if sm is None:
-    sm = messaging.SubMaster(['cameraOdometry', 'carState'])
+    sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll=['cameraOdometry'])
 
   if pm is None:
     pm = messaging.PubMaster(['liveCalibration'])
 
   calibrator = Calibrator(param_put=True)
 
-  send_counter = 0
   while 1:
-    sm.update()
+    sm.update(100)
-
-    # if no inputs still publish calibration
-    if not sm.updated['carState'] and not sm.updated['cameraOdometry']:
-      calibrator.send_data(pm)
-      continue
-
-    if sm.updated['carState']:
-      calibrator.handle_v_ego(sm['carState'].vEgo)
-      if send_counter % 25 == 0:
-        calibrator.send_data(pm)
-      send_counter += 1
 
     if sm.updated['cameraOdometry']:
+      calibrator.handle_v_ego(sm['carState'].vEgo)
       new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
-                                          sm['cameraOdometry'].rot,
+                                           sm['cameraOdometry'].rot,
-                                          sm['cameraOdometry'].transStd,
+                                           sm['cameraOdometry'].transStd,
-                                          sm['cameraOdometry'].rotStd)
+                                           sm['cameraOdometry'].rotStd)
 
       if DEBUG and new_rpy is not None:
         print('got new rpy', new_rpy)
 
+    # 4Hz driven by cameraOdometry
+    if sm.frame % 5 == 0:
+      calibrator.send_data(pm)
 
 def main(sm=None, pm=None):
   calibrationd_thread(sm, pm)

selfdrive/locationd/paramsd.py

@@ -13,8 +13,6 @@ from selfdrive.swaglog import cloudlog
 
 KalmanStatus = log.LiveLocationKalman.Status
 
-CARSTATE_DECIMATION = 5
-
 
 class ParamsLearner:
   def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
@@ -32,7 +30,6 @@ class ParamsLearner:
     self.speed = 0
     self.steering_pressed = False
     self.steering_angle = 0
-    self.carstate_counter = 0
 
     self.valid = True
 
@@ -51,18 +48,16 @@ class ParamsLearner:
         self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
 
     elif which == 'carState':
-      self.carstate_counter += 1
+      self.steering_angle = msg.steeringAngle
-      if self.carstate_counter % CARSTATE_DECIMATION == 0:
+      self.steering_pressed = msg.steeringPressed
-        self.steering_angle = msg.steeringAngle
+      self.speed = msg.vEgo
-        self.steering_pressed = msg.steeringPressed
-        self.speed = msg.vEgo
 
-        in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
+      in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
-        self.active = self.speed > 5 and in_linear_region
+      self.active = self.speed > 5 and in_linear_region
 
-        if self.active:
+      if self.active:
-          self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]]))
+        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]]))
-          self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
+        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
 
     if not self.active:
       # Reset time when stopped so uncertainty doesn't grow
@@ -72,7 +67,7 @@ class ParamsLearner:
 
 def main(sm=None, pm=None):
   if sm is None:
-    sm = messaging.SubMaster(['liveLocationKalman', 'carState'])
+    sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman'])
   if pm is None:
     pm = messaging.PubMaster(['liveParameters'])
 
@@ -111,12 +106,11 @@ def main(sm=None, pm=None):
     sm.update()
 
     for which, updated in sm.updated.items():
-      if not updated:
+      if updated:
-        continue
+        t = sm.logMonoTime[which] * 1e-9
-      t = sm.logMonoTime[which] * 1e-9
+        learner.handle_log(t, which, sm[which])
-      learner.handle_log(t, which, sm[which])
 
-    if sm.updated['carState'] and (learner.carstate_counter % CARSTATE_DECIMATION == 0):
+    if sm.updated['liveLocationKalman']:
       msg = messaging.new_message('liveParameters')
       msg.logMonoTime = sm.logMonoTime['carState']
 
@@ -135,7 +129,7 @@ def main(sm=None, pm=None):
         min_sr <= msg.liveParameters.steerRatio <= max_sr,
       ))
 
-      if learner.carstate_counter % 6000 == 0:   # once a minute
+      if sm.frame % 1200 == 0:  # once a minute
         params = {
           'carFingerprint': CP.carFingerprint,
           'steerRatio': msg.liveParameters.steerRatio,

selfdrive/monitoring/dmonitoringd.py

@@ -6,7 +6,7 @@ from common.params import Params
 import cereal.messaging as messaging
 from selfdrive.controls.lib.events import Events
 from selfdrive.monitoring.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION
-from selfdrive.locationd.calibration_helpers import Calibration
+from selfdrive.locationd.calibrationd import Calibration
 
 
 def dmonitoringd_thread(sm=None, pm=None):

selfdrive/test/process_replay/process_replay.py

@@ -201,16 +201,10 @@ def calibration_rcv_callback(msg, CP, cfg, fsm):
   # calibrationd publishes 1 calibrationData every 5 cameraOdometry packets.
   # should_recv always true to increment frame
   recv_socks = []
-  if msg.which() == 'carState' and ((fsm.frame + 1) % 25) == 0:
+  frame = fsm.frame + 1 # incrementing hasn't happened yet in SubMaster
+  if frame == 0 or (msg.which() == 'cameraOdometry' and (frame % 5) == 0):
     recv_socks = ["liveCalibration"]
-  return recv_socks, msg.which() == 'carState'
+  return recv_socks, fsm.frame == 0 or msg.which() == 'cameraOdometry'
-
-def paramsd_rcv_callback(msg, CP, cfg, fsm):
-  recv_socks = []
-  if msg.which() == 'carState' and ((fsm.frame + 2) % 5) == 0:
-    recv_socks = ["liveParameters"]
-  return recv_socks, msg.which() == 'carState'
-
 
 CONFIGS = [
   ProcessConfig(
@@ -283,12 +277,12 @@ CONFIGS = [
   ProcessConfig(
     proc_name="paramsd",
     pub_sub={
-      "carState": ["liveParameters"],
+      "liveLocationKalman": ["liveParameters"],
-      "liveLocationKalman": []
+      "carState": []
     },
     ignore=["logMonoTime", "valid"],
     init_callback=get_car_params,
-    should_recv_callback=paramsd_rcv_callback,
+    should_recv_callback=None,
     tolerance=NUMPY_TOLERANCE,
   ),
 ]

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-b47257005e7408737c66463c45f5db36153a849d
+5c2c99c4572d68f17ffa3a33acf8e4e205ca94c2

selfdrive/test/test_cpu_usage.py

@@ -19,17 +19,17 @@ def print_cpu_usage(first_proc, last_proc):
     ("./loggerd", 33.90),
     ("selfdrive.locationd.locationd", 29.5),
     ("selfdrive.controls.plannerd", 11.84),
-    ("selfdrive.locationd.paramsd", 11.53),
+    ("selfdrive.locationd.paramsd", 10.5),
     ("./_modeld", 7.12),
     ("selfdrive.controls.radard", 9.54),
     ("./camerad", 7.07),
     ("./_sensord", 6.17),
-    ("selfdrive.locationd.calibrationd", 6.0),
     ("./_ui", 5.82),
     ("./boardd", 3.63),
     ("./_dmonitoringmodeld", 2.67),
     ("selfdrive.logmessaged", 2.71),
     ("selfdrive.thermald.thermald", 2.41),
+    ("selfdrive.locationd.calibrationd", 2.0),
     ("selfdrive.monitoring.dmonitoringd", 1.90),
     ("./proclogd", 1.54),
     ("./_gpsd", 0.09),