AP_Periph: stop sending airspeed when unhealthy

This commit is contained in:
Andrew Tridgell 2020-12-08 14:04:53 +11:00
parent 2d75ef4f60
commit 74813e7761

View File

@ -175,6 +175,11 @@ void AP_Periph_FW::send_msp_airspeed(void)
if (msp.last_airspeed_ms == last_update_ms) {
return;
}
if (!airspeed.healthy()) {
// we don't report at all for an unhealthy sensor. This maps
// to unhealthy in the flight controller driver
return;
}
msp.last_airspeed_ms = last_update_ms;
p.instance = 0;