Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

OpticalFlow: switch order of sin_yaw, cos_yaw

Browse Source
This commit is contained in:
Randy Mackay 2013-04-02 15:49:31 +09:00
parent 7ad8670df7
commit 5bf55a9523
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
View File

@ -90,7 +90,7 @@ void AP_OpticalFlow::update_conversion_factors()
// updates internal lon and lat with estimation based on optical flow
void AP_OpticalFlow::update_position(float roll, float pitch,
float cos_yaw_x, float sin_yaw_y, float altitude)
float sin_yaw, float cos_yaw, float altitude)
{
float diff_roll = roll - _last_roll;
float diff_pitch = pitch - _last_pitch;
@ -119,8 +119,8 @@ void AP_OpticalFlow::update_position(float roll, float pitch,
y_cm = -change_y * avg_altitude * conv_factor;
// 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;
vlon = x_cm * cos_yaw + y_cm * sin_yaw;
vlat = y_cm * cos_yaw - x_cm * sin_yaw;
}
_last_altitude = altitude;

2
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -79,7 +79,7 @@ public:
virtual void update(uint32_t now);
// updates internal lon and lat with estimation based on optical flow
virtual void update_position(float roll,
float pitch, float cos_yaw_x, float sin_yaw_y, float altitude);
float pitch, float sin_yaw, float cos_yaw, float altitude);
protected:
// pointer to the last instantiated optical flow sensor. Will be turned