Plane: send EKF_STATUS_REPORT in extra3 stream

This commit is contained in:
Randy Mackay 2015-03-12 12:31:24 +09:00
parent 553261d4ce
commit cb1b4ab486

View File

@ -705,6 +705,13 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
#endif
break;
case MSG_EKF_STATUS_REPORT:
#if AP_AHRS_NAVEKF_AVAILABLE
CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT);
ahrs.get_NavEKF().send_status_report(chan);
#endif
break;
case MSG_RETRY_DEFERRED:
break; // just here to prevent a warning
@ -940,6 +947,7 @@ GCS_MAVLINK::data_stream_send(void)
send_message(MSG_BATTERY2);
send_message(MSG_MOUNT_STATUS);
send_message(MSG_OPTICAL_FLOW);
send_message(MSG_EKF_STATUS_REPORT);
}
}