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

AP_Compass: fixed a bug in the tilt compass calculation 

The simplification applied a few months ago was incorrect
This commit is contained in:
Andrew Tridgell 2013-08-18 21:08:34 +10:00
parent 3a5a15a15f
commit feac9d1306
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Compass/Compass.cpp
View File

@ -176,15 +176,19 @@ Compass::get_declination() const
return _declination.get();
}
/*
calculate a compass heading given the attitude from DCM and the mag vector
*/
float
Compass::calculate_heading(const Matrix3f &dcm_matrix) const
{
float cos_pitch_sq = 1.0f-(dcm_matrix.c.x*dcm_matrix.c.x);
// Tilt compensated magnetic field Y component:
float headY = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y;
// Tilt compensated magnetic field X component:
float headX = mag_x + dcm_matrix.c.x * (headY - mag_x * dcm_matrix.c.x);
float headX = mag_x * cos_pitch_sq - dcm_matrix.c.x * (mag_y * dcm_matrix.c.y + mag_z * dcm_matrix.c.z);
// magnetic heading
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.