SITL: fixed send of MAVLink2 pkt

This commit is contained in:
Andrew Tridgell 2016-08-17 08:29:35 +10:00 committed by Tom Pittenger
parent 19ac9bce48
commit 3ecf2850a8

View File

@ -244,18 +244,21 @@ void ADSB::send_report(void)
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
uint8_t saved_seq = chan0_status->current_tx_seq;
uint8_t saved_flags = chan0_status->flags;
chan0_status->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
chan0_status->current_tx_seq = mavlink.seq;
const mavlink_uavionix_adsb_transceiver_health_report_t health_report = {UAVIONIX_ADSB_RF_HEALTH_OK};
len = mavlink_msg_uavionix_adsb_transceiver_health_report_encode(vehicle_system_id,
MAV_COMP_ID_ADSB,
&msg, &health_report);
chan0_status->current_tx_seq = saved_seq;
chan0_status->flags = saved_flags;
uint8_t msgbuf[len];
len = mavlink_msg_to_send_buffer(msgbuf, &msg);
if (len > 0) {
mav_socket.send(msgbuf, len);
::printf("ADSBsim send tx health packet");
::printf("ADSBsim send tx health packet\n");
}
}