Skip to content

Commit

Permalink
feat: async serial
Browse files Browse the repository at this point in the history
  • Loading branch information
roby2014 committed Sep 21, 2024
1 parent 5f2ecbe commit cac901d
Show file tree
Hide file tree
Showing 3 changed files with 135 additions and 1 deletion.
3 changes: 2 additions & 1 deletion mavlink-core/Cargo.toml
Original file line number Diff line number Diff line change
Expand Up @@ -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"]
Expand All @@ -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"]

Expand Down
120 changes: 120 additions & 0 deletions mavlink-core/src/async_connection/direct_serial.rs
Original file line number Diff line number Diff line change
@@ -0,0 +1,120 @@
//! 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, MavHeader, MavlinkVersion, Message};

#[cfg(not(feature = "signing"))]
use crate::{read_versioned_msg_async, write_versioned_msg_async};
#[cfg(feature = "signing")]
use crate::{
read_versioned_msg_async_signed, write_versioned_msg_async_signed, SigningConfig, SigningData,
};

use super::AsyncMavConnection;

pub fn open(settings: &str) -> io::Result<AsyncSerialConnection> {
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::<u32>() 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<AsyncPeekReader<SerialStream>>,
sequence: Mutex<u8>,
protocol_version: MavlinkVersion,
#[cfg(feature = "signing")]
signing_data: Option<SigningData>,
}

#[async_trait::async_trait]
impl<M: Message + Sync + Send> AsyncMavConnection<M> 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<usize, crate::error::MessageWriteError> {
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<SigningConfig>) {
self.signing_data = signing_data.map(SigningData::from_config)
}
}
13 changes: 13 additions & 0 deletions mavlink-core/src/async_connection/mod.rs
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,9 @@ mod tcp;
#[cfg(feature = "udp")]
mod udp;

#[cfg(feature = "direct-serial")]
mod direct_serial;

mod file;

#[cfg(feature = "signing")]
Expand Down Expand Up @@ -70,6 +73,7 @@ pub trait AsyncMavConnection<M: Message + Sync + Send> {
/// * `udpin:<addr>:<port>` to create a UDP server, listening for incoming packets
/// * `udpout:<addr>:<port>` to create a UDP client
/// * `udpbcast:<addr>:<port>` to create a UDP broadcast
/// * `serial:<port>:<baudrate>` to create a serial connection
/// * `file:<path>` to extract file data
///
/// Serial is currently not supported for async connections, use [`crate::connect`] instead.
Expand Down Expand Up @@ -101,6 +105,15 @@ pub async fn connect_async<M: Message + Sync + Send>(
{
protocol_err
}
} else if cfg!(feature = "direct-serial") && address.starts_with("serial") {
#[cfg(feature = "direct-serial")]
{
Ok(Box::new(direct_serial::open(&address["serial:".len()..])?))
}
#[cfg(not(feature = "direct-serial"))]
{
protocol_err
}
} else if address.starts_with("file") {
Ok(Box::new(file::open(&address["file:".len()..]).await?))
} else {
Expand Down

0 comments on commit cac901d

Please sign in to comment.