From 0408ff0ab1924378f30760dbf9f7d40cefdfb6b4 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 0ea325cfdc..7d9e453c75 100644
--- a/libraries/AP_NavEKF/EKFGSF_yaw.cpp
+++ b/libraries/AP_NavEKF/EKFGSF_yaw.cpp
@@ -229,6 +229,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);
         }