From e374ec634de3ceb1cef519b8456c1c8e4f42c745 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 10 Jul 2016 18:43:28 +1000 Subject: [PATCH] AP_NavEKF2: Add separate horizontal/vertical local position interfaces --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 24 ++++++++++ libraries/AP_NavEKF2/AP_NavEKF2.h | 12 +++++ libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 50 +++++++++++++++++++++ libraries/AP_NavEKF2/AP_NavEKF2_core.h | 10 +++++ 4 files changed, 96 insertions(+) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index c9e532d31e..d4a91373fe 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -686,6 +686,30 @@ bool NavEKF2::getPosNED(int8_t instance, Vector3f &pos) return core[instance].getPosNED(pos); } +// Return the last calculated NE position relative to the reference point (m). +// If a calculated solution is not available, use the best available data and return false +// If false returned, do not use for flight control +bool NavEKF2::getPosNE(int8_t instance, Vector2f &posNE) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (!core) { + return false; + } + return core[instance].getPosNE(posNE); +} + +// Return the last calculated D position relative to the reference point (m). +// If a calculated solution is not available, use the best available data and return false +// If false returned, do not use for flight control +bool NavEKF2::getPosD(int8_t instance, float &posD) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (!core) { + return false; + } + return core[instance].getPosD(posD); +} + // return NED velocity in m/s void NavEKF2::getVelNED(int8_t instance, Vector3f &vel) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 1485695bc4..295764553d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -68,6 +68,18 @@ public: // If false returned, do not use for flight control bool getPosNED(int8_t instance, Vector3f &pos); + // Return the last calculated NE position relative to the reference point (m) for the specified instance. + // An out of range instance (eg -1) returns data for the the primary instance + // If a calculated solution is not available, use the best available data and return false + // If false returned, do not use for flight control + bool getPosNE(int8_t instance, Vector2f &posNE); + + // Return the last calculated D position relative to the reference point (m) for the specified instance. + // An out of range instance (eg -1) returns data for the the primary instance + // If a calculated solution is not available, use the best available data and return false + // If false returned, do not use for flight control + bool getPosD(int8_t instance, float &posD); + // return NED velocity in m/s for the specified instance // An out of range instance (eg -1) returns data for the the primary instance void getVelNED(int8_t instance, Vector3f &vel); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 62a7c02ce0..434a8386cd 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -249,7 +249,57 @@ bool NavEKF2_core::getPosNED(Vector3f &pos) const return false; } +// Return the last estimated NE position relative to the reference point (m). +// Return true if the estimate is valid +bool NavEKF2_core::getPosNE(Vector2f &posNE) const +{ + // There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available) + nav_filter_status status; + getFilterStatus(status); + if (PV_AidingMode != AID_NONE) { + // This is the normal mode of operation where we can use the EKF position states + posNE.x = outputDataNew.position.x; + posNE.y = outputDataNew.position.y; + return true; + } else { + // In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate + if(validOrigin) { + if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { + // If the origin has been set and we have GPS, then return the GPS position relative to the origin + const struct Location &gpsloc = _ahrs->get_gps().location(); + Vector2f tempPosNE = location_diff(EKF_origin, gpsloc); + posNE.x = tempPosNE.x; + posNE.y = tempPosNE.y; + return false; + } else { + // If no GPS fix is available, all we can do is provide the last known position + posNE.x = outputDataNew.position.x; + posNE.y = outputDataNew.position.y; + return false; + } + } else { + // If the origin has not been set, then we have no means of providing a relative position + posNE.x = 0.0f; + posNE.y = 0.0f; + return false; + } + } + return false; +} +// Return the last calculated D position relative to the reference point (m). +// Return true if the estimte is valid +bool NavEKF2_core::getPosD(float &posD) const +{ + // The EKF always has a height estimate regardless of mode of operation + posD = outputDataNew.position.z; + + // Return the current height solution status + nav_filter_status status; + getFilterStatus(status); + return status.flags.vert_pos; + +} // return the estimated height above ground level bool NavEKF2_core::getHAGL(float &HAGL) const { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 174a4c5bd1..88bd2f90f3 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -71,6 +71,16 @@ public: // If false returned, do not use for flight control bool getPosNED(Vector3f &pos) const; + // Return the last calculated NE position relative to the reference point (m). + // If a calculated solution is not available, use the best available data and return false + // If false returned, do not use for flight control + bool getPosNE(Vector2f &posNE) const; + + // Return the last calculated D position relative to the reference point (m). + // If a calculated solution is not available, use the best available data and return false + // If false returned, do not use for flight control + bool getPosD(float &posD) const; + // return NED velocity in m/s void getVelNED(Vector3f &vel) const;