mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-11 17:13:56 -03:00
Plane: use baro healthy()
Use baro last update time in place of healthy to determine whether HIL sensor updates have started
This commit is contained in:
parent
7686660c73
commit
7d7272520e
@ -216,7 +216,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
|
|||||||
// AHRS subsystem is unhealthy
|
// AHRS subsystem is unhealthy
|
||||||
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
|
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
|
||||||
}
|
}
|
||||||
if (barometer.healthy) {
|
if (barometer.healthy()) {
|
||||||
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
|
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
|
||||||
}
|
}
|
||||||
if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
|
if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
|
||||||
|
@ -456,8 +456,8 @@ static void check_short_failsafe()
|
|||||||
static void startup_INS_ground(bool do_accel_init)
|
static void startup_INS_ground(bool do_accel_init)
|
||||||
{
|
{
|
||||||
#if HIL_MODE != HIL_MODE_DISABLED
|
#if HIL_MODE != HIL_MODE_DISABLED
|
||||||
while (!barometer.healthy) {
|
while (barometer.get_last_update() == 0) {
|
||||||
// the barometer becomes healthy when we get the first
|
// the barometer begins updating when we get the first
|
||||||
// HIL_STATE message
|
// HIL_STATE message
|
||||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Waiting for first HIL_STATE message"));
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Waiting for first HIL_STATE message"));
|
||||||
delay(1000);
|
delay(1000);
|
||||||
|
@ -578,7 +578,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
|
|||||||
while(1) {
|
while(1) {
|
||||||
delay(100);
|
delay(100);
|
||||||
|
|
||||||
if (!barometer.healthy) {
|
if (!barometer.healthy()) {
|
||||||
cliSerial->println_P(PSTR("not healthy"));
|
cliSerial->println_P(PSTR("not healthy"));
|
||||||
} else {
|
} else {
|
||||||
cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %f Temperature: %.1f\n"),
|
cliSerial->printf_P(PSTR("Alt: %0.2fm, Raw: %f Temperature: %.1f\n"),
|
||||||
|
Loading…
Reference in New Issue
Block a user