mirror of https://github.com/ArduPilot/ardupilot
OptFlow: rename and restructure OpticalFlow class
AP_OpticalFlow.h becomes simply a file that includes all other optical flow header files. OpticalFlow class simplified to only return surface quality, raw output and velocity vector.
This commit is contained in:
parent
4bdd814dcc
commit
b64f9ed964
|
@ -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 <http://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/*
|
|
||||||
* 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;
|
|
||||||
}
|
|
|
@ -1,84 +1,7 @@
|
||||||
#ifndef __AP_OPTICALFLOW_H__
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
#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.
|
|
||||||
|
|
||||||
This program is distributed in the hope that it will be useful,
|
/// @file AP_OpticalFlow.h
|
||||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
/// @brief Catch-all header that defines all supported optical flow classes.
|
||||||
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.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 <AP_Math.h>
|
|
||||||
|
|
||||||
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;
|
|
||||||
};
|
|
||||||
|
|
||||||
#include "AP_OpticalFlow_ADNS3080.h"
|
#include "AP_OpticalFlow_ADNS3080.h"
|
||||||
|
#include "AP_OpticalFlow_PX4.h"
|
||||||
#endif
|
|
||||||
|
|
|
@ -0,0 +1,27 @@
|
||||||
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
#include <AP_Progmem.h>
|
||||||
|
#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;
|
||||||
|
};
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 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_Math.h>
|
||||||
|
#include <AP_AHRS.h>
|
||||||
|
|
||||||
|
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
|
Loading…
Reference in New Issue