Line data    Source code

       1             : /*
       2             :   support for sending UDP packets on MAVLink packet boundaries.
       3             :  */
       4             : 
       5             : #include "packetise.h"
       6             : 
       7             : #if AP_MAVLINK_PACKETISE_ENABLED
       8             : #include <GCS_MAVLink/GCS_MAVLink.h>
       9             : 
      10             : /*
      11             :   return the number of bytes to send for a packetised connection
      12             :  */
      13       93740 : uint16_t mavlink_packetise(ByteBuffer &writebuf, uint16_t n)
      14             : {
      15       93740 :     int16_t b = writebuf.peek(0);
      16       93736 :     if (b != MAVLINK_STX_MAVLINK1 && b != MAVLINK_STX) {
      17             :         /*
      18             :           we have a non-mavlink packet at the start of the
      19             :           buffer. Look ahead for a MAVLink start byte, up to 256 bytes
      20             :           ahead
      21             :          */
      22       64858 :         uint16_t limit = n>256?256:n;
      23             :         uint16_t i;
      24       64894 :         for (i=0; i<limit; i++) {
      25          40 :             b = writebuf.peek(i);
      26          40 :             if (b == MAVLINK_STX_MAVLINK1 || b == MAVLINK_STX) {
      27           4 :                 n = i;
      28           4 :                 break;
      29             :             }
      30             :         }
      31             :         // if we didn't find a MAVLink marker then limit the send size to 256
      32       64858 :         if (i == limit) {
      33       64854 :             n = limit;
      34             :         }
      35       64858 :         return n;
      36             :     }
      37             : 
      38             :     // cope with both MAVLink1 and MAVLink2 packets
      39       28878 :     uint8_t min_length = (b == MAVLINK_STX_MAVLINK1)?8:12;
      40             : 
      41             :     // this looks like a MAVLink packet - try to write on
      42             :     // packet boundaries when possible
      43       28878 :     if (n < min_length) {
      44             :         // we need to wait for more data to arrive
      45           3 :         return 0;
      46             :     }
      47             : 
      48             :     // the length of the packet is the 2nd byte
      49       28875 :     int16_t len = writebuf.peek(1);
      50       28877 :     if (b == MAVLINK_STX) {
      51             :         // This is Mavlink2. Check for signed packet with extra 13 bytes
      52       28877 :         int16_t incompat_flags = writebuf.peek(2);
      53       28881 :         if (incompat_flags & MAVLINK_IFLAG_SIGNED) {
      54           0 :             min_length += MAVLINK_SIGNATURE_BLOCK_LEN;
      55             :         }
      56             :     }
      57             : 
      58       28881 :     if (n < len+min_length) {
      59             :         // we don't have a full packet yet
      60           4 :         return 0;
      61             :     }
      62       28877 :     if (n > len+min_length) {
      63             :         // send just 1 packet at a time (so MAVLink packets
      64             :         // are aligned on UDP boundaries)
      65       26309 :         n = len+min_length;
      66             :     }
      67       28877 :     return n;
      68             : }
      69             : 
      70             : #endif // AP_MAVLINK_PACKETISE_ENABLED