ardupilot/libraries/AP_HAL/utility/packetise.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

71 lines
1.9 KiB
C++
Raw Normal View History

/*
support for sending UDP packets on MAVLink packet boundaries.
*/
#include "packetise.h"
#if AP_MAVLINK_PACKETISE_ENABLED
#include <GCS_MAVLink/GCS_MAVLink.h>
/*
return the number of bytes to send for a packetised connection
*/
uint16_t mavlink_packetise(ByteBuffer &writebuf, uint16_t n)
{
int16_t b = writebuf.peek(0);
if (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;
}
return n;
}
2018-12-05 05:59:26 -04:00
// cope with both MAVLink1 and MAVLink2 packets
uint8_t min_length = (b == MAVLINK_STX_MAVLINK1)?8:12;
2018-12-05 05:59:26 -04:00
// 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
return 0;
}
2018-12-05 05:59:26 -04:00
// the length of the packet is the 2nd byte
int16_t len = writebuf.peek(1);
if (b == MAVLINK_STX) {
2018-12-05 05:59:26 -04:00
// This is Mavlink2. 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
return 0;
}
if (n > len+min_length) {
// send just 1 packet at a time (so MAVLink packets
// are aligned on UDP boundaries)
n = len+min_length;
}
return n;
}
#endif // AP_MAVLINK_PACKETISE_ENABLED