From 966d59a26deffef6d928126778e5cc1bccf781c8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Jun 2020 11:25:47 +0900 Subject: [PATCH] AP_NavEKF2: protect against ExtNav being NaN --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 3109e64e7b..71fba1ed56 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -892,6 +892,11 @@ void NavEKF2_core::getTimingStatistics(struct ekf_timing &_timing) void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms) { + // protect against NaN + if (pos.is_nan() || isnan(posErr) || quat.is_nan() || isnan(angErr)) { + return; + } + // limit update rate to maximum allowed by sensor buffers and fusion process // don't try to write to buffer until the filter has been initialised if ((timeStamp_ms - extNavMeasTime_ms) < 20) { @@ -1014,6 +1019,11 @@ void NavEKF2_core::writeDefaultAirSpeed(float airspeed) void NavEKF2_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) { + // protect against NaN + if (vel.is_nan() || isnan(err)) { + return; + } + if ((timeStamp_ms - extNavVelMeasTime_ms) < 20) { return; }