diff --git a/cereal b/cereal index 61d7488e55..d66afca4ac 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 61d7488e55fcaa3760d9ccc9d6589b30cbe4a6c1 +Subproject commit d66afca4ac316456711cb80c8e8e2fe91431e1e2 diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 9f02c59722..a27e284a9c 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -126,13 +126,21 @@ class Calibrator(): def send_data(self, pm): calib = get_calib_from_vp(self.vp) + if self.valid_blocks > 0: + max_vp_calib = np.array(get_calib_from_vp(np.max(self.vps[:self.valid_blocks], axis=0))) + min_vp_calib = np.array(get_calib_from_vp(np.min(self.vps[:self.valid_blocks], axis=0))) + calib_spread = np.abs(max_vp_calib - min_vp_calib) + else: + calib_spread = np.zeros(3) extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height) cal_send = messaging.new_message('liveCalibration') + cal_send.liveCalibration.validBlocks = self.valid_blocks cal_send.liveCalibration.calStatus = self.cal_status cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()] cal_send.liveCalibration.rpyCalib = [float(x) for x in calib] + cal_send.liveCalibration.rpyCalibSpread = [float(x) for x in calib_spread] pm.send('liveCalibration', cal_send) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 8b8abe7eca..568aab2b73 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -3e8932a5dc0281b00463d40a5435f659259591c4 \ No newline at end of file +158743f002e82b62a67c6b91308db01d715f1643 \ No newline at end of file