diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index 549a28dcf2..6f71de57a1 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -896,12 +896,6 @@ static void medium_loop()
             if (g.compass_enabled && compass.read()) {
                 ahrs.set_compass(&compass);
                 // Calculate heading
-#if LITE == DISABLED                
-                Matrix3f m = ahrs.get_dcm_matrix();              
-                compass.calculate(m);
-#else
-                compass.calculate(0,0);  // roll = 0, pitch = 0
-#endif
                 compass.null_offsets();
             } else {
                 ahrs.set_compass(NULL);
@@ -1113,12 +1107,7 @@ static void update_GPS(void)
                   ground_course = ahrs.yaw_sensor;
                 } else {
 #endif                
-    long magheading;
-    magheading = ToDeg(compass.heading) * 100;
-    if (magheading > 36000)	  magheading -= 36000;
-    if (magheading < 0)           magheading += 36000;
-
-    ground_course = magheading;
+    ground_course = ToDeg(ahrs.yaw) * 100;
 #if LITE == DISABLED                  
                 }
 #endif
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index 1f67c1d6a8..0c44719156 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -192,7 +192,6 @@ static void init_ardupilot()
         } else {
             ahrs.set_compass(&compass);
             //compass.get_offsets();						// load offsets to account for airframe magnetic interference
-            compass.null_offsets_enable();
         }
 	}
 #else
diff --git a/APMrover2/test.pde b/APMrover2/test.pde
index 01f9b75a4e..c60f291e04 100644
--- a/APMrover2/test.pde
+++ b/APMrover2/test.pde
@@ -542,9 +542,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 			if(g.compass_enabled) {
 				medium_loopCounter++;
 				if(medium_loopCounter == 5){
-					if (compass.read()) {
-                        compass.calculate(ahrs.get_dcm_matrix());		// Calculate heading
-                    }
+					compass.read();
                     medium_loopCounter = 0;
 				}
 			}
@@ -581,7 +579,6 @@ test_mag(uint8_t argc, const Menu::arg *argv)
         Serial.println_P(PSTR("Compass initialisation failed!"));
         return 0;
     }
-    compass.null_offsets_enable();
     ahrs.set_compass(&compass);
     report_compass();
 
@@ -590,6 +587,8 @@ test_mag(uint8_t argc, const Menu::arg *argv)
     ahrs.reset();
 
 	int counter = 0;
+    float heading = 0;
+
 		//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
 
     print_hit_enter();
@@ -610,7 +609,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
                 if (compass.read()) {
                     // Calculate heading
                     Matrix3f m = ahrs.get_dcm_matrix();
-                    compass.calculate(m);
+                    heading = compass.calculate_heading(m);
                     compass.null_offsets();
                 }
                 medium_loopCounter = 0;
@@ -621,7 +620,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
                 if (compass.healthy) {
                     Vector3f maggy = compass.get_offsets();
                     Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
-                                    (wrap_360(ToDeg(compass.heading) * 100)) /100,
+                                    (wrap_360(ToDeg(heading) * 100)) /100,
                                     (int)compass.mag_x,
                                     (int)compass.mag_y,
                                     (int)compass.mag_z,