diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
deleted file mode 100644
index 749aa0e521..0000000000
--- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
+++ /dev/null
@@ -1,72 +0,0 @@
-/*
- 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 .
- */
-
-/*
- * ADC.cpp - Analog Digital Converter Base Class for Ardupilot Mega
- * Code by James Goppert. DIYDrones.com
- *
- */
-
-#include "AP_OpticalFlow.h"
-
-#define FORTYFIVE_DEGREES 0.78539816f
-
-// set_orientation - Rotation vector to transform sensor readings to the body
-// frame.
-void AP_OpticalFlow::set_orientation(enum Rotation rotation)
-{
- _orientation = rotation;
-}
-
-void AP_OpticalFlow::update()
-{
-}
-
-void AP_OpticalFlow::init()
-{
-}
-
-// updates internal lon and lat with estimation based on optical flow
-void AP_OpticalFlow::update_position(float roll, float pitch, float sin_yaw, float cos_yaw, float altitude)
-{
- float diff_roll = roll - _last_roll;
- float diff_pitch = pitch - _last_pitch;
- float change_x, change_y; // actual change in x, y coordinates
-
- // only update position if surface quality is good and angle is not
- // over 45 degrees
- if( surface_quality >= 10 && fabsf(roll) <= FORTYFIVE_DEGREES
- && fabsf(pitch) <= FORTYFIVE_DEGREES ) {
- altitude = max(altitude, 0);
-
- // change in position is actual change measured by sensor (i.e. dx, dy) minus expected change due to change in roll, pitch
- change_x = dx - (diff_roll * radians_to_pixels);
- change_y = dy - (-diff_pitch * radians_to_pixels);
-
- float avg_altitude = (altitude + _last_altitude)*0.5f;
-
- // convert raw change to horizontal movement in cm
- // perhaps this altitude should actually be the distance to the
- // ground? i.e. if we are very rolled over it should be longer?
- x_cm = -change_x * avg_altitude * conv_factor;
- // for example if you are leaned over at 45 deg the ground will
- // appear farther away and motion from opt flow sensor will be less
- y_cm = -change_y * avg_altitude * conv_factor;
- }
-
- _last_altitude = altitude;
- _last_roll = roll;
- _last_pitch = pitch;
-}
diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h
index 164fddb64b..cc99e686a9 100644
--- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h
@@ -1,84 +1,7 @@
-#ifndef __AP_OPTICALFLOW_H__
-#define __AP_OPTICALFLOW_H__
-/*
- 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.
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
- 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 .
- */
-
-/*
- * AP_OpticalFlow.cpp - OpticalFlow Base Class for Ardupilot Mega
- * Code by Randy Mackay. DIYDrones.com
- *
- * Methods:
- * init() : initializate sensor and library.
- * read : reads latest value from OpticalFlow and
- * stores values in x,y, surface_quality parameter
- * read_register() : reads a value from the sensor (will be
- * sensor specific)
- * write_register() : writes a value to one of the sensor's
- * register (will be sensor specific)
- */
-
-#include
-
-class AP_OpticalFlow
-{
-public:
- // constructor
- AP_OpticalFlow() {
- _flags.healthy = false;
- };
-
- virtual void init();
-
- // healthy - return true if the sensor is healthy
- bool healthy() const { return _flags.healthy; }
-
- // Rotation vector to transform sensor readings to the body frame.
- void set_orientation(enum Rotation rotation);
-
- // sets field of view of sensor
- void set_field_of_view(const float fov) { field_of_view = fov; };
-
- // read latest values from sensor and fill in x,y and totals.
- virtual void update();
-
- // updates internal lon and lat with estimation based on optical flow
- void update_position(float roll, float pitch, float sin_yaw, float cos_yaw, float altitude);
-
- // public variables
- uint8_t surface_quality; // image quality (below 15 you can't trust the dx,dy values returned)
- int16_t dx,dy; // rotated change in x and y position
- uint32_t last_update; // millis() time of last update
- float field_of_view; // field of view in Radians
-
- // public variables for reporting purposes
- float x_cm, y_cm; // x,y position in cm
-
-protected:
-
- struct AP_OpticalFlow_Flags {
- uint8_t healthy : 1; // true if sensor is healthy
- } _flags;
-
- enum Rotation _orientation;
- float conv_factor; // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude)
- float radians_to_pixels;
- float _last_roll;
- float _last_pitch;
- float _last_altitude;
-};
+/// @file AP_OpticalFlow.h
+/// @brief Catch-all header that defines all supported optical flow classes.
#include "AP_OpticalFlow_ADNS3080.h"
-
-#endif
+#include "AP_OpticalFlow_PX4.h"
diff --git a/libraries/AP_OpticalFlow/OpticalFlow.cpp b/libraries/AP_OpticalFlow/OpticalFlow.cpp
new file mode 100644
index 0000000000..6cbf92cc5f
--- /dev/null
+++ b/libraries/AP_OpticalFlow/OpticalFlow.cpp
@@ -0,0 +1,27 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include
+#include "OpticalFlow.h"
+
+const AP_Param::GroupInfo OpticalFlow::var_info[] PROGMEM = {
+ // @Param: ENABLE
+ // @DisplayName: Optical flow enable/disable
+ // @Description: Setting this to Enabled(1) will enable optical flow. Setting this to Disabled(0) will disable optical flow
+ // @Values: 0:Disabled, 1:Enabled
+ // @User: Standard
+ AP_GROUPINFO("_ENABLE", 0, OpticalFlow, _enabled, 0),
+
+ AP_GROUPEND
+};
+
+// default constructor
+OpticalFlow::OpticalFlow(const AP_AHRS &ahrs) :
+ _ahrs(ahrs),
+ _device_id(0),
+ _surface_quality(0),
+ _last_update(0)
+{
+ AP_Param::setup_object_defaults(this, var_info);
+
+ // healthy flag will be overwritten when init is called
+ _flags.healthy = false;
+};
diff --git a/libraries/AP_OpticalFlow/OpticalFlow.h b/libraries/AP_OpticalFlow/OpticalFlow.h
new file mode 100644
index 0000000000..2ff86b7238
--- /dev/null
+++ b/libraries/AP_OpticalFlow/OpticalFlow.h
@@ -0,0 +1,91 @@
+#ifndef __OpticalFlow_H__
+#define __OpticalFlow_H__
+/*
+ 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 .
+ */
+
+/*
+ * OpticalFlow.h - OpticalFlow Base Class for Ardupilot
+ * Code by Randy Mackay. DIYDrones.com
+ *
+ * Methods:
+ * init() : initializate sensor and library.
+ * read : reads latest value from OpticalFlow and
+ * stores values in x,y, surface_quality parameter
+ * read_register() : reads a value from the sensor (will be
+ * sensor specific)
+ * write_register() : writes a value to one of the sensor's
+ * register (will be sensor specific)
+ */
+
+#include
+#include
+
+class OpticalFlow
+{
+public:
+ // constructor
+ OpticalFlow(const AP_AHRS &ahrs);
+
+ // init - initialise sensor
+ virtual void init() {}
+
+ // enabled - returns true if optical flow is enabled
+ bool enabled() const { return _enabled; }
+
+ // healthy - return true if the sensor is healthy
+ bool healthy() const { return _flags.healthy; }
+
+ // read latest values from sensor and fill in x,y and totals.
+ virtual void update() {}
+
+ // quality - returns the surface quality as a measure from 0 ~ 255
+ uint8_t quality() const { return _surface_quality; }
+
+ // raw - returns the raw movement from the sensor
+ const Vector2i& raw() const { return _raw; }
+
+ // velocity - returns the velocity in m/s
+ const Vector2f& velocity() const { return _velocity; }
+
+ // device_id - returns device id
+ uint8_t device_id() const { return _device_id; }
+
+ // last_update() - returns system time of last sensor update
+ uint32_t last_update() const { return _last_update; }
+
+ // parameter var info table
+ static const struct AP_Param::GroupInfo var_info[];
+
+protected:
+
+ struct AP_OpticalFlow_Flags {
+ uint8_t healthy : 1; // true if sensor is healthy
+ } _flags;
+
+ // external references
+ const AP_AHRS &_ahrs; // ahrs object
+
+ // parameters
+ AP_Int8 _enabled; // enabled/disabled flag
+
+ // internal variables
+ uint8_t _device_id; // device id
+ uint8_t _surface_quality; // image quality (below 15 you can't trust the dx,dy values returned)
+ Vector2i _raw; // raw x,y values from sensor
+ Vector2f _velocity; // x, y velocity in m/s
+ uint32_t _last_update; // millis() time of last update
+};
+
+#endif