diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 11c4cccf5b..000f74dc76 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -156,11 +156,15 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan) if (ap.rc_receiver_present) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; } + if (copter.DataFlash.logging_present()) { // primary logging only (usually File) + control_sensors_present |= MAV_SYS_STATUS_LOGGING; + } // all present sensors enabled by default except altitude and position control and motors which we will set individually control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & - ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS); + ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & + ~MAV_SYS_STATUS_LOGGING); switch (control_mode) { case ALT_HOLD: @@ -187,6 +191,11 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan) control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS; } + if (copter.DataFlash.logging_enabled()) { + control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; + } + + // default to all healthy except baro, compass, gps and receiver which we set individually control_sensors_health = control_sensors_present & ~(MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_3D_MAG | @@ -226,6 +235,10 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan) control_sensors_health &= ~MAV_SYS_STATUS_AHRS; } + if (copter.DataFlash.logging_failed()) { + control_sensors_health &= ~MAV_SYS_STATUS_LOGGING; + } + int16_t battery_current = -1; int8_t battery_remaining = -1;