From ee56ae784369b26b2719fe93078d8a2d27520d11 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Ant=C3=B4nio=20Cardoso?= Date: Wed, 13 Mar 2024 20:24:36 -0300 Subject: [PATCH 1/2] src: mavlink: Remove sleeps from mavlink recv and send loops These sleeps on the mavlink-related part are _not_ needed, and they only clog the messages, making MCM lose messages, or snowball into an overloaded state. --- src/mavlink/manager.rs | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/src/mavlink/manager.rs b/src/mavlink/manager.rs index 34f4932c..25fce62b 100644 --- a/src/mavlink/manager.rs +++ b/src/mavlink/manager.rs @@ -74,8 +74,6 @@ impl Manager { fn receiver_loop(inner: Arc>) { loop { loop { - std::thread::sleep(std::time::Duration::from_millis(10)); - let Ok(inner_guard) = inner.read() else { break; // Break to trigger reconnection }; @@ -133,19 +131,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 { From c17f5e2290cd8053f648d5ab9ebbf2c947bf40e4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Ant=C3=B4nio=20Cardoso?= Date: Wed, 13 Mar 2024 20:27:05 -0300 Subject: [PATCH 2/2] src: mavlink: Add a filter by source component ID. --- src/mavlink/manager.rs | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/mavlink/manager.rs b/src/mavlink/manager.rs index 25fce62b..810d3922 100644 --- a/src/mavlink/manager.rs +++ b/src/mavlink/manager.rs @@ -104,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