From fd0b82f669a38c63643842192ef6dfb910190ea5 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 2 Jan 2015 20:45:10 +0900
Subject: [PATCH] Copter: add optflow_position_ok and use for loiter

This allows entering Loiter flight mode with only optical flow based
position
---
 ArduCopter/control_loiter.pde |  2 +-
 ArduCopter/system.pde         | 16 ++++++++++++++++
 2 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde
index 3963f33fbc..d0b7d212d5 100644
--- a/ArduCopter/control_loiter.pde
+++ b/ArduCopter/control_loiter.pde
@@ -7,7 +7,7 @@
 // loiter_init - initialise loiter controller
 static bool loiter_init(bool ignore_checks)
 {
-    if (position_ok() || ignore_checks) {
+    if (position_ok() || optflow_position_ok() || ignore_checks) {
 
         // set target to current position
         wp_nav.init_loiter_target();
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 4f0c489fa2..a3496a4a65 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -349,6 +349,22 @@ static bool position_ok()
     }
 }
 
+// optflow_position_ok - returns true if optical flow based position estimate is ok
+static bool optflow_position_ok()
+{
+#if OPTFLOW != ENABLED
+    return false;
+#else
+    // return immediately if optflow is not enabled
+    if (!optflow.enabled()) {
+        return false;
+    }
+
+    // get filter status from inertial nav or EKF
+    return inertial_nav.get_filter_status().flags.horiz_pos_rel;
+#endif
+}
+
 // update_auto_armed - update status of auto_armed flag
 static void update_auto_armed()
 {