N3_sub/libraries/AP_HAL/utility/packetise.cpp

/*
  support for sending UDP packets on MAVLink packet boundaries.
 */

#include <AP_HAL/AP_HAL.h>

#ifndef HAL_BOOTLOADER_BUILD
#include <GCS_MAVLink/GCS.h>
#include "packetise.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;
    }

    // cope with both MAVLink1 and MAVLink2 packets
    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
        return 0;
    }

    // the length of the packet is the 2nd byte
    int16_t len = writebuf.peek(1);
    if (b == MAVLINK_STX) {
        // 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 // HAL_BOOTLOADER_BUILD