From 4017bb39d1c492e7ab63bc93452a4a7bc1ec6671 Mon Sep 17 00:00:00 2001 From: "rmackay9@yahoo.com" Date: Sun, 22 May 2011 14:44:51 +0000 Subject: [PATCH] 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 | 16 +++++++----- libraries/AP_OpticalFlow/AP_OpticalFlow.h | 8 ++++-- .../AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h | 26 ++++++++++++------- 3 files changed, 32 insertions(+), 18 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 492b93e796..4306758e93 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/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; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index 8c490d4ad8..dec1d0bc28 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/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; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index 0e1589d19a..227067aaa9 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/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