AP_NavEKF3: add getVelInnovationsAndVariancesForSource

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -1407,6 +1407,19 @@ void NavEKF3::getStateVariances(int8_t instance, float stateVar[24]) const
     }
 }
 
+// get a source's velocity innovations for the specified instance.  Set instance to -1 for the primary instance
+// returns true on success and results are placed in innovations and variances arguments
+bool NavEKF3::getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const
+{
+    if (instance < 0 || instance >= num_cores) {
+        instance = primary;
+    }
+    if (core) {
+        return core[instance].getVelInnovationsAndVariancesForSource(source, innovations, variances);
+    }
+    return false;
+}
+
 // should we use the compass? This is public so it can be used for
 // reporting via ahrs.use_compass()
 bool NavEKF3::use_compass(void) const

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -198,6 +198,10 @@ public:
     // return the diagonals from the covariance matrix for the specified instance
     void getStateVariances(int8_t instance, float stateVar[24]) const;
 
+    // get a source's velocity innovations for the specified instance.  Set instance to -1 for the primary instance
+    // returns true on success and results are placed in innovations and variances arguments
+    bool getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED;
+
     // should we use the compass? This is public so it can be used for
     // reporting via ahrs.use_compass()
     bool use_compass(void) const;

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -513,6 +513,36 @@ void  NavEKF3_core::getStateVariances(float stateVar[24])
     }
 }
 
+// get a particular source's velocity innovations
+// returns true on success and results are placed in innovations and variances arguments
+bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const
+{
+    switch (source) {
+    case AP_NavEKF_Source::SourceXY::GPS:
+        // check for timeouts
+        if (AP_HAL::millis() - gpsVelInnovTime_ms > 500) {
+            return false;
+        }
+        innovations = gpsVelInnov;
+        variances = gpsVelVarInnov;
+        return true;
+    case AP_NavEKF_Source::SourceXY::EXTNAV:
+        // check for timeouts
+        if (AP_HAL::millis() - extNavVelInnovTime_ms > 500) {
+            return false;
+        }
+        innovations = extNavVelInnov;
+        variances = extNavVelVarInnov;
+        return true;
+    default:
+        // variances are not available for this source
+        return false;
+    }
+
+    // should never get here but just in case
+    return false;
+}
+
 /*
 return the filter fault status as a bitmasked integer
  0 = quaternions are NaN

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -211,6 +211,10 @@ public:
     // return the diagonals from the covariance matrix
     void getStateVariances(float stateVar[24]);
 
+    // get a particular source's velocity innovations
+    // returns true on success and results are placed in innovations and variances arguments
+    bool getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED;
+
     // should we use the compass? This is public so it can be used for
     // reporting via ahrs.use_compass()
     bool use_compass(void) const;