diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index cf9043f71a..6db7642f05 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -703,9 +703,10 @@ static void medium_loop()
 
 			#if HIL_MODE != HIL_MODE_ATTITUDE					// don't execute in HIL mode
 				if(g.compass_enabled){
-					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());
+                    }
 				}
 			#endif
 
diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde
index 4a63b5dcbe..dc37cf23f4 100644
--- a/ArduCopter/test.pde
+++ b/ArduCopter/test.pde
@@ -903,14 +903,17 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 
 		while(1){
 			delay(100);
-			compass.read();
-			compass.calculate(dcm.get_dcm_matrix());
-			Vector3f maggy = compass.get_offsets();
-			Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
-						(wrap_360(ToDeg(compass.heading) * 100)) /100,
-						compass.mag_x,
-						compass.mag_y,
-						compass.mag_z);
+			if (compass.read()) {
+                compass.calculate(dcm.get_dcm_matrix());
+                Vector3f maggy = compass.get_offsets();
+                Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
+                                (wrap_360(ToDeg(compass.heading) * 100)) /100,
+                                compass.mag_x,
+                                compass.mag_y,
+                                compass.mag_z);
+            } else {
+                Serial.println_P(PSTR("not healthy"));
+            }
 
 			if(Serial.available() > 0){
 				return (0);