mirror of https://github.com/ArduPilot/ardupilot
added sin and cos yaw from DCM, renamed "get" function because it doesn't return a value.
This commit is contained in:
Jason Short
parent
f79b8de5e8
commit
45778a4e81
|
|
@ -12,15 +12,11 @@
|
|||
|
||||
#define FORTYFIVE_DEGREES 0.78539816
|
||||
|
||||
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)
|
||||
bool
|
||||
bool
|
||||
AP_OpticalFlow::init(bool initCommAPI)
|
||||
{
|
||||
_orientation_matrix = Matrix3f(1,0,0,0,1,0,0,0,1);
|
||||
_orientation_matrix = Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1);
|
||||
update_conversion_factors();
|
||||
return true; // just return true by default
|
||||
}
|
||||
|
|
@ -38,28 +34,28 @@ int AP_OpticalFlow::read()
|
|||
}
|
||||
|
||||
// reads a value from the sensor (will be sensor specific)
|
||||
byte
|
||||
byte
|
||||
AP_OpticalFlow::read_register(byte address)
|
||||
{
|
||||
}
|
||||
|
||||
// writes a value to one of the sensor's register (will be sensor specific)
|
||||
void
|
||||
void
|
||||
AP_OpticalFlow::write_register(byte address, byte value)
|
||||
{
|
||||
}
|
||||
|
||||
// rotate raw values to arrive at final x,y,dx and dy values
|
||||
void
|
||||
void
|
||||
AP_OpticalFlow::apply_orientation_matrix()
|
||||
{
|
||||
Vector3f rot_vector;
|
||||
|
||||
|
||||
// next rotate dx and dy
|
||||
rot_vector = _orientation_matrix * Vector3f(raw_dx,raw_dy,0);
|
||||
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;
|
||||
|
|
@ -69,30 +65,26 @@ AP_OpticalFlow::apply_orientation_matrix()
|
|||
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;
|
||||
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)
|
||||
// 0.00615
|
||||
radians_to_pixels = (num_pixels * scaler) / field_of_view;
|
||||
// 162.99
|
||||
}
|
||||
|
||||
// updates internal lon and lat with estimation based on optical flow
|
||||
void
|
||||
AP_OpticalFlow::get_position(float roll, float pitch, float yaw, float altitude)
|
||||
AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, 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;
|
||||
float cos_yaw = cos(yaw);
|
||||
float sin_yaw = sin(yaw);
|
||||
int i;
|
||||
|
||||
float diff_roll = roll - _last_roll;
|
||||
float diff_pitch = pitch - _last_pitch;
|
||||
|
||||
// only update position if surface quality is good and angle is not over 45 degrees
|
||||
if( surface_quality >= 10 && fabs(roll) <= FORTYFIVE_DEGREES && fabs(pitch) <= FORTYFIVE_DEGREES ) {
|
||||
altitude = max(altitude, 0);
|
||||
// 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;
|
||||
|
|
@ -100,14 +92,36 @@ AP_OpticalFlow::get_position(float roll, float pitch, float yaw, float altitude)
|
|||
// convert raw change to horizontal movement in cm
|
||||
x_cm = -change_x * avg_altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer?
|
||||
y_cm = -change_y * 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
|
||||
|
||||
// use yaw to convert x and y into lon and lat
|
||||
lat += y_cm * cos_yaw - x_cm * sin_yaw;
|
||||
lng += x_cm * cos_yaw + y_cm * sin_yaw;
|
||||
|
||||
|
||||
vlon = x_cm * sin_yaw_y - y_cm * cos_yaw_x;
|
||||
vlat = y_cm * sin_yaw_y - x_cm * cos_yaw_x;
|
||||
}
|
||||
|
||||
// capture roll and pitch for next iteration
|
||||
_last_roll = roll;
|
||||
_last_pitch = pitch;
|
||||
_last_altitude = altitude;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
{
|
||||
// only update position if surface quality is good and angle is not over 45 degrees
|
||||
if( surface_quality >= 10 && fabs(_dcm->roll) <= FORTYFIVE_DEGREES && fabs(_dcm->pitch) <= FORTYFIVE_DEGREES ) {
|
||||
altitude = max(altitude, 0);
|
||||
Vector3f omega = _dcm->get_gyro();
|
||||
|
||||
// calculate expected x,y diff due to roll and pitch change
|
||||
float exp_change_x = omega.x * radians_to_pixels;
|
||||
float exp_change_y = -omega.y * radians_to_pixels;
|
||||
|
||||
// real estimated raw change from mouse
|
||||
float change_x = dx - exp_change_x;
|
||||
float change_y = dy - exp_change_y;
|
||||
|
||||
// convert raw change to horizontal movement in cm
|
||||
float x_cm = -change_x * altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer?
|
||||
float y_cm = -change_y * 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
|
||||
|
||||
vlon = (float)x_cm * sin_yaw_y - (float)y_cm * cos_yaw_x;
|
||||
vlat = (float)y_cm * sin_yaw_y - (float)x_cm * cos_yaw_x;
|
||||
}
|
||||
}
|
||||
|
||||
*/
|
||||
|
|
@ -15,11 +15,11 @@
|
|||
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 "WProgram.h"
|
||||
#include <AP_Math.h>
|
||||
#include <AP_DCM.h>
|
||||
#include <AP_Common.h>
|
||||
|
||||
// return value definition
|
||||
#define OPTICALFLOW_FAIL 0
|
||||
|
|
@ -37,12 +37,12 @@
|
|||
|
||||
class AP_OpticalFlow
|
||||
{
|
||||
public:
|
||||
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
|
||||
float lng, lat; // position as offsets from original position
|
||||
float vlon, vlat; // position as offsets from original 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
|
||||
|
|
@ -50,17 +50,19 @@ class AP_OpticalFlow
|
|||
// temp variables - delete me!
|
||||
float exp_change_x, exp_change_y;
|
||||
float change_x, change_y;
|
||||
float x_cm, y_cm;
|
||||
public:
|
||||
AP_OpticalFlow(); // Constructor
|
||||
float x_cm, y_cm;
|
||||
|
||||
//AP_OpticalFlow(AP_DCM *dcm);
|
||||
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
|
||||
|
||||
virtual void update_position(float roll, float pitch, float cos_yaw_x, float sin_yaw_y, float altitude); // updates internal lon and lat with estimation based on optical flow
|
||||
|
||||
|
||||
|
||||
//protected:
|
||||
Matrix3f _orientation_matrix;
|
||||
float conv_factor; // multiply this number by altitude and pixel change to get horizontal move (in same units as altitude)
|
||||
|
|
@ -70,6 +72,4 @@ class AP_OpticalFlow
|
|||
virtual void update_conversion_factors();
|
||||
};
|
||||
|
||||
#include "AP_OpticalFlow_ADNS3080.h"
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -33,18 +33,18 @@ union NumericIntType
|
|||
{
|
||||
int intValue;
|
||||
unsigned int uintValue;
|
||||
byte byteValue[2];
|
||||
byte byteValue[2];
|
||||
};
|
||||
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080()
|
||||
{
|
||||
num_pixels = ADNS3080_PIXELS_X;
|
||||
field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
|
||||
num_pixels = ADNS3080_PIXELS_X;
|
||||
field_of_view = AP_OPTICALFLOW_ADNS3080_08_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)
|
||||
|
|
@ -52,7 +52,7 @@ bool
|
|||
AP_OpticalFlow_ADNS3080::init(bool initCommAPI)
|
||||
{
|
||||
int retry = 0;
|
||||
|
||||
|
||||
pinMode(AP_SPI_DATAOUT,OUTPUT);
|
||||
pinMode(AP_SPI_DATAIN,INPUT);
|
||||
pinMode(AP_SPI_CLOCK,OUTPUT);
|
||||
|
|
@ -63,12 +63,12 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI)
|
|||
|
||||
// reset the device
|
||||
reset();
|
||||
|
||||
|
||||
// start the SPI library:
|
||||
if( initCommAPI ) {
|
||||
SPI.begin();
|
||||
}
|
||||
|
||||
|
||||
// check the sensor is functioning
|
||||
if( retry < 3 ) {
|
||||
if( read_register(ADNS3080_PRODUCT_ID) == 0x17 )
|
||||
|
|
@ -84,16 +84,16 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI)
|
|||
//
|
||||
byte
|
||||
AP_OpticalFlow_ADNS3080::backup_spi_settings()
|
||||
{
|
||||
{
|
||||
// store current spi values
|
||||
orig_spi_settings_spcr = SPCR & (DORD | CPOL | CPHA);
|
||||
orig_spi_settings_spsr = SPSR & SPI2X;
|
||||
|
||||
|
||||
// set the values that we need
|
||||
SPI.setBitOrder(MSBFIRST);
|
||||
SPI.setDataMode(SPI_MODE3);
|
||||
SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed
|
||||
|
||||
SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz. this is it's maximum speed
|
||||
|
||||
return orig_spi_settings_spcr;
|
||||
}
|
||||
|
||||
|
|
@ -102,7 +102,7 @@ byte
|
|||
AP_OpticalFlow_ADNS3080::restore_spi_settings()
|
||||
{
|
||||
byte temp;
|
||||
|
||||
|
||||
// restore SPSR
|
||||
temp = SPSR;
|
||||
temp &= ~SPI2X;
|
||||
|
|
@ -114,35 +114,35 @@ AP_OpticalFlow_ADNS3080::restore_spi_settings()
|
|||
temp &= ~(DORD | CPOL | CPHA); // zero out the important bits
|
||||
temp |= orig_spi_settings_spcr; // restore important bits
|
||||
SPCR = temp;
|
||||
|
||||
|
||||
return temp;
|
||||
}
|
||||
|
||||
// Read a register from the sensor
|
||||
byte
|
||||
AP_OpticalFlow_ADNS3080::read_register(byte address)
|
||||
AP_OpticalFlow_ADNS3080::read_register(byte address)
|
||||
{
|
||||
byte result = 0, junk = 0;
|
||||
|
||||
backup_spi_settings();
|
||||
|
||||
|
||||
// take the chip select low to select the device
|
||||
digitalWrite(ADNS3080_CHIP_SELECT, LOW);
|
||||
|
||||
|
||||
// send the device the register you want to read:
|
||||
junk = SPI.transfer(address);
|
||||
|
||||
|
||||
// small delay
|
||||
delayMicroseconds(50);
|
||||
|
||||
|
||||
// send a value of 0 to read the first byte returned:
|
||||
result = SPI.transfer(0x00);
|
||||
|
||||
|
||||
// take the chip select high to de-select:
|
||||
digitalWrite(ADNS3080_CHIP_SELECT, HIGH);
|
||||
|
||||
|
||||
restore_spi_settings();
|
||||
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -152,24 +152,24 @@ AP_OpticalFlow_ADNS3080::write_register(byte address, byte value)
|
|||
{
|
||||
byte junk = 0;
|
||||
|
||||
backup_spi_settings();
|
||||
|
||||
backup_spi_settings();
|
||||
|
||||
// take the chip select low to select the device
|
||||
digitalWrite(ADNS3080_CHIP_SELECT, LOW);
|
||||
|
||||
|
||||
// send register address
|
||||
junk = SPI.transfer(address | 0x80 );
|
||||
|
||||
|
||||
// small delay
|
||||
delayMicroseconds(50);
|
||||
|
||||
delayMicroseconds(50);
|
||||
|
||||
// send data
|
||||
junk = SPI.transfer(value);
|
||||
|
||||
|
||||
// take the chip select high to de-select:
|
||||
digitalWrite(ADNS3080_CHIP_SELECT, HIGH);
|
||||
|
||||
restore_spi_settings();
|
||||
|
||||
restore_spi_settings();
|
||||
}
|
||||
|
||||
// reset sensor by holding a pin high (or is it low?) for 10us.
|
||||
|
|
@ -187,7 +187,7 @@ 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 ) {
|
||||
raw_dx = ((char)read_register(ADNS3080_DELTA_X));
|
||||
|
|
@ -198,11 +198,11 @@ AP_OpticalFlow_ADNS3080::read()
|
|||
raw_dx = 0;
|
||||
raw_dy = 0;
|
||||
}
|
||||
|
||||
|
||||
last_update = millis();
|
||||
|
||||
|
||||
apply_orientation_matrix();
|
||||
|
||||
|
||||
return OPTICALFLOW_SUCCESS;
|
||||
}
|
||||
|
||||
|
|
@ -238,13 +238,13 @@ void
|
|||
AP_OpticalFlow_ADNS3080::set_resolution(int resolution)
|
||||
{
|
||||
byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
|
||||
|
||||
|
||||
if( resolution == ADNS3080_RESOLUTION_400 ) {
|
||||
regVal &= ~0x10;
|
||||
}else if( resolution == ADNS3080_RESOLUTION_1200) {
|
||||
}else if( resolution == ADNS3080_RESOLUTION_1200) {
|
||||
regVal |= 0x10;
|
||||
}
|
||||
|
||||
|
||||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
|
||||
}
|
||||
|
|
@ -273,7 +273,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate)
|
|||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER,0x1A);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
||||
|
||||
// decide what value to update in extended config
|
||||
regVal = (regVal & ~0x01);
|
||||
}else{
|
||||
|
|
@ -287,7 +287,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate_auto(bool auto_frame_rate)
|
|||
unsigned int
|
||||
AP_OpticalFlow_ADNS3080::get_frame_period()
|
||||
{
|
||||
NumericIntType aNum;
|
||||
NumericIntType aNum;
|
||||
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
|
||||
delayMicroseconds(50); // small delay
|
||||
aNum.byteValue[0] = read_register(ADNS3080_FRAME_PERIOD_LOWER);
|
||||
|
|
@ -300,11 +300,11 @@ AP_OpticalFlow_ADNS3080::set_frame_period(unsigned int period)
|
|||
{
|
||||
NumericIntType aNum;
|
||||
aNum.uintValue = period;
|
||||
|
||||
|
||||
// set frame rate to manual
|
||||
set_frame_rate_auto(false);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
||||
|
||||
// set specific frame period
|
||||
write_register(ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER,aNum.byteValue[0]);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
|
@ -325,7 +325,7 @@ AP_OpticalFlow_ADNS3080::set_frame_rate(unsigned int rate)
|
|||
{
|
||||
unsigned long clockSpeed = ADNS3080_CLOCK_SPEED;
|
||||
unsigned int period = (unsigned int)(clockSpeed / (unsigned long)rate);
|
||||
|
||||
|
||||
set_frame_period(period);
|
||||
}
|
||||
|
||||
|
|
@ -349,11 +349,11 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed)
|
|||
delayMicroseconds(50); // small delay
|
||||
if( auto_shutter_speed ) {
|
||||
// return shutter speed max to default
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c);
|
||||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,0x8c);
|
||||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,0x20);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
||||
delayMicroseconds(50); // small delay
|
||||
|
||||
// determine value to put into extended config
|
||||
regVal = regVal & ~0x02;
|
||||
}else{
|
||||
|
|
@ -361,41 +361,41 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed)
|
|||
regVal = regVal & ~0x02 | 0x02;
|
||||
}
|
||||
write_register(ADNS3080_EXTENDED_CONFIG, regVal);
|
||||
delayMicroseconds(50); // small delay
|
||||
delayMicroseconds(50); // small delay
|
||||
}
|
||||
|
||||
// get_shutter_speed_auto - returns true if shutter speed is adjusted automatically, false if manual
|
||||
unsigned int
|
||||
AP_OpticalFlow_ADNS3080::get_shutter_speed()
|
||||
{
|
||||
NumericIntType aNum;
|
||||
NumericIntType aNum;
|
||||
aNum.byteValue[1] = read_register(ADNS3080_SHUTTER_UPPER);
|
||||
delayMicroseconds(50); // small delay
|
||||
aNum.byteValue[0] = read_register(ADNS3080_SHUTTER_LOWER);
|
||||
return aNum.uintValue;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// 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)
|
||||
{
|
||||
NumericIntType aNum;
|
||||
aNum.uintValue = shutter_speed;
|
||||
|
||||
|
||||
// set shutter speed to manual
|
||||
set_shutter_speed_auto(false);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
||||
|
||||
// set specific shutter speed
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]);
|
||||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_LOWER,aNum.byteValue[0]);
|
||||
delayMicroseconds(50); // small delay
|
||||
write_register(ADNS3080_SHUTTER_MAX_BOUND_UPPER,aNum.byteValue[1]);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
||||
|
||||
// larger delay
|
||||
delay(50);
|
||||
|
||||
|
||||
// need to update frame period to cause shutter value to take effect
|
||||
aNum.byteValue[1] = read_register(ADNS3080_FRAME_PERIOD_UPPER);
|
||||
delayMicroseconds(50); // small delay
|
||||
|
|
@ -427,13 +427,13 @@ AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort)
|
|||
bool isFirstPixel = true;
|
||||
byte regValue;
|
||||
byte pixelValue;
|
||||
|
||||
|
||||
// write to frame capture register to force capture of frame
|
||||
write_register(ADNS3080_FRAME_CAPTURE,0x83);
|
||||
|
||||
|
||||
// wait 3 frame periods + 10 nanoseconds for frame to be captured
|
||||
delayMicroseconds(1510); // min frame speed is 2000 frames/second so 1 frame = 500 nano seconds. so 500 x 3 + 10 = 1510
|
||||
|
||||
|
||||
// display the pixel data
|
||||
for( i=0; i<ADNS3080_PIXELS_Y; i++ ) {
|
||||
for( j=0; j<ADNS3080_PIXELS_X; j++ ) {
|
||||
|
|
@ -450,7 +450,7 @@ AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort)
|
|||
}
|
||||
serPort->println();
|
||||
}
|
||||
|
||||
|
||||
// hardware reset to restore sensor to normal operation
|
||||
reset();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,8 +1,10 @@
|
|||
#ifndef AP_OPTICALFLOW_ADNS3080_H
|
||||
#define AP_OPTICALFLOW_ADNS3080_H
|
||||
|
||||
#include <AP_Math.h>
|
||||
#include <Stream.h>
|
||||
//#include <AP_Math.h>
|
||||
//#include <Stream.h>
|
||||
//#include <AP_Common.h>
|
||||
//#include "AP_OpticalFlow.h"
|
||||
#include "AP_OpticalFlow.h"
|
||||
|
||||
// orientations for ADNS3080 sensor
|
||||
|
|
@ -29,10 +31,10 @@
|
|||
#define ADNS3080_CHIP_SELECT 34 // PC3
|
||||
#define ADNS3080_RESET 40 // PG1 was 35 / PC2
|
||||
#else // normal arduino SPI pins...these need to be checked
|
||||
#define AP_SPI_DATAIN 12 //MISO
|
||||
#define AP_SPI_DATAIN 12 //MISO
|
||||
#define AP_SPI_DATAOUT 11 //MOSI
|
||||
#define AP_SPI_CLOCK 13 //SCK
|
||||
#define ADNS3080_CHIP_SELECT 10 //SS
|
||||
#define ADNS3080_CHIP_SELECT 10 //SS
|
||||
#define ADNS3080_RESET 9 //RESET
|
||||
#endif
|
||||
|
||||
|
|
@ -91,39 +93,40 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
|
|||
// bytes to store SPI settings
|
||||
byte orig_spi_settings_spcr;
|
||||
byte orig_spi_settings_spsr;
|
||||
|
||||
|
||||
// save and restore SPI settings
|
||||
byte backup_spi_settings();
|
||||
byte restore_spi_settings();
|
||||
|
||||
|
||||
bool _motion; // true if there has been motion
|
||||
|
||||
|
||||
public:
|
||||
AP_OpticalFlow_ADNS3080(); // Constructor
|
||||
AP_OpticalFlow_ADNS3080();
|
||||
//AP_OpticalFlow_ADNS3080(); // Constructor
|
||||
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
|
||||
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
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
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 -
|
||||
unsigned int get_frame_period(); // get_frame_period -
|
||||
void set_frame_period(unsigned int period);
|
||||
|
||||
unsigned int get_frame_rate();
|
||||
void set_frame_rate(unsigned int rate);
|
||||
|
||||
|
||||
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)
|
||||
|
||||
|
|
@ -131,7 +134,7 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
|
|||
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(Stream *serPort); // dumps a 30x30 image to the Serial port
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue