diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 1333180e..f2a534d3 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -7,7 +7,6 @@ and the image input into the neural network is not corrected for roll. ''' import os -import copy from typing import NoReturn import numpy as np import cereal.messaging as messaging @@ -70,7 +69,7 @@ class Calibrator(): if param_put and calibration_params: try: msg = log.Event.from_bytes(calibration_params) - rpy_init = list(msg.liveCalibration.rpyCalib) + rpy_init = np.array(msg.liveCalibration.rpyCalib) valid_blocks = msg.liveCalibration.validBlocks except Exception: cloudlog.exception("Error reading cached CalibrationParams") @@ -80,13 +79,15 @@ class Calibrator(): def reset(self, rpy_init=RPY_INIT, valid_blocks=0, smooth_from=None): if not np.isfinite(rpy_init).all(): - self.rpy = copy.copy(RPY_INIT) + self.rpy = RPY_INIT.copy() else: - self.rpy = rpy_init + self.rpy = rpy_init.copy() + if not np.isfinite(valid_blocks) or valid_blocks < 0: self.valid_blocks = 0 else: self.valid_blocks = valid_blocks + self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1)) self.idx = 0 diff --git a/selfdrive/locationd/test/test_calibrationd.py b/selfdrive/locationd/test/test_calibrationd.py index 030636dc..3612f482 100755 --- a/selfdrive/locationd/test/test_calibrationd.py +++ b/selfdrive/locationd/test/test_calibrationd.py @@ -2,6 +2,8 @@ import random import unittest +import numpy as np + import cereal.messaging as messaging from common.params import Params from selfdrive.locationd.calibrationd import Calibrator @@ -16,7 +18,7 @@ class TestCalibrationd(unittest.TestCase): Params().put("CalibrationParams", msg.to_bytes()) c = Calibrator(param_put=True) - self.assertEqual(list(msg.liveCalibration.rpyCalib), c.rpy) + np.testing.assert_allclose(msg.liveCalibration.rpyCalib, c.rpy) self.assertEqual(msg.liveCalibration.validBlocks, c.valid_blocks)