From 51624149e1d35f31ad504c4f5981a0e6d6229b09 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 23 Apr 2015 13:27:51 +1000
Subject: [PATCH] AP_InertialNav: Add function to return EKF height control
 limit

---
 .../AP_InertialNav/AP_InertialNav_NavEKF.cpp      | 15 +++++++++++++++
 libraries/AP_InertialNav/AP_InertialNav_NavEKF.h  |  8 ++++++++
 2 files changed, 23 insertions(+)

diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
index 5d9109cf73..2b34e7fdd7 100644
--- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
+++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
@@ -134,6 +134,21 @@ bool AP_InertialNav_NavEKF::get_hagl(float height) const
     return valid;
 }
 
+/**
+ * get_hgt_ctrl_limit - get maximum height to be observed by the control loops in cm and a validity flag
+ * this is used to limit height during optical flow navigation
+ * it will return invalid when no limiting is required
+ * @return
+ */
+bool AP_InertialNav_NavEKF::get_hgt_ctrl_limit(float limit) const
+{
+    // true when estimate is valid
+    bool valid = _ahrs_ekf.get_NavEKF().getHeightControlLimit(limit);
+    // convert height from m to cm
+    limit *= 100.0f;
+    return valid;
+}
+
 /**
  * get_velocity_z - returns the current climbrate.
  *
diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h
index d18b342ffd..da5c1fffe0 100644
--- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h
+++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h
@@ -94,6 +94,14 @@ public:
      */
     bool       get_hagl(float hagl) const;
 
+    /**
+     * get_hgt_ctrl_limit - get maximum height to be observed by the control loops in cm and a validity flag
+     * this is used to limit height during optical flow navigation
+     * it will return invalid when no limiting is required
+     * @return
+     */
+    bool       get_hgt_ctrl_limit(float limit) const;
+
     /**
      * get_velocity_z - returns the current climbrate.
      *