aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_OpticalFlow: split library into frontend/backend

Browse Source 
this will make it easier to add a SITL backend
This commit is contained in:
Andrew Tridgell 2015-01-02 19:30:32 +11:00
parent a9eec29e45
commit 11ff12dfd3
9 changed files with 237 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -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"

37
libraries/AP_OpticalFlow/AP_OpticalFlow_HIL.cpp Normal file
View File

@ -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)
{
}

22
libraries/AP_OpticalFlow/AP_OpticalFlow_HIL.h Normal file
View File

@ -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

45
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
View File

@ -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);
}
}

9
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.h
View File

@ -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

45
libraries/AP_OpticalFlow/OpticalFlow.cpp
View File

@ -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);
}
}

64
libraries/AP_OpticalFlow/OpticalFlow.h
View File

@ -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

26
libraries/AP_OpticalFlow/OpticalFlow_backend.cpp Normal file
View File

@ -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();
}

50
libraries/AP_OpticalFlow/OpticalFlow_backend.h Normal file
View File

@ -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__