From 1f11c6add6d2d5776228009fd9cf8f452c96cd8a Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Thu, 7 Nov 2024 14:06:53 -0300 Subject: [PATCH] AP_NavEKF3: ignore VelZ of body odometry if SRCn_VELZ is set to None --- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 8f73f8ae02c71..33dff0dac2d57 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/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(); }