yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink app: fix mavlink forwarding issue. (#4907)

Browse Source
This commit is contained in:
Henry Zhang
2016-06-25 17:02:02 +08:00
committed by Lorenz Meier
parent 705a08bbbd
commit ae6600e48f
2 changed files with 1 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
src/modules/mavlink/mavlink_main.cpp
View File

@@ -993,58 +993,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
pthread_mutex_unlock(&_send_mutex);
}
void
Mavlink::resend_message(mavlink_message_t *msg)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
pthread_mutex_lock(&_send_mutex);
unsigned buf_free = get_free_tx_buf();
_last_write_try_time = hrt_absolute_time();
unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
/* check if there is space in the buffer, let it overflow else */
if (buf_free < packet_len) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
pthread_mutex_unlock(&_send_mutex);
return;
}
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header and payload */
memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len);
/* checksum */
buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);
if (_uart_fd >= 0) {
/* send message to UART */
ssize_t ret = ::write(_uart_fd, buf, packet_len);
if (ret != (int) packet_len) {
count_txerr();
count_txerrbytes(packet_len);
} else {
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
}
pthread_mutex_unlock(&_send_mutex);
}
void
Mavlink::find_broadcast_address()
{