diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 1efb3ee564..ddb2813a56 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -39,6 +39,7 @@ #include "UARTDriver.h" #include "SITL_State.h" +#include 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,23 +531,39 @@ void UARTDriver::_timer_tick(void) _check_reconnect(); return; } - uint32_t navail; ssize_t nwritten; - const uint8_t *readptr = _writebuffer.readptr(navail); - if (readptr && navail > 0) { - if (!_use_send_recv) { - nwritten = ::write(_fd, readptr, navail); - if (nwritten == -1 && errno != EAGAIN && _uart_path) { - close(_fd); - _fd = -1; - _connected = false; + 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 { - nwritten = send(_fd, readptr, navail, MSG_DONTWAIT); } - if (nwritten > 0) { - _writebuffer.advance(nwritten); + } else { + uint32_t navail; + const uint8_t *readptr = _writebuffer.readptr(navail); + if (readptr && navail > 0) { + if (!_use_send_recv) { + nwritten = ::write(_fd, readptr, navail); + if (nwritten == -1 && errno != EAGAIN && _uart_path) { + close(_fd); + _fd = -1; + _connected = false; + } + } else { + nwritten = send(_fd, readptr, navail, MSG_DONTWAIT); + } + if (nwritten > 0) { + _writebuffer.advance(nwritten); + } } } diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index 4785e29833..e83024045d 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -112,6 +112,7 @@ private: SITL_State *_sitlState; uint64_t _receive_timestamp; bool _is_udp; + bool _packetise; }; #endif