AP_Airspeed: show health status in examples

This commit is contained in:
Andrew Tridgell 2015-11-04 14:53:59 +11:00
parent e7efaa45c5
commit a3ca732403

View File

@ -35,6 +35,8 @@ void setup()
hal.console->println("ArduPilot Airspeed library test"); hal.console->println("ArduPilot Airspeed library test");
AP_Param::set_object_value(&airspeed, airspeed.var_info, "_PIN", 65); AP_Param::set_object_value(&airspeed, airspeed.var_info, "_PIN", 65);
AP_Param::set_object_value(&airspeed, airspeed.var_info, "_ENABLE", 1);
AP_Param::set_object_value(&airspeed, airspeed.var_info, "_USE", 1);
airspeed.init(); airspeed.init();
airspeed.calibrate(false); airspeed.calibrate(false);
@ -46,7 +48,7 @@ void loop(void)
if((hal.scheduler->millis() - timer) > 100) { if((hal.scheduler->millis() - timer) > 100) {
timer = hal.scheduler->millis(); timer = hal.scheduler->millis();
airspeed.read(); airspeed.read();
hal.console->printf("airspeed %.2f\n", airspeed.get_airspeed()); hal.console->printf("airspeed %.2f healthy=%u\n", airspeed.get_airspeed(), airspeed.healthy());
} }
hal.scheduler->delay(1); hal.scheduler->delay(1);
} }