From e7e91b1c3bad2e3477d9fb14ea2b6dd2eb3c1fbd Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 13 Oct 2020 11:29:35 +0900
Subject: [PATCH] AP_NavEKF3: always calculate extNav innovations and variances

---
 libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 6 ++++++
 libraries/AP_NavEKF3/AP_NavEKF3_core.h           | 3 +++
 2 files changed, 9 insertions(+)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
index 927d37d497..aa081546ba 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
@@ -426,6 +426,12 @@ void NavEKF3_core::SelectVelPosFusion()
     extNavVelToFuse = storedExtNavVel.recall(extNavVelDelayed, imuDataDelayed.time_ms);
     if (extNavVelToFuse) {
         CorrectExtNavVelForSensorOffset(extNavVelDelayed);
+
+        // calculate innovations and variances for reporting purposes only
+        CalculateVelInnovationsAndVariances(extNavVelDelayed.vel, extNavVelDelayed.err, frontend->extNavVelVarAccScale, extNavVelInnov, extNavVelVarInnov);
+
+        // record time innovations were calculated (for timeout checks)
+        extNavVelInnovTime_ms = AP_HAL::millis();
     }
 
     // Read GPS data from the sensor
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index 071cd0999c..2091182b1d 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -1352,6 +1352,9 @@ private:
     uint32_t extNavVelMeasTime_ms;      // time external navigation velocity measurements were accepted for input to the data buffer (msec)
     bool extNavVelToFuse;               // true when there is new external navigation velocity to fuse
     bool useExtNavVel;                  // true if external nav velocity should be used
+    Vector3f extNavVelInnov;            // external nav velocity innovations
+    Vector3f extNavVelVarInnov;         // external nav velocity innovation variances
+    uint32_t extNavVelInnovTime_ms;     // system time that external nav velocity innovations were recorded (to detect timeouts)
 
     // flags indicating severe numerical errors in innovation variance calculation for different fusion operations
     struct {