From be04be9b431ba46ffd1bde9e8ed59503a454f06f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 2 May 2015 17:32:57 +1000 Subject: [PATCH] AP_NavEKF: Fix bug preventing home position being set by copter The interface definition has been modified so that it returns true for a position obtained usin geither the normal inertial navigation calculation, or a raw GPS measurement. This enables this function to be used to set a home position before flight. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 7 +++++-- libraries/AP_NavEKF/AP_NavEKF.h | 5 +++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index a0961db406..79bd4f1dc1 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -3755,7 +3755,9 @@ bool NavEKF::getMagOffsets(Vector3f &magOffsets) const } // Return the last calculated latitude, longitude and height in WGS-84 -// If a calculated location isn't available, return false and the raw GPS measurement or last known position if available +// If a calculated location isn't available, return a raw GPS measurement +// The status will return true if a calculation or raw measurement is available +// The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control bool NavEKF::getLLH(struct Location &loc) const { if(validOrigin) { @@ -3778,11 +3780,12 @@ bool NavEKF::getLLH(struct Location &loc) const // we have a GPS position fix to return const struct Location &gpsloc = _ahrs->get_gps().location(); loc = gpsloc; + return true; } else { // if no GPS fix, provide last known position before entering the mode location_offset(loc, lastKnownPositionNE.x, lastKnownPositionNE.y); + return false; } - return false; } } else { // If no origin has been defined for the EKF, then we cannot use its position states so return a raw diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 9b2c566399..465c172894 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -154,8 +154,9 @@ public: bool getMagOffsets(Vector3f &magOffsets) const; // Return the last calculated latitude, longitude and height in WGS-84 - // If a calculated location isn't available, return false and the raw GPS measurement or last known position if available - // If false returned, do not use for flight control + // If a calculated location isn't available, return a raw GPS measurement + // The status will return true if a calculation or raw measurement is available + // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control bool getLLH(struct Location &loc) const; // return the latitude and longitude and height used to set the NED origin