mirror of https://github.com/ArduPilot/ardupilot
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:
parent
877e965e31
commit
8a0843e69a
|
@ -118,7 +118,8 @@ Compass::calculate(const Matrix3f &dcm_matrix)
|
||||||
// Tilt compensated magnetic field Y component:
|
// Tilt compensated magnetic field Y component:
|
||||||
headY = mag_y*dcm_matrix.c.z/cos_pitch - mag_z*dcm_matrix.c.y/cos_pitch;
|
headY = mag_y*dcm_matrix.c.z/cos_pitch - mag_z*dcm_matrix.c.y/cos_pitch;
|
||||||
// magnetic heading
|
// 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)
|
// Declination correction (if supplied)
|
||||||
if( fabs(_declination) > 0.0 )
|
if( fabs(_declination) > 0.0 )
|
||||||
|
|
Loading…
Reference in New Issue