mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-30 11:02:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Laikad: reset est pos when in bad state (#29559)

Browse Source 
* reset est pos when cant delay correct

* was just for debug
old-commit-hash: aa585b17578a38b294ee748b172e6ad28f8266ca
This commit is contained in:
Harald Schäfer
2023-08-23 13:23:17 -07:00
committed by GitHub
parent 5c213d7a38
commit ca666855d5
1 changed files with 5 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/laikad.py
+5
View File
@@ -269,6 +269,11 @@ class Laikad:
est_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
correct_delay = False
corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog, correct_delay=correct_delay)
# If many measurements weren't corrected, position may be garbage, so reset
if len(processed_measurements) >= 8 and len(corrected_measurements) < 5:
cloudlog.error("Didn't correct enough measurements, resetting estimate position")
self.last_fix_pos = None
self.last_fix_t = None
return corrected_measurements
def calc_fix(self, t, measurements):