diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index dbc7f359a9..34d3d2b03d 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: