mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-06-26 18:22:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert "Dont use erratic current block (#23211)" (#23221)

Browse Source 
This reverts commit 5c76900f565c8e55f5738efb8fdf5d6a846dbe0e.
old-commit-hash: 837823ff58
This commit is contained in:
HaraldSchafer
2021-12-14 13:59:39 -08:00
committed by GitHub
parent 6a7849aea3
commit 4708268c7d
1 changed files with 3 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/calibrationd.py
+3 -9
View File
@@ -99,16 +99,10 @@ class Calibrator():
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 @@ class Calibrator():
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()