diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index b530b081d2..de4d2de2f9 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -621,11 +621,10 @@ static void medium_loop()
             }
 
 			#if HIL_MODE != HIL_MODE_ATTITUDE
-				if(g.compass_enabled){
-					compass.read();     // Read magnetometer
-					compass.calculate(dcm.get_dcm_matrix());  // Calculate heading
-					compass.null_offsets(dcm.get_dcm_matrix());
-				}
+            if (g.compass_enabled && compass.read()) {
+                compass.calculate(dcm.get_dcm_matrix());  // Calculate heading
+                compass.null_offsets(dcm.get_dcm_matrix());
+            }
 			#endif
 /*{
 Serial.print(dcm.roll_sensor, DEC);	Serial.printf_P(PSTR("\t"));
diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde
index 11e1fc0a34..1b899c8558 100644
--- a/ArduPlane/test.pde
+++ b/ArduPlane/test.pde
@@ -547,9 +547,10 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 			if(g.compass_enabled) {
 				medium_loopCounter++;
 				if(medium_loopCounter == 5){
-					compass.read();		 				// Read magnetometer
-					compass.calculate(dcm.get_dcm_matrix());		// Calculate heading
-					medium_loopCounter = 0;
+					if (compass.read()) {
+                        compass.calculate(dcm.get_dcm_matrix());		// Calculate heading
+                    }
+                    medium_loopCounter = 0;
 				}
 			}
 
@@ -610,23 +611,28 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 
             medium_loopCounter++;
             if(medium_loopCounter == 5){
-                compass.read();		 				// Read magnetometer
-                compass.calculate(dcm.get_dcm_matrix());		// Calculate heading
-                compass.null_offsets(dcm.get_dcm_matrix());
+                if (compass.read()) {
+                    compass.calculate(dcm.get_dcm_matrix());		// Calculate heading
+                    compass.null_offsets(dcm.get_dcm_matrix());
+                }
                 medium_loopCounter = 0;
             }
 
 			counter++;
 			if (counter>20) {
-                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,
-                                compass.mag_x,
-                                compass.mag_y,
-                                compass.mag_z,
-                                maggy.x,
-                                maggy.y,
-                                maggy.z);
+                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,
+                                    compass.mag_x,
+                                    compass.mag_y,
+                                    compass.mag_z,
+                                    maggy.x,
+                                    maggy.y,
+                                    maggy.z);
+                } else {
+                    Serial.println_P(PSTR("compass not healthy"));
+                }
                 counter=0;
             }
 		}