From 34c7555fd14feed4c9b3265cd0d9286dc1715865 Mon Sep 17 00:00:00 2001 From: "rmackay9@yahoo.com" Date: Wed, 11 May 2011 12:49:19 +0000 Subject: [PATCH] AP_OpticalFlow - added set_orientation git-svn-id: https://arducopter.googlecode.com/svn/trunk@2245 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_OpticalFlow/AP_OpticalFlow.cpp | 79 +++++++++++++++- libraries/AP_OpticalFlow/AP_OpticalFlow.h | 43 ++++++--- .../AP_OpticalFlow_ADNS3080.cpp | 90 ++++++++++++------- .../AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h | 41 ++++++--- .../AP_OpticalFlow_test.pde | 9 +- 5 files changed, 205 insertions(+), 57 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 875cbdfba2..492b93e796 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -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; } \ No newline at end of file diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index 87a6dabcad..8c490d4ad8 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -18,29 +18,52 @@ */ -#include -#include -#include -#include "WConstants.h" +#include "WProgram.h" +#include // 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" diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index 326f0c5b4d..71cd7110f3 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -24,7 +24,7 @@ */ #include "AP_OpticalFlow_ADNS3080.h" -#include +#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; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index 2d8ec27968..0e1589d19a 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h @@ -1,8 +1,27 @@ #ifndef AP_OPTICALFLOW_ADNS3080_H #define AP_OPTICALFLOW_ADNS3080_H +#include +#include #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 diff --git a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde index 2b6c674e13..aca9007132 100644 --- a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde +++ b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde @@ -3,7 +3,7 @@ Code by Randy Mackay. DIYDrones.com */ -#include +#include // ArduPilot Mega Vector/Matrix math Library #include // 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; //}