From 83775554ea61ae498b4732f0e8b00a3a06b910ab Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 16 Nov 2014 10:36:33 +1100 Subject: [PATCH] AP_AHRS: Publish EKF ground speed limit --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 9 ++++++++- libraries/AP_AHRS/AP_AHRS_NavEKF.h | 3 +++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 91eebf8848..08b641ca4e 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -324,7 +324,14 @@ void AP_AHRS_NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlo // inhibit GPS useage uint8_t AP_AHRS_NavEKF::setInhibitGPS(void) { - return EKF.setInhibitGPS();; + return EKF.setInhibitGPS(); } + +// get speed limit +float AP_AHRS_NavEKF::getSpeedLimit(void) +{ + return EKF.getSpeedLimit(); +} + #endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 1ee3441b84..6313a09e35 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -107,6 +107,9 @@ public: // inibit GPS useage uint8_t setInhibitGPS(void); + // get speed limit + float getSpeedLimit(void); + void set_ekf_use(bool setting) { _ekf_use.set(setting); } // is the AHRS subsystem healthy?