Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_SITL: factor _timer_tick into read/write methods

Browse Source
This commit is contained in:
Peter Barker 2023-07-18 12:04:09 +10:00 committed by Andrew Tridgell
parent 0de7544650
commit 2cb3265131
2 changed files with 43 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_HAL_SITL/UARTDriver.cpp
View File

@ -796,7 +796,7 @@ uint16_t UARTDriver::read_from_async_csv(uint8_t *buffer, uint16_t space)
return i; return i;
} }
void UARTDriver::_timer_tick(void) void UARTDriver::handle_writing_from_writebuffer_to_device()
{ {
if (!_connected) { if (!_connected) {
_check_reconnect(); _check_reconnect();
@ -809,12 +809,12 @@ void UARTDriver::_timer_tick(void)
if (_sitl && _sitl->telem_baudlimit_enable) { if (_sitl && _sitl->telem_baudlimit_enable) {
// limit byte rate to configured baudrate // limit byte rate to configured baudrate
uint32_t now = AP_HAL::micros(); uint32_t now = AP_HAL::micros();
float dt = 1.0e-6 * (now - last_tick_us); float dt = 1.0e-6 * (now - last_write_tick_us);
max_bytes = _uart_baudrate * dt / 10; max_bytes = _uart_baudrate * dt / 10;
if (max_bytes == 0) { if (max_bytes == 0) {
return; return;
} }
last_tick_us = now; last_write_tick_us = now;
} }
#endif #endif
if (_packetise) { if (_packetise) {
@ -856,13 +856,37 @@ void UARTDriver::_timer_tick(void)
} }
} }
} }
}
void UARTDriver::handle_reading_from_device_to_readbuffer()
{
if (!_connected) {
_check_reconnect();
return;
}
uint32_t space = _readbuffer.space(); uint32_t space = _readbuffer.space();
if (space == 0) { if (space == 0) {
return; return;
} }
uint32_t max_bytes = 10000;
#if !defined(HAL_BUILD_AP_PERIPH)
SITL::SIM *_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_read_tick_us);
max_bytes = _uart_baudrate * dt / 10;
if (max_bytes == 0) {
return;
}
last_read_tick_us = now;
}
#endif
space = MIN(space, max_bytes); space = MIN(space, max_bytes);
char buf[space]; char buf[space];
ssize_t nread = 0; ssize_t nread = 0;
if (_mc_fd >= 0) { if (_mc_fd >= 0) {
@ -929,6 +953,13 @@ void UARTDriver::_timer_tick(void)
} }
} }
void UARTDriver::_timer_tick(void)
{
handle_writing_from_writebuffer_to_device();
handle_reading_from_device_to_readbuffer();
}
/* /*
return timestamp estimate in microseconds for when the start of return timestamp estimate in microseconds for when the start of
a nbytes packet arrived on the uart. This should be treated as a a nbytes packet arrived on the uart. This should be treated as a

9
libraries/AP_HAL_SITL/UARTDriver.h
View File

@ -106,7 +106,10 @@ private:
bool _is_udp; bool _is_udp;
bool _packetise; bool _packetise;
uint16_t _mc_myport; uint16_t _mc_myport;
uint32_t last_tick_us;
// for baud-rate limiting:
uint32_t last_read_tick_us;
uint32_t last_write_tick_us;
SITL::SerialDevice *_sim_serial_device; SITL::SerialDevice *_sim_serial_device;
@ -134,6 +137,10 @@ protected:
void _end() override; void _end() override;
void _flush() override; void _flush() override;
bool _discard_input() override; bool _discard_input() override;
private:
void handle_writing_from_writebuffer_to_output();
void handle_reading_from_device_to_readbuffer();
}; };
#endif #endif