mavlink-core: support for recv_raw in Connections

mavlink · Sep 24, 2024 · 1b9a274 · 1b9a274
1 parent 137c77a
commit 1b9a274
Show file tree

Hide file tree

Showing 7 changed files with 114 additions and 7 deletions.
diff --git 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"] }
+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
@@ -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<M: Message + Sync + Send> AsyncMavConnection<M> for AsyncSerialConnection {
  result
  }
 
+ async fn recv_raw(&self) -> Result<MAVLinkRawMessage, crate::error::MessageReadError> {
+ 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::<M, _>(port.deref_mut()).await?)
+ }
+ MavlinkVersion::V2 => {
+ MAVLinkRawMessage::V2(read_v2_raw_message_async::<M, _>(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
@@ -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<M: Message + Sync + Send> AsyncMavConnection<M> for AsyncFileConnection {
  }
  }
 
+ async fn recv_raw(&self) -> Result<MAVLinkRawMessage, crate::error::MessageReadError> {
+ 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::<M, _>(file.deref_mut()).await?)
+ }
+ MavlinkVersion::V2 => {
+ MAVLinkRawMessage::V2(read_v2_raw_message_async::<M, _>(file.deref_mut()).await?)
+ }
+ };
+ #[cfg(feature = "signing")]
+ let result = todo!();
+
+ Ok(result)
+ }
+
  async fn send(&self, _header: &MavHeader, _data: &M) -> Result<usize, MessageWriteError> {
  Ok(0)
  }

diff --git 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<M: Message + Sync + Send> {
  /// 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<MAVLinkRawMessage, crate::error::MessageReadError>;
+
  /// 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
@@ -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<M: Message + Sync + Send> AsyncMavConnection<M> for AsyncTcpConnection {
  result
  }
 
+ async fn recv_raw(&self) -> Result<MAVLinkRawMessage, crate::error::MessageReadError> {
+ 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::<M, _>(reader.deref_mut()).await?)
+ }
+ MavlinkVersion::V2 => {
+ MAVLinkRawMessage::V2(read_v2_raw_message_async::<M, _>(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
@@ -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<M: Message + Sync + Send> AsyncMavConnection<M> for AsyncUdpConnection {
  }
  }
 
+ async fn recv_raw(&self) -> Result<MAVLinkRawMessage, crate::error::MessageReadError> {
+ 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::<M, _>(reader.deref_mut()).await?,
+ ),
+ MavlinkVersion::V2 => MAVLinkRawMessage::V2(
+ read_v2_raw_message_async::<M, _>(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
@@ -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;