diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index 5bf97b903c..75f4f449f5 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -29,6 +29,7 @@ serial = { version = "0.4", 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 } [features] "std" = ["byteorder/std"] @@ -41,7 +42,7 @@ async-trait = { version = "0.1.18", optional = true } "embedded" = ["dep:embedded-io", "dep:embedded-io-async"] "embedded-hal-02" = ["dep:nb", "dep:embedded-hal-02"] "serde" = ["dep:serde", "dep:serde_arrays"] -"tokio-1" = ["dep:tokio", "dep:async-trait"] +"tokio-1" = ["dep:tokio", "dep:async-trait", "dep:tokio-serial"] "signing" = ["dep:sha2"] default = ["std", "tcp", "udp", "direct-serial", "serde"] diff --git a/mavlink-core/src/async_connection/direct_serial.rs b/mavlink-core/src/async_connection/direct_serial.rs new file mode 100644 index 0000000000..3edd6f2c12 --- /dev/null +++ b/mavlink-core/src/async_connection/direct_serial.rs @@ -0,0 +1,121 @@ +//! Async Serial MAVLINK connection + +use core::ops::DerefMut; +use std::io; + +use tokio::sync::Mutex; +use tokio_serial::{SerialPort, SerialPortBuilderExt, SerialStream}; + +use crate::{ + async_peek_reader::AsyncPeekReader, read_versioned_msg_async_signed, + write_versioned_msg_async_signed, MavHeader, MavlinkVersion, Message, +}; + +#[cfg(not(feature = "signing"))] +use crate::{read_versioned_msg, write_versioned_msg}; +#[cfg(feature = "signing")] +use crate::{SigningConfig, SigningData}; + +use super::AsyncMavConnection; + +pub fn open(settings: &str) -> io::Result { + let settings_toks: Vec<&str> = settings.split(':').collect(); + if settings_toks.len() < 2 { + return Err(io::Error::new( + io::ErrorKind::AddrNotAvailable, + "Incomplete port settings", + )); + } + + let Ok(baud) = settings_toks[1].parse::() else { + return Err(io::Error::new( + io::ErrorKind::AddrNotAvailable, + "Invalid baud rate", + )); + }; + + let port_name = settings_toks[0]; + let mut port = tokio_serial::new(port_name, baud).open_native_async()?; + port.set_data_bits(tokio_serial::DataBits::Eight)?; + port.set_parity(tokio_serial::Parity::None)?; + port.set_stop_bits(tokio_serial::StopBits::One)?; + port.set_flow_control(tokio_serial::FlowControl::None)?; + + Ok(AsyncSerialConnection { + port: Mutex::new(AsyncPeekReader::new(port)), + sequence: Mutex::new(0), + protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, + }) +} + +pub struct AsyncSerialConnection { + port: Mutex>, + sequence: Mutex, + protocol_version: MavlinkVersion, + #[cfg(feature = "signing")] + signing_data: Option, +} + +#[async_trait::async_trait] +impl AsyncMavConnection for AsyncSerialConnection { + async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { + let mut port = self.port.lock().await; + + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg_async(port.deref_mut(), self.protocol_version).await; + #[cfg(feature = "signing")] + let result = read_versioned_msg_async_signed( + port.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ) + .await; + result + } + + async fn send( + &self, + header: &MavHeader, + data: &M, + ) -> Result { + let mut port = self.port.lock().await; + let mut sequence = self.sequence.lock().await; + + let header = MavHeader { + sequence: *sequence, + system_id: header.system_id, + component_id: header.component_id, + }; + + *sequence = sequence.wrapping_add(1); + + #[cfg(not(feature = "signing"))] + let result = + write_versioned_msg_async(port.reader_mut(), self.protocol_version, header, data).await; + #[cfg(feature = "signing")] + let result = write_versioned_msg_async_signed( + port.reader_mut(), + self.protocol_version, + header, + data, + self.signing_data.as_ref(), + ) + .await; + result + } + + fn set_protocol_version(&mut self, version: MavlinkVersion) { + self.protocol_version = version; + } + + fn get_protocol_version(&self) -> MavlinkVersion { + self.protocol_version + } + + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option) { + self.signing_data = signing_data.map(SigningData::from_config) + } +} diff --git a/mavlink-core/src/async_connection/mod.rs b/mavlink-core/src/async_connection/mod.rs index 35e2fa0e78..b9c1f7d053 100644 --- a/mavlink-core/src/async_connection/mod.rs +++ b/mavlink-core/src/async_connection/mod.rs @@ -8,6 +8,9 @@ mod tcp; #[cfg(feature = "udp")] mod udp; +#[cfg(feature = "direct-serial")] +mod direct_serial; + mod file; #[cfg(feature = "signing")]