From ca666855d5ff9f91880e77be7a98b0f2e06309cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Wed, 23 Aug 2023 13:23:17 -0700 Subject: [PATCH] Laikad: reset est pos when in bad state (#29559) * reset est pos when cant delay correct * was just for debug old-commit-hash: aa585b17578a38b294ee748b172e6ad28f8266ca --- selfdrive/locationd/laikad.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 6c9c199db1..42d9303fd6 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -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):