|
|
|
@ -12,7 +12,7 @@ import capnp |
|
|
|
|
import numpy as np |
|
|
|
|
from typing import List, NoReturn, Optional |
|
|
|
|
|
|
|
|
|
from cereal import car, log |
|
|
|
|
from cereal import log |
|
|
|
|
import cereal.messaging as messaging |
|
|
|
|
from common.conversions import Conversions as CV |
|
|
|
|
from common.params import Params, put_nonblocking |
|
|
|
@ -62,7 +62,7 @@ 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)) |
|
|
|
|
self.not_car = False |
|
|
|
|
|
|
|
|
|
# Read saved calibration |
|
|
|
|
params = Params() |
|
|
|
@ -192,7 +192,7 @@ class Calibrator: |
|
|
|
|
liveCalibration.rpyCalib = smooth_rpy.tolist() |
|
|
|
|
liveCalibration.rpyCalibSpread = self.calib_spread.tolist() |
|
|
|
|
|
|
|
|
|
if self.CP.notCar: |
|
|
|
|
if self.not_car: |
|
|
|
|
extrinsic_matrix = get_view_frame_from_road_frame(0, 0, 0, model_height) |
|
|
|
|
liveCalibration.validBlocks = INPUTS_NEEDED |
|
|
|
|
liveCalibration.calStatus = Calibration.CALIBRATED |
|
|
|
@ -212,7 +212,7 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m |
|
|
|
|
set_realtime_priority(1) |
|
|
|
|
|
|
|
|
|
if sm is None: |
|
|
|
|
sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll=['cameraOdometry']) |
|
|
|
|
sm = messaging.SubMaster(['cameraOdometry', 'carState', 'carParams'], poll=['cameraOdometry']) |
|
|
|
|
|
|
|
|
|
if pm is None: |
|
|
|
|
pm = messaging.PubMaster(['liveCalibration']) |
|
|
|
@ -223,6 +223,8 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m |
|
|
|
|
timeout = 0 if sm.frame == -1 else 100 |
|
|
|
|
sm.update(timeout) |
|
|
|
|
|
|
|
|
|
calibrator.not_car = sm['carParams'].notCar |
|
|
|
|
|
|
|
|
|
if sm.updated['cameraOdometry']: |
|
|
|
|
calibrator.handle_v_ego(sm['carState'].vEgo) |
|
|
|
|
new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, |
|
|
|
|