HAL_SITL: use mavlink_packetise()

libraries/AP_HAL_SITL/UARTDriver.cpp

@@ -39,6 +39,7 @@
 
 #include "UARTDriver.h"
 #include "SITL_State.h"
+#include <AP_HAL/utility/packetise.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -399,6 +400,7 @@ void UARTDriver::_udp_start_client(const char *address, uint16_t port)
     }
 
     _is_udp = true;
+    _packetise = true;
     _connected = true;
 }
 
@@ -529,9 +531,24 @@ void UARTDriver::_timer_tick(void)
         _check_reconnect();
         return;
     }
-    uint32_t navail;
     ssize_t nwritten;
 
+    if (_packetise) {
+        uint16_t n = _writebuffer.available();
+        if (n > 0) {
+            n = mavlink_packetise(_writebuffer, n);
+        }
+        if (n > 0) {
+            // keep as a single UDP packet
+            uint8_t tmpbuf[n];
+            _writebuffer.peekbytes(tmpbuf, n);
+            ssize_t ret = send(_fd, tmpbuf, n, MSG_DONTWAIT);
+            if (ret > 0) {
+                _writebuffer.advance(ret);
+            }
+        }
+    } else {
+        uint32_t navail;
         const uint8_t *readptr = _writebuffer.readptr(navail);
         if (readptr && navail > 0) {
             if (!_use_send_recv) {
@@ -548,6 +565,7 @@ void UARTDriver::_timer_tick(void)
                 _writebuffer.advance(nwritten);
             }
         }
+    }
 
     uint32_t space = _readbuffer.space();
     if (space == 0) {

libraries/AP_HAL_SITL/UARTDriver.h

@@ -112,6 +112,7 @@ private:
     SITL_State *_sitlState;
     uint64_t _receive_timestamp;
     bool _is_udp;
+    bool _packetise;
 };
 
 #endif