From ec6412f5d4d415611322d148fea9579e2d402077 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Tue, 14 Dec 2021 13:58:56 -0800 Subject: [PATCH] Revert "Dont use erratic current block (#23211)" This reverts commit 19335bb6a48f0e3558c837557a1ae7c66b8b8587. --- selfdrive/locationd/calibrationd.py | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 0ba2bad7e40f61..604f4f68d92967 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -99,16 +99,10 @@ def reset(self, rpy_init=RPY_INIT, valid_blocks=0, smooth_from=None): self.old_rpy = smooth_from self.old_rpy_weight = 1.0 - def get_valid_idxs(self, ): - # exclude current block_idx from validity window - before_current = list(range(self.block_idx)) - after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks)) - return before_current + after_current - def update_status(self): if self.valid_blocks > 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)) + max_rpy_calib = np.array(np.max(self.rpys[:self.valid_blocks], axis=0)) + min_rpy_calib = np.array(np.min(self.rpys[:self.valid_blocks], axis=0)) self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib) else: self.calib_spread = np.zeros(3) @@ -164,7 +158,7 @@ def handle_cam_odom(self, trans, rot, trans_std, rot_std): self.valid_blocks = max(self.block_idx, self.valid_blocks) self.block_idx = self.block_idx % INPUTS_WANTED if self.valid_blocks > 0: - self.rpy = np.mean(self.rpys[self.get_valid_idxs()], axis=0) + self.rpy = np.mean(self.rpys[:self.valid_blocks], axis=0) self.update_status()