From a0be4f4c8d13497930915b8215dfc20e1a6693d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Dec 2018 20:59:26 +1100 Subject: [PATCH] AP_HAL: fixed build and comments --- libraries/AP_HAL/utility/packetise.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL/utility/packetise.cpp b/libraries/AP_HAL/utility/packetise.cpp index 66eb7abed3..67e1584a55 100644 --- a/libraries/AP_HAL/utility/packetise.cpp +++ b/libraries/AP_HAL/utility/packetise.cpp @@ -2,8 +2,11 @@ support for sending UDP packets on MAVLink packet boundaries. */ -#include +#include + +#ifndef HAL_BOOTLOADER_BUILD #include +#include "packetise.h" /* return the number of bytes to send for a packetised connection @@ -33,7 +36,9 @@ uint16_t mavlink_packetise(ByteBuffer &writebuf, uint16_t n) 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) { @@ -41,12 +46,10 @@ uint16_t mavlink_packetise(ByteBuffer &writebuf, uint16_t n) return 0; } - // 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 + // the length of the packet is the 2nd byte int16_t len = writebuf.peek(1); if (b == MAVLINK_STX) { - // check for signed packet with extra 13 bytes + // 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; @@ -64,3 +67,4 @@ uint16_t mavlink_packetise(ByteBuffer &writebuf, uint16_t n) } return n; } +#endif // HAL_BOOTLOADER_BUILD