Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04: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

18
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
View File

@ -18,16 +18,16 @@ AP_OpticalFlow* AP_OpticalFlow::_sensor = NULL; // pointer to the last instanti
bool
AP_OpticalFlow::init(bool initCommAPI)
{
_orientation_matrix = Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1);
update_conversion_factors();
return true; // just return true by default
_orientation = ROTATION_NONE;
update_conversion_factors();
return true; // just return true by default
}
// set_orientation - Rotation vector to transform sensor readings to the body frame.
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
@ -54,10 +54,12 @@ AP_OpticalFlow::write_register(byte address, byte value)
void
AP_OpticalFlow::apply_orientation_matrix()
{
Vector3f rot_vector;
Vector3f rot_vector;
rot_vector(raw_dx, raw_dy, 0);
// next rotate dx and dy
rot_vector = _orientation_matrix * Vector3f(raw_dx, raw_dy, 0);
rot_vector.rotate(_orientation);
dx = rot_vector.x;
dy = rot_vector.y;
@ -108,4 +110,4 @@ AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float
_last_altitude = altitude;
_last_roll = roll;
_last_pitch = pitch;
}
}

14
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -21,16 +21,6 @@
#include <AP_Math.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
{
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 byte read_register(byte address);
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
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
@ -61,7 +51,7 @@ class AP_OpticalFlow
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
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 radians_to_pixels;
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"
// orientations for ADNS3080 sensor
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD AP_OPTICALFLOW_ROTATION_YAW_180
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT AP_OPTICALFLOW_ROTATION_YAW_135
#define AP_OPTICALFLOW_ADNS3080_PINS_RIGHT AP_OPTICALFLOW_ROTATION_YAW_90
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_RIGHT AP_OPTICALFLOW_ROTATION_YAW_45
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK AP_OPTICALFLOW_ROTATION_NONE
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_LEFT AP_OPTICALFLOW_ROTATION_YAW_315
#define AP_OPTICALFLOW_ADNS3080_PINS_LEFT AP_OPTICALFLOW_ROTATION_YAW_270
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_LEFT AP_OPTICALFLOW_ROTATION_YAW_225
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD ROTATION_YAW_180
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_RIGHT ROTATION_YAW_135
#define AP_OPTICALFLOW_ADNS3080_PINS_RIGHT ROTATION_YAW_90
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_RIGHT ROTATION_YAW_45
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK ROTATION_NONE
#define AP_OPTICALFLOW_ADNS3080_PINS_BACK_LEFT ROTATION_YAW_315
#define AP_OPTICALFLOW_ADNS3080_PINS_LEFT ROTATION_YAW_270
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_LEFT ROTATION_YAW_225
// field of view of ADNS3080 sensor lenses
#define AP_OPTICALFLOW_ADNS3080_08_FOV 0.202458 // 11.6 degrees