diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index a27e284a9c..b32bbedb30 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -156,7 +156,7 @@ def calibrationd_thread(sm=None, pm=None): send_counter = 0 while 1: - sm.update() + sm.update(wait_for="cameraOdometry") # if no inputs still publish calibration if not sm.updated['carState'] and not sm.updated['cameraOdometry']: @@ -165,9 +165,6 @@ def calibrationd_thread(sm=None, pm=None): if sm.updated['carState']: calibrator.handle_v_ego(sm['carState'].vEgo) - if send_counter % 25 == 0: - calibrator.send_data(pm) - send_counter += 1 if sm.updated['cameraOdometry']: new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, @@ -175,6 +172,10 @@ def calibrationd_thread(sm=None, pm=None): sm['cameraOdometry'].transStd, sm['cameraOdometry'].rotStd) + if send_counter % 5 == 0: + calibrator.send_data(pm) + send_counter += 1 + if DEBUG and new_vp is not None: print('got new vp', new_vp)