|
|
@ -7,12 +7,10 @@ and the image input into the neural network is not corrected for roll. |
|
|
|
''' |
|
|
|
''' |
|
|
|
|
|
|
|
|
|
|
|
import os |
|
|
|
import os |
|
|
|
import capnp |
|
|
|
|
|
|
|
import copy |
|
|
|
import copy |
|
|
|
import json |
|
|
|
|
|
|
|
import numpy as np |
|
|
|
import numpy as np |
|
|
|
import cereal.messaging as messaging |
|
|
|
import cereal.messaging as messaging |
|
|
|
from cereal import car, log |
|
|
|
from cereal import log |
|
|
|
from selfdrive.hardware import TICI |
|
|
|
from selfdrive.hardware import TICI |
|
|
|
from common.params import Params, put_nonblocking |
|
|
|
from common.params import Params, put_nonblocking |
|
|
|
from common.transformations.model import model_height |
|
|
|
from common.transformations.model import model_height |
|
|
@ -68,25 +66,13 @@ class Calibrator(): |
|
|
|
rpy_init = RPY_INIT |
|
|
|
rpy_init = RPY_INIT |
|
|
|
valid_blocks = 0 |
|
|
|
valid_blocks = 0 |
|
|
|
|
|
|
|
|
|
|
|
cached_params = params.get("CarParamsCache") |
|
|
|
|
|
|
|
if cached_params is not None: |
|
|
|
|
|
|
|
CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) |
|
|
|
|
|
|
|
cached_params = car.CarParams.from_bytes(cached_params) |
|
|
|
|
|
|
|
if cached_params.carFingerprint != CP.carFingerprint: |
|
|
|
|
|
|
|
calibration_params = None |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if param_put and calibration_params: |
|
|
|
if param_put and calibration_params: |
|
|
|
try: |
|
|
|
try: |
|
|
|
msg = log.Event.from_bytes(calibration_params) |
|
|
|
msg = log.Event.from_bytes(calibration_params) |
|
|
|
rpy_init = list(msg.liveCalibration.rpyCalib) |
|
|
|
rpy_init = list(msg.liveCalibration.rpyCalib) |
|
|
|
valid_blocks = msg.liveCalibration.validBlocks |
|
|
|
valid_blocks = msg.liveCalibration.validBlocks |
|
|
|
except (ValueError, capnp.lib.capnp.KjException): |
|
|
|
|
|
|
|
# TODO: remove this after next release |
|
|
|
|
|
|
|
calibration_params = json.loads(calibration_params) |
|
|
|
|
|
|
|
rpy_init = calibration_params["calib_radians"] |
|
|
|
|
|
|
|
valid_blocks = calibration_params['valid_blocks'] |
|
|
|
|
|
|
|
except Exception: |
|
|
|
except Exception: |
|
|
|
cloudlog.exception("CalibrationParams file found but error encountered") |
|
|
|
cloudlog.exception("Error reading cached CalibrationParams") |
|
|
|
|
|
|
|
|
|
|
|
self.reset(rpy_init, valid_blocks) |
|
|
|
self.reset(rpy_init, valid_blocks) |
|
|
|
self.update_status() |
|
|
|
self.update_status() |
|
|
|