mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
AP_OpticalFlow - added set_orientation
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2245 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com
parent
57338c488a
commit
4ce3cb9be8
@ -10,13 +10,24 @@
|
||||
|
||||
#include "AP_OpticalFlow.h"
|
||||
|
||||
AP_OpticalFlow::AP_OpticalFlow() : x(0),y(0),surface_quality(0),dx(0),dy(0)
|
||||
AP_OpticalFlow::AP_OpticalFlow() : raw_dx(0),raw_dy(0),x(0),y(0),surface_quality(0),dx(0),dy(0),last_update(0),field_of_view(1),scaler(0),num_pixels(0)
|
||||
{
|
||||
}
|
||||
|
||||
// init - initCommAPI 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)
|
||||
void AP_OpticalFlow::init(boolean initCommAPI)
|
||||
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
|
||||
}
|
||||
|
||||
// set_orientation - Rotation vector to transform sensor readings to the body frame.
|
||||
void
|
||||
AP_OpticalFlow::set_orientation(const Matrix3f &rotation_matrix)
|
||||
{
|
||||
_orientation_matrix = rotation_matrix;
|
||||
}
|
||||
|
||||
// read latest values from sensor and fill in x,y and totals
|
||||
@ -25,11 +36,71 @@ int AP_OpticalFlow::read()
|
||||
}
|
||||
|
||||
// reads a value from the sensor (will be sensor specific)
|
||||
byte AP_OpticalFlow::read_register(byte address)
|
||||
byte
|
||||
AP_OpticalFlow::read_register(byte address)
|
||||
{
|
||||
}
|
||||
|
||||
// writes a value to one of the sensor's register (will be sensor specific)
|
||||
void AP_OpticalFlow::write_register(byte address, byte value)
|
||||
void
|
||||
AP_OpticalFlow::write_register(byte address, byte value)
|
||||
{
|
||||
}
|
||||
|
||||
// rotate raw values to arrive at final x,y,dx and dy values
|
||||
void
|
||||
AP_OpticalFlow::apply_orientation_matrix()
|
||||
{
|
||||
Vector3f rot_vector;
|
||||
|
||||
// next rotate dx and dy
|
||||
rot_vector = _orientation_matrix * Vector3f(raw_dx,raw_dy,0);
|
||||
dx = rot_vector.x;
|
||||
dy = rot_vector.y;
|
||||
|
||||
// add rotated values to totals (perhaps this is pointless as we need to take into account yaw, roll, pitch)
|
||||
x += dx;
|
||||
y += dy;
|
||||
}
|
||||
|
||||
// updatse conversion factors that are dependent upon field_of_view
|
||||
void
|
||||
AP_OpticalFlow::update_conversion_factors()
|
||||
{
|
||||
conv_factor = (1.0/(float)(num_pixels*scaler))*2.0*tan(field_of_view/2.0); // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude)
|
||||
radians_to_pixels = (num_pixels*scaler) / field_of_view;
|
||||
}
|
||||
|
||||
// updates internal lon and lat with estimation based on optical flow
|
||||
void
|
||||
AP_OpticalFlow::get_position(float roll, float pitch, float yaw, float altitude)
|
||||
{
|
||||
float diff_roll = roll - _last_roll;
|
||||
float diff_pitch = pitch - _last_pitch;
|
||||
float avg_altitude = (altitude + _last_altitude)/2;
|
||||
float exp_change_x, exp_change_y;
|
||||
float change_x, change_y;
|
||||
float x_cm, y_cm;
|
||||
int i;
|
||||
|
||||
// calculate expected x,y diff due to roll and pitch change
|
||||
exp_change_x = diff_roll * radians_to_pixels;
|
||||
exp_change_y = diff_pitch * radians_to_pixels;
|
||||
|
||||
// real estimated raw change from mouse
|
||||
change_x = dx - exp_change_x;
|
||||
change_y = dy - exp_change_y;
|
||||
|
||||
// convert raw change to horizontal movement in cm
|
||||
x_cm = change_x * avg_altitude * conv_factor;
|
||||
y_cm = change_y * avg_altitude * conv_factor;
|
||||
|
||||
// use yaw to convert x and y into lon and lat
|
||||
lng += y_cm * cos(yaw) + x_cm * sin(yaw);
|
||||
lat += x_cm * cos(yaw) + y_cm * sin(yaw);
|
||||
|
||||
// capture roll and pitch for next iteration
|
||||
_last_roll = roll;
|
||||
_last_pitch = pitch;
|
||||
_last_altitude = altitude;
|
||||
}
|
||||
@ -18,29 +18,52 @@
|
||||
|
||||
*/
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <inttypes.h>
|
||||
#include <math.h>
|
||||
#include "WConstants.h"
|
||||
#include "WProgram.h"
|
||||
#include <AP_Math.h>
|
||||
|
||||
// return value definition
|
||||
#define OPTICALFLOW_FAIL 0
|
||||
#define OPTICALFLOW_SUCCESS 1
|
||||
|
||||
// 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:
|
||||
int x, y; // total x,y position
|
||||
int dx, dy; // change in x and y position
|
||||
public:
|
||||
int raw_dx, raw_dy; // raw sensor change in x and y position (i.e. unrotated)
|
||||
int surface_quality; // image quality (below 15 you really can't trust the x,y values returned)
|
||||
int x,y; // total x,y position
|
||||
int dx,dy; // rotated change in x and y position
|
||||
unsigned long lng, lat; // position
|
||||
unsigned long last_update; // millis() time of last update
|
||||
float field_of_view; // field of view in Radians
|
||||
float scaler; // number returned from sensor when moved one pixel
|
||||
int num_pixels; // number of pixels of resolution in the sensor
|
||||
public:
|
||||
AP_OpticalFlow(); // Constructor
|
||||
virtual void init(boolean 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 int read(); // read latest values from sensor and fill in x,y and totals
|
||||
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_field_of_view(const float fov) { field_of_view = fov; update_conversion_factors(); }; // sets field of view of sensor
|
||||
virtual int read(); // read latest values from sensor and fill in x,y and totals
|
||||
virtual void get_position(float roll, float pitch, float yaw, float altitude); // updates internal lon and lat with estimation based on optical flow
|
||||
|
||||
private:
|
||||
protected:
|
||||
Matrix3f _orientation_matrix;
|
||||
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_yaw, _last_altitude;
|
||||
virtual void apply_orientation_matrix(); // rotate raw values to arrive at final x,y,dx and dy values
|
||||
virtual void update_conversion_factors();
|
||||
};
|
||||
|
||||
#include "AP_OpticalFlow_ADNS3080.h"
|
||||
|
||||
@ -24,7 +24,7 @@
|
||||
*/
|
||||
|
||||
#include "AP_OpticalFlow_ADNS3080.h"
|
||||
#include <avr/interrupt.h>
|
||||
#include "WProgram.h"
|
||||
#include "../SPI/SPI.h"
|
||||
|
||||
#define AP_SPI_TIMEOUT 1000
|
||||
@ -40,12 +40,16 @@ union NumericIntType
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080()
|
||||
{
|
||||
num_pixels = ADNS3080_PIXELS_X;
|
||||
field_of_view = AP_OPTICALFLOW_ADNS3080_12_FOV;
|
||||
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
// init - initialise sensor
|
||||
// initCommAPI parameter controls whether SPI interface is initialised (set to false if other devices are on the SPI bus and have already initialised the interface)
|
||||
void AP_OpticalFlow_ADNS3080::init(boolean initCommAPI)
|
||||
bool
|
||||
AP_OpticalFlow_ADNS3080::init(bool initCommAPI)
|
||||
{
|
||||
pinMode(AP_SPI_DATAOUT,OUTPUT);
|
||||
pinMode(AP_SPI_DATAIN,INPUT);
|
||||
@ -70,7 +74,8 @@ void AP_OpticalFlow_ADNS3080::init(boolean initCommAPI)
|
||||
//
|
||||
// backup_spi_settings - checks current SPI settings (clock speed, etc), sets values to what we need
|
||||
//
|
||||
byte AP_OpticalFlow_ADNS3080::backup_spi_settings()
|
||||
byte
|
||||
AP_OpticalFlow_ADNS3080::backup_spi_settings()
|
||||
{
|
||||
// store current spi values
|
||||
orig_spi_settings_spcr = SPCR & (DORD | CPOL | CPHA);
|
||||
@ -85,7 +90,8 @@ byte AP_OpticalFlow_ADNS3080::backup_spi_settings()
|
||||
}
|
||||
|
||||
// restore_spi_settings - restores SPI settings (clock speed, etc) to what their values were before the sensor used the bus
|
||||
byte AP_OpticalFlow_ADNS3080::restore_spi_settings()
|
||||
byte
|
||||
AP_OpticalFlow_ADNS3080::restore_spi_settings()
|
||||
{
|
||||
byte temp;
|
||||
|
||||
@ -105,7 +111,8 @@ byte AP_OpticalFlow_ADNS3080::restore_spi_settings()
|
||||
}
|
||||
|
||||
// Read a register from the sensor
|
||||
byte AP_OpticalFlow_ADNS3080::read_register(byte address)
|
||||
byte
|
||||
AP_OpticalFlow_ADNS3080::read_register(byte address)
|
||||
{
|
||||
byte result = 0, junk = 0;
|
||||
|
||||
@ -132,7 +139,8 @@ byte AP_OpticalFlow_ADNS3080::read_register(byte address)
|
||||
}
|
||||
|
||||
// write a value to one of the sensor's registers
|
||||
void AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
|
||||
void
|
||||
AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
|
||||
{
|
||||
byte junk = 0;
|
||||
|
||||
@ -157,7 +165,8 @@ void AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
|
||||
}
|
||||
|
||||
// reset sensor by holding a pin high (or is it low?) for 10us.
|
||||
void AP_OpticalFlow_ADNS3080::reset()
|
||||
void
|
||||
AP_OpticalFlow_ADNS3080::reset()
|
||||
{
|
||||
digitalWrite(ADNS3080_RESET,HIGH); // reset sensor
|
||||
delayMicroseconds(10);
|
||||
@ -165,35 +174,40 @@ void AP_OpticalFlow_ADNS3080::reset()
|
||||
}
|
||||
|
||||
// read latest values from sensor and fill in x,y and totals
|
||||
int AP_OpticalFlow_ADNS3080::read()
|
||||
int
|
||||
AP_OpticalFlow_ADNS3080::read()
|
||||
{
|
||||
surface_quality = (unsigned int)read_register(ADNS3080_SQUAL);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
||||
// check for movement, update x,y values
|
||||
if( (read_register(ADNS3080_MOTION) & 0x80) != 0 ) {
|
||||
dx = ((char)read_register(ADNS3080_DELTA_X));
|
||||
raw_dx = ((char)read_register(ADNS3080_DELTA_X));
|
||||
delayMicroseconds(50); // small delay
|
||||
dy = ((char)read_register(ADNS3080_DELTA_Y));
|
||||
x+=dx;
|
||||
y+=dy;
|
||||
raw_dy = ((char)read_register(ADNS3080_DELTA_Y));
|
||||
_motion = true;
|
||||
}else{
|
||||
dx = 0;
|
||||
dy = 0;
|
||||
raw_dx = 0;
|
||||
raw_dy = 0;
|
||||
}
|
||||
|
||||
last_update = millis();
|
||||
|
||||
apply_orientation_matrix();
|
||||
|
||||
return OPTICALFLOW_SUCCESS;
|
||||
}
|
||||
|
||||
// get_led_always_on - returns true if LED is always on, false if only on when required
|
||||
boolean AP_OpticalFlow_ADNS3080::get_led_always_on()
|
||||
bool
|
||||
AP_OpticalFlow_ADNS3080::get_led_always_on()
|
||||
{
|
||||
return ( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x40) > 0 );
|
||||
}
|
||||
|
||||
// set_led_always_on - set parameter to true if you want LED always on, otherwise false for only when required
|
||||
void AP_OpticalFlow_ADNS3080::set_led_always_on( boolean alwaysOn )
|
||||
void
|
||||
AP_OpticalFlow_ADNS3080::set_led_always_on( bool alwaysOn )
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
|
||||
regVal = regVal & 0xBf | (alwaysOn << 6);
|
||||
@ -202,7 +216,8 @@ void AP_OpticalFlow_ADNS3080::set_led_always_on( boolean alwaysOn )
|
||||
}
|
||||
|
||||
// returns resolution (either 400 or 1200 counts per inch)
|
||||
int AP_OpticalFlow_ADNS3080::get_resolution()
|
||||
int
|
||||
AP_OpticalFlow_ADNS3080::get_resolution()
|
||||
{
|
||||
if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
|
||||
return 400;
|
||||
@ -211,7 +226,8 @@ int AP_OpticalFlow_ADNS3080::get_resolution()
|
||||
}
|
||||
|
||||
// set parameter to 400 or 1200 counts per inch
|
||||
void AP_OpticalFlow_ADNS3080::set_resolution(int resolution)
|
||||
void
|
||||
AP_OpticalFlow_ADNS3080::set_resolution(int resolution)
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
|
||||
|
||||
@ -226,7 +242,8 @@ void AP_OpticalFlow_ADNS3080::set_resolution(int resolution)
|
||||
}
|
||||
|
||||
// get_frame_rate_auto - return whether frame rate is set to "auto" or manual
|
||||
boolean AP_OpticalFlow_ADNS3080::get_frame_rate_auto()
|
||||
bool
|
||||
AP_OpticalFlow_ADNS3080::get_frame_rate_auto()
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||
if( regVal & 0x01 > 0 ) {
|
||||
@ -237,7 +254,8 @@ boolean AP_OpticalFlow_ADNS3080::get_frame_rate_auto()
|
||||
}
|
||||
|
||||
// set_frame_rate_auto - set frame rate to auto (true) or manual (false)
|
||||
void AP_OpticalFlow_ADNS3080::set_frame_rate_auto(boolean auto_frame_rate)
|
||||
void
|
||||
AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate)
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||
delayMicroseconds(50); // small delay
|
||||
@ -258,7 +276,8 @@ void AP_OpticalFlow_ADNS3080::set_frame_rate_auto(boolean auto_frame_rate)
|
||||
}
|
||||
|
||||
// get frame period
|
||||
unsigned int AP_OpticalFlow_ADNS3080::get_frame_period()
|
||||
unsigned int
|
||||
AP_OpticalFlow_ADNS3080::get_frame_period()
|
||||
{
|
||||
NumericIntType aNum;
|
||||
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
|
||||
@ -268,7 +287,8 @@ unsigned int AP_OpticalFlow_ADNS3080::get_frame_period()
|
||||
}
|
||||
|
||||
// set frame period
|
||||
void AP_OpticalFlow_ADNS3080::set_frame_period(unsigned int period)
|
||||
void
|
||||
AP_OpticalFlow_ADNS3080::set_frame_period(unsigned int period)
|
||||
{
|
||||
NumericIntType aNum;
|
||||
aNum.uintValue = period;
|
||||
@ -284,14 +304,16 @@ void AP_OpticalFlow_ADNS3080::set_frame_period(unsigned int period)
|
||||
|
||||
}
|
||||
|
||||
unsigned int AP_OpticalFlow_ADNS3080::get_frame_rate()
|
||||
unsigned int
|
||||
AP_OpticalFlow_ADNS3080::get_frame_rate()
|
||||
{
|
||||
unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
|
||||
unsigned int rate = clockSpeed / get_frame_period();
|
||||
return rate;
|
||||
}
|
||||
|
||||
void AP_OpticalFlow_ADNS3080::set_frame_rate(unsigned int rate)
|
||||
void
|
||||
AP_OpticalFlow_ADNS3080::set_frame_rate(unsigned int rate)
|
||||
{
|
||||
unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
|
||||
unsigned int period = (unsigned int)(clockSpeed / (unsigned long)rate);
|
||||
@ -300,7 +322,8 @@ void AP_OpticalFlow_ADNS3080::set_frame_rate(unsigned int rate)
|
||||
}
|
||||
|
||||
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
||||
boolean AP_OpticalFlow_ADNS3080::get_shutter_speed_auto()
|
||||
bool
|
||||
AP_OpticalFlow_ADNS3080::get_shutter_speed_auto()
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||
if( (regVal & 0x02) > 0 ) {
|
||||
@ -311,7 +334,8 @@ boolean AP_OpticalFlow_ADNS3080::get_shutter_speed_auto()
|
||||
}
|
||||
|
||||
// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
|
||||
void AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(boolean auto_shutter_speed)
|
||||
void
|
||||
AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed)
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
|
||||
delayMicroseconds(50); // small delay
|
||||
@ -333,7 +357,8 @@ void AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(boolean auto_shutter_speed)
|
||||
}
|
||||
|
||||
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
||||
unsigned int AP_OpticalFlow_ADNS3080::get_shutter_speed()
|
||||
unsigned int
|
||||
AP_OpticalFlow_ADNS3080::get_shutter_speed()
|
||||
{
|
||||
NumericIntType aNum;
|
||||
aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER);
|
||||
@ -344,7 +369,8 @@ unsigned int AP_OpticalFlow_ADNS3080::get_shutter_speed()
|
||||
|
||||
|
||||
// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
|
||||
unsigned int AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_speed)
|
||||
unsigned int
|
||||
AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_speed)
|
||||
{
|
||||
NumericIntType aNum;
|
||||
aNum.uintValue = shutter_speed;
|
||||
@ -374,7 +400,8 @@ unsigned int AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_spe
|
||||
}
|
||||
|
||||
// clear_motion - will cause the Delta_X, Delta_Y, and internal motion registers to be cleared
|
||||
void AP_OpticalFlow_ADNS3080::clear_motion()
|
||||
void
|
||||
AP_OpticalFlow_ADNS3080::clear_motion()
|
||||
{
|
||||
write_register(ADNS3080_MOTION_CLEAR,0xFF); // writing anything to this register will clear the sensor's motion registers
|
||||
x = 0;
|
||||
@ -385,10 +412,11 @@ void AP_OpticalFlow_ADNS3080::clear_motion()
|
||||
}
|
||||
|
||||
// get_pixel_data - captures an image from the sensor and stores it to the pixe_data array
|
||||
int AP_OpticalFlow_ADNS3080::print_pixel_data(HardwareSerial *serPort)
|
||||
int
|
||||
AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort)
|
||||
{
|
||||
int i,j;
|
||||
boolean isFirstPixel = true;
|
||||
bool isFirstPixel = true;
|
||||
byte regValue;
|
||||
byte pixelValue;
|
||||
|
||||
|
||||
@ -1,8 +1,27 @@
|
||||
#ifndef AP_OPTICALFLOW_ADNS3080_H
|
||||
#define AP_OPTICALFLOW_ADNS3080_H
|
||||
|
||||
#include <AP_Math.h>
|
||||
#include <Stream.h>
|
||||
#include "AP_OpticalFlow.h"
|
||||
#include "HardwareSerial.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
|
||||
|
||||
// field of view of ADNS3080 sensor lenses
|
||||
#define AP_OPTICALFLOW_ADNS3080_04_FOV 80
|
||||
#define AP_OPTICALFLOW_ADNS3080_08_FOV 50
|
||||
#define AP_OPTICALFLOW_ADNS3080_12_FOV 20
|
||||
|
||||
// scaler - value returned when sensor is moved equivalent of 1 pixel
|
||||
#define AP_OPTICALFLOW_ADNS3080_SCALER 10.5
|
||||
|
||||
// We use Serial Port 2 in SPI Mode
|
||||
#define AP_SPI_DATAIN 50 // MISO // PB3
|
||||
@ -71,27 +90,27 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
|
||||
byte backup_spi_settings();
|
||||
byte restore_spi_settings();
|
||||
|
||||
boolean _motion; // true if there has been motion
|
||||
bool _motion; // true if there has been motion
|
||||
|
||||
public:
|
||||
AP_OpticalFlow_ADNS3080(); // Constructor
|
||||
void init(boolean 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)
|
||||
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)
|
||||
byte read_register(byte address);
|
||||
void write_register(byte address, byte value);
|
||||
void reset(); // reset sensor by holding a pin high (or is it low?) for 10us.
|
||||
int read(); // read latest values from sensor and fill in x,y and totals, return OPTICALFLOW_SUCCESS on successful read
|
||||
|
||||
// ADNS3080 specific features
|
||||
boolean motion() { if( _motion ) { _motion = false; return true; }else{ return false; } } // return true if there has been motion since the last time this was called
|
||||
bool motion() { if( _motion ) { _motion = false; return true; }else{ return false; } } // return true if there has been motion since the last time this was called
|
||||
|
||||
boolean get_led_always_on(); // returns true if LED is always on, false if only on when required
|
||||
void set_led_always_on( boolean alwaysOn ); // set parameter to true if you want LED always on, otherwise false for only when required
|
||||
bool get_led_always_on(); // returns true if LED is always on, false if only on when required
|
||||
void set_led_always_on( bool alwaysOn ); // set parameter to true if you want LED always on, otherwise false for only when required
|
||||
|
||||
int get_resolution(); // returns resolution (either 400 or 1200 counts per inch)
|
||||
void set_resolution(int resolution); // set parameter to 400 or 1200 counts per inch
|
||||
|
||||
boolean get_frame_rate_auto(); // get_frame_rate_auto - return true if frame rate is set to "auto", false if manual
|
||||
void set_frame_rate_auto(boolean auto_frame_rate); // set_frame_rate_auto(boolean) - set frame rate to auto (true), or manual (false)
|
||||
bool get_frame_rate_auto(); // get_frame_rate_auto - return true if frame rate is set to "auto", false if manual
|
||||
void set_frame_rate_auto(bool auto_frame_rate); // set_frame_rate_auto(bool) - set frame rate to auto (true), or manual (false)
|
||||
|
||||
unsigned int get_frame_period(); // get_frame_period -
|
||||
void set_frame_period(unsigned int period);
|
||||
@ -99,15 +118,15 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
|
||||
unsigned int get_frame_rate();
|
||||
void set_frame_rate(unsigned int rate);
|
||||
|
||||
boolean get_shutter_speed_auto(); // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
||||
void set_shutter_speed_auto(boolean auto_shutter_speed); // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
|
||||
bool get_shutter_speed_auto(); // get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
||||
void set_shutter_speed_auto(bool auto_shutter_speed); // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
|
||||
|
||||
unsigned int get_shutter_speed();
|
||||
unsigned int set_shutter_speed(unsigned int shutter_speed);
|
||||
|
||||
void clear_motion(); // will cause the x,y, dx, dy, and the sensor's motion registers to be cleared
|
||||
|
||||
int print_pixel_data(HardwareSerial *serPort); // dumps a 30x30 image to the Serial port
|
||||
int print_pixel_data(Stream *serPort); // dumps a 30x30 image to the Serial port
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
Code by Randy Mackay. DIYDrones.com
|
||||
*/
|
||||
|
||||
#include <avr/interrupt.h>
|
||||
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
#include <SPI.h> // Arduino SPI library
|
||||
#include "AP_OpticalFlow.h" // ArduCopter OpticalFlow Library
|
||||
|
||||
@ -17,6 +17,8 @@ void setup()
|
||||
delay(1000);
|
||||
|
||||
flowSensor.init(); // flowSensor initialization
|
||||
flowSensor.set_orientation(AP_OPTICALFLOW_ADNS3080_PINS_FORWARD);
|
||||
flowSensor.set_field_of_view(AP_OPTICALFLOW_ADNS3080_12_FOV);
|
||||
|
||||
delay(1000);
|
||||
}
|
||||
@ -293,6 +295,7 @@ void display_motion()
|
||||
|
||||
while( !Serial.available() ) {
|
||||
flowSensor.read();
|
||||
flowSensor.get_position(0,0,0,100);
|
||||
|
||||
// x,y,squal
|
||||
//if( flowSensor.motion() || first_time ) {
|
||||
@ -306,6 +309,10 @@ void display_motion()
|
||||
Serial.print(flowSensor.dy,DEC);
|
||||
Serial.print("\tsqual:");
|
||||
Serial.print(flowSensor.surface_quality,DEC);
|
||||
Serial.print("\tlat:");
|
||||
Serial.print(flowSensor.lat,DEC);
|
||||
Serial.print("\tlng:");
|
||||
Serial.print(flowSensor.lng,DEC);
|
||||
Serial.println();
|
||||
first_time = false;
|
||||
//}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user