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

AP_NavEKF3: don't reset EKx_GPS_TYPE when GPS has no vertical velocity 

setting the parameter to 1 causes the following issues:

 - the GPS may not have vertical velocity at the time the parameter
   set happens, but may get it later when the GPS is fully configured

 - we may switch between GPS modules which do/don't have vertical
   velocity

 - the user may download parameters after the set(1), and end up with
   incorrect parameters they may later load onto the vehicle,
   permanently disabling use of vertical velocity
This commit is contained in:
Andrew Tridgell 2020-11-15 20:38:05 +11:00
parent 79148498d3
commit 603e0c090d
2 changed files with 4 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -245,7 +245,8 @@ void NavEKF3_core::ResetHeight(void)
// Reset the vertical velocity state using GPS vertical velocity if we are airborne // 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 // Check that GPS vertical velocity data is available and can be used
if (inFlight && !gpsNotAvailable && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) { if (inFlight && !gpsNotAvailable && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) &&
!frontend->inhibitGpsVertVelUse && dal.gps().have_vertical_velocity(selected_gps)) {
stateStruct.velocity.z = gpsDataNew.vel.z; stateStruct.velocity.z = gpsDataNew.vel.z;
} else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) { } else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
stateStruct.velocity.z = extNavVelDelayed.vel.z; stateStruct.velocity.z = extNavVelDelayed.vel.z;
@ -720,7 +721,8 @@ void NavEKF3_core::FuseVelPosNED()
// test velocity measurements // test velocity measurements
uint8_t imax = 2; 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 inhibited by the user or if we are using synthetic data
if ((!frontend->sources.haveVelZSource() || PV_AidingMode != AID_ABSOLUTE || frontend->inhibitGpsVertVelUse) && !useExtNavVel) { if ((!frontend->sources.haveVelZSource() || PV_AidingMode != AID_ABSOLUTE ||
frontend->inhibitGpsVertVelUse || !dal.gps().have_vertical_velocity(selected_gps)) && !useExtNavVel) {
imax = 1; imax = 1;
} }
float innovVelSumSq = 0; // sum of squares of velocity innovations float innovVelSumSq = 0; // sum of squares of velocity innovations

10
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -83,16 +83,6 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt; gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f); gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD); gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
} else if (frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !gps.have_vertical_velocity(preferred_gps)) {
// If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
gpsVertVelFail = true;
// if we have a 3D fix with no vertical velocity and
// EK3_GPS_TYPE=0 then change it to 1. It means the GPS is not
// capable of giving a vertical velocity
if (gps.status(preferred_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D) {
frontend->sources.setVelZSource(AP_NavEKF_Source::SourceZ::NONE);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EK3: Changed EK3_GPS_TYPE to 1");
}
} else { } else {
gpsVertVelFail = false; gpsVertVelFail = false;
} }