From 34249ffa4262bf0cbf9411d31366c980771b9139 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Thu, 7 Aug 2014 10:30:42 +1000
Subject: [PATCH] Copter : EKF optical flow fusion support preliminary changes

Assumes optflow sensor has been modified to push data at a low rate, eg 10Hz
and velocity data is angular velocity, not linear velocity
---
 ArduCopter/ArduCopter.pde | 30 +++++++++++++++++++++++++++++-
 1 file changed, 29 insertions(+), 1 deletion(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index b6bec3310a..6ac27ac94f 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -849,7 +849,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { throttle_loop,         2,     450 },
     { update_GPS,            2,     900 },
 #if OPTFLOW == ENABLED
-    { update_optflow,        2,     100 },
+    { update_optical_flow,   1,     100 },
 #endif
     { update_batt_compass,  10,     720 },
     { read_aux_switches,    10,      50 },
@@ -1188,6 +1188,34 @@ static void one_hz_loop()
 #endif
 }
 
+// called at 100hz but data from sensor only arrives at 20 Hz
+#if OPTFLOW == ENABLED
+static void update_optical_flow(void)
+{
+    static uint32_t last_of_update = 0;
+
+    // exit immediately if not enabled
+    if (!optflow.enabled()) {
+        return;
+    }
+
+    // read from sensor
+    optflow.update();
+
+    // write to log and send to EKF if new data has arrived
+    if (optflow.last_update() != last_of_update) {
+        last_of_update = optflow.last_update();
+        uint8_t quality = optflow.quality();
+        Vector2f raw = optflow.velocity();
+        float ground_distance_m = optflow.ground_distance_m();
+        ahrs.writeOptFlowMeas(quality, raw, ground_distance_m, last_of_update);
+         if (g.log_bitmask & MASK_LOG_OPTFLOW) {
+            Log_Write_Optflow();
+        }
+    }
+}
+#endif  // OPTFLOW == ENABLED
+
 // called at 50hz
 static void update_GPS(void)
 {