From d242339f2eca1b66fa2fc87e17d874253b33b9e6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 30 Nov 2020 21:01:20 +1100 Subject: [PATCH] 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 | 3 ++- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 8 ++++---- libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 1 + 4 files changed, 8 insertions(+), 6 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index c19f44c824..2e68ae87e3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index e090514e1b..8c62eaa3b4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 9d3289c9bf..55a0ecf3d2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/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); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index e97666d5b4..526a962ef9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/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 {