fixed build of OpticalFlow code

ArduCopter/ArduCopter.pde

@@ -61,7 +61,7 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
 #include <APM_PI.h>            	// PI library
 #include <RC_Channel.h>     // RC Channel Library
 #include <AP_RangeFinder.h>	// Range finder library
-#include <AP_OpticalFlow_ADNS3080.h> // Optical Flow library
+#include <AP_OpticalFlow.h> // Optical Flow library
 #include <ModeFilter.h>
 #include <GCS_MAVLink.h>    // MAVLink GCS definitions
 #include <memcheck.h>

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

@@ -31,12 +31,14 @@ AP_OpticalFlow::set_orientation(const Matrix3f &rotation_matrix)
 // read latest values from sensor and fill in x,y and totals
 int AP_OpticalFlow::read()
 {
+	return 0;
 }
 
 // reads a value from the sensor (will be sensor specific)
 byte
 AP_OpticalFlow::read_register(byte address)
 {
+	return 0;
 }
 
 // writes a value to one of the sensor's register (will be sensor specific)
@@ -89,6 +91,8 @@ AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float
 		change_x = dx - exp_change_x;
 		change_y = dy - exp_change_y;
 
+		float avg_altitude = (altitude + _last_altitude)*0.5;
+
 		// 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
@@ -97,6 +101,10 @@ AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float
 		vlon =  x_cm * sin_yaw_y - y_cm * cos_yaw_x;
 		vlat =  y_cm * sin_yaw_y - x_cm * cos_yaw_x;
 	}
+
+	_last_altitude = altitude;
+	_last_roll = roll;
+	_last_pitch = pitch;
 }
 
 

libraries/AP_OpticalFlow/AP_OpticalFlow.h

@@ -67,9 +67,11 @@ class AP_OpticalFlow
 	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;
+	float _last_roll, _last_pitch, _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"
+
 #endif

libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp

@@ -73,9 +73,9 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI)
 	if( retry < 3 ) {
 	    if( read_register(ADNS3080_PRODUCT_ID) == 0x17 )
 	        return true;
-		else
 	    retry++;
-	}else
+	}
+
 	return false;
 }
 
@@ -218,7 +218,7 @@ void
 AP_OpticalFlow_ADNS3080::set_led_always_on( bool alwaysOn )
 {
     byte regVal = read_register(ADNS3080_CONFIGURATION_BITS);
-    regVal = regVal & 0xBf | (alwaysOn << 6);
+    regVal = (regVal & 0xbf) | (alwaysOn << 6);
 	delayMicroseconds(50);  // small delay
 	write_register(ADNS3080_CONFIGURATION_BITS, regVal);
 }
@@ -254,7 +254,7 @@ bool
 AP_OpticalFlow_ADNS3080::get_frame_rate_auto()
 {
     byte regVal = read_register(ADNS3080_EXTENDED_CONFIG);
-	if( regVal & 0x01 > 0 ) {
+    if( (regVal & 0x01) != 0 ) {
 	    return false;
 	}else{
 	    return true;
@@ -355,10 +355,10 @@ AP_OpticalFlow_ADNS3080::set_shutter_speed_auto(bool auto_shutter_speed)
 		delayMicroseconds(50);  // small delay
 
 		// determine value to put into extended config
-        regVal = regVal & ~0x02;
+        regVal &= ~0x02;
 	}else{
 	    // determine value to put into extended config
-	    regVal = regVal & ~0x02 | 0x02;
+	    regVal |= 0x02;
 	}
 	write_register(ADNS3080_EXTENDED_CONFIG, regVal);
 	delayMicroseconds(50);  // small delay
@@ -377,7 +377,7 @@ AP_OpticalFlow_ADNS3080::get_shutter_speed()
 
 
 // set_shutter_speed_auto - set shutter speed to auto (true), or manual (false)
-unsigned int
+void
 AP_OpticalFlow_ADNS3080::set_shutter_speed(unsigned int shutter_speed)
 {
     NumericIntType aNum;
@@ -420,7 +420,7 @@ AP_OpticalFlow_ADNS3080::clear_motion()
 }
 
 // get_pixel_data - captures an image from the sensor and stores it to the pixe_data array
-int
+void
 AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort)
 {
     int i,j;
@@ -438,7 +438,7 @@ AP_OpticalFlow_ADNS3080::print_pixel_data(Stream *serPort)
 	for( i=0; i<ADNS3080_PIXELS_Y; i++ ) {
 	    for( j=0; j<ADNS3080_PIXELS_X; j++ ) {
 		    regValue = read_register(ADNS3080_FRAME_CAPTURE);
-			if( isFirstPixel && (regValue & 0x40 == 0) ) {
+		    if( isFirstPixel && (regValue & 0x40) == 0 ) {
 			    serPort->println("failed to find first pixel");
 			}
 			isFirstPixel = false;

libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h

@@ -4,7 +4,6 @@
 //#include <AP_Math.h>
 //#include <Stream.h>
 //#include <AP_Common.h>
-//#include "AP_OpticalFlow.h"
 #include "AP_OpticalFlow.h"
 
 // orientations for ADNS3080 sensor
@@ -131,11 +130,11 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
 	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 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
+	void print_pixel_data(Stream *serPort); // dumps a 30x30 image to the Serial port
 };
 
 #endif