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

AP_Optflow - small fix to lat/lon calculation and made example sketch work again

This commit is contained in:
rmackay9 2011-10-01 21:51:14 +09:00
parent 30f4ced9df
commit 1ff58aaa81
2 changed files with 18 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
View File

@ -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;

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

@ -3,10 +3,23 @@
Code by Randy Mackay. DIYDrones.com
*/
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <SPI.h> // Arduino SPI library
#include <SPI.h> // Arduino SPI library
#include <AP_DCM.h> // 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 ) {