Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_OpticalFlow - small but important fixes for position calcs 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3040 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com 2011-08-07 13:19:18 +00:00
parent 9b9402a264
commit 998b00693c
4 changed files with 8 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
View File

@ -88,10 +88,10 @@ AP_OpticalFlow::get_position(float roll, float pitch, float yaw, float altitude)
int i; int i;
// only update position if surface quality is good and angle is not over 45 degrees // only update position if surface quality is good and angle is not over 45 degrees
if( surface_quality >= 20 && fabs(roll) <= FORTYFIVE_DEGREES && fabs(pitch) <= FORTYFIVE_DEGREES ) { if( surface_quality >= 10 && fabs(roll) <= FORTYFIVE_DEGREES && fabs(pitch) <= FORTYFIVE_DEGREES ) {
// calculate expected x,y diff due to roll and pitch change // calculate expected x,y diff due to roll and pitch change
exp_change_x = diff_roll * radians_to_pixels; exp_change_x = diff_roll * radians_to_pixels;
exp_change_y = diff_pitch * radians_to_pixels; exp_change_y = -diff_pitch * radians_to_pixels;
// real estimated raw change from mouse // real estimated raw change from mouse
change_x = dx - exp_change_x; change_x = dx - exp_change_x;

2
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
View File

@ -41,7 +41,7 @@ union NumericIntType
AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080() AP_OpticalFlow_ADNS3080::AP_OpticalFlow_ADNS3080()
{ {
num_pixels = ADNS3080_PIXELS_X; num_pixels = ADNS3080_PIXELS_X;
field_of_view = AP_OPTICALFLOW_ADNS3080_12_FOV; field_of_view = AP_OPTICALFLOW_ADNS3080_08_FOV;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER; scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
} }

8
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h
View File

@ -16,12 +16,10 @@
#define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_LEFT AP_OPTICALFLOW_ROTATION_YAW_225 #define AP_OPTICALFLOW_ADNS3080_PINS_FORWARD_LEFT AP_OPTICALFLOW_ROTATION_YAW_225
// field of view of ADNS3080 sensor lenses // field of view of ADNS3080 sensor lenses
#define AP_OPTICALFLOW_ADNS3080_04_FOV 1.396263 // 80 degrees #define AP_OPTICALFLOW_ADNS3080_08_FOV 0.202458 // 11.6 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 // scaler - value returned when sensor is moved equivalent of 1 pixel
#define AP_OPTICALFLOW_ADNS3080_SCALER 3.4 // was 10.5 #define AP_OPTICALFLOW_ADNS3080_SCALER 1.1
// We use Serial Port 2 in SPI Mode // We use Serial Port 2 in SPI Mode
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
@ -29,7 +27,7 @@
#define AP_SPI_DATAOUT 51 // MOSI // PB2 #define AP_SPI_DATAOUT 51 // MOSI // PB2
#define AP_SPI_CLOCK 52 // SCK // PB1 #define AP_SPI_CLOCK 52 // SCK // PB1
#define ADNS3080_CHIP_SELECT 34 // PC3 #define ADNS3080_CHIP_SELECT 34 // PC3
#define ADNS3080_RESET 35 // PC2 #define ADNS3080_RESET 40 // PG1 was 35 / PC2
#else // normal arduino SPI pins...these need to be checked #else // normal arduino SPI pins...these need to be checked
#define AP_SPI_DATAIN 12 //MISO #define AP_SPI_DATAIN 12 //MISO
#define AP_SPI_DATAOUT 11 //MOSI #define AP_SPI_DATAOUT 11 //MOSI

6
libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde
View File

@ -20,7 +20,7 @@ void setup()
if( flowSensor.init() == false ) if( flowSensor.init() == false )
Serial.println("Failed to initialise ADNS3080"); Serial.println("Failed to initialise ADNS3080");
flowSensor.set_orientation(AP_OPTICALFLOW_ADNS3080_PINS_FORWARD); flowSensor.set_orientation(AP_OPTICALFLOW_ADNS3080_PINS_FORWARD);
flowSensor.set_field_of_view(AP_OPTICALFLOW_ADNS3080_12_FOV); flowSensor.set_field_of_view(AP_OPTICALFLOW_ADNS3080_08_FOV);
delay(1000); delay(1000);
} }
@ -311,10 +311,6 @@ void display_motion()
Serial.print(flowSensor.dy,DEC); Serial.print(flowSensor.dy,DEC);
Serial.print("\tsqual:"); Serial.print("\tsqual:");
Serial.print(flowSensor.surface_quality,DEC); Serial.print(flowSensor.surface_quality,DEC);
Serial.print("\tlat:");
Serial.print(flowSensor.lat,DEC);
Serial.print("\tlng:");
Serial.print(flowSensor.lng,DEC);
Serial.println(); Serial.println();
first_time = false; first_time = false;
//} //}