AP_OpticalFlow - fixed some bugs in get_position

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2380 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

@@ -78,9 +78,11 @@ 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;
+    //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;
     
     // calculate expected x,y diff due to roll and pitch change
@@ -92,12 +94,12 @@ AP_OpticalFlow::get_position(float roll, float pitch, float yaw, float altitude)
 	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;
+	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
-    lng += y_cm * cos(yaw) + x_cm * sin(yaw);
-	lat += x_cm * cos(yaw) + y_cm * sin(yaw);
+    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
     _last_roll = roll;

libraries/AP_OpticalFlow/AP_OpticalFlow.h

@@ -42,11 +42,15 @@ class AP_OpticalFlow
 	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	
+    float lng, lat;       // 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
 	int num_pixels;      // number of pixels of resolution in the sensor
+	// 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
 	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)
@@ -57,7 +61,7 @@ class AP_OpticalFlow
     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
 	
-protected:
+//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;

libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h

@@ -16,19 +16,27 @@
 #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
+#define AP_OPTICALFLOW_ADNS3080_04_FOV 1.396263 // 80 degrees
+#define AP_OPTICALFLOW_ADNS3080_08_FOV 0.872665 // 50 degrees
+#define AP_OPTICALFLOW_ADNS3080_12_FOV 0.349066 // 20 degrees
 
 // scaler - value returned when sensor is moved equivalent of 1 pixel
-#define AP_OPTICALFLOW_ADNS3080_SCALER  10.5
+#define AP_OPTICALFLOW_ADNS3080_SCALER  3.4   // was 10.5
 
 // We use Serial Port 2 in SPI Mode
-#define AP_SPI_DATAIN      50    // MISO  // PB3
-#define AP_SPI_DATAOUT     51    // MOSI  // PB2
-#define AP_SPI_CLOCK       52    // SCK   // PB1
-#define ADNS3080_CHIP_SELECT 32    // PC5 
-#define ADNS3080_RESET       33    // PC6
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+	#define AP_SPI_DATAIN      50    // MISO  // PB3
+	#define AP_SPI_DATAOUT     51    // MOSI  // PB2
+	#define AP_SPI_CLOCK       52    // SCK   // PB1
+	#define ADNS3080_CHIP_SELECT 32  // PC5  // should be changed to be 34/PC3
+	#define ADNS3080_RESET       35  // PC2
+#else  // normal arduino SPI pins...these need to be checked
+	#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_RESET       9   //RESET
+#endif
 
 // ADNS3080 hardware config
 #define ADNS3080_PIXELS_X                 30