From 2b681f2f13bbc549fd26ed76fe260ce58ced5056 Mon Sep 17 00:00:00 2001
From: Julien BERAUD <julien.beraud@parrot.com>
Date: Mon, 23 Nov 2015 19:45:20 +0100
Subject: [PATCH] AP_HAL: Add support for an Optflow driver

This is meant to be used for onboard optical flow
---
 libraries/AP_HAL/AP_HAL.h           |  1 +
 libraries/AP_HAL/AP_HAL_Namespace.h |  1 +
 libraries/AP_HAL/HAL.h              |  8 +++++--
 libraries/AP_HAL/OpticalFlow.h      | 34 +++++++++++++++++++++++++++++
 4 files changed, 42 insertions(+), 2 deletions(-)
 create mode 100644 libraries/AP_HAL/OpticalFlow.h

diff --git a/libraries/AP_HAL/AP_HAL.h b/libraries/AP_HAL/AP_HAL.h
index 53a13db1e7..62eb5d3416 100644
--- a/libraries/AP_HAL/AP_HAL.h
+++ b/libraries/AP_HAL/AP_HAL.h
@@ -20,6 +20,7 @@
 #include "Scheduler.h"
 #include "Semaphores.h"
 #include "Util.h"
+#include "OpticalFlow.h"
 
 #include "utility/Print.h"
 #include "utility/Stream.h"
diff --git a/libraries/AP_HAL/AP_HAL_Namespace.h b/libraries/AP_HAL/AP_HAL_Namespace.h
index bd4da93bd4..23cbe76939 100644
--- a/libraries/AP_HAL/AP_HAL_Namespace.h
+++ b/libraries/AP_HAL/AP_HAL_Namespace.h
@@ -26,6 +26,7 @@ namespace AP_HAL {
     class RCOutput;
     class Scheduler;
     class Semaphore;
+    class OpticalFlow;
     
     class Util;
 
diff --git a/libraries/AP_HAL/HAL.h b/libraries/AP_HAL/HAL.h
index 990260ca8e..075f86fa9d 100644
--- a/libraries/AP_HAL/HAL.h
+++ b/libraries/AP_HAL/HAL.h
@@ -12,6 +12,7 @@
 #include "Storage.h"
 #include "UARTDriver.h"
 #include "system.h"
+#include "OpticalFlow.h"
 
 class AP_HAL::HAL {
 public:
@@ -31,7 +32,8 @@ public:
         AP_HAL::RCInput*    _rcin,
         AP_HAL::RCOutput*   _rcout,
         AP_HAL::Scheduler*  _scheduler,
-        AP_HAL::Util*       _util)
+        AP_HAL::Util*       _util,
+        AP_HAL::OpticalFlow* _opticalflow)
         :
         uartA(_uartA),
         uartB(_uartB),
@@ -49,7 +51,8 @@ public:
         rcin(_rcin),
         rcout(_rcout),
         scheduler(_scheduler),
-        util(_util)
+        util(_util),
+        opticalflow(_opticalflow)
     {
         AP_HAL::init();
     }
@@ -89,6 +92,7 @@ public:
     AP_HAL::RCOutput*   rcout;
     AP_HAL::Scheduler*  scheduler;
     AP_HAL::Util*       util;
+    AP_HAL::OpticalFlow* opticalflow;
 };
 
 #endif // __AP_HAL_HAL_H__
diff --git a/libraries/AP_HAL/OpticalFlow.h b/libraries/AP_HAL/OpticalFlow.h
new file mode 100644
index 0000000000..f9634c133a
--- /dev/null
+++ b/libraries/AP_HAL/OpticalFlow.h
@@ -0,0 +1,34 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef __AP_HAL_OPTICALFLOW_H__
+#define __AP_HAL_OPTICALFLOW_H__
+
+class AP_HAL::OpticalFlow {
+public:
+    FUNCTOR_TYPEDEF(Gyro_Cb, void, float&, float&, float&);
+    virtual void init(Gyro_Cb) = 0;
+    class Data_Frame {
+    public:
+        float pixel_flow_x_integral;
+        float pixel_flow_y_integral;
+        float gyro_x_integral;
+        float gyro_y_integral;
+        uint32_t delta_time;
+        uint8_t quality;
+    };
+    virtual bool read(Data_Frame& frame) = 0;
+};
+
+#endif