diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index bc0f0a0396..fdbc4b1a65 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -59,6 +59,7 @@ class Calibrator(): self.valid_blocks = 0 self.cal_status = Calibration.UNCALIBRATED self.just_calibrated = False + self.v_ego = 0 # Read calibration calibration_params = Params().get("CalibrationParams") @@ -88,8 +89,11 @@ class Calibrator(): if start_status == Calibration.UNCALIBRATED and end_status == Calibration.CALIBRATED: self.just_calibrated = True + def handle_v_ego(self, v_ego): + self.v_ego = v_ego + def handle_cam_odom(self, trans, rot, trans_std, rot_std): - straight_and_fast = ((trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) + straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or (self.valid_blocks < INPUTS_NEEDED)) if straight_and_fast and certain_if_calib: @@ -132,7 +136,7 @@ class Calibrator(): def calibrationd_thread(sm=None, pm=None): if sm is None: - sm = messaging.SubMaster(['cameraOdometry']) + sm = messaging.SubMaster(['cameraOdometry', 'carState']) if pm is None: pm = messaging.PubMaster(['liveCalibration']) @@ -143,18 +147,22 @@ def calibrationd_thread(sm=None, pm=None): while 1: sm.update() + 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, sm['cameraOdometry'].rot, sm['cameraOdometry'].transStd, sm['cameraOdometry'].rotStd) - if DEBUG and new_vp is not None: - print('got new vp', new_vp) - # decimate outputs for efficiency - 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) + + # decimate outputs for efficiency def main(sm=None, pm=None): diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index b8288987b1..106d871e5f 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -92,6 +92,7 @@ class FakeSubMaster(messaging.SubMaster): wait_for_event(self.update_ready) self.update_ready.clear() + def update_msgs(self, cur_time, msgs): wait_for_event(self.update_called) self.update_called.clear() @@ -186,8 +187,14 @@ def radar_rcv_callback(msg, CP, cfg, fsm): def calibration_rcv_callback(msg, CP, cfg, fsm): # calibrationd publishes 1 calibrationData every 5 cameraOdometry packets. # should_recv always true to increment frame - recv_socks = ["liveCalibration"] if (fsm.frame + 1) % 5 == 0 else [] - return recv_socks, True + if msg.which() == 'carState': + if ((fsm.frame +1)% 25) == 0: + recv_socks = ["liveCalibration"] + else: + recv_socks = [] + return recv_socks, True + else: + return [], False CONFIGS = [ @@ -225,7 +232,8 @@ CONFIGS = [ ProcessConfig( proc_name="calibrationd", pub_sub={ - "cameraOdometry": ["liveCalibration"] + "carState": ["liveCalibration"], + "cameraOdometry": [] }, ignore=["logMonoTime", "valid"], init_callback=get_car_params, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index c86a6d04cb..70ee41a975 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -9b4b08487380a6741646e22e191fa5bd1615f2d3 +ea2820c0708a95e8f392d503c7b76090ade380b0 \ No newline at end of file