aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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 // updates internal lon and lat with estimation based on optical flow
void AP_OpticalFlow::update_position(float roll, float pitch, 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_roll = roll - _last_roll;
float diff_pitch = pitch - _last_pitch; 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; y_cm = -change_y * avg_altitude * conv_factor;
// convert x/y movements into lon/lat movement // convert x/y movements into lon/lat movement
vlon = x_cm * sin_yaw_y + y_cm * cos_yaw_x; vlon = x_cm * cos_yaw + y_cm * sin_yaw;
vlat = y_cm * sin_yaw_y - x_cm * cos_yaw_x; vlat = y_cm * cos_yaw - x_cm * sin_yaw;
} }
_last_altitude = altitude; _last_altitude = altitude;

2
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -79,7 +79,7 @@ public:
virtual void update(uint32_t now); virtual void update(uint32_t now);
// updates internal lon and lat with estimation based on optical flow // updates internal lon and lat with estimation based on optical flow
virtual void update_position(float roll, 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: protected:
// pointer to the last instantiated optical flow sensor. Will be turned // pointer to the last instantiated optical flow sensor. Will be turned