aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: add support for passthrough with baud changes

Browse Source
This commit is contained in:
bugobliterator 2021-07-10 23:44:23 +05:30 committed by Andrew Tridgell
parent d622aad592
commit f3bc75c538
1 changed files with 37 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -5265,14 +5265,16 @@ void GCS::update_passthru(void)
{ {
WITH_SEMAPHORE(_passthru.sem); WITH_SEMAPHORE(_passthru.sem);
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
uint32_t baud1, baud2;
bool enabled = AP::serialmanager().get_passthru(_passthru.port1, _passthru.port2, _passthru.timeout_s, bool enabled = AP::serialmanager().get_passthru(_passthru.port1, _passthru.port2, _passthru.timeout_s,
_passthru.baud1, _passthru.baud2); baud1, baud2);
if (enabled && !_passthru.enabled) { if (enabled && !_passthru.enabled) {
_passthru.start_ms = now; _passthru.start_ms = now;
_passthru.last_ms = 0; _passthru.last_ms = 0;
_passthru.enabled = true; _passthru.enabled = true;
_passthru.last_port1_data_ms = now; _passthru.last_port1_data_ms = now;
_passthru.baud1 = baud1;
_passthru.baud2 = baud2;
gcs().send_text(MAV_SEVERITY_INFO, "Passthru enabled"); gcs().send_text(MAV_SEVERITY_INFO, "Passthru enabled");
if (!_passthru.timer_installed) { if (!_passthru.timer_installed) {
_passthru.timer_installed = true; _passthru.timer_installed = true;
@ -5282,6 +5284,15 @@ void GCS::update_passthru(void)
_passthru.enabled = false; _passthru.enabled = false;
_passthru.port1->lock_port(0, 0); _passthru.port1->lock_port(0, 0);
_passthru.port2->lock_port(0, 0); _passthru.port2->lock_port(0, 0);
// Restore original baudrates
if (_passthru.baud1 != baud1) {
_passthru.port1->end();
_passthru.port1->begin(baud1);
}
if (_passthru.baud2 != baud2) {
_passthru.port2->end();
_passthru.port2->begin(baud2);
}
gcs().send_text(MAV_SEVERITY_INFO, "Passthru disabled"); gcs().send_text(MAV_SEVERITY_INFO, "Passthru disabled");
} else if (enabled && } else if (enabled &&
_passthru.timeout_s && _passthru.timeout_s &&
@ -5291,6 +5302,15 @@ void GCS::update_passthru(void)
_passthru.port1->lock_port(0, 0); _passthru.port1->lock_port(0, 0);
_passthru.port2->lock_port(0, 0); _passthru.port2->lock_port(0, 0);
AP::serialmanager().disable_passthru(); AP::serialmanager().disable_passthru();
// Restore original baudrates
if (_passthru.baud1 != baud1) {
_passthru.port1->end();
_passthru.port1->begin(baud1);
}
if (_passthru.baud2 != baud2) {
_passthru.port2->end();
_passthru.port2->begin(baud2);
}
gcs().send_text(MAV_SEVERITY_INFO, "Passthru timed out"); gcs().send_text(MAV_SEVERITY_INFO, "Passthru timed out");
} }
} }
@ -5323,6 +5343,21 @@ void GCS::passthru_timer(void)
_passthru.port1->lock_port(lock_key, lock_key); _passthru.port1->lock_port(lock_key, lock_key);
_passthru.port2->lock_port(lock_key, lock_key); _passthru.port2->lock_port(lock_key, lock_key);
// Check for requested Baud rates over USB
uint32_t baud = _passthru.port1->get_usb_baud();
if (_passthru.baud2 != baud && baud != 0) {
_passthru.baud2 = baud;
_passthru.port2->end();
_passthru.port2->begin_locked(baud, lock_key);
}
baud = _passthru.port2->get_usb_baud();
if (_passthru.baud1 != baud && baud != 0) {
_passthru.baud1 = baud;
_passthru.port1->end();
_passthru.port1->begin_locked(baud, lock_key);
}
int16_t b; int16_t b;
uint8_t buf[64]; uint8_t buf[64];
uint8_t nbytes = 0; uint8_t nbytes = 0;