From 06029854f18fd55a07390c5c762b9cf0146c775c Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Sat, 11 Mar 2023 17:01:55 +0000
Subject: [PATCH] AP_NavEKF: ensure gyro biases are numbers

avoid errors during compass mot
---
 libraries/AP_NavEKF/EKFGSF_yaw.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.cpp b/libraries/AP_NavEKF/EKFGSF_yaw.cpp
index e58150600f..2bba0d49d1 100644
--- a/libraries/AP_NavEKF/EKFGSF_yaw.cpp
+++ b/libraries/AP_NavEKF/EKFGSF_yaw.cpp
@@ -230,6 +230,11 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx)
     if (spinRate_squared < sq(0.175f)) {
         AHRS[mdl_idx].gyro_bias -= tilt_error_gyro_correction * (EKFGSF_gyroBiasGain * angle_dt);
 
+        // sanity check
+        if (AHRS[mdl_idx].gyro_bias.is_nan()) {
+            AHRS[mdl_idx].gyro_bias.zero();
+        }
+
         for (uint8_t i = 0; i < 3; i++) {
             AHRS[mdl_idx].gyro_bias[i] = constrain_ftype(AHRS[mdl_idx].gyro_bias[i], -gyro_bias_limit, gyro_bias_limit);
         }