From 8bd3b9a7b84d593e3047d78dd72d3f55b50e160d Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 3 May 2021 14:13:31 +0200 Subject: [PATCH] paramsd: check NaN in inputs and outputs (#20804) old-commit-hash: 0b4d28b9d697ad7b55495993bd4a7d2800c4e79b --- selfdrive/locationd/paramsd.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index d47c64cc61..0da862b6d8 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -38,6 +38,8 @@ class ParamsLearner: 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 (not math.isnan(yaw_rate)) + yaw_rate_valid = yaw_rate_valid and (not math.isnan(yaw_rate_std)) if self.active: if msg.inputsOK and msg.posenetOK and yaw_rate_valid: @@ -126,13 +128,17 @@ def main(sm=None, pm=None): learner.handle_log(t, which, sm[which]) if sm.updated['liveLocationKalman']: + x = learner.kf.x + if any(map(math.isnan, x)): + cloudlog.error("NaN in liveParameters estimate. Resetting to default values") + learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0) + continue + msg = messaging.new_message('liveParameters') msg.logMonoTime = sm.logMonoTime['carState'] msg.liveParameters.posenetValid = True msg.liveParameters.sensorValid = True - - x = learner.kf.x msg.liveParameters.steerRatio = float(x[States.STEER_RATIO]) msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) msg.liveParameters.angleOffsetAverageDeg = math.degrees(x[States.ANGLE_OFFSET])