From f4c18e0f9cedeb100fc795a6ed9cd7b43ee6781d Mon Sep 17 00:00:00 2001
From: nkruzan <nickkruzan@gmail.com>
Date: Sun, 23 May 2021 17:58:16 -0500
Subject: [PATCH] AP_InertialSensor_Invensense: set reset count to 1 if 10s has
 passed since last reset

---
 libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
index d87e3aa991..b45255e1eb 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
@@ -168,6 +168,11 @@ void AP_InertialSensor_Invensense::_fifo_reset(bool log_error)
             INTERNAL_ERROR(AP_InternalError::error_t::imu_reset);
             reset_count = 0;
         }
+    } else if (log_error &&
+        !hal.scheduler->in_expected_delay() &&
+        now - last_reset_ms > 10000) {
+        //if last reset was more than 10s ago consider this the first reset
+        reset_count = 1;
     }
     last_reset_ms = now;