diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 183a049c45..a2e8a40cb4 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -306,7 +306,6 @@ static void NOINLINE send_meminfo(mavlink_channel_t chan)
 
 static void NOINLINE send_location(mavlink_channel_t chan)
 {
-#if QUATERNION_ENABLE == DISABLED
     Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
     mavlink_msg_global_position_int_send(
         chan,
@@ -319,7 +318,6 @@ static void NOINLINE send_location(mavlink_channel_t chan)
         g_gps->ground_speed * rot.b.x,  // Y speed cm/s
         g_gps->ground_speed * rot.c.x,
         g_gps->ground_course);          // course in 1/100 degree
-#endif
 }
 
 static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde
index fedb2fdb50..85c8e4ae3f 100644
--- a/ArduPlane/test.pde
+++ b/ArduPlane/test.pde
@@ -535,11 +535,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 				if(medium_loopCounter == 5){
 					if (compass.read()) {
                         // Calculate heading
-#if QUATERNION_ENABLE == ENABLED
-                        compass.calculate(dcm.roll, dcm.pitch);
-#else
                         compass.calculate(dcm.get_dcm_matrix());
-#endif
                     }
                     medium_loopCounter = 0;
 				}
@@ -605,12 +601,9 @@ test_mag(uint8_t argc, const Menu::arg *argv)
             if(medium_loopCounter == 5){
                 if (compass.read()) {
                     // Calculate heading
-#if QUATERNION_ENABLE == ENABLED
-                    compass.calculate(dcm.roll, dcm.pitch);
-#else
-                    compass.calculate(dcm.get_dcm_matrix());
-                    compass.null_offsets(dcm.get_dcm_matrix());
-#endif
+                    Matrix3f m = dcm.get_dcm_matrix();
+                    compass.calculate(m);
+                    compass.null_offsets(m);
                 }
                 medium_loopCounter = 0;
             }