ACM: check compass health before using it

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
+					if (compass.read()) {
                         compass.calculate(dcm.get_dcm_matrix());  	// Calculate heading
                         compass.null_offsets(dcm.get_dcm_matrix());
+                    }
 				}
 			#endif
 

ArduCopter/test.pde

@@ -903,7 +903,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 
 		while(1){
 			delay(100);
-			compass.read();
+			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"),
@@ -911,6 +911,9 @@ test_mag(uint8_t argc, const Menu::arg *argv)
                                 compass.mag_x,
                                 compass.mag_y,
                                 compass.mag_z);
+            } else {
+                Serial.println_P(PSTR("not healthy"));
+            }
 
 			if(Serial.available() > 0){
 				return (0);