AP_NavEKF3: added have_vz flag to GPS buffer data

this ensures that we record GPS vertical velocity status for every
sample correctly

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -580,6 +580,7 @@ void NavEKF3_core::readGpsData()
 
             // read the NED velocity from the GPS
             gpsDataNew.vel = gps.velocity(selected_gps);
+            gpsDataNew.have_vz = gps.have_vertical_velocity(selected_gps);
 
             // position and velocity are not yet corrected for sensor position
             gpsDataNew.corrected = false;
@@ -625,7 +626,7 @@ void NavEKF3_core::readGpsData()
             }
 
             // Check if GPS can output vertical velocity, vertical velocity use is permitted and set GPS fusion mode accordingly
-            if (gps.have_vertical_velocity(selected_gps) && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS)) {
+            if (gpsDataNew.have_vz && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS)) {
                 useGpsVertVel = true;
             } else {
                 useGpsVertVel = false;

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -246,7 +246,7 @@ void NavEKF3_core::ResetHeight(void)
     // Reset the vertical velocity state using GPS vertical velocity if we are airborne
     // Check that GPS vertical velocity data is available and can be used
     if (inFlight && !gpsNotAvailable && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) &&
-        dal.gps().have_vertical_velocity(selected_gps)) {
+        gpsDataNew.have_vz) {
         stateStruct.velocity.z =  gpsDataNew.vel.z;
     } else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
         stateStruct.velocity.z = extNavVelDelayed.vel.z;
@@ -659,7 +659,7 @@ void NavEKF3_core::FuseVelPosNED()
         // if vertical GPS velocity data and an independent height source is being used, check to see if the GPS vertical velocity and altimeter
         // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
         // the accelerometers and we should disable the GPS and barometer innovation consistency checks.
-        if (useGpsVertVel && fuseVelData && (frontend->sources.getPosZSource() != AP_NavEKF_Source::SourceZ::GPS)) {
+        if (gpsDataDelayed.have_vz && fuseVelData && (frontend->sources.getPosZSource() != AP_NavEKF_Source::SourceZ::GPS)) {
             // calculate innovations for height and vertical GPS vel measurements
             const float hgtErr  = stateStruct.position.z - velPosObs[5];
             const float velDErr = stateStruct.velocity.z - velPosObs[2];
@@ -720,9 +720,9 @@ void NavEKF3_core::FuseVelPosNED()
         if (fuseVelData) {
             // test velocity measurements
             uint8_t imax = 2;
-            // Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
+            // Don't fuse vertical velocity observations if disabled in sources or not available
             if ((!frontend->sources.haveVelZSource() || PV_AidingMode != AID_ABSOLUTE ||
-                 !dal.gps().have_vertical_velocity(selected_gps)) && !useExtNavVel) {
+                 !gpsDataDelayed.have_vz) && !useExtNavVel) {
                 imax = 1;
             }
             float innovVelSumSq = 0; // sum of squares of velocity innovations

libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

@@ -78,7 +78,7 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
 
     // Check that the vertical GPS vertical velocity is reasonable after noise filtering
     bool gpsVertVelFail;
-    if (gps.have_vertical_velocity(preferred_gps) && onGround) {
+    if (gpsDataNew.have_vz && onGround) {
         // check that the average vertical GPS velocity is close to zero
         gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
         gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -514,6 +514,7 @@ private:
         Vector3f    vel;            // velocity of the GPS antenna in local NED earth frame (m/sec)
         uint8_t     sensor_idx;     // unique integer identifying the GPS sensor
         bool        corrected;      // true when the position and velocity have been corrected for sensor position
+        bool        have_vz;        // true when vertical velocity is valid
     };
 
     struct mag_elements : EKF_obs_element_t {