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 )