aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_Linux: use mavlink_packetise()

Browse Source
This commit is contained in:
Andrew Tridgell 2018-12-05 16:58:26 +11:00
parent d01f5ae6c8
commit 63705f858b
1 changed files with 5 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
libraries/AP_HAL_Linux/UARTDriver.cpp
View File

@ -25,6 +25,7 @@
#include "UDPDevice.h"
#include <GCS_MAVLink/GCS.h>
#include <AP_HAL/utility/packetise.h>
extern const AP_HAL::HAL& hal;
@ -378,58 +379,10 @@ bool UARTDriver::_write_pending_bytes(void)
// write any pending bytes
uint32_t available_bytes = _writebuf.available();
uint16_t n = available_bytes;
int16_t b = _writebuf.peek(0);
if (_packetise && n > 0 &&
b != MAVLINK_STX_MAVLINK1 && b != MAVLINK_STX) {
/*
we have a non-mavlink packet at the start of the
buffer. Look ahead for a MAVLink start byte, up to 256 bytes
ahead
*/
uint16_t limit = n>256?256:n;
uint16_t i;
for (i=0; i<limit; i++) {
b = _writebuf.peek(i);
if (b == MAVLINK_STX_MAVLINK1 || b == MAVLINK_STX) {
n = i;
break;
}
}
// if we didn't find a MAVLink marker then limit the send size to 256
if (i == limit) {
n = limit;
}
}
b = _writebuf.peek(0);
if (_packetise && n > 0 &&
(b == MAVLINK_STX_MAVLINK1 || b == MAVLINK_STX)) {
uint8_t min_length = (b == MAVLINK_STX_MAVLINK1)?8:12;
// this looks like a MAVLink packet - try to write on
// packet boundaries when possible
if (n < min_length) {
// we need to wait for more data to arrive
n = 0;
} else {
// the length of the packet is the 2nd byte, and mavlink
// packets have a 6 byte header plus 2 byte checksum,
// giving len+8 bytes
int16_t len = _writebuf.peek(1);
if (b == MAVLINK_STX) {
// check for signed packet with extra 13 bytes
int16_t incompat_flags = _writebuf.peek(2);
if (incompat_flags & MAVLINK_IFLAG_SIGNED) {
min_length += MAVLINK_SIGNATURE_BLOCK_LEN;
}
}
if (n < len+min_length) {
// we don't have a full packet yet
n = 0;
} else if (n > len+min_length) {
// send just 1 packet at a time (so MAVLink packets
// are aligned on UDP boundaries)
n = len+min_length;
}
}
if (_packetise && n > 0) {
// send on MAVLink packet boundaries if possible
n = mavlink_packetise(_writebuf, n);
}
if (n > 0) {