diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 43b36510da5001..ae191bbebbed3d 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -108,9 +108,12 @@ def get_valid_idxs(self): return before_current + after_current def update_status(self): - if len(self.get_valid_idxs()) > 0: - max_rpy_calib = np.array(np.max(self.rpys[self.get_valid_idxs()], axis=0)) - min_rpy_calib = np.array(np.min(self.rpys[self.get_valid_idxs()], axis=0)) + valid_idxs = self.get_valid_idxs() + if valid_idxs: + rpys = self.rpys[valid_idxs] + self.rpy = np.mean(rpys, axis=0) + max_rpy_calib = np.array(np.max(rpys, axis=0)) + min_rpy_calib = np.array(np.min(rpys, axis=0)) self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib) else: self.calib_spread = np.zeros(3) @@ -165,8 +168,6 @@ def handle_cam_odom(self, trans, rot, trans_std, rot_std): self.block_idx += 1 self.valid_blocks = max(self.block_idx, self.valid_blocks) self.block_idx = self.block_idx % INPUTS_WANTED - if len(self.get_valid_idxs()) > 0: - self.rpy = np.mean(self.rpys[self.get_valid_idxs()], axis=0) self.update_status()