calibrationd: convert existing calibration to numpy (#23494)
* calibrationd: convert existing calibration to numpy * use numpy functionslocal_plotjuggler
parent
35ec6ac1cb
commit
8444f8267d
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
||||
|
||||
|
|
Loading…
Reference in New Issue