diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h
index 73a3628f7104f6357047bb600b59e5567b72b2b2..42f21f69037bd064b6dac00b860c0104b89ffb45 100644
--- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h
@@ -3,4 +3,4 @@
/// @file AP_OpticalFlow.h
/// @brief Catch-all header that defines all supported optical flow classes.
-#include "AP_OpticalFlow_PX4.h"
+#include "OpticalFlow.h"
diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_HIL.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_HIL.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b5acafaf3067d8ea8a2b03540dd2f8b6a867abad
--- /dev/null
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_HIL.cpp
@@ -0,0 +1,37 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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/>.
+ */
+
+/*
+ * AP_OpticalFlow_HIL.cpp - HIL emulation of optical flow sensor.
+ * This is a dummy class, with the work done in setHIL()
+ */
+
+#include <AP_HAL.h>
+#include "OpticalFlow.h"
+
+extern const AP_HAL::HAL& hal;
+
+AP_OpticalFlow_HIL::AP_OpticalFlow_HIL(OpticalFlow &_frontend) :
+ OpticalFlow_backend(_frontend)
+{}
+
+void AP_OpticalFlow_HIL::init(void)
+{
+}
+
+void AP_OpticalFlow_HIL::update(void)
+{
+}
diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_HIL.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_HIL.h
new file mode 100644
index 0000000000000000000000000000000000000000..14ae83205af7d1e290f08bb733ec53e5ddef9dac
--- /dev/null
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_HIL.h
@@ -0,0 +1,22 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef AP_OpticalFlow_HIL_H
+#define AP_OpticalFlow_HIL_H
+
+#include "OpticalFlow.h"
+
+class AP_OpticalFlow_HIL : public OpticalFlow_backend
+{
+public:
+ /// constructor
+ AP_OpticalFlow_HIL(OpticalFlow &_frontend);
+
+ // init - initialise the sensor
+ void init();
+
+ // update - read latest values from sensor and fill in x,y and totals.
+ void update(void);
+};
+
+#endif // AP_OpticalFlow_HIL_H
+
diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
index 7c6d656f8a6d46b1af76ea92c19df0adf07c3490..6266ec0195578056ed8a733ebc1f4df7c8d54123 100644
--- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
@@ -20,11 +20,10 @@
*/
#include <AP_HAL.h>
+#include "OpticalFlow.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
-#include "AP_OpticalFlow_PX4.h"
-
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
@@ -35,7 +34,10 @@
extern const AP_HAL::HAL& hal;
-// Public Methods //////////////////////////////////////////////////////////////
+AP_OpticalFlow_PX4::AP_OpticalFlow_PX4(OpticalFlow &_frontend) :
+OpticalFlow_backend(_frontend)
+{}
+
void AP_OpticalFlow_PX4::init(void)
{
@@ -50,39 +52,38 @@ void AP_OpticalFlow_PX4::init(void)
if (ioctl(_fd, SENSORIOCSPOLLRATE, 10) != 0) {
hal.console->printf("Unable to set flow rate to 10Hz\n");
}
-
- // if we got this far, the sensor must be healthy
- _flags.healthy = true;
}
// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_PX4::update(void)
{
- // return immediately if not healthy
- if (!_flags.healthy) {
+ // return immediately if not initialised
+ if (_fd == -1) {
return;
}
struct optical_flow_s report;
- while (::read(_fd, &report, sizeof(optical_flow_s)) == sizeof(optical_flow_s) && report.timestamp != _last_timestamp) {
- _device_id = report.sensor_id;
- _surface_quality = report.quality;
+ while (::read(_fd, &report, sizeof(optical_flow_s)) == sizeof(optical_flow_s) &&
+ report.timestamp != _last_timestamp) {
+ struct OpticalFlow::OpticalFlow_state state;
+ state.device_id = report.sensor_id;
+ state.surface_quality = report.quality;
if (report.integration_timespan > 0) {
- float flowScaleFactorX = 1.0f + 0.001f * float(_flowScalerX);
- float flowScaleFactorY = 1.0f + 0.001f * float(_flowScalerY);
+ const Vector2f flowScaler = _flowScaler();
+ float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
+ float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
float integralToRate = 1e6f / float(report.integration_timespan);
- _flowRate.x = flowScaleFactorX * integralToRate * float(report.pixel_flow_x_integral); // rad/sec measured optically about the X sensor axis
- _flowRate.y = flowScaleFactorY * integralToRate * float(report.pixel_flow_y_integral); // rad/sec measured optically about the Y sensor axis
- _bodyRate.x = integralToRate * float(report.gyro_x_rate_integral); // rad/sec measured inertially about the X sensor axis
- _bodyRate.y = integralToRate * float(report.gyro_y_rate_integral); // rad/sec measured inertially about the Y sensor axis
+ state.flowRate.x = flowScaleFactorX * integralToRate * float(report.pixel_flow_x_integral); // rad/sec measured optically about the X sensor axis
+ state.flowRate.y = flowScaleFactorY * integralToRate * float(report.pixel_flow_y_integral); // rad/sec measured optically about the Y sensor axis
+ state.bodyRate.x = integralToRate * float(report.gyro_x_rate_integral); // rad/sec measured inertially about the X sensor axis
+ state.bodyRate.y = integralToRate * float(report.gyro_y_rate_integral); // rad/sec measured inertially about the Y sensor axis
} else {
- _flowRate.x = 0.0f;
- _flowRate.y = 0.0f;
- _bodyRate.x = 0.0f;
- _bodyRate.y = 0.0f;
+ state.flowRate.zero();
+ state.bodyRate.zero();
}
_last_timestamp = report.timestamp;
- _last_update = hal.scheduler->millis();
+
+ _update_frontend(state);
}
}
diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h
index c40b336d79789d0beaa24d9cc2970f75aaf44a53..68304b0c0f97eb807a7e7473e4af23ec5e0ade0a 100644
--- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h
@@ -5,18 +5,17 @@
#include "OpticalFlow.h"
-class AP_OpticalFlow_PX4 : public OpticalFlow
+class AP_OpticalFlow_PX4 : public OpticalFlow_backend
{
public:
-
/// constructor
- AP_OpticalFlow_PX4(const AP_AHRS &ahrs) : OpticalFlow(ahrs) {};
+ AP_OpticalFlow_PX4(OpticalFlow &_frontend);
// init - initialise the sensor
- virtual void init();
+ void init();
// update - read latest values from sensor and fill in x,y and totals.
- virtual void update(void);
+ void update(void);
private:
int _fd; // file descriptor for sensor
diff --git a/libraries/AP_OpticalFlow/OpticalFlow.cpp b/libraries/AP_OpticalFlow/OpticalFlow.cpp
index ddd97549d51d6aaa14d12026412f832522866d08..7262c72f081dde59e73cc81221327d2e65f69db1 100644
--- a/libraries/AP_OpticalFlow/OpticalFlow.cpp
+++ b/libraries/AP_OpticalFlow/OpticalFlow.cpp
@@ -1,7 +1,10 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
#include <AP_Progmem.h>
#include "OpticalFlow.h"
+extern const AP_HAL::HAL& hal;
+
const AP_Param::GroupInfo OpticalFlow::var_info[] PROGMEM = {
// @Param: ENABLE
// @DisplayName: Optical flow enable/disable
@@ -30,14 +33,44 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] PROGMEM = {
};
// default constructor
-OpticalFlow::OpticalFlow(const AP_AHRS &ahrs) :
- _ahrs(ahrs),
- _device_id(0),
- _surface_quality(0),
- _last_update(0)
+OpticalFlow::OpticalFlow(void) :
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+ backend(new AP_OpticalFlow_PX4(*this))
+#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+ backend(new AP_OpticalFlow_HIL(*this))
+#else
+ backend(NULL)
+#endif
{
AP_Param::setup_object_defaults(this, var_info);
- // healthy flag will be overwritten when init is called
+ memset(&_state, 0, sizeof(_state));
+
+ // healthy flag will be overwritten on update
_flags.healthy = false;
};
+
+void OpticalFlow::init(void)
+{
+ if (backend != NULL) {
+ backend->init();
+ } else {
+ _enabled = 0;
+ }
+}
+
+void OpticalFlow::update(void)
+{
+ if (backend != NULL) {
+ backend->update();
+ }
+ // only healthy if the data is less than 0.5s old
+ _flags.healthy = (_last_update_ms - hal.scheduler->millis() < 500);
+}
+
+void OpticalFlow::setHIL(const struct OpticalFlow::OpticalFlow_state &state)
+{
+ if (backend) {
+ backend->_update_frontend(state);
+ }
+}
diff --git a/libraries/AP_OpticalFlow/OpticalFlow.h b/libraries/AP_OpticalFlow/OpticalFlow.h
index f44465c556d9bf3f890816b39eaefc51f7eb44fb..6cd7479e6b8d40341be6c0835cb5d867b6033c39 100644
--- a/libraries/AP_OpticalFlow/OpticalFlow.h
+++ b/libraries/AP_OpticalFlow/OpticalFlow.h
@@ -18,76 +18,82 @@
/*
* 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 <AP_HAL.h>
#include <AP_Math.h>
-#include <AP_AHRS.h>
+
+class OpticalFlow_backend;
class OpticalFlow
{
+ friend class OpticalFlow_backend;
+
public:
// constructor
- OpticalFlow(const AP_AHRS &ahrs);
+ OpticalFlow(void);
// init - initialise sensor
- virtual void init() {}
+ void init(void);
// 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; }
+ bool healthy() const { return backend != NULL && _flags.healthy; }
// read latest values from sensor and fill in x,y and totals.
- virtual void update() {}
+ void update(void);
// quality - returns the surface quality as a measure from 0 ~ 255
- uint8_t quality() const { return _surface_quality; }
+ uint8_t quality() const { return _state.surface_quality; }
// raw - returns the raw movement from the sensor
- const Vector2f& flowRate() const { return _flowRate; }
+ const Vector2f& flowRate() const { return _state.flowRate; }
// velocity - returns the velocity in m/s
- const Vector2f& bodyRate() const { return _bodyRate; }
+ const Vector2f& bodyRate() const { return _state.bodyRate; }
// device_id - returns device id
- uint8_t device_id() const { return _device_id; }
+ uint8_t device_id() const { return _state.device_id; }
// last_update() - returns system time of last sensor update
- uint32_t last_update() const { return _last_update; }
+ uint32_t last_update() const { return _last_update_ms; }
// parameter var info table
static const struct AP_Param::GroupInfo var_info[];
-protected:
+ struct OpticalFlow_state {
+ uint8_t device_id; // device id
+ uint8_t surface_quality; // image quality (below TBD you can't trust the dx,dy values returned)
+ Vector2f flowRate; // optical flow angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.
+ Vector2f bodyRate; // body inertial angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.
+ };
+
+ // support for HIL/SITL
+ void setHIL(const struct OpticalFlow_state &state);
+
+private:
+ OpticalFlow_backend *backend;
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
AP_Int16 _flowScalerX; // X axis flow scale factor correction - parts per thousand
AP_Int16 _flowScalerY; // Y axis flow scale factor correction - parts per thousand
- // internal variables
- uint8_t _device_id; // device id
- uint8_t _surface_quality; // image quality (below TBD you can't trust the dx,dy values returned)
- Vector2f _flowRate; // optical flow angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.
- Vector2f _bodyRate; // body inertial angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.
- uint32_t _last_update; // millis() time of last update
+
+ // state filled in by backend
+ struct OpticalFlow_state _state;
+
+ uint32_t _last_update_ms; // millis() time of last update
};
+#include "OpticalFlow_backend.h"
+#include "AP_OpticalFlow_HIL.h"
+#include "AP_OpticalFlow_PX4.h"
+
#endif
diff --git a/libraries/AP_OpticalFlow/OpticalFlow_backend.cpp b/libraries/AP_OpticalFlow/OpticalFlow_backend.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3ebb13079b488ebbed8e839dbc90872c347c4059
--- /dev/null
+++ b/libraries/AP_OpticalFlow/OpticalFlow_backend.cpp
@@ -0,0 +1,26 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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/>.
+ */
+
+#include "OpticalFlow.h"
+
+extern const AP_HAL::HAL& hal;
+
+// update the frontend
+void OpticalFlow_backend::_update_frontend(const struct OpticalFlow::OpticalFlow_state &state)
+{
+ frontend._state = state;
+ frontend._last_update_ms = hal.scheduler->millis();
+}
diff --git a/libraries/AP_OpticalFlow/OpticalFlow_backend.h b/libraries/AP_OpticalFlow/OpticalFlow_backend.h
new file mode 100644
index 0000000000000000000000000000000000000000..ce5c002ddf440969c92aa4e432505e0a13c851e8
--- /dev/null
+++ b/libraries/AP_OpticalFlow/OpticalFlow_backend.h
@@ -0,0 +1,50 @@
+#ifndef __OpticalFlow_backend_H__
+#define __OpticalFlow_backend_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 <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ OpticalFlow backend class for ArduPilot
+ */
+
+#include "OpticalFlow.h"
+
+class OpticalFlow_backend
+{
+ friend class OpticalFlow;
+
+public:
+ // constructor
+ OpticalFlow_backend(OpticalFlow &_frontend) : frontend(_frontend) {}
+
+ // init - initialise sensor
+ virtual void init() = 0;
+
+ // read latest values from sensor and fill in x,y and totals.
+ virtual void update() = 0;
+
+protected:
+ // access to frontend
+ OpticalFlow &frontend;
+
+ // update the frontend
+ void _update_frontend(const struct OpticalFlow::OpticalFlow_state &state);
+
+ // get the flow scaling parameters
+ Vector2f _flowScaler(void) const { return Vector2f(frontend._flowScalerX, frontend._flowScalerY); }
+};
+
+#endif // __OpticalFlow_backend_H__
+