src: mavlink: Add a filter by GCS ids

src/mavlink/manager.rs

@@ -11,6 +11,7 @@ use crate::settings;
 
 lazy_static! {
     static ref MANAGER: Arc<Mutex<Manager>> = Default::default();
+    static ref GCS_IDS: Arc<RwLock<Vec<u8>>> = Default::default();
 }
 
 pub struct Manager {
@@ -104,6 +105,29 @@ impl Manager {
 
                 trace!("Message received: {header:?}, {message:?}");
 
+                // Early filter non-GCS messages to avoid passing unwanted ones to the camera componenets.
+                // Here we register the GCSs when they send a heartbeat
+                let componenet_id = header.component_id;
+                let registered_gcs_ids = GCS_IDS.read().unwrap().clone();
+                if !registered_gcs_ids.contains(&componenet_id) {
+                    let MavMessage::HEARTBEAT(heartbeat) = &message else {
+                        trace!("Message dropped: {header:?}, {message:?}");
+                        continue;
+                    };
+
+                    if heartbeat.mavtype != mavlink::common::MavType::MAV_TYPE_GCS {
+                        trace!("Message dropped: {header:?}, {message:?}");
+                        continue;
+                    }
+
+                    GCS_IDS.write().unwrap().push(componenet_id);
+                    debug!(
+                        "New GCS {componenet_id} was registered to the accepted componenet ids: {registered_gcs_ids:?}"
+                    );
+                }
+
+                debug!("Message accepted: {header:?}, {message:?}");
+
                 // Send the received message to the cameras
                 if let Err(error) = inner_guard
                     .sender