added sin and cos yaw from DCM, renamed "get" function because it doesn't return a value.

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

@@ -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
 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
 }
@@ -56,7 +52,7 @@ 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;
 
@@ -69,26 +65,22 @@ 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)
+	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;
+	// 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_roll 	= roll  - _last_roll;
-    float diff_pitch = pitch - _last_pitch;
+    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;
 
 	// 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;
@@ -101,13 +93,35 @@ AP_OpticalFlow::get_position(float roll, float pitch, float yaw, float altitude)
 		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;
-	}
 
-    // capture roll and pitch for next iteration
+		vlon =  x_cm * sin_yaw_y - y_cm * cos_yaw_x;
-    _last_roll = roll;
+		vlat =  y_cm * sin_yaw_y - x_cm * cos_yaw_x;
-    _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;
+	}
+}
+
+*/

libraries/AP_OpticalFlow/AP_OpticalFlow.h

@@ -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
@@ -51,15 +51,17 @@ class AP_OpticalFlow
     float exp_change_x, exp_change_y;
     float change_x, change_y;
 	float x_cm, y_cm;
-  public:
+
-	AP_OpticalFlow();  // Constructor
+	//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;
@@ -70,6 +72,4 @@ class AP_OpticalFlow
 	virtual void update_conversion_factors();
 };
 
-#include "AP_OpticalFlow_ADNS3080.h"
-
 #endif

libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp

@@ -36,15 +36,15 @@ union NumericIntType
    byte        byteValue[2];
 };
 
-
+	// Constructors ////////////////////////////////////////////////////////////////
-// Constructors ////////////////////////////////////////////////////////////////
 AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080()
 {
-    num_pixels = ADNS3080_PIXELS_X;
+	num_pixels = ADNS3080_PIXELS_X;
-    field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
+	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)

libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h

@@ -1,8 +1,10 @@
 #ifndef AP_OPTICALFLOW_ADNS3080_H
 #define AP_OPTICALFLOW_ADNS3080_H
 
-#include <AP_Math.h>
+//#include <AP_Math.h>
-#include <Stream.h>
+//#include <Stream.h>
+//#include <AP_Common.h>
+//#include "AP_OpticalFlow.h"
 #include "AP_OpticalFlow.h"
 
 // orientations for ADNS3080 sensor
@@ -99,7 +101,8 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
 	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);