diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 4c8486236d..ca9ea76998 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -13,8 +13,8 @@ from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose MIN_LAG_VEL = 20.0 MAX_SANE_LAG = 3.0 MIN_ABS_YAW_RATE_DEG = 1 -MOVING_CORR_WINDOW = 120.0 -MIN_OKAY_WINDOW = 20.0 +MOVING_CORR_WINDOW = 300.0 +MIN_OKAY_WINDOW = 25.0 MIN_NCC = 0.95 @@ -145,7 +145,7 @@ def main(): while True: sm.update() if sm.all_checks(): - for which in sm.services: + for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]): if sm.updated[which]: t = sm.logMonoTime[which] * 1e-9 estimator.handle_log(t, which, sm[which])