AP_NavEKF3: ignore VelZ of body odometry if SRCn_VELZ is set to None

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -2031,7 +2031,10 @@ void NavEKF3_core::SelectBodyOdomFusion()
     // Check for body odometry data (aka visual position delta) at the fusion time horizon
     const bool bodyOdomDataToFuse = storedBodyOdm.recall(bodyOdmDataDelayed, imuDataDelayed.time_ms);
     if (bodyOdomDataToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV)) {
-
+        // If VelZ source is set to none, don't use the odometry data for Z velocity
+        if (!frontend->sources.haveVelZSource()) {
+            bodyOdmDataDelayed.vel.z = stateStruct.velocity.z;
+        }
         // Fuse data into the main filter
         FuseBodyVel();
     }