spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

calibrationd: Don't repeatedly call get_valid_idxs() (#23473) 

* optimize

* rpy_init should be np.array

* rename to rpys
pull/23503/head
Dean Lee 2022-01-13 07:34:17 +08:00 committed by GitHub
parent de572c3755
commit e5f9af4a6e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
selfdrive/locationd/calibrationd.py
View File

@ -108,9 +108,12 @@ class Calibrator():
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 @@ class Calibrator():
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()