diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 91bcd69f96..90d96c97ba 100644 --- a/Tools/AP_Periph/can.cpp +++ b/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(); diff --git a/Tools/AP_Periph/msp.cpp b/Tools/AP_Periph/msp.cpp index 3d44680ba3..c8c2de118c 100644 --- a/Tools/AP_Periph/msp.cpp +++ b/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);