diff --git a/src/mavlink.rs b/src/mavlink.rs index 116f3059f1423ee208bdcde881ca80182f94acae..6e0c003f4f8b22ba24392ac27216606084442085 100644 --- a/src/mavlink.rs +++ b/src/mavlink.rs @@ -1,3 +1,8 @@ +//! This module contains all the structs and functions to work with Mavlink messages. +//! +//! It serves also as an abstraction wrapper around the `skyward_mavlink` crate, facilitating +//! rapid switching between different mavlink versions and profiles (_dialects_). + mod base; mod message_broker; mod reflection; @@ -7,4 +12,5 @@ pub use base::*; pub use message_broker::{MessageBroker, MessageView}; pub use reflection::ReflectionContext; +/// Default port for the Ethernet connection pub const DEFAULT_ETHERNET_PORT: u16 = 42069; diff --git a/src/mavlink/base.rs b/src/mavlink/base.rs index f80c2a1eeef30fc14e36ab2f74f84baf226551a1..8b7a4113627a4960345cddb4afa64c8468d08caf 100644 --- a/src/mavlink/base.rs +++ b/src/mavlink/base.rs @@ -1,5 +1,6 @@ -//! This module is a wrapper around the `skyward_mavlink` crate, facilitates -//! rapid switching between different mavlink versions and profiles. +//! Wrapper around the `skyward_mavlink` crate +//! +//! This facilitates rapid switching between different mavlink versions and profiles. //! //! In addition, it provides few utility functions to work with mavlink messages. @@ -23,6 +24,7 @@ pub struct TimedMessage { } impl TimedMessage { + /// Create a new `TimedMessage` instance with the given message and the current time pub fn just_received(message: MavMessage) -> Self { Self { message, @@ -31,6 +33,7 @@ impl TimedMessage { } } +/// Extract fields from a MavLink message using string keys pub fn extract_from_message<K, T>( message: &MavMessage, fields: impl IntoIterator<Item = K>, @@ -50,7 +53,7 @@ where .collect() } -/// Helper function to read a stream of bytes and return an iterator of MavLink messages +/// Read a stream of bytes and return an iterator of MavLink messages pub fn byte_parser(buf: &[u8]) -> impl Iterator<Item = (MavHeader, MavMessage)> + '_ { let mut reader = PeekReader::new(buf); std::iter::from_fn(move || read_v1_msg(&mut reader).ok()) diff --git a/src/mavlink/message_broker.rs b/src/mavlink/message_broker.rs index 0a70820950aebeb1e1477b1ac3d6b3db5a7bfaa2..a04d075ad3f5d8420e64f0a450d997b4e285e838 100644 --- a/src/mavlink/message_broker.rs +++ b/src/mavlink/message_broker.rs @@ -1,3 +1,10 @@ +//! Message broker module, responsible for managing the messages received from +//! the Mavlink listener. +//! +//! The `MessageBroker` struct is the main entry point for this module, and it +//! is responsible for listening to incoming messages from the Mavlink listener, +//! storing them in a map, and updating the views that are interested in them. + use std::{ collections::{HashMap, VecDeque}, num::NonZeroUsize, @@ -17,16 +24,35 @@ use crate::mavlink::byte_parser; use super::{Message, TimedMessage}; +/// Maximum size of the UDP buffer const UDP_BUFFER_SIZE: usize = 65527; +/// Trait for a view that fetch Mavlink messages. +/// +/// This trait should be implemented by any view that wants to interact with the +/// `MessageBroker` and get updates on the messages it is interested in. pub trait MessageView { + /// Returns the widget ID of the view fn widget_id(&self) -> &egui::Id; + /// Returns the message ID of interest for the view fn id_of_interest(&self) -> u32; + /// Returns whether the view is cache valid or not, i.e. if it can be + /// updated or needs to be re-populated from scratch fn is_valid(&self) -> bool; + /// Populates the view with the initial messages. This method is called when + /// the cache is invalid and the view needs to be populated from the stored + /// map of messages fn populate_view(&mut self, msg_slice: &[TimedMessage]); + /// Updates the view with new messages. This method is called when the cache + /// is valid, hence the view only needs to be updated with the new messages fn update_view(&mut self, msg_slice: &[TimedMessage]); } +/// Responsible for storing & dispatching the Mavlink message received. +/// +/// It listens to incoming messages, stores them in a map, and updates the views +/// that are interested in them. It should be used as a singleton in the +/// application. #[derive(Debug)] pub struct MessageBroker { // == Messages == @@ -48,6 +74,7 @@ pub struct MessageBroker { } impl MessageBroker { + /// Creates a new `MessageBroker` with the given channel size and Egui context. pub fn new(channel_size: NonZeroUsize, ctx: egui::Context) -> Self { let (tx, rx) = ring_channel(channel_size); Self { @@ -61,6 +88,8 @@ impl MessageBroker { } } + /// Refreshes the view given as argument. It handles automatically the cache + /// validity based on `is_valid` method of the view. pub fn refresh_view<V: MessageView>(&mut self, view: &mut V) { self.process_incoming_msgs(); if !view.is_valid() || !self.update_queues.contains_key(view.widget_id()) { @@ -70,6 +99,8 @@ impl MessageBroker { } } + /// Stop the listener task from listening to incoming messages, if it is + /// running. pub fn stop_listening(&mut self) { self.running_flag.store(false, Ordering::Relaxed); if let Some(t) = self.task.take() { @@ -77,6 +108,9 @@ impl MessageBroker { } } + /// Start a listener task that listens to incoming messages from the given + /// Ethernet port, and accumulates them in a ring buffer, read only when + /// views request a refresh. pub fn listen_from_ethernet_port(&mut self, port: u16) { // Stop the current listener if it exists self.stop_listening(); @@ -112,10 +146,13 @@ impl MessageBroker { self.task = Some(handle); } + /// Clears all the messages stored in the broker. Useful in message replay + /// scenarios. pub fn clear(&mut self) { self.messages.clear(); } + /// Init a view in case of cache invalidation or first time initialization. fn init_view<V: MessageView>(&mut self, view: &mut V) { if let Some(messages) = self.messages.get(&view.id_of_interest()) { view.populate_view(messages); @@ -124,6 +161,7 @@ impl MessageBroker { .insert(*view.widget_id(), (view.id_of_interest(), VecDeque::new())); } + /// Update a view with new messages, used when the cache is valid. fn update_view<V: MessageView>(&mut self, view: &mut V) { if let Some((_, queue)) = self.update_queues.get_mut(view.widget_id()) { while let Some(msg) = queue.pop_front() { @@ -132,6 +170,8 @@ impl MessageBroker { } } + /// Process the incoming messages from the Mavlink listener, storing them in + /// the messages map and updating the update queues. fn process_incoming_msgs(&mut self) { while let Ok(message) = self.rx.try_recv() { // first update the update queues diff --git a/src/mavlink/reflection.rs b/src/mavlink/reflection.rs index ae2394adc63e7ee27df2800293fe35f619ba5c63..7fa531ee79242ff8337a80e5afb3004b2592fc38 100644 --- a/src/mavlink/reflection.rs +++ b/src/mavlink/reflection.rs @@ -1,15 +1,25 @@ +//! Reflection utilities for MAVLink messages. +//! +//! This module provides a reflection context that allows to query information about MAVLink messages +//! and their fields. This is useful for dynamically generating UI elements based on the available +//! messages and fields. + use std::collections::HashMap; use mavlink_bindgen::parser::{MavProfile, MavType}; use super::MAVLINK_PROFILE_SERIALIZED; +/// Reflection context for MAVLink messages. +/// +/// This struct provides methods to query information about MAVLink messages and their fields. pub struct ReflectionContext { mavlink_profile: MavProfile, id_name_map: HashMap<u32, String>, } impl ReflectionContext { + /// Create a new reflection context. pub fn new() -> Self { let profile: MavProfile = serde_json::from_str(MAVLINK_PROFILE_SERIALIZED) .expect("Failed to deserialize MavProfile"); @@ -24,10 +34,12 @@ impl ReflectionContext { } } + /// Get the name of a message by its ID. pub fn get_name_from_id(&self, message_id: u32) -> Option<&str> { self.id_name_map.get(&message_id).map(|s| s.as_str()) } + /// Get all message names in a sorted vector. pub fn sorted_messages(&self) -> Vec<&str> { let mut msgs: Vec<&str> = self .mavlink_profile @@ -39,6 +51,7 @@ impl ReflectionContext { msgs } + /// Get all field names for a message by its ID. pub fn get_fields_by_id(&self, message_id: u32) -> Vec<&str> { self.mavlink_profile .messages @@ -53,6 +66,7 @@ impl ReflectionContext { .collect() } + /// Get all plottable field names for a message by its ID. pub fn get_plottable_fields_by_id(&self, message_id: u32) -> Vec<&str> { self.mavlink_profile .messages @@ -82,6 +96,7 @@ impl ReflectionContext { .collect() } + /// Get all field names for a message by its name. pub fn get_fields_by_name(&self, message_name: &str) -> Vec<&str> { self.mavlink_profile .messages