Calibrationd: unit tests (#27988)

* Add calibrationd unit tests

* more tests

selfdrive/locationd/test/test_calibrationd.py

@@ -6,7 +6,7 @@ import numpy as np
 
 import cereal.messaging as messaging
 from common.params import Params
-from selfdrive.locationd.calibrationd import Calibrator
+from selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, MAX_YAW_RATE_FILTER
 
 
 class TestCalibrationd(unittest.TestCase):
@@ -22,5 +22,78 @@ class TestCalibrationd(unittest.TestCase):
     self.assertEqual(msg.liveCalibration.validBlocks, c.valid_blocks)
 
 
+  def test_calibration_basics(self):
+    c = Calibrator(param_put=False)
+    for _ in range(BLOCK_SIZE * INPUTS_WANTED):
+      c.handle_v_ego(MIN_SPEED_FILTER + 1)
+      c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [1e-3, 1e-3, 1e-3])
+    self.assertEqual(c.valid_blocks, INPUTS_WANTED)
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+    c.reset()
+
+  def test_calibration_low_speed_reject(self):
+    c = Calibrator(param_put=False)
+    for _ in range(BLOCK_SIZE * INPUTS_WANTED):
+      c.handle_v_ego(MIN_SPEED_FILTER - 1)
+      c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [1e-3, 1e-3, 1e-3])
+    for _ in range(BLOCK_SIZE * INPUTS_WANTED):
+      c.handle_v_ego(MIN_SPEED_FILTER + 1)
+      c. handle_cam_odom([MIN_SPEED_FILTER - 1, 0.0, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [1e-3, 1e-3, 1e-3])
+    self.assertEqual(c.valid_blocks, 0)
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+
+
+  def test_calibration_yaw_rate_reject(self):
+    c = Calibrator(param_put=False)
+    for _ in range(BLOCK_SIZE * INPUTS_WANTED):
+      c.handle_v_ego(MIN_SPEED_FILTER + 1)
+      c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
+                         [0.0, 0.0, MAX_YAW_RATE_FILTER ],
+                         [0.0, 0.0, 0.0],
+                         [1e-3, 1e-3, 1e-3])
+    self.assertEqual(c.valid_blocks, 0)
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+
+  
+  def test_calibration_speed_std_reject(self):
+    c = Calibrator(param_put=False)
+    for _ in range(BLOCK_SIZE * INPUTS_WANTED):
+      c.handle_v_ego(MIN_SPEED_FILTER + 1)
+      c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [1e3, 1e3, 1e3])
+    self.assertEqual(c.valid_blocks, INPUTS_NEEDED)
+    np.testing.assert_allclose(c.rpy, np.zeros(3))
+
+
+  def test_calibration_auto_reset(self):
+    c = Calibrator(param_put=False)
+    for _ in range(BLOCK_SIZE * INPUTS_WANTED):
+      c.handle_v_ego(MIN_SPEED_FILTER + 1)
+      c. handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [1e-3, 1e-3, 1e-3])
+    self.assertEqual(c.valid_blocks, INPUTS_WANTED)
+    np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0])
+    for _ in range(BLOCK_SIZE):
+      c.handle_v_ego(MIN_SPEED_FILTER + 1)
+      c.handle_cam_odom([MIN_SPEED_FILTER + 1, -0.05 * MIN_SPEED_FILTER, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [0.0, 0.0, 0.0],
+                         [1e-3, 1e-3, 1e-3])
+    self.assertEqual(c.valid_blocks, INPUTS_NEEDED)
+    np.testing.assert_allclose(c.rpy, [0.0, 0.0, -0.05], atol=1e-2)
+
 if __name__ == "__main__":
   unittest.main()