diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index f5e7d34cab..82b2410b00 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -19,8 +19,10 @@ rust-version = "1.65.0" [dependencies] crc-any = { workspace = true, default-features = false } byteorder = { workspace = true, default-features = false } -embedded-hal = { version = "0.2", optional = true } nb = { version = "1.0", optional = true } +embedded-hal-02 = { version = "0.2", optional = true, package = "embedded-hal" } +embedded-io = { version = "0.6.1", optional = true } +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 } @@ -30,6 +32,10 @@ serial = { version = "0.4", optional = true } "udp" = [] "tcp" = [] "direct-serial" = ["serial"] -"embedded" = ["embedded-hal", "nb"] +# NOTE: Only one of 'embedded' and 'embedded-hal-02' features can be enabled. +# Use "embedded' feature to enable embedded-hal=1.0 (embedded-io and embedded-io-async is part of embedded-hal). +# Use 'embedded-hal-0.2' feature to enable deprecated embedded-hal=0.2.3 (some hals is not supports embedded-hal=1.0 yet). +"embedded" = ["dep:embedded-io", "dep:embedded-io-async"] +"embedded-hal-02" = ["dep:nb", "dep:embedded-hal-02"] "serde" = ["dep:serde", "dep:serde_arrays"] default = ["std", "tcp", "udp", "direct-serial", "serde"] diff --git a/mavlink-core/src/embedded.rs b/mavlink-core/src/embedded.rs index 574ae2b826..96917e506c 100644 --- a/mavlink-core/src/embedded.rs +++ b/mavlink-core/src/embedded.rs @@ -1,30 +1,45 @@ use crate::error::*; +#[cfg(all(feature = "embedded", feature = "embedded-hal-02"))] +const _: () = panic!("Only one of 'embedded' and 'embedded-hal-02' features can be enabled."); + /// Replacement for std::io::Read + byteorder::ReadBytesExt in no_std envs pub trait Read { - fn read_u8(&mut self) -> Result; + fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), MessageReadError>; +} +#[cfg(all(feature = "embedded", not(feature = "embedded-hal-02")))] +impl Read for R { + fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), MessageReadError> { + embedded_io::Read::read_exact(self, buf).map_err(|_| MessageReadError::Io) + } +} + +#[cfg(all(feature = "embedded-hal-02", not(feature = "embedded")))] +impl> Read for R { fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), MessageReadError> { for byte in buf { - *byte = self.read_u8()?; + *byte = nb::block!(self.read()).map_err(|_| MessageReadError::Io)?; } Ok(()) } } -impl> Read for R { - fn read_u8(&mut self) -> Result { - nb::block!(self.read()).map_err(|_| MessageReadError::Io) - } -} - /// Replacement for std::io::Write + byteorder::WriteBytesExt in no_std envs pub trait Write { fn write_all(&mut self, buf: &[u8]) -> Result<(), MessageWriteError>; } -impl> Write for W { +#[cfg(all(feature = "embedded", not(feature = "embedded-hal-02")))] +impl Write for W { + fn write_all(&mut self, buf: &[u8]) -> Result<(), MessageWriteError> { + embedded_io::Write::write_all(self, buf).map_err(|_| MessageWriteError::Io) + } +} + +#[cfg(all(feature = "embedded-hal-02", not(feature = "embedded")))] +impl> Write for W { fn write_all(&mut self, buf: &[u8]) -> Result<(), MessageWriteError> { for byte in buf { nb::block!(self.write(*byte)).map_err(|_| MessageWriteError::Io)?; diff --git a/mavlink-core/src/error.rs b/mavlink-core/src/error.rs index b60136bd09..49394f8d64 100644 --- a/mavlink-core/src/error.rs +++ b/mavlink-core/src/error.rs @@ -32,7 +32,7 @@ impl Error for ParserError {} pub enum MessageReadError { #[cfg(feature = "std")] Io(std::io::Error), - #[cfg(feature = "embedded")] + #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] Io, Parse(ParserError), } @@ -42,7 +42,7 @@ impl Display for MessageReadError { match self { #[cfg(feature = "std")] Self::Io(e) => write!(f, "Failed to read message: {e:#?}"), - #[cfg(feature = "embedded")] + #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] Self::Io => write!(f, "Failed to read message"), Self::Parse(e) => write!(f, "Failed to read message: {e:#?}"), } @@ -69,7 +69,7 @@ impl From for MessageReadError { pub enum MessageWriteError { #[cfg(feature = "std")] Io(std::io::Error), - #[cfg(feature = "embedded")] + #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] Io, } @@ -78,7 +78,7 @@ impl Display for MessageWriteError { match self { #[cfg(feature = "std")] Self::Io(e) => write!(f, "Failed to write message: {e:#?}"), - #[cfg(feature = "embedded")] + #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] Self::Io => write!(f, "Failed to write message"), } } diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 0e6b653157..ef18bfa994 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -49,9 +49,9 @@ pub mod error; #[cfg(feature = "std")] pub use self::connection::{connect, MavConnection}; -#[cfg(feature = "embedded")] +#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] pub mod embedded; -#[cfg(feature = "embedded")] +#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] use embedded::{Read, Write}; pub const MAX_FRAME_SIZE: usize = 280; @@ -395,7 +395,49 @@ pub fn read_v1_raw_message( } } -/// Read a MAVLink v1 message from a Read stream. +/// Async read a raw buffer with the mavlink message +/// V1 maximum size is 263 bytes: `` +/// +/// # Example +/// See mavlink/examples/embedded-async-read full example for details. +#[cfg(feature = "embedded")] +pub async fn read_v1_raw_message_async( + reader: &mut impl embedded_io_async::Read, +) -> Result { + loop { + // search for the magic framing value indicating start of mavlink message + let mut byte = [0u8]; + loop { + reader + .read_exact(&mut byte) + .await + .map_err(|_| error::MessageReadError::Io)?; + if byte[0] == MAV_STX { + break; + } + } + + let mut message = MAVLinkV1MessageRaw::new(); + + message.0[0] = MAV_STX; + reader + .read_exact(message.mut_header()) + .await + .map_err(|_| error::MessageReadError::Io)?; + reader + .read_exact(message.mut_payload_and_checksum()) + .await + .map_err(|_| error::MessageReadError::Io)?; + + // retry if CRC failed after previous STX + // (an STX byte may appear in the middle of a message) + if message.has_valid_crc::() { + return Ok(message); + } + } +} + +/// Read a MAVLink v1 message from a Read stream. pub fn read_v1_msg( r: &mut PeekReader, ) -> Result<(MavHeader, M), error::MessageReadError> { @@ -419,6 +461,34 @@ pub fn read_v1_msg( .map_err(|err| err.into()) } +/// Async read a MAVLink v1 message from a Read stream. +/// +/// NOTE: it will be add ~80KB to firmware flash size because all *_DATA::deser methods will be add to firmware. +/// Use `*_DATA::ser` methods manually to prevent it. +#[cfg(feature = "embedded")] +pub async fn read_v1_msg_async( + r: &mut impl embedded_io_async::Read, +) -> Result<(MavHeader, M), error::MessageReadError> { + let message = read_v1_raw_message_async::(r).await?; + + M::parse( + MavlinkVersion::V1, + u32::from(message.message_id()), + message.payload(), + ) + .map(|msg| { + ( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + msg, + ) + }) + .map_err(|err| err.into()) +} + const MAVLINK_IFLAG_SIGNED: u8 = 0x01; #[derive(Debug, Copy, Clone, PartialEq, Eq)] @@ -621,6 +691,48 @@ pub fn read_v2_raw_message( } } +/// Async read a raw buffer with the mavlink message +/// V2 maximum size is 280 bytes: `` +/// +/// # Example +/// See mavlink/examples/embedded-async-read full example for details. +#[cfg(feature = "embedded")] +pub async fn read_v2_raw_message_async( + reader: &mut impl embedded_io_async::Read, +) -> Result { + loop { + // search for the magic framing value indicating start of mavlink message + let mut byte = [0u8]; + loop { + reader + .read_exact(&mut byte) + .await + .map_err(|_| error::MessageReadError::Io)?; + if byte[0] == MAV_STX_V2 { + break; + } + } + + let mut message = MAVLinkV2MessageRaw::new(); + + message.0[0] = MAV_STX; + reader + .read_exact(message.mut_header()) + .await + .map_err(|_| error::MessageReadError::Io)?; + reader + .read_exact(message.mut_payload_and_checksum_and_sign()) + .await + .map_err(|_| error::MessageReadError::Io)?; + + // retry if CRC failed after previous STX + // (an STX byte may appear in the middle of a message) + if message.has_valid_crc::() { + return Ok(message); + } + } +} + /// Read a MAVLink v2 message from a Read stream. pub fn read_v2_msg( read: &mut PeekReader, @@ -641,6 +753,34 @@ pub fn read_v2_msg( .map_err(|err| err.into()) } +/// Async read a MAVLink v2 message from a Read stream. +/// +/// NOTE: it will be add ~80KB to firmware flash size because all *_DATA::deser methods will be add to firmware. +/// Use `*_DATA::deser` methods manually to prevent it. +#[cfg(feature = "embedded")] +pub async fn read_v2_msg_async( + r: &mut R, +) -> Result<(MavHeader, M), error::MessageReadError> { + let message = read_v2_raw_message_async::(r).await?; + + M::parse( + MavlinkVersion::V2, + u32::from(message.message_id()), + message.payload(), + ) + .map(|msg| { + ( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + msg, + ) + }) + .map_err(|err| err.into()) +} + /// Write a message using the given mavlink version pub fn write_versioned_msg( w: &mut W, @@ -654,6 +794,23 @@ pub fn write_versioned_msg( } } +/// Async write a message using the given mavlink version +/// +/// NOTE: it will be add ~70KB to firmware flash size because all *_DATA::ser methods will be add to firmware. +/// Use `*_DATA::ser` methods manually to prevent it. +#[cfg(feature = "embedded")] +pub async fn write_versioned_msg_async( + w: &mut impl embedded_io_async::Write, + version: MavlinkVersion, + header: MavHeader, + data: &M, +) -> Result { + match version { + MavlinkVersion::V2 => write_v2_msg_async(w, header, data).await, + MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await, + } +} + /// Write a MAVLink v2 message to a Write stream. pub fn write_v2_msg( w: &mut W, @@ -671,6 +828,29 @@ pub fn write_v2_msg( Ok(len) } +/// Async write a MAVLink v2 message to a Write stream. +/// +/// NOTE: it will be add ~70KB to firmware flash size because all *_DATA::ser methods will be add to firmware. +/// Use `*_DATA::ser` methods manually to prevent it. +#[cfg(feature = "embedded")] +pub async fn write_v2_msg_async( + w: &mut impl embedded_io_async::Write, + header: MavHeader, + data: &M, +) -> Result { + let mut message_raw = MAVLinkV2MessageRaw::new(); + message_raw.serialize_message(header, data); + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2; + + w.write_all(&message_raw.0[..len]) + .await + .map_err(|_| error::MessageWriteError::Io)?; + + Ok(len) +} + /// Write a MAVLink v1 message to a Write stream. pub fn write_v1_msg( w: &mut W, @@ -687,3 +867,26 @@ pub fn write_v1_msg( Ok(len) } + +/// Write a MAVLink v1 message to a Write stream. +/// +/// NOTE: it will be add ~70KB to firmware flash size because all *_DATA::ser methods will be add to firmware. +/// Use `*_DATA::ser` methods manually to prevent it. +#[cfg(feature = "embedded")] +pub async fn write_v1_msg_async( + w: &mut impl embedded_io_async::Write, + header: MavHeader, + data: &M, +) -> Result { + let mut message_raw = MAVLinkV1MessageRaw::new(); + message_raw.serialize_message(header, data); + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2; + + w.write_all(&message_raw.0[..len]) + .await + .map_err(|_| error::MessageWriteError::Io)?; + + Ok(len) +} diff --git a/mavlink-core/src/peek_reader.rs b/mavlink-core/src/peek_reader.rs index b760b2a8a3..5a5a8a11c5 100644 --- a/mavlink-core/src/peek_reader.rs +++ b/mavlink-core/src/peek_reader.rs @@ -11,7 +11,7 @@ //! The main type `PeekReader`does not implement [`std::io::Read`] itself, as there is no added benefit //! in doing so. //! -#[cfg(feature = "embedded")] +#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] use crate::embedded::Read; #[cfg(feature = "std")] diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index 08cf4e1ab1..c21c797b0d 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -93,7 +93,11 @@ serde_arrays = { version = "0.1.0", optional = true } "udp" = ["mavlink-core/udp"] "tcp" = ["mavlink-core/tcp"] "direct-serial" = ["mavlink-core/direct-serial"] +# NOTE: Only one of 'embedded' and 'embedded-hal-02' features can be enabled. +# Use "embedded' feature to enable embedded-hal=1.0 (embedded-io and embedded-io-async is part of embedded-hal). +# Use 'embedded-hal-0.2' feature to enable deprecated embedded-hal=0.2.3 (some hals is not supports embedded-hal=1.0 yet). "embedded" = ["mavlink-core/embedded"] +"embedded-hal-02" = ["mavlink-core/embedded-hal-02"] "serde" = ["mavlink-core/serde", "dep:serde", "dep:serde_arrays"] default = ["std", "tcp", "udp", "direct-serial", "serde", "ardupilotmega"] diff --git a/mavlink/examples/embedded-async-read/.cargo/config.toml b/mavlink/examples/embedded-async-read/.cargo/config.toml new file mode 100644 index 0000000000..2c0f3e9d15 --- /dev/null +++ b/mavlink/examples/embedded-async-read/.cargo/config.toml @@ -0,0 +1,7 @@ +[target.thumbv7em-none-eabihf] +rustflags = [ + "-C", "link-arg=-Tlink.x", +] + +[build] +target = "thumbv7em-none-eabihf" diff --git a/mavlink/examples/embedded-async-read/Cargo.toml b/mavlink/examples/embedded-async-read/Cargo.toml new file mode 100644 index 0000000000..20712a4307 --- /dev/null +++ b/mavlink/examples/embedded-async-read/Cargo.toml @@ -0,0 +1,44 @@ +[package] +name = "mavlink-embedded-async-read" +edition = "2021" +authors = ["Patrick José Pereira "] +version = "0.1.0" + +[profile.release] +opt-level = 'z' # Optimize for binary size, but also turn off loop vectorization. +lto = true # Performs "fat" LTO which attempts to perform optimizations across all crates within the dependency graph + +[dependencies] +cortex-m = { version = "0.7", features = [ + "inline-asm", + "critical-section-single-core", +] } # Low level access to Cortex-M processors +cortex-m-rt = "0.7" # Startup code and minimal runtime for Cortex-M microcontrollers +rtt-target = "0.5" +panic-rtt-target = "0.1" # Panic handler +static_cell = "2.1" + +embassy-time = { version = "0.3", features = ["tick-hz-32_768"] } +embassy-executor = { version = "0.5", features = [ + "arch-cortex-m", + "executor-thread", + "executor-interrupt", + "integrated-timers", +] } +embassy-stm32 = { version = "0.1", features = [ + "memory-x", + "stm32f446re", + "time-driver-any", +] } + +[dependencies.mavlink] # MAVLink library (wait for 0.9.0 version) +path = "../../" +features = ["common", "embedded"] +default-features = false + +[patch.crates-io] +embassy-time-driver = { git = "https://github.com/embassy-rs/embassy.git", rev = "86c48dde4192cabcad22faa10cabb4dc5f035c0a" } +embassy-time-queue-driver = { git = "https://github.com/embassy-rs/embassy.git", rev = "86c48dde4192cabcad22faa10cabb4dc5f035c0a" } +embassy-stm32 = { git = "https://github.com/embassy-rs/embassy.git", rev = "86c48dde4192cabcad22faa10cabb4dc5f035c0a" } + +[workspace] diff --git a/mavlink/examples/embedded-async-read/README.md b/mavlink/examples/embedded-async-read/README.md new file mode 100644 index 0000000000..8bbf41c145 --- /dev/null +++ b/mavlink/examples/embedded-async-read/README.md @@ -0,0 +1,11 @@ +# rust-MAVLink Embedded async example (with reading loop) +### How to run: +- Install cargo flash: + - cargo install cargo-flash +- Install target + - rustup target add thumbv7em-none-eabihf +- Check if we can build the project + - cargo build +- Connect your STM32f446re board +- Flash it! + - cargo flash --chip stm32f446RETx --release --log info diff --git a/mavlink/examples/embedded-async-read/src/main.rs b/mavlink/examples/embedded-async-read/src/main.rs new file mode 100644 index 0000000000..af76a2640a --- /dev/null +++ b/mavlink/examples/embedded-async-read/src/main.rs @@ -0,0 +1,93 @@ +//! Target board: stm32f446RETx (stm32nucleo) +//! Manual: https://www.st.com/resource/en/reference_manual/dm00043574-stm32f303xb-c-d-e-stm32f303x6-8-stm32f328x8-stm32f358xc-stm32f398xe-advanced-arm-based-mcus-stmicroelectronics.pdf +#![no_main] +#![no_std] + +// Panic handler +use panic_rtt_target as _; + +use embassy_executor::Spawner; +use embassy_stm32::{bind_interrupts, mode::Async, peripherals::*, usart}; +use embassy_time::Timer; +use mavlink; +use mavlink::common::{MavMessage, HEARTBEAT_DATA}; +use mavlink::{read_v2_raw_message_async, MAVLinkV2MessageRaw, MavlinkVersion, MessageData}; +use rtt_target::{rprintln, rtt_init_print}; +use static_cell::ConstStaticCell; + +bind_interrupts!(struct Irqs { + USART1 => usart::InterruptHandler; +}); + +#[embassy_executor::main] +async fn main(spawner: Spawner) { + rtt_init_print!(); + + // Peripherals access + let p = embassy_stm32::init(embassy_stm32::Config::default()); + + // Create an interface USART2 with 115200 baudrate + let mut config = usart::Config::default(); + config.baudrate = 115200; + let serial = usart::Uart::new( + p.USART1, p.PA10, p.PA9, Irqs, p.DMA2_CH7, p.DMA2_CH2, config, + ) + .unwrap(); + + // Break serial in TX and RX (not used) + let (mut tx, rx) = serial.split(); + + // Create our mavlink header and heartbeat message + let header = mavlink::MavHeader { + system_id: 1, + component_id: 1, + sequence: 42, + }; + let heartbeat = mavlink::common::HEARTBEAT_DATA { + custom_mode: 0, + mavtype: mavlink::common::MavType::MAV_TYPE_SUBMARINE, + autopilot: mavlink::common::MavAutopilot::MAV_AUTOPILOT_ARDUPILOTMEGA, + base_mode: mavlink::common::MavModeFlag::empty(), + system_status: mavlink::common::MavState::MAV_STATE_STANDBY, + mavlink_version: 0x3, + }; + + // Spawn Rx loop + spawner.spawn(rx_task(rx)).unwrap(); + + // Main loop + loop { + // Write the raw heartbeat message to reduce firmware flash size (using Message::ser will be add ~70KB because + // all *_DATA::ser methods will be add to firmware). + let mut raw = MAVLinkV2MessageRaw::new(); + raw.serialize_message_data(header, &heartbeat); + tx.write(raw.raw_bytes()).await.unwrap(); + + // Delay for 1 second + Timer::after_millis(1000).await; + } +} + +#[embassy_executor::task] +pub async fn rx_task(rx: usart::UartRx<'static, Async>) { + // Make ring-buffered RX (over DMA) + static BUF_MEMORY: ConstStaticCell<[u8; 1024]> = ConstStaticCell::new([0; 1024]); + let mut rx_buffered = rx.into_ring_buffered(BUF_MEMORY.take()); + + loop { + // Read raw message to reduce firmware flash size (using read_v2_msg_async will be add ~80KB because + // all *_DATA::deser methods will be add to firmware). + let raw = read_v2_raw_message_async::(&mut rx_buffered) + .await + .unwrap(); + rprintln!("Read raw message: msg_id={}", raw.message_id()); + + match raw.message_id() { + HEARTBEAT_DATA::ID => { + let heartbeat = HEARTBEAT_DATA::deser(MavlinkVersion::V2, raw.payload()).unwrap(); + rprintln!("heartbeat: {:?}", heartbeat); + } + _ => {} + } + } +} diff --git a/mavlink/examples/embedded/Cargo.toml b/mavlink/examples/embedded/Cargo.toml index 717a0c94d1..4b0801d66d 100644 --- a/mavlink/examples/embedded/Cargo.toml +++ b/mavlink/examples/embedded/Cargo.toml @@ -18,7 +18,7 @@ stm32f3xx-hal = { version = "0.9", features = ["stm32f303xe"] } # HAL for stm32f [dependencies.mavlink] # MAVLink library (wait for 0.9.0 version) path = "../../" -features = ["ardupilotmega", "embedded"] +features = ["ardupilotmega", "embedded-hal-02"] default-features = false [workspace] \ No newline at end of file