AP_Periph: check compass and baro health before using

Tools/AP_Periph/can.cpp

@@ -1231,6 +1231,9 @@ void AP_Periph_FW::can_mag_update(void)
     if (last_mag_update_ms == compass.last_update_ms()) {
         return;
     }
+    if (!compass.healthy()) {
+        return;
+    }
 
     last_mag_update_ms = compass.last_update_ms();
     const Vector3f &field = compass.get_field();

Tools/AP_Periph/msp.cpp

@@ -129,6 +129,10 @@ void AP_Periph_FW::send_msp_baro(void)
         return;
     }
     msp.last_baro_ms = baro.get_last_update(0);
+    if (!baro.healthy()) {
+        // don't send any data
+        return;
+    }
 
     p.instance = 0;
     p.time_ms = msp.last_baro_ms;
@@ -150,6 +154,9 @@ void AP_Periph_FW::send_msp_compass(void)
     if (msp.last_mag_ms == compass.last_update_ms(0)) {
         return;
     }
+    if (!compass.healthy()) {
+        return;
+    }
     msp.last_mag_ms = compass.last_update_ms(0);
 
     const Vector3f &field = compass.get_field(0);