From 1b9a274f9a1edb5b58a18ef22318960bc19bc23f Mon Sep 17 00:00:00 2001 From: roby2014 Date: Tue, 24 Sep 2024 15:41:16 +0100 Subject: [PATCH] mavlink-core: support for recv_raw in Connections --- mavlink-core/Cargo.toml | 18 ++++++++++-- .../src/async_connection/direct_serial.rs | 22 ++++++++++++++- mavlink-core/src/async_connection/file.rs | 20 +++++++++++++ mavlink-core/src/async_connection/mod.rs | 4 ++- mavlink-core/src/async_connection/tcp.rs | 22 ++++++++++++++- mavlink-core/src/async_connection/udp.rs | 28 ++++++++++++++++++- mavlink-core/src/lib.rs | 7 +++++ 7 files changed, 114 insertions(+), 7 deletions(-) diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index 413572b61f..ac75eceb1f 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -26,7 +26,12 @@ embedded-io-async = { version = "0.6.1", optional = true } serde = { version = "1.0.115", optional = true, features = ["derive"] } serde_arrays = { version = "0.1.0", optional = true } serial = { version = "0.4", optional = true } -tokio = { version = "1.0", default-features = false, features = ["io-util", "net", "sync", "fs"], optional = true } +tokio = { version = "1.0", default-features = false, features = [ + "io-util", + "net", + "sync", + "fs", +], optional = true } sha2 = { version = "0.10", optional = true } async-trait = { version = "0.1.18", optional = true } tokio-serial = { version = "5.4.4", default-features = false, optional = true } @@ -44,7 +49,14 @@ tokio-serial = { version = "5.4.4", default-features = false, optional = true } "serde" = ["dep:serde", "dep:serde_arrays"] "tokio-1" = ["dep:tokio", "dep:async-trait", "dep:tokio-serial"] "signing" = ["dep:sha2"] -default = ["std", "tcp", "udp", "direct-serial", "serde"] +default = ["std", "tcp", "udp", "direct-serial", "serde", "tokio-1"] [dev-dependencies] -tokio = { version = "1.0", default-features = false, features = ["io-util", "net", "sync", "fs", "macros", "rt"] } \ No newline at end of file +tokio = { version = "1.0", default-features = false, features = [ + "io-util", + "net", + "sync", + "fs", + "macros", + "rt", +] } diff --git a/mavlink-core/src/async_connection/direct_serial.rs b/mavlink-core/src/async_connection/direct_serial.rs index d9b2fe1de2..52cf590f11 100644 --- a/mavlink-core/src/async_connection/direct_serial.rs +++ b/mavlink-core/src/async_connection/direct_serial.rs @@ -6,7 +6,10 @@ use std::io; use tokio::sync::Mutex; use tokio_serial::{SerialPort, SerialPortBuilderExt, SerialStream}; -use crate::{async_peek_reader::AsyncPeekReader, MavHeader, MavlinkVersion, Message}; +use crate::{ + async_peek_reader::AsyncPeekReader, read_v1_raw_message_async, read_v2_raw_message_async, + MAVLinkRawMessage, MAVLinkV2MessageRaw, MavHeader, MavlinkVersion, Message, +}; #[cfg(not(feature = "signing"))] use crate::{read_versioned_msg_async, write_versioned_msg_async}; @@ -74,6 +77,23 @@ impl AsyncMavConnection for AsyncSerialConnection { result } + async fn recv_raw(&self) -> Result { + let mut port = self.port.lock().await; + #[cfg(not(feature = "signing"))] + let result = match self.protocol_version { + MavlinkVersion::V1 => { + MAVLinkRawMessage::V1(read_v1_raw_message_async::(port.deref_mut()).await?) + } + MavlinkVersion::V2 => { + MAVLinkRawMessage::V2(read_v2_raw_message_async::(port.deref_mut()).await?) + } + }; + #[cfg(feature = "signing")] + let result = todo!(); + + Ok(result) + } + async fn send( &self, header: &MavHeader, diff --git a/mavlink-core/src/async_connection/file.rs b/mavlink-core/src/async_connection/file.rs index 5dae56d99e..2bf6c3d6af 100644 --- a/mavlink-core/src/async_connection/file.rs +++ b/mavlink-core/src/async_connection/file.rs @@ -6,6 +6,9 @@ use super::AsyncMavConnection; use crate::error::{MessageReadError, MessageWriteError}; use crate::{async_peek_reader::AsyncPeekReader, MavHeader, MavlinkVersion, Message}; +use crate::{ + read_v1_raw_message_async, read_v2_raw_message_async, MAVLinkRawMessage, MAVLinkV2MessageRaw, +}; use tokio::fs::File; use tokio::io; @@ -64,6 +67,23 @@ impl AsyncMavConnection for AsyncFileConnection { } } + async fn recv_raw(&self) -> Result { + let mut file = self.file.lock().await; + #[cfg(not(feature = "signing"))] + let result = match self.protocol_version { + MavlinkVersion::V1 => { + MAVLinkRawMessage::V1(read_v1_raw_message_async::(file.deref_mut()).await?) + } + MavlinkVersion::V2 => { + MAVLinkRawMessage::V2(read_v2_raw_message_async::(file.deref_mut()).await?) + } + }; + #[cfg(feature = "signing")] + let result = todo!(); + + Ok(result) + } + async fn send(&self, _header: &MavHeader, _data: &M) -> Result { Ok(0) } diff --git a/mavlink-core/src/async_connection/mod.rs b/mavlink-core/src/async_connection/mod.rs index b2eb5c6757..38880c14d6 100644 --- a/mavlink-core/src/async_connection/mod.rs +++ b/mavlink-core/src/async_connection/mod.rs @@ -1,6 +1,6 @@ use tokio::io; -use crate::{MavFrame, MavHeader, MavlinkVersion, Message}; +use crate::{MAVLinkRawMessage, MAVLinkV2MessageRaw, MavFrame, MavHeader, MavlinkVersion, Message}; #[cfg(feature = "tcp")] mod tcp; @@ -24,6 +24,8 @@ pub trait AsyncMavConnection { /// Yield until a valid frame is received, ignoring invalid messages. async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError>; + async fn recv_raw(&self) -> Result; + /// Send a mavlink message async fn send( &self, diff --git a/mavlink-core/src/async_connection/tcp.rs b/mavlink-core/src/async_connection/tcp.rs index 4cd1f9ac9a..6f7562f99b 100644 --- a/mavlink-core/src/async_connection/tcp.rs +++ b/mavlink-core/src/async_connection/tcp.rs @@ -2,7 +2,10 @@ use super::{get_socket_addr, AsyncMavConnection}; use crate::async_peek_reader::AsyncPeekReader; -use crate::{MavHeader, MavlinkVersion, Message}; +use crate::{ + read_v1_raw_message, read_v1_raw_message_async, read_v2_raw_message_async, MAVLinkRawMessage, + MAVLinkV2MessageRaw, MavHeader, MavlinkVersion, Message, +}; use core::ops::DerefMut; use tokio::io; @@ -112,6 +115,23 @@ impl AsyncMavConnection for AsyncTcpConnection { result } + async fn recv_raw(&self) -> Result { + let mut reader = self.reader.lock().await; + #[cfg(not(feature = "signing"))] + let result = match self.protocol_version { + MavlinkVersion::V1 => { + MAVLinkRawMessage::V1(read_v1_raw_message_async::(reader.deref_mut()).await?) + } + MavlinkVersion::V2 => { + MAVLinkRawMessage::V2(read_v2_raw_message_async::(reader.deref_mut()).await?) + } + }; + #[cfg(feature = "signing")] + let result = todo!(); + + Ok(result) + } + async fn send( &self, header: &MavHeader, diff --git a/mavlink-core/src/async_connection/udp.rs b/mavlink-core/src/async_connection/udp.rs index 3f06b83e1d..e8d0f4ec70 100644 --- a/mavlink-core/src/async_connection/udp.rs +++ b/mavlink-core/src/async_connection/udp.rs @@ -9,7 +9,10 @@ use tokio::{ sync::Mutex, }; -use crate::{async_peek_reader::AsyncPeekReader, MavHeader, MavlinkVersion, Message}; +use crate::{ + async_peek_reader::AsyncPeekReader, read_v1_raw_message_async, read_v2_raw_message_async, + MAVLinkRawMessage, MAVLinkV2MessageRaw, MavHeader, MavlinkVersion, Message, +}; use super::{get_socket_addr, AsyncMavConnection}; @@ -176,6 +179,29 @@ impl AsyncMavConnection for AsyncUdpConnection { } } + async fn recv_raw(&self) -> Result { + let mut reader = self.reader.lock().await; + loop { + #[cfg(not(feature = "signing"))] + let result = match self.protocol_version { + MavlinkVersion::V1 => MAVLinkRawMessage::V1( + read_v1_raw_message_async::(reader.deref_mut()).await?, + ), + MavlinkVersion::V2 => MAVLinkRawMessage::V2( + read_v2_raw_message_async::(reader.deref_mut()).await?, + ), + }; + #[cfg(feature = "signing")] + let result = todo!(); + if self.server { + if let addr @ Some(_) = reader.reader_ref().last_recv_address { + self.writer.lock().await.dest = addr; + } + } + return Ok(result); + } + } + async fn send( &self, header: &MavHeader, diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 8af70ad89b..5057d1ad36 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -127,6 +127,13 @@ pub enum MavlinkVersion { V2, } +/// MAVLink raw message types wrapper +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +pub enum MAVLinkRawMessage { + V1(MAVLinkV1MessageRaw), + V2(MAVLinkV2MessageRaw), +} + /// Message framing marker for mavlink v1 pub const MAV_STX: u8 = 0xFE;