diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 48dbbc2417..cf1ea3fe78 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1418,6 +1418,13 @@ bool NavEKF3::using_external_yaw(void) const return core[primary].using_external_yaw(); } +// check if configured to use GPS for horizontal position estimation +bool NavEKF3::configuredToUseGPSForPosXY(void) const +{ + // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity, 3 = do not use GPS + return (sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); +} + // write the raw optical flow measurements // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality // rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes. diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 36dca597c3..2009acd487 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -387,7 +387,10 @@ public: // are we using an external yaw source? This is needed by AHRS attitudes_consistent check bool using_external_yaw(void) const; - + + // check if configured to use GPS for horizontal position estimation + bool configuredToUseGPSForPosXY(void) const; + // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available. void writeDefaultAirSpeed(float airspeed);