Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: added have_vz flag to GPS buffer data 

this ensures that we record GPS vertical velocity status for every
sample correctly
This commit is contained in:
Andrew Tridgell 2020-11-30 21:01:20 +11:00
parent 4e4a044d5d
commit d242339f2e
4 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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;

8
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -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

2
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -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);

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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 {