From 1ff58aaa8144b05b97b35741b5fa9f60d74aa2d2 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sat, 1 Oct 2011 21:51:14 +0900 Subject: [PATCH] AP_Optflow - small fix to lat/lon calculation and made example sketch work again --- libraries/AP_OpticalFlow/AP_OpticalFlow.cpp | 6 +++--- .../AP_OpticalFlow_test/AP_OpticalFlow_test.pde | 17 +++++++++++++++-- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index da47395701..25dbec3fec 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -97,9 +97,9 @@ AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float 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 - - vlon = x_cm * sin_yaw_y - y_cm * cos_yaw_x; - vlat = y_cm * sin_yaw_y - x_cm * cos_yaw_x; + // convert x/y movements into lon/lat movement + 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; diff --git a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde index 057e9d5353..ef8c604a2c 100644 --- a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde +++ b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde @@ -3,10 +3,23 @@ Code by Randy Mackay. DIYDrones.com */ +#include +#include #include // ArduPilot Mega Vector/Matrix math Library -#include // Arduino SPI library +#include // Arduino SPI library +#include // ArduCopter DCM Library #include "AP_OpticalFlow.h" // ArduCopter OpticalFlow Library +//////////////////////////////////////////////////////////////////////////////// +// Serial ports +//////////////////////////////////////////////////////////////////////////////// +// +// Note that FastSerial port buffers are allocated at ::begin time, +// so there is not much of a penalty to defining ports that we don't +// use. +// +FastSerialPort0(Serial); // FTDI/console + AP_OpticalFlow_ADNS3080 flowSensor; void setup() @@ -297,7 +310,7 @@ void display_motion() while( !Serial.available() ) { flowSensor.read(); - flowSensor.get_position(0,0,0,100); + flowSensor.update_position(0,0,0,1,100); // x,y,squal //if( flowSensor.motion() || first_time ) {