zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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.
mission-4.1.18
Randy Mackay 11 years ago
parent
4bdd814dcc
commit
b64f9ed964
4 changed files with 122 additions and 153 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 72
      libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
  2. 85
      libraries/AP_OpticalFlow/AP_OpticalFlow.h
  3. 27
      libraries/AP_OpticalFlow/OpticalFlow.cpp
  4. 91
      libraries/AP_OpticalFlow/OpticalFlow.h

72
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
Unescape View File

@ -1,72 +0,0 @@ @@ -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;
}

85
libraries/AP_OpticalFlow/AP_OpticalFlow.h
Unescape View File

@ -1,84 +1,7 @@ @@ -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"

27
libraries/AP_OpticalFlow/OpticalFlow.cpp
Unescape View File

@ -0,0 +1,27 @@ @@ -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;
};

91
libraries/AP_OpticalFlow/OpticalFlow.h
Unescape View File

@ -0,0 +1,91 @@ @@ -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
Write Preview
Loading…
Cancel
Save