diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 174100517c..51c84b847d 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -12,7 +12,7 @@ import capnp import numpy as np from typing import List, NoReturn, Optional -from cereal import log +from cereal import car, log import cereal.messaging as messaging from common.conversions import Conversions as CV from common.params import Params, put_nonblocking @@ -63,6 +63,8 @@ class Calibrator: def __init__(self, param_put: bool = False): self.param_put = param_put + self.CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) + # Read saved calibration params = Params() calibration_params = params.get("CalibrationParams") @@ -182,12 +184,24 @@ class Calibrator: extrinsic_matrix = get_view_frame_from_road_frame(0, smooth_rpy[1], smooth_rpy[2], model_height) msg = messaging.new_message('liveCalibration') - msg.liveCalibration.validBlocks = self.valid_blocks - msg.liveCalibration.calStatus = self.cal_status - msg.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) - msg.liveCalibration.extrinsicMatrix = extrinsic_matrix.flatten().tolist() - msg.liveCalibration.rpyCalib = smooth_rpy.tolist() - msg.liveCalibration.rpyCalibSpread = self.calib_spread.tolist() + liveCalibration = msg.liveCalibration + + liveCalibration.validBlocks = self.valid_blocks + liveCalibration.calStatus = self.cal_status + liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) + liveCalibration.extrinsicMatrix = extrinsic_matrix.flatten().tolist() + liveCalibration.rpyCalib = smooth_rpy.tolist() + liveCalibration.rpyCalibSpread = self.calib_spread.tolist() + + if self.CP.notCar: + extrinsic_matrix = get_view_frame_from_road_frame(0, 0, 0, model_height) + liveCalibration.validBlocks = INPUTS_NEEDED + liveCalibration.calStatus = Calibration.CALIBRATED + liveCalibration.calPerc = 100. + liveCalibration.extrinsicMatrix = extrinsic_matrix.flatten().tolist() + liveCalibration.rpyCalib = [0, 0, 0] + liveCalibration.rpyCalibSpread = self.calib_spread.tolist() + return msg def send_data(self, pm: messaging.PubMaster) -> None: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index efc74e2102..b9e38e7ae8 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f7a6dd296e4747595ec4ef453c58800a88128be7 \ No newline at end of file +f6c93d48d14fa7be45961f7325a7957f53126671 \ No newline at end of file