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

AP_Compass: more efficient calculate_heading() implementation 

Thanks to piersh for this improvement. See

b5f0635455 (commitcomment-3171806)
This commit is contained in:
Andrew Tridgell 2013-05-09 09:19:01 +10:00
parent 1b2d8636ca
commit f78de63a09
1 changed files with 4 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_Compass/Compass.cpp
View File

@ -216,29 +216,14 @@ Compass::calculate_heading(float roll, float pitch)
float float
Compass::calculate_heading(const Matrix3f &dcm_matrix) Compass::calculate_heading(const Matrix3f &dcm_matrix)
{ {
float headX; float headY = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y;
float headY;
float cos_pitch = safe_sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x));
float heading;
// sinf(pitch) = - dcm_matrix(3,1)
// cosf(pitch)*sinf(roll) = - dcm_matrix(3,2)
// cosf(pitch)*cosf(roll) = - dcm_matrix(3,3)
if (cos_pitch == 0.0f) {
// we are pointing straight up or down so don't update our
// heading using the compass. Wait for the next iteration when
// we hopefully will have valid values again.
return 0;
}
// Tilt compensated magnetic field X component: // Tilt compensated magnetic field X component:
headX = mag_x*cos_pitch - mag_y*dcm_matrix.c.y*dcm_matrix.c.x/cos_pitch - mag_z*dcm_matrix.c.z*dcm_matrix.c.x/cos_pitch; float headX = mag_x + dcm_matrix.c.x * (headY - mag_x * dcm_matrix.c.x);
// Tilt compensated magnetic field Y component:
headY = mag_y*dcm_matrix.c.z/cos_pitch - mag_z*dcm_matrix.c.y/cos_pitch;
// magnetic heading // magnetic heading
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S. // 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
heading = constrain_float(atan2f(-headY,headX), -3.15f, 3.15f); float heading = constrain_float(atan2f(-headY,headX), -3.15f, 3.15f);
// Declination correction (if supplied) // Declination correction (if supplied)
if( fabsf(_declination) > 0.0f ) if( fabsf(_declination) > 0.0f )