From b56b68ce10c98056b258c9e684ca264a78999c49 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sat, 15 Nov 2014 17:44:25 +1100
Subject: [PATCH] AP_NavEKF: Add public method reporting horizontal speed limit

This is required if using optical flow, as depending on height, the speed must be limited to prevent the sensor saturating
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 11 +++++++++++
 libraries/AP_NavEKF/AP_NavEKF.h   |  3 +++
 2 files changed, 14 insertions(+)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 69af6bd9f0..e7063aacd5 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -3622,6 +3622,17 @@ uint8_t NavEKF::setInhibitGPS(void)
     }
 }
 
+// return the horizontal speed limit in m/s set by optical flow limitations
+// allow 1.0 rad/sec margin for angular motion
+float NavEKF::getSpeedLimit(void) const
+{
+    if (useOptFlow()) {
+        return max((_maxFlowRate - 1.0f), 0.0f) * (flowStates[1] - state.position[2]);
+    } else {
+        return 400.0f; //return 80% of max filter speed
+    }
+}
+
 // return weighting of first IMU in blending function
 void NavEKF::getIMU1Weighting(float &ret) const
 {
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 9fb7c1e4b7..9ff1483af8 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -119,6 +119,9 @@ public:
     // This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms)
     uint8_t setInhibitGPS(void);
 
+    // return the horizontal speed limit in m/s set by optical flow limitations
+    float getSpeedLimit(void) const;
+
     // return weighting of first IMU in blending function
     void getIMU1Weighting(float &ret) const;