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:
Randy Mackay
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__
|
||||
#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 <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;
|
||||
};
|
||||
/// @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"
|
||||
|
|
|
|||
|
|
@ -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