HAL_SITL: support limiting baudrate of simulated telemetry

This commit is contained in:
Andrew Tridgell 2019-01-25 11:26:38 +11:00
parent 56683ba803
commit d176776357
2 changed files with 19 additions and 2 deletions

View File

@ -57,8 +57,9 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
const char *path = _sitlState->_uart_path[_portNumber]; const char *path = _sitlState->_uart_path[_portNumber];
// default to 1MBit if (baud != 0) {
_uart_baudrate = 1000000U; _uart_baudrate = baud;
}
if (strcmp(path, "GPS1") == 0) { if (strcmp(path, "GPS1") == 0) {
/* gps */ /* gps */
@ -612,9 +613,22 @@ void UARTDriver::_timer_tick(void)
return; return;
} }
ssize_t nwritten; ssize_t nwritten;
uint32_t max_bytes = 10000;
SITL::SITL *_sitl = AP::sitl();
if (_sitl && _sitl->telem_baudlimit_enable) {
// limit byte rate to configured baudrate
uint32_t now = AP_HAL::micros();
float dt = 1.0e-6 * (now - last_tick_us);
max_bytes = _uart_baudrate * dt / 10;
if (max_bytes == 0) {
return;
}
last_tick_us = now;
}
if (_packetise) { if (_packetise) {
uint16_t n = _writebuffer.available(); uint16_t n = _writebuffer.available();
n = MIN(n, max_bytes);
if (n > 0) { if (n > 0) {
n = mavlink_packetise(_writebuffer, n); n = mavlink_packetise(_writebuffer, n);
} }
@ -631,6 +645,7 @@ void UARTDriver::_timer_tick(void)
uint32_t navail; uint32_t navail;
const uint8_t *readptr = _writebuffer.readptr(navail); const uint8_t *readptr = _writebuffer.readptr(navail);
if (readptr && navail > 0) { if (readptr && navail > 0) {
navail = MIN(navail, max_bytes);
if (!_use_send_recv) { if (!_use_send_recv) {
nwritten = ::write(_fd, readptr, navail); nwritten = ::write(_fd, readptr, navail);
if (nwritten == -1 && errno != EAGAIN && _uart_path) { if (nwritten == -1 && errno != EAGAIN && _uart_path) {
@ -651,6 +666,7 @@ void UARTDriver::_timer_tick(void)
if (space == 0) { if (space == 0) {
return; return;
} }
space = MIN(space, max_bytes);
char buf[space]; char buf[space];
ssize_t nread = 0; ssize_t nread = 0;

View File

@ -123,6 +123,7 @@ private:
bool _is_udp; bool _is_udp;
bool _packetise; bool _packetise;
uint16_t _mc_myport; uint16_t _mc_myport;
uint32_t last_tick_us;
}; };
#endif #endif