From d6a575a8605130a59575ec7668e0d0eb90626595 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 5 May 2021 17:42:11 +0200 Subject: [PATCH] paramsd: stricter checks on locationd yaw rate outputs --- selfdrive/locationd/paramsd.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index dbc7f359a..34d3d2b03 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -34,13 +34,14 @@ class ParamsLearner: def handle_log(self, t, which, msg): if which == 'liveLocationKalman': - yaw_rate = msg.angularVelocityCalibrated.value[2] yaw_rate_std = msg.angularVelocityCalibrated.std[2] + yaw_rate_valid = msg.angularVelocityCalibrated.valid yaw_rate_valid = yaw_rate_valid and math.isfinite(yaw_rate) yaw_rate_valid = yaw_rate_valid and math.isfinite(yaw_rate_std) - yaw_rate_valid = yaw_rate_valid and yaw_rate_std < 1e6 + yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10 # rad/s + yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1 # rad/s if self.active: if msg.inputsOK and msg.posenetOK and yaw_rate_valid: