Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

OpticalFlow: adapt optical flow library to new rotation system

Browse Source
This commit is contained in:
Andrew Tridgell 2012-03-10 13:57:06 +11:00
parent 7aa6ba2c86
commit 5203df2cf8
3 changed files with 20 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
View File

@ -18,16 +18,16 @@ AP_OpticalFlow* AP_OpticalFlow::_sensor = NULL; // pointer to the last instanti
bool bool
AP_OpticalFlow::init(bool initCommAPI) AP_OpticalFlow::init(bool initCommAPI)
{ {
_orientation_matrix = Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1); _orientation = ROTATION_NONE;
update_conversion_factors(); update_conversion_factors();
return true; // just return true by default return true; // just return true by default
} }
// set_orientation - Rotation vector to transform sensor readings to the body frame. // set_orientation - Rotation vector to transform sensor readings to the body frame.
void void
AP_OpticalFlow::set_orientation(const Matrix3f &rotation_matrix) AP_OpticalFlow::set_orientation(enum Rotation rotation)
{ {
_orientation_matrix = rotation_matrix; _orientation = rotation;
} }
// read value from the sensor. Should be overridden by derived class // read value from the sensor. Should be overridden by derived class
@ -55,9 +55,11 @@ void
AP_OpticalFlow::apply_orientation_matrix() AP_OpticalFlow::apply_orientation_matrix()
{ {
Vector3f rot_vector; Vector3f rot_vector;
rot_vector(raw_dx, raw_dy, 0);
// next rotate dx and dy // next rotate dx and dy
rot_vector = _orientation_matrix * Vector3f(raw_dx, raw_dy, 0); rot_vector.rotate(_orientation);
dx = rot_vector.x; dx = rot_vector.x;
dy = rot_vector.y; dy = rot_vector.y;

14
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -21,16 +21,6 @@
#include <AP_Math.h> #include <AP_Math.h>
#include <AP_Common.h> #include <AP_Common.h>
// standard rotation matrices
#define AP_OPTICALFLOW_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)
#define AP_OPTICALFLOW_ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1)
class AP_OpticalFlow class AP_OpticalFlow
{ {
public: public:
@ -53,7 +43,7 @@ class AP_OpticalFlow
virtual bool init(bool initCommAPI = true); // parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface) virtual bool init(bool initCommAPI = true); // parameter controls whether I2C/SPI interface is initialised (set to false if other devices are on the I2C/SPI bus and have already initialised the interface)
virtual byte read_register(byte address); virtual byte read_register(byte address);
virtual void write_register(byte address, byte value); virtual void write_register(byte address, byte value);
virtual void set_orientation(const Matrix3f &rotation_matrix); // Rotation vector to transform sensor readings to the body frame. virtual void set_orientation(enum Rotation rotation); // Rotation vector to transform sensor readings to the body frame.
virtual void set_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); }; // sets field of view of sensor virtual void set_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); }; // sets field of view of sensor
static void read(uint32_t ) { if( _sensor != NULL ) _sensor->update(); }; // call to update all attached sensors static void read(uint32_t ) { if( _sensor != NULL ) _sensor->update(); }; // call to update all attached sensors
virtual bool update(); // read latest values from sensor and fill in x,y and totals. returns true on success virtual bool update(); // read latest values from sensor and fill in x,y and totals. returns true on success
@ -61,7 +51,7 @@ class AP_OpticalFlow
protected: protected:
static AP_OpticalFlow *_sensor; // pointer to the last instantiated optical flow sensor. Will be turned into a table if we ever add support for more than one sensor static AP_OpticalFlow *_sensor; // pointer to the last instantiated optical flow sensor. Will be turned into a table if we ever add support for more than one sensor
Matrix3f _orientation_matrix; enum Rotation _orientation;
float conv_factor; // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude) 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 radians_to_pixels;
float _last_roll, _last_pitch, _last_altitude; float _last_roll, _last_pitch, _last_altitude;

16
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h
View File

@ -4,14 +4,14 @@
#include "AP_OpticalFlow.h" #include "AP_OpticalFlow.h"
// orientations for ADNS3080 sensor // orientations for ADNS3080 sensor
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD AP_OPTICALFLOW_ROTATION_YAW_180 #define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD ROTATION_YAW_180
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT AP_OPTICALFLOW_ROTATION_YAW_135 #define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT ROTATION_YAW_135
#define AP_OPTICALFLOW_ADNS3080_PINS_RIGHT AP_OPTICALFLOW_ROTATION_YAW_90 #define AP_OPTICALFLOW_ADNS3080_PINS_RIGHT ROTATION_YAW_90
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_RIGHT AP_OPTICALFLOW_ROTATION_YAW_45 #define AP_OPTICALFLOW_ADNS3080_PINS_BACK_RIGHT ROTATION_YAW_45
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK AP_OPTICALFLOW_ROTATION_NONE #define AP_OPTICALFLOW_ADNS3080_PINS_BACK ROTATION_NONE
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_LEFT AP_OPTICALFLOW_ROTATION_YAW_315 #define AP_OPTICALFLOW_ADNS3080_PINS_BACK_LEFT ROTATION_YAW_315
#define AP_OPTICALFLOW_ADNS3080_PINS_LEFT AP_OPTICALFLOW_ROTATION_YAW_270 #define AP_OPTICALFLOW_ADNS3080_PINS_LEFT ROTATION_YAW_270
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_LEFT AP_OPTICALFLOW_ROTATION_YAW_225 #define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_LEFT ROTATION_YAW_225
// field of view of ADNS3080 sensor lenses // field of view of ADNS3080 sensor lenses
#define AP_OPTICALFLOW_ADNS3080_08_FOV 0.202458 // 11.6 degrees #define AP_OPTICALFLOW_ADNS3080_08_FOV 0.202458 // 11.6 degrees