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

Added a limit to the atan function to calc the heading. Just trying to avoid, bad values screwing up the DCM. - Jason 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2777 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-07-08 03:56:04 +00:00
parent 877e965e31
commit 8a0843e69a
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_Compass/Compass.cpp
View File

@ -75,7 +75,7 @@ Compass::calculate(float roll, float pitch)
float sin_roll;
float cos_pitch;
float sin_pitch;
cos_roll = cos(roll);
cos_roll = cos(roll);
sin_roll = sin(roll);
cos_pitch = cos(pitch);
sin_pitch = sin(pitch);
@ -118,7 +118,8 @@ Compass::calculate(const Matrix3f &dcm_matrix)
// 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
heading = atan2(-headY,headX);
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
heading = constrain(atan2(-headY,headX), -3.15, 3.15);
// Declination correction (if supplied)
if( fabs(_declination) > 0.0 )