From f8beca190cba45c5e97f0542e447d9f4c5f057bd Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 22 Nov 2020 14:24:01 +1100
Subject: [PATCH] AP_GPS: fixed constrained NaN in EKF3 caused by bad GPS
 blending

if the accuracies reported are very low then we can do a division by
zero and this results in a constraining NaN for GPS vertical velocity
filter in NavEKF3_core::calcGpsGoodToAlign
---
 libraries/AP_GPS/AP_GPS.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 68028a9669..9ba2eac4b4 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -1541,6 +1541,10 @@ bool AP_GPS::calc_blend_weights(void)
         }
     }
 
+    if (!is_positive(sum_of_all_weights)) {
+        return false;
+    }
+
     // calculate an overall weight
     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
         _blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights;