From 11ff12dfd35e336f715cbcdc0e9f580c5d4342dc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Jan 2015 19:30:32 +1100 Subject: [PATCH] AP_OpticalFlow: split library into frontend/backend this will make it easier to add a SITL backend --- libraries/AP_OpticalFlow/AP_OpticalFlow.h | 2 +- .../AP_OpticalFlow/AP_OpticalFlow_HIL.cpp | 37 +++++++++++ libraries/AP_OpticalFlow/AP_OpticalFlow_HIL.h | 22 +++++++ .../AP_OpticalFlow/AP_OpticalFlow_PX4.cpp | 45 ++++++------- libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h | 9 ++- libraries/AP_OpticalFlow/OpticalFlow.cpp | 45 +++++++++++-- libraries/AP_OpticalFlow/OpticalFlow.h | 64 ++++++++++--------- .../AP_OpticalFlow/OpticalFlow_backend.cpp | 26 ++++++++ .../AP_OpticalFlow/OpticalFlow_backend.h | 50 +++++++++++++++ 9 files changed, 237 insertions(+), 63 deletions(-) create mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_HIL.cpp create mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_HIL.h create mode 100644 libraries/AP_OpticalFlow/OpticalFlow_backend.cpp create mode 100644 libraries/AP_OpticalFlow/OpticalFlow_backend.h diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index 73a3628f71..42f21f6903 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 0000000000..b5acafaf30 --- /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 . + */ + +/* + * AP_OpticalFlow_HIL.cpp - HIL emulation of optical flow sensor. + * This is a dummy class, with the work done in setHIL() + */ + +#include +#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 0000000000..14ae83205a --- /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 7c6d656f8a..6266ec0195 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp @@ -20,11 +20,10 @@ */ #include +#include "OpticalFlow.h" #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 -#include "AP_OpticalFlow_PX4.h" - #include #include #include @@ -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 c40b336d79..68304b0c0f 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 ddd97549d5..7262c72f08 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 #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 f44465c556..6cd7479e6b 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 #include -#include + +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 0000000000..3ebb13079b --- /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 . + */ + +#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 0000000000..ce5c002ddf --- /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 . + */ + +/* + 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__ +