GCS_MAVLink: Avoid serial passthrough buffer exhausted/lost data

Just don't read more than we can write.
This commit is contained in:
Brad Bosch 2024-04-25 11:29:32 -05:00 committed by Randy Mackay
parent 280b2536b3
commit a156e3e9d8
1 changed files with 2 additions and 2 deletions

View File

@ -6636,14 +6636,14 @@ void GCS::passthru_timer(void)
uint8_t buf[64];
// read from port1, and write to port2
int16_t nbytes = _passthru.port1->read_locked(buf, sizeof(buf), lock_key);
int16_t nbytes = _passthru.port1->read_locked(buf, MIN(sizeof(buf),_passthru.port2->txspace()), lock_key);
if (nbytes > 0) {
_passthru.last_port1_data_ms = AP_HAL::millis();
_passthru.port2->write_locked(buf, nbytes, lock_key);
}
// read from port2, and write to port1
nbytes = _passthru.port2->read_locked(buf, sizeof(buf), lock_key);
nbytes = _passthru.port2->read_locked(buf, MIN(sizeof(buf),_passthru.port1->txspace()), lock_key);
if (nbytes > 0) {
_passthru.port1->write_locked(buf, nbytes, lock_key);
}