From 3c69404d5bb3d0ae922fb968f9e1eb03e2ec2d7a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 17 Sep 2011 13:38:18 +1000 Subject: [PATCH] fixed build of OpticalFlow code --- ArduCopter/ArduCopter.pde | 2 +- libraries/AP_OpticalFlow/AP_OpticalFlow.cpp | 10 ++++++++- libraries/AP_OpticalFlow/AP_OpticalFlow.h | 4 +++- .../AP_OpticalFlow_ADNS3080.cpp | 22 +++++++++---------- .../AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h | 5 ++--- 5 files changed, 26 insertions(+), 17 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index b5aac40ecb..e6d3defd2c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -61,7 +61,7 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List #include // PI library #include // RC Channel Library #include // Range finder library -#include // Optical Flow library +#include // Optical Flow library #include #include // MAVLink GCS definitions #include diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 2613acae3e..da47395701 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/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; } @@ -124,4 +132,4 @@ AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float } } -*/ \ No newline at end of file +*/ diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index 79f1bb215f..7d57977174 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/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 diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index f42d1acb9c..b387e9ed91 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -73,10 +73,10 @@ AP_OpticalFlow_ADNS3080::init(bool initCommAPI) if( retry < 3 ) { if( read_register(ADNS3080_PRODUCT_ID) == 0x17 ) return true; - else - retry++; - }else - return false; + retry++; + } + + 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; iprintln("failed to find first pixel"); } isFirstPixel = false; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index e5c296f186..cdb6a2e975 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h @@ -4,7 +4,6 @@ //#include //#include //#include -//#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