From 0d31d9be10fb28384917d15e638d1cfa697f0b8c Mon Sep 17 00:00:00 2001 From: jasonshort Date: Fri, 8 Jul 2011 03:56:04 +0000 Subject: [PATCH] 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 --- libraries/AP_Compass/Compass.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 49ea187547..86af0c21ce 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -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 )