Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_HAL_SITL/UARTDriver.cpp
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];
// default to 1MBit
_uart_baudrate = 1000000U;
if (baud != 0) {
_uart_baudrate = baud;
}
if (strcmp(path, "GPS1") == 0) {
/* gps */
@ -612,9 +613,22 @@ void UARTDriver::_timer_tick(void)
return;
}
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) {
uint16_t n = _writebuffer.available();
n = MIN(n, max_bytes);
if (n > 0) {
n = mavlink_packetise(_writebuffer, n);
}
@ -631,6 +645,7 @@ void UARTDriver::_timer_tick(void)
uint32_t navail;
const uint8_t *readptr = _writebuffer.readptr(navail);
if (readptr && navail > 0) {
navail = MIN(navail, max_bytes);
if (!_use_send_recv) {
nwritten = ::write(_fd, readptr, navail);
if (nwritten == -1 && errno != EAGAIN && _uart_path) {
@ -651,6 +666,7 @@ void UARTDriver::_timer_tick(void)
if (space == 0) {
return;
}
space = MIN(space, max_bytes);
char buf[space];
ssize_t nread = 0;

1
libraries/AP_HAL_SITL/UARTDriver.h
View File

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