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
-					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
 

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);