From 3201a8dbcaea92aa92322b9fdde02fd484882b3e Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 12 Jul 2014 11:34:55 +0900
Subject: [PATCH] Copter: integrate optflow_enable param move to optflow class

---
 ArduCopter/GCS_Mavlink.pde |  2 +-
 ArduCopter/sensors.pde     |  7 ++++++-
 ArduCopter/setup.pde       |  2 +-
 ArduCopter/system.pde      |  4 +---
 ArduCopter/test.pde        | 11 ++++++-----
 5 files changed, 15 insertions(+), 11 deletions(-)

diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 419e22e0ee..9a67ad4fb3 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -153,7 +153,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
     }
 #if OPTFLOW == ENABLED
-    if (g.optflow_enabled) {
+    if (optflow.enabled()) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
     }
 #endif
diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde
index 43aac53278..10b7cfac50 100644
--- a/ArduCopter/sensors.pde
+++ b/ArduCopter/sensors.pde
@@ -93,9 +93,14 @@ static void init_compass()
 static void init_optflow()
 {
 #if OPTFLOW == ENABLED
+    // exit immediately if not enabled
+    if (!optflow.enabled()) {
+        return;
+    }
+
+    // initialise sensor and display error on failure
     optflow.init();
     if (!optflow.healthy()) {
-        g.optflow_enabled = false;
         cliSerial->print_P(PSTR("Failed to Init OptFlow\n"));
         Log_Write_Error(ERROR_SUBSYSTEM_OPTFLOW,ERROR_CODE_FAILED_TO_INITIALISE);
     }
diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde
index f1e2fb4804..009b673e2f 100644
--- a/ArduCopter/setup.pde
+++ b/ArduCopter/setup.pde
@@ -386,7 +386,7 @@ void report_optflow()
     cliSerial->printf_P(PSTR("OptFlow\n"));
     print_divider();
 
-    print_enabled(g.optflow_enabled);
+    print_enabled(optflow.enabled());
 
     print_blanks(2);
  #endif     // OPTFLOW == ENABLED
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index d796f0fb0e..c37ef21e6e 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -222,9 +222,7 @@ static void init_ardupilot()
     pos_control.set_dt(MAIN_LOOP_SECONDS);
 
     // init the optical flow sensor
-    if(g.optflow_enabled) {
-        init_optflow();
-    }
+    init_optflow();
 
     // initialise inertial nav
     inertial_nav.init();
diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde
index d59c10248a..15be4b828c 100644
--- a/ArduCopter/test.pde
+++ b/ArduCopter/test.pde
@@ -198,17 +198,18 @@ static int8_t
 test_optflow(uint8_t argc, const Menu::arg *argv)
 {
 #if OPTFLOW == ENABLED
-    if(g.optflow_enabled) {
-        cliSerial->printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID));
+    if(optflow.enabled()) {
+        cliSerial->printf_P(PSTR("dev id: %d\t"),(int)optflow.device_id());
         print_hit_enter();
 
         while(1) {
             delay(200);
             optflow.update();
+            const Vector2i& raw = optflow.raw();
             cliSerial->printf_P(PSTR("dx:%d\t dy:%d\t squal:%d\n"),
-                            optflow.dx,
-                            optflow.dy,
-                            optflow.surface_quality);
+                            (int)raw.x,
+                            (int)raw.y,
+                            (int)optflow.quality());
 
             if(cliSerial->available() > 0) {
                 return (0);