Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix mavlink unresponsiveness / lateness. #359

Merged
merged 2 commits into from Mar 14, 2024
Merged

Fix mavlink unresponsiveness / lateness. #359

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ee56ae7
src: mavlink: Remove sleeps from mavlink recv and send loops
joaoantoniocardoso Mar 13, 2024
c17f5e2
src: mavlink: Add a filter by source component ID.
joaoantoniocardoso Mar 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
29 changes: 20 additions & 9 deletions src/mavlink/manager.rs
View file
Open in desktop
Expand Up @@ -74,8 +74,6 @@ impl Manager {
fn receiver_loop(inner: Arc<RwLock<Connection>>) {
loop {
loop {
std::thread::sleep(std::time::Duration::from_millis(10));

let Ok(inner_guard) = inner.read() else {
break; // Break to trigger reconnection
};
Expand Down Expand Up @@ -106,6 +104,19 @@ impl Manager {

trace!("Message received: {header:?}, {message:?}");

// Early filter non-GCS messages to avoid passing unwanted ones to the camera componenets.
let allowed_component_ids = [
mavlink::common::MavComponent::MAV_COMP_ID_ALL as u8,
mavlink::common::MavComponent::MAV_COMP_ID_SYSTEM_CONTROL as u8,
mavlink::common::MavComponent::MAV_COMP_ID_MISSIONPLANNER as u8,
];
if !allowed_component_ids.contains(&header.component_id) {
trace!("Message dropped: {header:?}, {message:?}");
continue;
}

debug!("Message accepted: {header:?}, {message:?}");

// Send the received message to the cameras
if let Err(error) = inner_guard
.sender
Expand Down Expand Up @@ -133,19 +144,19 @@ impl Manager {

loop {
loop {
std::thread::sleep(std::time::Duration::from_millis(10));

// Receive answer from the cameras
let (header, message) = match receiver.try_recv() {
let (header, message) = match receiver.blocking_recv() {
Ok(Message::ToBeSent(message)) => message,
Err(broadcast::error::TryRecvError::Closed) => {
Ok(Message::Received(_)) => continue,
Err(broadcast::error::RecvError::Closed) => {
unreachable!(
"Closed channel: This should never happen, this channel is static!"
);
}
// Since we are sharing a singel channel to both send and receive, and we don't care
// when the channel is empty or lagged, we can safely ignore anything else here.
_ => continue,
Err(broadcast::error::RecvError::Lagged(samples)) => {
warn!("Channel is lagged behind by {samples} messages. Expect degraded performance on the mavlink responsiviness.");
continue;
}
};

let Ok(inner_guard) = inner.read() else {
Expand Down