diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 0df4d93921..ddaecd5c2d 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -185,7 +185,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) // default to all healthy except compass, gps and receiver which we set individually control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS & ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER); - if (g.compass_enabled && compass.healthy() && ahrs.use_compass()) { + if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; } if (g_gps != NULL && g_gps->status() > GPS::NO_GPS && (!gps_glitch.glitching()||ap.usb_connected)) { diff --git a/ArduCopter/compassmot.pde b/ArduCopter/compassmot.pde index 2204e78d9c..3d9a089893 100644 --- a/ArduCopter/compassmot.pde +++ b/ArduCopter/compassmot.pde @@ -38,7 +38,7 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan) // check compass health compass.read(); - if (!compass.healthy()) { + if (!compass.healthy(0)) { gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH, PSTR("check compass")); ap.compass_mot = false; return 1; @@ -128,7 +128,7 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan) last_send_time = millis(); // main run while there is no user input and the compass is healthy - while (command_ack_start == command_ack_counter && compass.healthy() && motors.armed()) { + while (command_ack_start == command_ack_counter && compass.healthy(0) && motors.armed()) { // 50hz loop if (millis() - last_run_time < 20) { // grab some compass values diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 2ecd099ffe..33201bed33 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -248,7 +248,7 @@ static void pre_arm_checks(bool display_failure) // check Compass if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_COMPASS)) { // check the compass is healthy - if(!compass.healthy()) { + if(!compass.healthy(0)) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not healthy")); }