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

@@ -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;
+}

libraries/AP_OpticalFlow/AP_OpticalFlow.h

@@ -18,29 +18,52 @@
 
 */
 
-#include <stdlib.h>
+#include "WProgram.h"
-#include <inttypes.h>
+#include <AP_Math.h>
-#include <math.h>
-#include "WConstants.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 raw_dx, raw_dy;   // raw sensor change in x and y position (i.e. unrotated)
-	int dx, dy;  // change in x and y position
 	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 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 int read(); // read latest values from sensor and fill in x,y and totals
 	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"

libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp

@@ -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));
+		raw_dy = ((char)read_register(ADNS3080_DELTA_Y));
-		x+=dx;
-		y+=dy;
 		_motion = true;
 	}else{
-	    dx = 0;
+	    raw_dx = 0;
-		dy = 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;
 	

libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h

@@ -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
+	bool 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
+	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
+	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(boolean auto_frame_rate);  // set_frame_rate_auto(boolean) - set frame rate to auto (true), or manual (false)
+	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
+	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(boolean auto_shutter_speed); 	// set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
+	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

libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde

@@ -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;
 		//}