From 232fc8a64db00752a35b78a5758021567eddea05 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 29 Sep 2015 10:58:54 +1000 Subject: [PATCH] AP_AHRS: added send_ekf_status_report() --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 13 +++++++++++++ libraries/AP_AHRS/AP_AHRS_NavEKF.h | 3 +++ 2 files changed, 16 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 923a888f42..5d462013aa 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -709,5 +709,18 @@ bool AP_AHRS_NavEKF::resetHeightDatum(void) return false; } +// send a EKF_STATUS_REPORT for current EKF +void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan) +{ + switch (active_EKF_type()) { + case EKF_TYPE1: + default: + return EKF1.send_status_report(chan); + + case EKF_TYPE2: + return EKF2.send_status_report(chan); + } +} + #endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index e67403bb0d..98111ad62e 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -152,6 +152,9 @@ public: // Returns true if the height datum reset has been performed // If using a range finder for height no reset is performed and it returns false bool resetHeightDatum(void); + + // send a EKF_STATUS_REPORT for current EKF + void send_ekf_status_report(mavlink_channel_t chan); private: enum EKF_TYPE {EKF_TYPE_NONE, EKF_TYPE1, EKF_TYPE2};