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

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
-    if param_put:
-      calibration_params = Params().get("CalibrationParams")
-    else:
-      calibration_params = None
-    if calibration_params:
+    # Read saved calibration
+    calibration_params = Params().get("CalibrationParams")
+    if param_put and 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 = False
-    if start_status == Calibration.UNCALIBRATED and end_status != Calibration.UNCALIBRATED:
-      self.just_calibrated = True
+    self.just_calibrated = start_status == Calibration.UNCALIBRATED and self.cal_status != Calibration.UNCALIBRATED
 
   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()
-
-    # 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
+    sm.update(100)
 
     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'].transStd,
-                                          sm['cameraOdometry'].rotStd)
+                                           sm['cameraOdometry'].rot,
+                                           sm['cameraOdometry'].transStd,
+                                           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
-      if self.carstate_counter % CARSTATE_DECIMATION == 0:
-        self.steering_angle = msg.steeringAngle
-        self.steering_pressed = msg.steeringPressed
-        self.speed = msg.vEgo
+      self.steering_angle = msg.steeringAngle
+      self.steering_pressed = msg.steeringPressed
+      self.speed = msg.vEgo
 
-        in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
-        self.active = self.speed > 5 and in_linear_region
+      in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
+      self.active = self.speed > 5 and in_linear_region
 
-        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.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
+      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.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:
-        continue
-      t = sm.logMonoTime[which] * 1e-9
-      learner.handle_log(t, which, sm[which])
+      if updated:
+        t = sm.logMonoTime[which] * 1e-9
+        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,