Browse Source

Copter: use baro healthy()

mission-4.1.18
Randy Mackay 11 years ago
parent
commit
7686660c73
  1. 2
      ArduCopter/GCS_Mavlink.pde
  2. 2
      ArduCopter/motors.pde
  3. 4
      ArduCopter/system.pde
  4. 2
      ArduCopter/test.pde

2
ArduCopter/GCS_Mavlink.pde

@ -187,7 +187,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
MAV_SYS_STATUS_SENSOR_3D_MAG | MAV_SYS_STATUS_SENSOR_3D_MAG |
MAV_SYS_STATUS_SENSOR_GPS | MAV_SYS_STATUS_SENSOR_GPS |
MAV_SYS_STATUS_SENSOR_RC_RECEIVER); MAV_SYS_STATUS_SENSOR_RC_RECEIVER);
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()) {

2
ArduCopter/motors.pde

@ -240,7 +240,7 @@ static void pre_arm_checks(bool display_failure)
// check Baro // check Baro
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
// barometer health check // barometer health check
if(!barometer.healthy) { if(!barometer.healthy()) {
if (display_failure) { if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Baro not healthy")); gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Baro not healthy"));
} }

4
ArduCopter/system.pde

@ -248,8 +248,8 @@ static void init_ardupilot()
#endif // CLI_ENABLED #endif // CLI_ENABLED
#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);

2
ArduCopter/test.pde

@ -60,7 +60,7 @@ test_baro(uint8_t argc, const Menu::arg *argv)
delay(100); delay(100);
alt = read_barometer(); alt = read_barometer();
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…
Cancel
Save