diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000000..83fc85e850 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,13 @@ +version: 2 +updates: +- package-ecosystem: cargo + directory: "/" + schedule: + interval: daily + ignore: + - dependency-name: "embedded-hal" + versions: ["^0.2"] +- package-ecosystem: github-actions + directory: "/" + schedule: + interval: daily diff --git a/.github/workflows/deploy.yml b/.github/workflows/deploy.yml index 98154469f0..89244914c3 100644 --- a/.github/workflows/deploy.yml +++ b/.github/workflows/deploy.yml @@ -10,12 +10,31 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@master - - uses: actions-rs/toolchain@v1.0.1 + with: + fetch-depth: 0 + ref: ${{ github.event.repository.default_branch }} + - uses: actions-rs/toolchain@v1.0.7 with: toolchain: stable override: true - name: Build run: cargo build - - uses: katyo/publish-crates@v1 - with: - registry-token: ${{ secrets.CARGO }} + - name: Extract version from tag + id: get_version + run: echo "::set-output name=version::${GITHUB_REF/refs\/tags\//}" + - name: Commit version changes + run: | + git config --global user.name 'github-actions[bot]' + git config --global user.email '41898282+github-actions[bot]@users.noreply.github.com' + - name: Set and publish workspace crates + env: + CARGO_REGISTRY_TOKEN: ${{ secrets.CARGO }} + run: | + cargo install cargo-workspaces + cargo workspaces version custom ${{ steps.get_version.outputs.version }} \ + --exact --yes --no-git-tag --no-git-push \ + -m "Commit new release ${{ steps.get_version.outputs.version }}" + cargo workspaces publish --yes --no-verify --publish-as-is + - name: Push commit + run: | + git push origin ${{ github.event.repository.default_branch }} diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 20df235010..1eb228de28 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -3,81 +3,135 @@ name: Test all targets on: [push, pull_request] jobs: - quick-tests: + formatting: runs-on: ubuntu-latest steps: - - uses: actions/checkout@master - - uses: actions-rs/toolchain@v1.0.1 - with: - toolchain: stable - target: ${{ matrix.TARGET }} - override: true - - name: Check Type - run: cargo fmt -- --check - - name: Run internal tests - run: cargo test --verbose --features all-dialects -- --nocapture - - name: Build mavlink-dump - run: cargo build --verbose --bin mavlink-dump --features ardupilotmega + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@stable + with: + components: rustfmt + - name: Run rustfmt + run: cargo fmt --all -- --check + + linting: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@master + with: + toolchain: nightly-2024-09-10 + components: clippy + - uses: actions-rs-plus/clippy-check@v2 + with: + args: --all --all-targets --features format-generated-code --features signing --features tokio-1 + + internal-tests: + runs-on: ubuntu-latest + strategy: + matrix: + dialect: ["ardupilotmega", "asluav", "matrixpilot", "minimal", "paparazzi", "python_array_test", "slugs", "standard", "test", "ualberta", "uavionix", "icarous", "common"] + signing: ["", "--features signing"] + steps: + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@stable + - name: Run internal tests + run: cargo test --verbose --features ${{ matrix.dialect }} ${{ matrix.signing }} -- --nocapture + + mavlink-dump: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@stable + - name: Build mavlink-dump + run: cargo build --verbose --example mavlink-dump + + msrv: + runs-on: ubuntu-latest + strategy: + matrix: + features: ["", "--features serde,tokio-1", "--features signing"] + steps: + - uses: actions/checkout@master + - name: Get MSRV from Cargo.toml + run: | + MSRV=$(grep 'rust-version' Cargo.toml | sed 's/.*= *"\(.*\)".*/\1/') + echo "MSRV=$MSRV" >> $GITHUB_ENV + - uses: dtolnay/rust-toolchain@master + with: + toolchain: ${{ env.MSRV }} + - uses: taiki-e/install-action@cargo-no-dev-deps + - run: cargo no-dev-deps check --all --lib --bins ${{ matrix.features }} build: - needs: quick-tests + needs: [formatting, linting, internal-tests, mavlink-dump, msrv] runs-on: ${{ matrix.os }} strategy: fail-fast: false matrix: include: - - os: macos-latest - TARGET: x86_64-apple-darwin - FEATURES: --features all-dialects + - os: macos-latest + TARGET: x86_64-apple-darwin + FEATURES: --features ardupilotmega - - os: ubuntu-latest - TARGET: arm-unknown-linux-musleabihf - FLAGS: --features ardupilotmega + - os: ubuntu-latest + TARGET: arm-unknown-linux-musleabihf + FLAGS: --features ardupilotmega - - os: ubuntu-latest - TARGET: armv7-unknown-linux-musleabihf - FLAGS: --features ardupilotmega + - os: ubuntu-latest + TARGET: armv7-unknown-linux-musleabihf + FLAGS: --features ardupilotmega - - os: ubuntu-latest - TARGET: x86_64-unknown-linux-musl - FLAGS: --features all-dialects + - os: ubuntu-latest + TARGET: x86_64-unknown-linux-musl + FLAGS: --features ardupilotmega - - os: ubuntu-latest - TARGET: x86_64-unknown-linux-musl - FLAGS: --features all-dialects,emit-extensions + - os: ubuntu-latest + TARGET: x86_64-unknown-linux-musl + FLAGS: --features ardupilotmega,emit-description,emit-extensions - - os: ubuntu-latest - TARGET: thumbv7m-none-eabi - FLAGS: --no-default-features --features embedded + - os: ubuntu-latest + TARGET: thumbv7m-none-eabi + FLAGS: --no-default-features --features embedded - - os: windows-latest - TARGET: x86_64-pc-windows-msvc - FLAGS: --features all-dialects + - os: windows-latest + TARGET: x86_64-pc-windows-msvc + FLAGS: --features ardupilotmega steps: - - name: Building ${{ matrix.TARGET }} - run: echo "${{ matrix.TARGET }}" - - uses: actions/checkout@master - - uses: actions-rs/toolchain@v1.0.1 - with: - toolchain: stable - target: ${{ matrix.TARGET }} - override: true - - uses: actions-rs/cargo@v1 - with: - use-cross: true - command: build - args: --verbose --release --target=${{ matrix.TARGET }} ${{ matrix.FLAGS }} + - name: Building ${{ matrix.TARGET }} + run: echo "${{ matrix.TARGET }}" + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@stable + with: + target: ${{ matrix.TARGET }} + - uses: actions-rs/cargo@v1 + with: + use-cross: true + command: build + args: --verbose --release --package=mavlink --target=${{ matrix.TARGET }} ${{ matrix.FLAGS }} test-embedded-size: needs: build runs-on: ubuntu-latest + steps: + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@nightly + with: + target: thumbv7em-none-eabihf + - name: Build + run: cargo +nightly build --target thumbv7em-none-eabihf --manifest-path mavlink/examples/embedded/Cargo.toml --out-dir $PWD --release -Z unstable-options + + docs: + needs: internal-tests + runs-on: ubuntu-latest steps: - uses: actions/checkout@master - - uses: actions-rs/toolchain@v1.0.1 + - uses: dtolnay/rust-toolchain@nightly + - name: Build docs + run: cargo doc --features "default all-dialects emit-description emit-extensions format-generated-code tokio-1 signing" + - name: Deploy + uses: peaceiris/actions-gh-pages@v3 + if: ${{ github.ref == 'refs/heads/master' }} with: - toolchain: nightly - target: thumbv7em-none-eabihf - override: true - - name: Build - run: cargo +nightly build --target thumbv7em-none-eabihf --manifest-path examples/embedded/Cargo.toml --out-dir $PWD --release -Z unstable-options + github_token: ${{ secrets.GITHUB_TOKEN }} + publish_dir: ./target/doc \ No newline at end of file diff --git a/.gitmodules b/.gitmodules index 3fbebb0955..80bff682ff 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ -[submodule "mavlink"] - path = mavlink +[submodule "mavlink/mavlink"] + path = mavlink/mavlink url = https://github.com/mavlink/mavlink diff --git a/.idea/workspace.xml b/.idea/workspace.xml new file mode 100644 index 0000000000..2a7f278391 --- /dev/null +++ b/.idea/workspace.xml @@ -0,0 +1,128 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1707604355173 + + + + + + \ No newline at end of file diff --git a/Cargo.toml b/Cargo.toml index d542f56f8a..71ebeb23ac 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,82 +1,14 @@ +[workspace] +members = ["mavlink", "mavlink-bindgen", "mavlink-core"] +resolver = "1" -[package] -name = "mavlink" -version = "0.10.1" -authors = ["Todd Stellanova", "Michal Podhradsky", "Kevin Mehall", "Tim Ryan", "Patrick José Pereira", "Ibiyemi Abiodun"] -build = "build/main.rs" -description = "Implements the MAVLink data interchange format for UAVs." -readme = "README.md" -license = "MIT/Apache-2.0" -repository = "https://github.com/mavlink/rust-mavlink" -edition = "2018" - -[build-dependencies] -crc-any = "2.3.0" -bytes = { version = "1.0", default-features = false } -xml-rs = "0.2" -quote = "0.3" -lazy_static = "1.2.0" -serde = { version = "1.0.115", optional = true, features = ["derive"] } - -[[bin]] -name = "mavlink-dump" -required-features = ["ardupilotmega"] - -[dependencies] -crc-any = "2.3.5" -bytes = { version = "1.0", default-features = false } +[workspace.dependencies] +crc-any = { version = "2.3.5", default-features = false } num-traits = { version = "0.2", default-features = false } -num-derive = "0.3.2" +num-derive = "0.4" bitflags = "1.2.1" -serial = { version = "0.4", optional = true } -serde = { version = "1.0.115", optional = true, features = ["derive"] } byteorder = { version = "1.3.4", default-features = false } -embedded-hal = { version = "0.2", optional = true } -nb = { version = "0.1", optional = true } - -[features] -"ardupilotmega" = ["common", "icarous", "uavionix"] -"asluav" = ["common"] -"autoquad" = ["common"] -"matrixpilot" = ["common"] -"minimal" = [] -"paparazzi" = ["common"] -"python_array_test" = ["common"] -"slugs" = ["common"] -"standard" = ["common"] -"test" = [] -"ualberta" = ["common"] -"uavionix" = ["common"] -"icarous" = [] -"common" = [] - -"all-dialects" = [ - "ardupilotmega", - "asluav", - "autoquad", - "matrixpilot", - "minimal", - "paparazzi", - "python_array_test", - "slugs", - "standard", - "test", - "ualberta", - "uavionix", - "icarous", - "common", -] - -"emit-description" = [] -"emit-extensions" = [] -"std" = ["byteorder/std"] -"udp" = [] -"tcp" = [] -"direct-serial" = [] -"embedded" = ["embedded-hal", "nb"] -default= ["std", "tcp", "udp", "direct-serial", "serial", "serde", "common"] -# build with all features on docs.rs so that users viewing documentation -# can see everything -[package.metadata.docs.rs] -features = ["default", "all-dialects"] +[workspace.package] +edition = "2021" +rust-version = "1.70.0" diff --git a/README.md b/README.md index 0e179fb6d6..689c6bf1f5 100644 --- a/README.md +++ b/README.md @@ -4,27 +4,45 @@ [![Crate info](https://img.shields.io/crates/v/mavlink.svg)](https://crates.io/crates/mavlink) [![Documentation](https://docs.rs/mavlink/badge.svg)](https://docs.rs/mavlink) -Rust implementation of the [MAVLink](http://qgroundcontrol.org/mavlink/start) UAV messaging protocol, +Rust implementation of the [MAVLink](https://mavlink.io/en) UAV messaging protocol, with bindings for all message sets. Add to your Cargo.toml: ``` -mavlink = "0.10.1" +mavlink = "0.12.2" ``` ## Examples -See [src/bin/mavlink-dump.rs](src/bin/mavlink-dump.rs) for a usage example. +See [examples/](mavlink/examples/mavlink-dump/src/main.rs) for different usage examples. + +### mavlink-dump +[examples/mavlink-dump](mavlink/examples/mavlink-dump/src/main.rs) contains an executable example that can be used to test message reception. + +It can be executed directly by running: +``` +cargo run --example mavlink-dump [options] +``` It's also possible to install the working example via `cargo` command line: ```sh -cargo install mavlink +cargo install --path examples/mavlink-dump +``` + +It can then be executed by running: +``` +mavlink-dump [options] +``` + +Execution call example: +```sh +mavlink-dump udpin:127.0.0.1:14540 ``` ### Community projects Check some projects built by the community: - [mavlink2rest](https://github.com/patrickelectric/mavlink2rest): A REST server that provides easy and friendly access to mavlink messages. -- [mavlink-camera-manager](https://github.com/patrickelectric/mavlink-camera-manager): Extensible cross-platform camera server. +- [mavlink-camera-manager](https://github.com/mavlink/mavlink-camera-manager): Extensible cross-platform camera server. ## License diff --git a/build/binder.rs b/build/binder.rs deleted file mode 100644 index d3b663c510..0000000000 --- a/build/binder.rs +++ /dev/null @@ -1,23 +0,0 @@ -use quote::Ident; -use std::io::Write; - -pub fn generate(modules: Vec, out: &mut W) { - let modules_tokens = modules.into_iter().map(|module| { - let module_ident = Ident::from(module.clone()); - - quote! { - #[allow(non_camel_case_types)] - #[allow(non_snake_case)] - #[allow(unused_variables)] - #[allow(unused_mut)] - #[cfg(feature = #module)] - pub mod #module_ident; - } - }); - - let tokens = quote! { - #(#modules_tokens)* - }; - - writeln!(out, "{}", tokens).unwrap(); -} diff --git a/build/main.rs b/build/main.rs deleted file mode 100644 index a4750ebead..0000000000 --- a/build/main.rs +++ /dev/null @@ -1,113 +0,0 @@ -#![recursion_limit = "256"] -#[macro_use] -extern crate quote; - -extern crate xml; - -mod binder; -mod parser; -mod util; - -use crate::util::to_module_name; -use std::env; -use std::fs::{read_dir, File}; -use std::path::{Path, PathBuf}; -use std::process::Command; - -pub fn main() { - let src_dir = Path::new(env!("CARGO_MANIFEST_DIR")); - - // Update and init submodule - match Command::new("git") - .arg("submodule") - .arg("update") - .arg("--init") - .current_dir(&src_dir) - .status() - { - Ok(_) => {} - Err(error) => eprintln!("{}", error), - } - - // find & apply patches to XML definitions to avoid crashes - let mut patch_dir = src_dir.to_path_buf(); - patch_dir.push("build/patches"); - let mut mavlink_dir = src_dir.to_path_buf(); - mavlink_dir.push("mavlink"); - - if let Ok(dir) = read_dir(patch_dir) { - for entry in dir { - if let Ok(entry) = entry { - match Command::new("git") - .arg("apply") - .arg(entry.path().as_os_str()) - .current_dir(&mavlink_dir) - .status() - { - Ok(_) => (), - Err(error) => eprintln!("{}", error), - } - } - } - } - - let mut definitions_dir = src_dir.to_path_buf(); - definitions_dir.push("mavlink/message_definitions/v1.0"); - - let out_dir = env::var("OUT_DIR").unwrap(); - - let mut modules = vec![]; - - for entry in read_dir(&definitions_dir).expect("could not read definitions directory") { - let entry = entry.expect("could not read directory entry"); - - let definition_file = entry.file_name(); - let module_name = to_module_name(&definition_file); - - let mut definition_rs = PathBuf::from(&module_name); - definition_rs.set_extension("rs"); - - modules.push(module_name); - - let in_path = Path::new(&definitions_dir).join(&definition_file); - let mut inf = File::open(&in_path).unwrap(); - - let dest_path = Path::new(&out_dir).join(definition_rs); - let mut outf = File::create(&dest_path).unwrap(); - - // generate code - parser::generate(&mut inf, &mut outf); - - // format code - match Command::new("rustfmt") - .arg(dest_path.as_os_str()) - .current_dir(&out_dir) - .status() - { - Ok(_) => (), - Err(error) => eprintln!("{}", error), - } - - // Re-run build if definition file changes - println!("cargo:rerun-if-changed={}", entry.path().to_string_lossy()); - } - - // output mod.rs - { - let dest_path = Path::new(&out_dir).join("mod.rs"); - let mut outf = File::create(&dest_path).unwrap(); - - // generate code - binder::generate(modules, &mut outf); - - // format code - match Command::new("rustfmt") - .arg(dest_path.as_os_str()) - .current_dir(&out_dir) - .status() - { - Ok(_) => (), - Err(error) => eprintln!("{}", error), - } - } -} diff --git a/mavlink b/mavlink deleted file mode 160000 index 6723147f26..0000000000 --- a/mavlink +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 6723147f26d2625383a588c88633c568c9e9b929 diff --git a/mavlink-bindgen/Cargo.toml b/mavlink-bindgen/Cargo.toml new file mode 100644 index 0000000000..2c23aeefaa --- /dev/null +++ b/mavlink-bindgen/Cargo.toml @@ -0,0 +1,33 @@ +[package] +name = "mavlink-bindgen" +version = "0.13.2" +edition.workspace = true +license = "MIT/Apache-2.0" +description = "Library used by rust-mavlink." +readme = "README.md" +repository = "https://github.com/mavlink/rust-mavlink" +rust-version.workspace = true + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +crc-any = { workspace = true, default-features = false } +quick-xml = "0.36" +quote = "1" +proc-macro2 = "1.0.43" +lazy_static = "1.2.0" +serde = { version = "1.0.115", optional = true, features = ["derive"] } +clap = { version = "~4.3.24", optional = true, default-features =false, features = ["derive", "help", "usage", "error-context"] } +thiserror = "1.0.56" + +# Required to support MSRV 1.65.0 +clap_lex = { version = "=0.5.0", optional=true } +clap_builder = { version = "~4.3.24", optional=true} +anstyle = { version = "=1.0.2", optional=true } +anstyle-query = { version = "=1.0.0", optional=true } +anstyle-parse = { version = "=0.2.1", optional=true } + +[features] +cli = ["dep:clap", "dep:clap_lex", "dep:clap_builder", "dep:anstyle", "dep:anstyle-query", "dep:anstyle-parse"] +emit-extensions = [] +emit-description = [] diff --git a/mavlink-bindgen/README.md b/mavlink-bindgen/README.md new file mode 100644 index 0000000000..c813b5fc3a --- /dev/null +++ b/mavlink-bindgen/README.md @@ -0,0 +1,98 @@ +# mavlink-bindgen + +[![Build status](https://github.com/mavlink/rust-mavlink/actions/workflows/test.yml/badge.svg)](https://github.com/mavlink/rust-mavlink/actions/workflows/test.yml) +[![Crate info](https://img.shields.io/crates/v/mavlink-bindgen.svg)](https://crates.io/crates/mavlink-bindgen) +[![Documentation](https://docs.rs/mavlink-bindgen/badge.svg)](https://docs.rs/mavlink-bindgen) + +Library and CLI for generating code for the Rust implementation of the [MAVLink](https://mavlink.io/en) UAV messaging protocol. + +`mavlink-bindgen` can be used to create MAVLink bindings for Rust. This is used from `build.rs` in the [mavlink](https://crates.io/crates/mavlink) crate to create bindings from the standard MAVLink dialects in . + +## Usage + +`mavlink-bindgen` can be used as a code generator from `build.rs` as done is the `mavlink` crate for a custom MAVLink dialect or as a CLI tool to generate rust binding from XML dialect definitions. The generated code will depend on the [mavlink-core](https://crates.io/crates/mavlink-core) crate in both use cases. Each dialect generated will be locked behind a feature flag of the same name, that must be enabled when using the generated code. + +### CLI + +Build the binary using cargo with `cli` feature enabled: + +```shell +cd mavlink-bindgen +cargo build --features cli +``` + +Alternatively you can build and install `mavlink-bindgen` to your locally installed crates: + +```shell +cargo install mavlink-bindgen --features cli +``` + +To generate code using the resulting binary: + +```shell +mavlink-bindgen --format-generated-code message_definitions mavlink_dialects +``` + +The full command line options are shown below. + +```shell +Usage: mavlink-bindgen [OPTIONS] + +Arguments: + Path to the directory containing the MAVLink dialect definitions + Path to the directory where the code is generated into, must already exist + +Options: + --format-generated-code format code generated code + --emit-cargo-build-messages prints cargo build message indicating when the code has to be rebuild + -h, --help Print help +``` + +The output dir will contain a `mod.rs` file with each dialect in its own file locked behind a feature flag. + +### Library as build dependency + +Add to your Cargo.toml: + +```toml +mavlink-bindgen = "0.13.1" +``` + +Add a `build/main.rs` or `build.rs` to your project if it does not already exist. Then add the following to the `main` function to generate the code: + +```rs +let out_dir = env::var("OUT_DIR").unwrap(); +let result = match mavlink_bindgen::generate(definitions_dir, out_dir) { + Ok(r) => r, + Err(e) => { + eprintln!("{e}"); + return ExitCode::FAILURE; + } +}; +``` + +If the generated code should be formated use + +```rs + mavlink_bindgen::format_generated_code(&result); +``` + +To tell cargo when to regenerate code from the definitions use: + +```rs + mavlink_bindgen::emit_cargo_build_messages(&result); +``` + +Finally include the generated code into the `lib.rs` or `main.rs` : + +```rs +#![cfg_attr(not(feature = "std"), no_std)] +// include generate definitions +include!(concat!(env!("OUT_DIR"), "/mod.rs")); + +pub use mavlink_core::*; +``` + +Since each dialect is locked behind a feature flag these need to be enabled for the dialects to become available when using the generated code. + +This approach is used by the `mavlink` crate see its build script for an example. diff --git a/mavlink-bindgen/src/binder.rs b/mavlink-bindgen/src/binder.rs new file mode 100644 index 0000000000..b9a911cc1c --- /dev/null +++ b/mavlink-bindgen/src/binder.rs @@ -0,0 +1,25 @@ +use quote::{format_ident, quote}; +use std::io::Write; + +pub fn generate(modules: Vec<&str>, out: &mut W) { + let modules_tokens = modules.into_iter().map(|module| { + let module_ident = format_ident!("{}", module); + + quote! { + #[allow(non_camel_case_types)] + #[allow(clippy::derive_partial_eq_without_eq)] + #[allow(clippy::field_reassign_with_default)] + #[allow(non_snake_case)] + #[allow(clippy::unnecessary_cast)] + #[allow(clippy::bad_bit_mask)] + #[cfg(feature = #module)] + pub mod #module_ident; + } + }); + + let tokens = quote! { + #(#modules_tokens)* + }; + + writeln!(out, "{tokens}").unwrap(); +} diff --git a/mavlink-bindgen/src/cli.rs b/mavlink-bindgen/src/cli.rs new file mode 100644 index 0000000000..5ff37b26f1 --- /dev/null +++ b/mavlink-bindgen/src/cli.rs @@ -0,0 +1,34 @@ +use std::path::PathBuf; + +use clap::Parser; +use mavlink_bindgen::{emit_cargo_build_messages, format_generated_code, generate, BindGenError}; + +#[derive(Parser)] +/// Generate Rust bindings from MAVLink message dialect XML files. +struct Cli { + /// Path to the directory containing the MAVLink dialect definitions. + definitions_dir: PathBuf, + /// Path to the directory where the code is generated into, must already exist. + destination_dir: PathBuf, + /// format code generated code + #[arg(long)] + format_generated_code: bool, + /// prints cargo build messages indicating when the code has to be rebuild + #[arg(long)] + emit_cargo_build_messages: bool, +} + +pub fn main() -> Result<(), BindGenError> { + let args = Cli::parse(); + let result = generate(args.definitions_dir, args.destination_dir)?; + + if args.format_generated_code { + format_generated_code(&result); + } + + if args.emit_cargo_build_messages { + emit_cargo_build_messages(&result); + } + + Ok(()) +} diff --git a/mavlink-bindgen/src/error.rs b/mavlink-bindgen/src/error.rs new file mode 100644 index 0000000000..fe32023389 --- /dev/null +++ b/mavlink-bindgen/src/error.rs @@ -0,0 +1,29 @@ +use thiserror::Error; + +#[derive(Error, Debug)] +pub enum BindGenError { + /// Represents a failure to read the MAVLink definitions directory. + #[error("Could not read definitions directory {path}: {source}")] + CouldNotReadDefinitionsDirectory { + source: std::io::Error, + path: std::path::PathBuf, + }, + /// Represents a failure to read a MAVLink definition file. + #[error("Could not read definition file {path}: {source}")] + CouldNotReadDefinitionFile { + source: std::io::Error, + path: std::path::PathBuf, + }, + /// Represents a failure to read a directory entry in the MAVLink definitions directory. + #[error("Could not read MAVLink definitions directory entry {path}: {source}")] + CouldNotReadDirectoryEntryInDefinitionsDirectory { + source: std::io::Error, + path: std::path::PathBuf, + }, + /// Represents a failure to create a Rust file for the generated MAVLink bindings. + #[error("Could not create Rust bindings file {dest_path}: {source}")] + CouldNotCreateRustBindingsFile { + source: std::io::Error, + dest_path: std::path::PathBuf, + }, +} diff --git a/mavlink-bindgen/src/lib.rs b/mavlink-bindgen/src/lib.rs new file mode 100644 index 0000000000..23aae4b1d0 --- /dev/null +++ b/mavlink-bindgen/src/lib.rs @@ -0,0 +1,129 @@ +pub use crate::error::BindGenError; +use std::fs::{read_dir, File}; +use std::io::BufWriter; +use std::ops::Deref; +use std::path::{Path, PathBuf}; +use std::process::Command; + +pub mod binder; +pub mod error; +pub mod parser; +mod util; + +#[derive(Debug)] +pub struct GeneratedBinding { + pub module_name: String, + pub mavlink_xml: PathBuf, + pub rust_module: PathBuf, +} + +#[derive(Debug)] +pub struct GeneratedBindings { + pub bindings: Vec, + pub mod_rs: PathBuf, +} + +/// Generate Rust MAVLink dialect binding for dialects present in `definitions_dir` into `destination_dir`. +/// +/// If successful returns paths of generated bindings linked to their dialect definitions files. +pub fn generate, P2: AsRef>( + definitions_dir: P1, + destination_dir: P2, +) -> Result { + _generate(definitions_dir.as_ref(), destination_dir.as_ref()) +} + +fn _generate( + definitions_dir: &Path, + destination_dir: &Path, +) -> Result { + let mut bindings = vec![]; + + for entry_maybe in read_dir(definitions_dir).map_err(|source| { + BindGenError::CouldNotReadDefinitionsDirectory { + source, + path: definitions_dir.to_path_buf(), + } + })? { + let entry = entry_maybe.map_err(|source| { + BindGenError::CouldNotReadDirectoryEntryInDefinitionsDirectory { + source, + path: definitions_dir.to_path_buf(), + } + })?; + + let definition_file = PathBuf::from(entry.file_name()); + let module_name = util::to_module_name(&definition_file); + + let definition_rs = PathBuf::from(&module_name).with_extension("rs"); + + let dest_path = destination_dir.join(definition_rs); + let mut outf = BufWriter::new(File::create(&dest_path).map_err(|source| { + BindGenError::CouldNotCreateRustBindingsFile { + source, + dest_path: dest_path.clone(), + } + })?); + + // generate code + parser::generate(definitions_dir, &definition_file, &mut outf)?; + + bindings.push(GeneratedBinding { + module_name, + mavlink_xml: entry.path(), + rust_module: dest_path, + }); + } + + // output mod.rs + { + let dest_path = destination_dir.join("mod.rs"); + let mut outf = File::create(&dest_path).map_err(|source| { + BindGenError::CouldNotCreateRustBindingsFile { + source, + dest_path: dest_path.clone(), + } + })?; + + // generate code + binder::generate( + bindings + .iter() + .map(|binding| binding.module_name.deref()) + .collect(), + &mut outf, + ); + + Ok(GeneratedBindings { + bindings, + mod_rs: dest_path, + }) + } +} + +/// Formats generated code using `rustfmt`. +pub fn format_generated_code(result: &GeneratedBindings) { + if let Err(error) = Command::new("rustfmt") + .args( + result + .bindings + .iter() + .map(|binding| binding.rust_module.clone()), + ) + .arg(result.mod_rs.clone()) + .status() + { + eprintln!("{error}"); + } +} + +/// Prints definitions for cargo that describe which files the generated code depends on, indicating when it has to be regenerated. +pub fn emit_cargo_build_messages(result: &GeneratedBindings) { + for binding in &result.bindings { + // Re-run build if definition file changes + println!( + "cargo:rerun-if-changed={}", + binding.mavlink_xml.to_string_lossy() + ); + } +} diff --git a/mavlink-bindgen/src/main.rs b/mavlink-bindgen/src/main.rs new file mode 100644 index 0000000000..f2c68ba787 --- /dev/null +++ b/mavlink-bindgen/src/main.rs @@ -0,0 +1,20 @@ +#![recursion_limit = "256"] + +use std::process::ExitCode; + +#[cfg(feature = "cli")] +mod cli; + +fn main() -> ExitCode { + #[cfg(feature = "cli")] + if let Err(e) = cli::main() { + eprintln!("{e}"); + return ExitCode::FAILURE; + } + + #[cfg(not(feature = "cli"))] + panic!("Compiled without cli feature"); + + #[cfg(feature = "cli")] + ExitCode::SUCCESS +} diff --git a/build/parser.rs b/mavlink-bindgen/src/parser.rs similarity index 50% rename from build/parser.rs rename to mavlink-bindgen/src/parser.rs index b621bcf3f0..dce7263129 100644 --- a/build/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -1,43 +1,71 @@ use crc_any::CRCu16; use std::cmp::Ordering; +use std::collections::hash_map::Entry; +use std::collections::{HashMap, HashSet}; use std::default::Default; -use std::io::{Read, Write}; -use std::u32; +use std::fs::File; +use std::io::{BufReader, Write}; +use std::path::{Path, PathBuf}; +use std::str::FromStr; -use xml::reader::{EventReader, XmlEvent}; +use quick_xml::{events::Event, Reader}; -use quote::{Ident, Tokens}; +use proc_macro2::{Ident, TokenStream}; +use quote::{format_ident, quote}; -use crate::util::to_module_name; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; +use crate::error::BindGenError; + #[derive(Debug, PartialEq, Clone, Default)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct MavProfile { - pub includes: Vec, - pub messages: Vec, - pub enums: Vec, + pub messages: HashMap, + pub enums: HashMap, } impl MavProfile { + fn add_message(&mut self, message: &MavMessage) { + match self.messages.entry(message.name.clone()) { + Entry::Occupied(entry) => { + assert!( + entry.get() == message, + "Message '{}' defined twice but definitions are different", + message.name + ); + } + Entry::Vacant(entry) => { + entry.insert(message.clone()); + } + } + } + + fn add_enum(&mut self, enm: &MavEnum) { + match self.enums.entry(enm.name.clone()) { + Entry::Occupied(entry) => { + entry.into_mut().try_combine(enm); + } + Entry::Vacant(entry) => { + entry.insert(enm.clone()); + } + } + } + /// Go over all fields in the messages, and if you encounter an enum, /// update this enum with information about whether it is a bitmask, and what /// is the desired width of such. fn update_enums(mut self) -> Self { - for msg in &self.messages { + for msg in self.messages.values() { for field in &msg.fields { - if let Some(ref enum_name) = field.enumtype { - // it is an enum - if let Some(ref dsp) = field.display { - // it is a bitmask - if dsp == "bitmask" { - // find the corresponding enum - for mut enm in &mut self.enums { - if enm.name == *enum_name { - // this is the right enum - enm.bitfield = Some(field.mavtype.rust_type()); - } + if let Some(enum_name) = &field.enumtype { + // it is a bitmask + if let Some("bitmask") = &field.display.as_deref() { + // find the corresponding enum + for enm in self.enums.values_mut() { + if enm.name == *enum_name { + // this is the right enum + enm.bitfield = Some(field.mavtype.rust_primitive_type()); } } } @@ -58,108 +86,61 @@ impl MavProfile { // } /// Simple header comment - fn emit_comments(&self) -> Ident { - Ident::from(format!( - "// This file was automatically generated, do not edit \n" - )) - } - - /// Emit includes - fn emit_includes(&self) -> Vec { - self.includes - .iter() - .map(|i| Ident::from(to_module_name(i))) - .collect::>() + fn emit_comments(&self) -> TokenStream { + quote!(#![doc = "This file was automatically generated, do not edit"]) } /// Emit rust messages - fn emit_msgs(&self) -> Vec { - self.messages - .iter() - .map(|d| d.emit_rust()) - .collect::>() + fn emit_msgs(&self) -> Vec { + self.messages.values().map(|d| d.emit_rust()).collect() } /// Emit rust enums - fn emit_enums(&self) -> Vec { - self.enums - .iter() - .map(|d| d.emit_rust()) - .collect::>() + fn emit_enums(&self) -> Vec { + self.enums.values().map(|d| d.emit_rust()).collect() } /// Get list of original message names - fn emit_enum_names(&self) -> Vec { + fn emit_enum_names(&self) -> Vec { self.messages - .iter() + .values() .map(|msg| { - let name = Ident::from(msg.name.clone()); + let name = format_ident!("{}", msg.name); quote!(#name) }) - .collect::>() + .collect() } /// Emit message names with "_DATA" at the end - fn emit_struct_names(&self) -> Vec { + fn emit_struct_names(&self) -> Vec { self.messages - .iter() + .values() .map(|msg| msg.emit_struct_name()) - .collect::>() + .collect() } - /// A list of message IDs - fn emit_msg_ids(&self) -> Vec { - self.messages - .iter() - .map(|msg| { - let id = Ident::from(msg.id.to_string()); - quote!(#id) - }) - .collect::>() - } - - /// CRC values needed for mavlink parsing - fn emit_msg_crc(&self) -> Vec { - self.messages - .iter() - .map(|msg| { - let crc = Ident::from(extra_crc(&msg).to_string()); - quote!(#crc) - }) - .collect::>() - } - - fn emit_rust(&self) -> Tokens { + fn emit_rust(&self) -> TokenStream { //TODO verify that id_width of u8 is OK even in mavlink v1 - let id_width = Ident::from("u32"); + let id_width = format_ident!("u32"); let comment = self.emit_comments(); let msgs = self.emit_msgs(); - let includes = self.emit_includes(); let enum_names = self.emit_enum_names(); let struct_names = self.emit_struct_names(); let enums = self.emit_enums(); - let msg_ids = self.emit_msg_ids(); - let msg_crc = self.emit_msg_crc(); - - let mav_message = self.emit_mav_message(&enum_names, &struct_names, &includes); - let mav_message_from_includes = self.emit_mav_message_from_includes(&includes); - let mav_message_parse = - self.emit_mav_message_parse(&enum_names, &struct_names, &msg_ids, &includes); - let mav_message_crc = self.emit_mav_message_crc(&id_width, &msg_ids, &msg_crc, &includes); - let mav_message_name = self.emit_mav_message_name(&enum_names, &includes); - let mav_message_id = self.emit_mav_message_id(&enum_names, &msg_ids, &includes); - let mav_message_id_from_name = - self.emit_mav_message_id_from_name(&enum_names, &msg_ids, &includes); + + let mav_message = self.emit_mav_message(&enum_names, &struct_names); + let mav_message_parse = self.emit_mav_message_parse(&enum_names, &struct_names); + let mav_message_crc = self.emit_mav_message_crc(&id_width, &struct_names); + let mav_message_name = self.emit_mav_message_name(&enum_names, &struct_names); + let mav_message_id = self.emit_mav_message_id(&enum_names, &struct_names); + let mav_message_id_from_name = self.emit_mav_message_id_from_name(&struct_names); let mav_message_default_from_id = - self.emit_mav_message_default_from_id(&enum_names, &msg_ids, &includes); - let mav_message_serialize = self.emit_mav_message_serialize(&enum_names, &includes); + self.emit_mav_message_default_from_id(&enum_names, &struct_names); + let mav_message_serialize = self.emit_mav_message_serialize(&enum_names); quote! { #comment - use crate::MavlinkVersion; - #[allow(unused_imports)] - use bytes::{Buf, BufMut, Bytes, BytesMut}; #[allow(unused_imports)] use num_derive::FromPrimitive; #[allow(unused_imports)] @@ -171,19 +152,11 @@ impl MavProfile { #[allow(unused_imports)] use bitflags::bitflags; - use crate::{Message, error::*}; - #[allow(unused_imports)] - use crate::{#(#includes::*),*}; + use mavlink_core::{MavlinkVersion, Message, MessageData, bytes::Bytes, bytes_mut::BytesMut}; #[cfg(feature = "serde")] use serde::{Serialize, Deserialize}; - #[cfg(not(feature = "std"))] - use alloc::vec::Vec; - - #[cfg(not(feature = "std"))] - use alloc::string::ToString; - #(#enums)* #(#msgs)* @@ -191,8 +164,6 @@ impl MavProfile { #[derive(Clone, PartialEq, Debug)] #mav_message - #mav_message_from_includes - impl Message for MavMessage { #mav_message_parse #mav_message_name @@ -205,99 +176,42 @@ impl MavProfile { } } - fn emit_mav_message( - &self, - enums: &Vec, - structs: &Vec, - includes: &Vec, - ) -> Tokens { - let includes = includes.into_iter().map(|include| { - quote! { - #include(crate::#include::MavMessage) - } - }); - + fn emit_mav_message(&self, enums: &[TokenStream], structs: &[TokenStream]) -> TokenStream { quote! { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde", serde(tag = "type"))] + #[repr(u32)] pub enum MavMessage { #(#enums(#structs),)* - #(#includes,)* } } } - fn emit_mav_message_from_includes(&self, includes: &Vec) -> Tokens { - let froms = includes.into_iter().map(|include| { - quote! { - impl From for MavMessage { - fn from(message: crate::#include::MavMessage) -> Self { - MavMessage::#include(message) - } - } - } - }); - - quote! { - #(#froms)* - } - } - fn emit_mav_message_parse( &self, - enums: &Vec, - structs: &Vec, - ids: &Vec, - includes: &Vec, - ) -> Tokens { - let id_width = Ident::from("u32"); - - // try parsing all included message variants if it doesn't land in the id - // range for this message - let includes_branches = includes.into_iter().map(|i| { - quote! { - if let Ok(msg) = crate::#i::MavMessage::parse(version, id, payload) { - return Ok(MavMessage::#i(msg)) - } - } - }); + enums: &[TokenStream], + structs: &[TokenStream], + ) -> TokenStream { + let id_width = format_ident!("u32"); quote! { - fn parse(version: MavlinkVersion, id: #id_width, payload: &[u8]) -> Result { + fn parse(version: MavlinkVersion, id: #id_width, payload: &[u8]) -> Result { match id { - #(#ids => #structs::deser(version, payload).map(|s| MavMessage::#enums(s)),)* + #(#structs::ID => #structs::deser(version, payload).map(Self::#enums),)* _ => { - #(#includes_branches)* - Err(ParserError::UnknownMessage { id }) + Err(::mavlink_core::error::ParserError::UnknownMessage { id }) }, } } } } - fn emit_mav_message_crc( - &self, - id_width: &Ident, - ids: &Vec, - crc: &Vec, - includes: &Vec, - ) -> Tokens { - let includes_branch = includes.into_iter().map(|include| { - quote! { - match crate::#include::MavMessage::extra_crc(id) { - 0 => {}, - any => return any - } - } - }); - + fn emit_mav_message_crc(&self, id_width: &Ident, structs: &[TokenStream]) -> TokenStream { quote! { fn extra_crc(id: #id_width) -> u8 { match id { - #(#ids => #crc,)* + #(#structs::ID => #structs::EXTRA_CRC,)* _ => { - #(#includes_branch)* - 0 }, } @@ -305,72 +219,33 @@ impl MavProfile { } } - fn emit_mav_message_name(&self, enums: &Vec, includes: &Vec) -> Tokens { - let enum_names = enums - .iter() - .map(|enum_name| { - let name = Ident::from(format!("\"{}\"", enum_name)); - quote!(#name) - }) - .collect::>(); - + fn emit_mav_message_name(&self, enums: &[TokenStream], structs: &[TokenStream]) -> TokenStream { quote! { fn message_name(&self) -> &'static str { match self { - #(MavMessage::#enums(..) => #enum_names,)* - #(MavMessage::#includes(msg) => msg.message_name(),)* + #(Self::#enums(..) => #structs::NAME,)* } } } } - fn emit_mav_message_id( - &self, - enums: &Vec, - ids: &Vec, - includes: &Vec, - ) -> Tokens { - let id_width = Ident::from("u32"); + fn emit_mav_message_id(&self, enums: &[TokenStream], structs: &[TokenStream]) -> TokenStream { + let id_width = format_ident!("u32"); quote! { fn message_id(&self) -> #id_width { match self { - #(MavMessage::#enums(..) => #ids,)* - #(MavMessage::#includes(msg) => msg.message_id(),)* + #(Self::#enums(..) => #structs::ID,)* } } } } - fn emit_mav_message_id_from_name( - &self, - enums: &Vec, - ids: &Vec, - includes: &Vec, - ) -> Tokens { - let includes_branch = includes.into_iter().map(|include| { - quote! { - match crate::#include::MavMessage::message_id_from_name(name) { - Ok(name) => return Ok(name), - Err(..) => {} - } - } - }); - - let enum_names = enums - .iter() - .map(|enum_name| { - let name = Ident::from(format!("\"{}\"", enum_name)); - quote!(#name) - }) - .collect::>(); - + fn emit_mav_message_id_from_name(&self, structs: &[TokenStream]) -> TokenStream { quote! { fn message_id_from_name(name: &str) -> Result { match name { - #(#enum_names => Ok(#ids),)* + #(#structs::NAME => Ok(#structs::ID),)* _ => { - #(#includes_branch)* - Err("Invalid message name.") } } @@ -380,53 +255,33 @@ impl MavProfile { fn emit_mav_message_default_from_id( &self, - enums: &Vec, - ids: &Vec, - includes: &Vec, - ) -> Tokens { - let data_name = enums - .iter() - .map(|enum_name| { - let name = Ident::from(format!("{}_DATA", enum_name)); - quote!(#name) - }) - .collect::>(); - - let includes_branches = includes.into_iter().map(|include| { - quote! { - if let Ok(msg) = crate::#include::MavMessage::default_message_from_id(id) { - return Ok(MavMessage::#include(msg)); - } - } - }); - + enums: &[TokenStream], + structs: &[TokenStream], + ) -> TokenStream { quote! { - fn default_message_from_id(id: u32) -> Result { + fn default_message_from_id(id: u32) -> Result { match id { - #(#ids => Ok(MavMessage::#enums(#data_name::default())),)* + #(#structs::ID => Ok(Self::#enums(#structs::default())),)* _ => { - #(#includes_branches)* - - return Err("Invalid message id."); + Err("Invalid message id.") } } } } } - fn emit_mav_message_serialize(&self, enums: &Vec, includes: &Vec) -> Tokens { + fn emit_mav_message_serialize(&self, enums: &Vec) -> TokenStream { quote! { - fn ser(&self) -> Vec { + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { match self { - #(&MavMessage::#enums(ref body) => body.ser(),)* - #(&MavMessage::#includes(ref msg) => msg.ser(),)* + #(Self::#enums(body) => body.ser(version, bytes),)* } } } } } -#[derive(Debug, PartialEq, Clone, Default)] +#[derive(Debug, PartialEq, Eq, Clone, Default)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct MavEnum { pub name: String, @@ -437,48 +292,95 @@ pub struct MavEnum { } impl MavEnum { - fn has_enum_values(&self) -> bool { - self.entries.iter().all(|x| x.value.is_some()) + fn try_combine(&mut self, enm: &Self) { + if self.name == enm.name { + for enum_entry in &enm.entries { + let found_entry = self.entries.iter().find(|elem| { + elem.name == enum_entry.name && elem.value.unwrap() == enum_entry.value.unwrap() + }); + match found_entry { + Some(entry) => panic!("Enum entry {} already exists", entry.name), + None => self.entries.push(enum_entry.clone()), + } + } + } } - fn emit_defs(&self) -> Vec { - let mut cnt = 0; + fn emit_defs(&self) -> Vec { + let mut cnt = 0u32; self.entries .iter() .map(|enum_entry| { - let name = Ident::from(enum_entry.name.clone()); + let name = format_ident!("{}", enum_entry.name.clone()); let value; - if !self.has_enum_values() { - value = Ident::from(cnt.to_string()); + + #[cfg(feature = "emit-description")] + let description = if let Some(description) = enum_entry.description.as_ref() { + quote!(#[doc = #description]) + } else { + quote!() + }; + + #[cfg(not(feature = "emit-description"))] + let description = quote!(); + + if enum_entry.value.is_none() { cnt += 1; + value = quote!(#cnt); } else { - value = Ident::from(enum_entry.value.unwrap().to_string()); + let tmp_value = enum_entry.value.unwrap(); + cnt = cnt.max(tmp_value); + let tmp = TokenStream::from_str(&tmp_value.to_string()).unwrap(); + value = quote!(#tmp); }; if self.bitfield.is_some() { - quote!(const #name = #value;) + quote! { + #description + const #name = #value; + } } else { - quote!(#name = #value,) + quote! { + #description + #name = #value, + } } }) - .collect::>() + .collect() } - fn emit_name(&self) -> Tokens { - let name = Ident::from(self.name.clone()); + fn emit_name(&self) -> TokenStream { + let name = format_ident!("{}", self.name); quote!(#name) } - fn emit_rust(&self) -> Tokens { + fn emit_const_default(&self) -> TokenStream { + let default = format_ident!("{}", self.entries[0].name); + quote!(pub const DEFAULT: Self = Self::#default;) + } + + fn emit_rust(&self) -> TokenStream { let defs = self.emit_defs(); - let default = Ident::from(self.entries[0].name.clone()); let enum_name = self.emit_name(); + let const_default = self.emit_const_default(); + + #[cfg(feature = "emit-description")] + let description = if let Some(description) = self.description.as_ref() { + let desc = description.to_string(); + quote!(#[doc = #desc]) + } else { + quote!() + }; + + #[cfg(not(feature = "emit-description"))] + let description = quote!(); let enum_def; if let Some(width) = self.bitfield.clone() { - let width = Ident::from(width); + let width = format_ident!("{}", width); enum_def = quote! { bitflags!{ #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] + #description pub struct #enum_name: #width { #(#defs)* } @@ -489,6 +391,8 @@ impl MavEnum { #[derive(Debug, Copy, Clone, PartialEq, FromPrimitive, ToPrimitive)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde", serde(tag = "type"))] + #[repr(u32)] + #description pub enum #enum_name { #(#defs)* } @@ -498,16 +402,20 @@ impl MavEnum { quote! { #enum_def + impl #enum_name { + #const_default + } + impl Default for #enum_name { fn default() -> Self { - #enum_name::#default + Self::DEFAULT } } } } } -#[derive(Debug, PartialEq, Clone, Default)] +#[derive(Debug, PartialEq, Eq, Clone, Default)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct MavEnumEntry { pub value: Option, @@ -528,12 +436,12 @@ pub struct MavMessage { impl MavMessage { /// Return Token of "MESSAGE_NAME_DATA /// for mavlink struct data - fn emit_struct_name(&self) -> Tokens { - let name = Ident::from(format!("{}_DATA", self.name)); + fn emit_struct_name(&self) -> TokenStream { + let name = format_ident!("{}", format!("{}_DATA", self.name)); quote!(#name) } - fn emit_name_types(&self) -> (Vec, usize) { + fn emit_name_types(&self) -> (Vec, usize) { let mut encoded_payload_len: usize = 0; let field_toks = self .fields @@ -546,59 +454,90 @@ impl MavMessage { let description = field.emit_description(); #[cfg(not(feature = "emit-description"))] - let description = Ident::from(""); + let description = quote!(); // From MAVLink specification: // If sent by an implementation that doesn't have the extensions fields // then the recipient will see zero values for the extensions fields. let serde_default = if field.is_extension { - Ident::from(r#"#[cfg_attr(feature = "serde", serde(default))]"#) + if field.enumtype.is_some() { + quote!(#[cfg_attr(feature = "serde", serde(default))]) + } else { + quote!(#[cfg_attr(feature = "serde", serde(default = "crate::RustDefault::rust_default"))]) + } } else { - Ident::from("") + quote!() + }; + + let serde_with_attr = if matches!(field.mavtype, MavType::Array(_, _)) { + quote!(#[cfg_attr(feature = "serde", serde(with = "serde_arrays"))]) + } else { + quote!() }; quote! { #description #serde_default + #serde_with_attr #nametype } }) - .collect::>(); + .collect::>(); (field_toks, encoded_payload_len) } /// Generate description for the given message #[cfg(feature = "emit-description")] - fn emit_description(&self) -> Tokens { - let mut desc = String::from(format!("\n/// id: {}\n", self.id)); + fn emit_description(&self) -> TokenStream { + let mut ts = TokenStream::new(); + let desc = format!("id: {}", self.id); + ts.extend(quote!(#[doc = #desc])); if let Some(val) = self.description.clone() { - desc = desc + &format!("/// {}.\n", val); + let doc = &format!("{val}."); + ts.extend(quote!(#[doc = #doc])); } - let desc = Ident::from(desc); - quote!(#desc) + ts } - fn emit_serialize_vars(&self) -> Tokens { - let ser_vars = self - .fields - .iter() - .map(|f| f.rust_writer()) - .collect::>(); + fn emit_serialize_vars(&self) -> TokenStream { + let ser_vars = self.fields.iter().map(|f| f.rust_writer()); + quote! { - let mut _tmp = Vec::new(); + let mut __tmp = BytesMut::new(bytes); + + // TODO: these lints are produced on a couple of cubepilot messages + // because they are generated as empty structs with no fields and + // therefore Self::ENCODED_LEN is 0. This itself is a bug because + // cubepilot.xml has unclosed tags in fields, which the parser has + // bad time handling. It should probably be fixed in both the parser + // and mavlink message definitions. However, until it's done, let's + // skip the lints. + #[allow(clippy::absurd_extreme_comparisons)] + #[allow(unused_comparisons)] + if __tmp.remaining() < Self::ENCODED_LEN { + panic!( + "buffer is too small (need {} bytes, but got {})", + Self::ENCODED_LEN, + __tmp.remaining(), + ) + } + #(#ser_vars)* - _tmp + if matches!(version, MavlinkVersion::V2) { + let len = __tmp.len(); + ::mavlink_core::utils::remove_trailing_zeroes(&bytes[..len]) + } else { + __tmp.len() + } } } - fn emit_deserialize_vars(&self) -> Tokens { + fn emit_deserialize_vars(&self) -> TokenStream { let deser_vars = self .fields .iter() .map(|f| f.rust_reader()) - .collect::>(); - - let encoded_len_name = Ident::from(format!("{}_DATA::ENCODED_LEN", self.name)); + .collect::>(); if deser_vars.is_empty() { // struct has no fields @@ -607,42 +546,65 @@ impl MavMessage { } } else { quote! { - let avail_len = _input.len(); - - // fast zero copy - let mut buf = BytesMut::from(_input); + let avail_len = __input.len(); - // handle payload length truncuation due to empty fields - if avail_len < #encoded_len_name { + let mut payload_buf = [0; Self::ENCODED_LEN]; + let mut buf = if avail_len < Self::ENCODED_LEN { //copy available bytes into an oversized buffer filled with zeros - let mut payload_buf = [0; #encoded_len_name]; - payload_buf[0..avail_len].copy_from_slice(_input); - buf = BytesMut::from(&payload_buf[..]); - } + payload_buf[0..avail_len].copy_from_slice(__input); + Bytes::new(&payload_buf) + } else { + // fast zero copy + Bytes::new(__input) + }; - let mut _struct = Self::default(); + let mut __struct = Self::default(); #(#deser_vars)* - Ok(_struct) + Ok(__struct) + } + } + } + + fn emit_default_impl(&self) -> TokenStream { + let msg_name = self.emit_struct_name(); + quote! { + impl Default for #msg_name { + fn default() -> Self { + Self::DEFAULT.clone() + } } } } - fn emit_rust(&self) -> Tokens { + fn emit_const_default(&self) -> TokenStream { + let initializers = self + .fields + .iter() + .map(|field| field.emit_default_initializer()); + quote!(pub const DEFAULT: Self = Self { #(#initializers)* };) + } + + fn emit_rust(&self) -> TokenStream { let msg_name = self.emit_struct_name(); + let id = self.id; + let name = self.name.clone(); + let extra_crc = extra_crc(self); let (name_types, msg_encoded_len) = self.emit_name_types(); let deser_vars = self.emit_deserialize_vars(); let serialize_vars = self.emit_serialize_vars(); + let const_default = self.emit_const_default(); + let default_impl = self.emit_default_impl(); #[cfg(feature = "emit-description")] let description = self.emit_description(); #[cfg(not(feature = "emit-description"))] - let description = Ident::from(""); + let description = quote!(); quote! { #description - #[derive(Debug, Clone, PartialEq, Default)] + #[derive(Debug, Clone, PartialEq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct #msg_name { #(#name_types)* @@ -650,12 +612,24 @@ impl MavMessage { impl #msg_name { pub const ENCODED_LEN: usize = #msg_encoded_len; + #const_default + } + + #default_impl - pub fn deser(version: MavlinkVersion, _input: &[u8]) -> Result { + impl MessageData for #msg_name { + type Message = MavMessage; + + const ID: u32 = #id; + const NAME: &'static str = #name; + const EXTRA_CRC: u8 = #extra_crc; + const ENCODED_LEN: usize = #msg_encoded_len; + + fn deser(_version: MavlinkVersion, __input: &[u8]) -> Result { #deser_vars } - pub fn ser(&self) -> Vec { + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { #serialize_vars } } @@ -676,126 +650,134 @@ pub struct MavField { impl MavField { /// Emit rust name of a given field - fn emit_name(&self) -> Tokens { - let name = Ident::from(self.name.clone()); + fn emit_name(&self) -> TokenStream { + let name = format_ident!("{}", self.name); quote!(#name) } /// Emit rust type of the field - fn emit_type(&self) -> Tokens { + fn emit_type(&self) -> TokenStream { let mavtype; - match self.mavtype { - MavType::Array(_, _) => { - mavtype = Ident::from(self.mavtype.rust_type()); - } - _ => match self.enumtype { - Some(ref enumname) => { - mavtype = Ident::from(enumname.clone()); - } - _ => { - mavtype = Ident::from(self.mavtype.rust_type()); - } - }, + if matches!(self.mavtype, MavType::Array(_, _)) { + let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap(); + mavtype = quote!(#rt); + } else if let Some(enumname) = &self.enumtype { + let en = TokenStream::from_str(enumname).unwrap(); + mavtype = quote!(#en); + } else { + let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap(); + mavtype = quote!(#rt); } - quote!(#mavtype) + mavtype } /// Generate description for the given field #[cfg(feature = "emit-description")] - fn emit_description(&self) -> Tokens { - let mut desc = Vec::new(); + fn emit_description(&self) -> TokenStream { + let mut ts = TokenStream::new(); if let Some(val) = self.description.clone() { - desc.push(format!("\n/// {}.", val)); + let desc = format!("{val}."); + ts.extend(quote!(#[doc = #desc])); } - desc.push("\n".to_string()); - let desc: String = desc.iter().map(|s| s.to_string()).collect(); - let desc = Ident::from(desc); - quote!(#desc) + ts } /// Combine rust name and type of a given field - fn emit_name_type(&self) -> Tokens { + fn emit_name_type(&self) -> TokenStream { let name = self.emit_name(); let fieldtype = self.emit_type(); quote!(pub #name: #fieldtype,) } /// Emit writer - fn rust_writer(&self) -> Tokens { + fn rust_writer(&self) -> TokenStream { let mut name = "self.".to_string() + &self.name.clone(); - if let Some(_) = &self.enumtype { - if let Some(dsp) = &self.display { - // potentially a bitflag - if dsp == "bitmask" { - // it is a bitflag - name += ".bits()"; - } else { - panic!("Display option not implemented"); - } - } else { - match self.mavtype { - MavType::Array(_, _) => {} // cast are not necessary for arrays - _ => { - // an enum, have to use "*foo as u8" cast - name += " as "; - name += &self.mavtype.rust_type(); + if self.enumtype.is_some() { + // casts are not necessary for arrays, because they are currently + // generated as primitive arrays + if !matches!(self.mavtype, MavType::Array(_, _)) { + if let Some(dsp) = &self.display { + // potentially a bitflag + if dsp == "bitmask" { + // it is a bitflag + name += ".bits()"; + } else { + panic!("Display option not implemented"); } + } else { + // an enum, have to use "*foo as u8" cast + name += " as "; + name += &self.mavtype.rust_type(); } } } - let name = Ident::from(name); - let buf = Ident::from("_tmp"); - self.mavtype.rust_writer(name, buf) + let ts = TokenStream::from_str(&name).unwrap(); + let name = quote!(#ts); + let buf = format_ident!("__tmp"); + self.mavtype.rust_writer(&name, buf) } /// Emit reader - fn rust_reader(&self) -> Tokens { - let name = Ident::from("_struct.".to_string() + &self.name.clone()); - let buf = Ident::from("buf"); + fn rust_reader(&self) -> TokenStream { + let _name = TokenStream::from_str(&self.name).unwrap(); + + let name = quote!(__struct.#_name); + let buf = format_ident!("buf"); if let Some(enum_name) = &self.enumtype { + // TODO: handle enum arrays properly, rather than just generating + // primitive arrays + if let MavType::Array(_t, _size) = &self.mavtype { + return self.mavtype.rust_reader(&name, buf); + } if let Some(dsp) = &self.display { if dsp == "bitmask" { // bitflags - let tmp = self - .mavtype - .rust_reader(Ident::from("let tmp"), buf.clone()); - let enum_name_ident = Ident::from(enum_name.clone()); + let tmp = self.mavtype.rust_reader("e!(let tmp), buf); + let enum_name_ident = format_ident!("{}", enum_name); quote! { #tmp #name = #enum_name_ident::from_bits(tmp & #enum_name_ident::all().bits()) - .ok_or(ParserError::InvalidFlag { flag_type: #enum_name.to_string(), value: tmp as u32 })?; + .ok_or(::mavlink_core::error::ParserError::InvalidFlag { flag_type: #enum_name, value: tmp as u32 })?; } } else { panic!("Display option not implemented"); } } else { - match &self.mavtype { - MavType::Array(_t, _size) => { - return self.mavtype.rust_reader(name, buf); - } - _ => {} - } // handle enum by FromPrimitive - let tmp = self - .mavtype - .rust_reader(Ident::from("let tmp"), buf.clone()); - let val = Ident::from("from_".to_string() + &self.mavtype.rust_type()); + let tmp = self.mavtype.rust_reader("e!(let tmp), buf); + let val = format_ident!("from_{}", &self.mavtype.rust_type()); quote!( #tmp #name = FromPrimitive::#val(tmp) - .ok_or(ParserError::InvalidEnum { enum_type: #enum_name.to_string(), value: tmp as u32 })?; + .ok_or(::mavlink_core::error::ParserError::InvalidEnum { enum_type: #enum_name, value: tmp as u32 })?; ) } } else { - self.mavtype.rust_reader(name, buf) + self.mavtype.rust_reader(&name, buf) + } + } + + fn emit_default_initializer(&self) -> TokenStream { + let field = self.emit_name(); + // FIXME: Is this actually expected behaviour?? + if matches!(self.mavtype, MavType::Array(_, _)) { + let default_value = self.mavtype.emit_default_value(); + quote!(#field: #default_value,) + } else if let Some(enumname) = &self.enumtype { + let ty = TokenStream::from_str(enumname).unwrap(); + quote!(#field: #ty::DEFAULT,) + } else { + let default_value = self.mavtype.emit_default_value(); + quote!(#field: #default_value,) } } } -#[derive(Debug, PartialEq, Clone)] +#[derive(Debug, PartialEq, Clone, Default)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub enum MavType { UInt8MavlinkVersion, + #[default] UInt8, UInt16, UInt32, @@ -810,14 +792,8 @@ pub enum MavType { Array(Box, usize), } -impl Default for MavType { - fn default() -> MavType { - MavType::UInt8 - } -} - impl MavType { - fn parse_type(s: &str) -> Option { + fn parse_type(s: &str) -> Option { use self::MavType::*; match s { "uint8_t_mavlink_version" => Some(UInt8MavlinkVersion), @@ -834,10 +810,10 @@ impl MavType { "Double" => Some(Double), "double" => Some(Double), _ => { - if s.ends_with("]") { - let start = s.find("[")?; + if s.ends_with(']') { + let start = s.find('[')?; let size = s[start + 1..(s.len() - 1)].parse::().ok()?; - let mtype = MavType::parse_type(&s[0..start])?; + let mtype = Self::parse_type(&s[0..start])?; Some(Array(Box::new(mtype), size)) } else { None @@ -847,10 +823,10 @@ impl MavType { } /// Emit reader of a given type - pub fn rust_reader(&self, val: Ident, buf: Ident) -> Tokens { + pub fn rust_reader(&self, val: &TokenStream, buf: Ident) -> TokenStream { use self::MavType::*; - match self.clone() { - Char => quote! {#val = #buf.get_u8() as char;}, + match self { + Char => quote! {#val = #buf.get_u8();}, UInt8 => quote! {#val = #buf.get_u8();}, UInt16 => quote! {#val = #buf.get_u16_le();}, UInt32 => quote! {#val = #buf.get_u32_le();}, @@ -862,24 +838,12 @@ impl MavType { Int64 => quote! {#val = #buf.get_i64_le();}, Float => quote! {#val = #buf.get_f32_le();}, Double => quote! {#val = #buf.get_f64_le();}, - Array(t, size) => { - if size > 32 { - // it is a vector - let r = t.rust_reader(Ident::from("let val"), buf.clone()); - quote! { - for _ in 0..#size { - #r - #val.push(val); - } - } - } else { - // handle as a slice - let r = t.rust_reader(Ident::from("let val"), buf.clone()); - quote! { - for idx in 0..#size { - #r - #val[idx] = val; - } + Array(t, _) => { + let r = t.rust_reader("e!(let val), buf); + quote! { + for v in &mut #val { + #r + *v = val; } } } @@ -887,12 +851,12 @@ impl MavType { } /// Emit writer of a given type - pub fn rust_writer(&self, val: Ident, buf: Ident) -> Tokens { + pub fn rust_writer(&self, val: &TokenStream, buf: Ident) -> TokenStream { use self::MavType::*; - match self.clone() { + match self { UInt8MavlinkVersion => quote! {#buf.put_u8(#val);}, UInt8 => quote! {#buf.put_u8(#val);}, - Char => quote! {#buf.put_u8(#val as u8);}, + Char => quote! {#buf.put_u8(#val);}, UInt16 => quote! {#buf.put_u16_le(#val);}, UInt32 => quote! {#buf.put_u32_le(#val);}, Int8 => quote! {#buf.put_i8(#val);}, @@ -903,7 +867,7 @@ impl MavType { Int64 => quote! {#buf.put_i64_le(#val);}, Double => quote! {#buf.put_f64_le(#val);}, Array(t, _size) => { - let w = t.rust_writer(Ident::from("*val"), buf.clone()); + let w = t.rust_writer("e!(*val), buf); quote! { for val in &#val { #w @@ -916,7 +880,7 @@ impl MavType { /// Size of a given Mavtype fn len(&self) -> usize { use self::MavType::*; - match self.clone() { + match self { UInt8MavlinkVersion | UInt8 | Int8 | Char => 1, UInt16 | Int16 => 2, UInt32 | Int32 | Float => 4, @@ -928,7 +892,7 @@ impl MavType { /// Used for ordering of types fn order_len(&self) -> usize { use self::MavType::*; - match self.clone() { + match self { UInt8MavlinkVersion | UInt8 | Int8 | Char => 1, UInt16 | Int16 => 2, UInt32 | Int32 | Float => 4, @@ -940,7 +904,7 @@ impl MavType { /// Used for crc calculation pub fn primitive_type(&self) -> String { use self::MavType::*; - match self.clone() { + match self { UInt8MavlinkVersion => "uint8_t".into(), UInt8 => "uint8_t".into(), Int8 => "int8_t".into(), @@ -961,10 +925,10 @@ impl MavType { /// Used for generating struct fields. pub fn rust_type(&self) -> String { use self::MavType::*; - match self.clone() { + match self { UInt8 | UInt8MavlinkVersion => "u8".into(), Int8 => "i8".into(), - Char => "char".into(), + Char => "u8".into(), UInt16 => "u16".into(), Int16 => "i16".into(), UInt32 => "u32".into(), @@ -973,18 +937,42 @@ impl MavType { UInt64 => "u64".into(), Int64 => "i64".into(), Double => "f64".into(), - Array(t, size) => { - if size > 32 { - // we have to use a vector to make our lives easier - format!("Vec<{}> /* {} elements */", t.rust_type(), size) - } else { - // we can use a slice, as Rust derives lot of thinsg for slices <= 32 elements - format!("[{};{}]", t.rust_type(), size) - } + Array(t, size) => format!("[{};{}]", t.rust_type(), size), + } + } + + pub fn emit_default_value(&self) -> TokenStream { + use self::MavType::*; + match self { + UInt8 | UInt8MavlinkVersion => quote!(0_u8), + Int8 => quote!(0_i8), + Char => quote!(0_u8), + UInt16 => quote!(0_u16), + Int16 => quote!(0_i16), + UInt32 => quote!(0_u32), + Int32 => quote!(0_i32), + Float => quote!(0.0_f32), + UInt64 => quote!(0_u64), + Int64 => quote!(0_i64), + Double => quote!(0.0_f64), + Array(ty, size) => { + let default_value = ty.emit_default_value(); + quote!([#default_value; #size]) } } } + /// Return rust equivalent of the primitive type of a MavType. The primitive + /// type is the type itself for all except arrays, in which case it is the + /// element type. + pub fn rust_primitive_type(&self) -> String { + use self::MavType::*; + match self { + Array(t, _) => t.rust_primitive_type(), + _ => self.rust_type(), + } + } + /// Compare two MavTypes pub fn compare(&self, other: &Self) -> Ordering { let len = self.order_len(); @@ -992,7 +980,7 @@ impl MavType { } } -#[derive(Debug, PartialEq, Clone, Copy)] +#[derive(Debug, PartialEq, Eq, Clone, Copy)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde", serde(tag = "type"))] pub enum MavXmlElement { @@ -1013,24 +1001,24 @@ pub enum MavXmlElement { Extensions, } -fn identify_element(s: &str) -> Option { +const fn identify_element(s: &[u8]) -> Option { use self::MavXmlElement::*; match s { - "version" => Some(Version), - "mavlink" => Some(Mavlink), - "dialect" => Some(Dialect), - "include" => Some(Include), - "enums" => Some(Enums), - "enum" => Some(Enum), - "entry" => Some(Entry), - "description" => Some(Description), - "param" => Some(Param), - "messages" => Some(Messages), - "message" => Some(Message), - "field" => Some(Field), - "deprecated" => Some(Deprecated), - "wip" => Some(Wip), - "extensions" => Some(Extensions), + b"version" => Some(Version), + b"mavlink" => Some(Mavlink), + b"dialect" => Some(Dialect), + b"include" => Some(Include), + b"enums" => Some(Enums), + b"enum" => Some(Enum), + b"entry" => Some(Entry), + b"description" => Some(Description), + b"param" => Some(Param), + b"messages" => Some(Messages), + b"message" => Some(Message), + b"field" => Some(Field), + b"deprecated" => Some(Deprecated), + b"wip" => Some(Wip), + b"extensions" => Some(Extensions), _ => None, } } @@ -1039,7 +1027,7 @@ fn is_valid_parent(p: Option, s: MavXmlElement) -> bool { use self::MavXmlElement::*; match s { Version => p == Some(Mavlink), - Mavlink => p == None, + Mavlink => p.is_none(), Dialect => p == Some(Mavlink), Include => p == Some(Mavlink), Enums => p == Some(Mavlink), @@ -1056,70 +1044,82 @@ fn is_valid_parent(p: Option, s: MavXmlElement) -> bool { } } -pub fn parse_profile(file: &mut dyn Read) -> MavProfile { - let mut stack: Vec = vec![]; +pub fn parse_profile( + definitions_dir: &Path, + definition_file: &Path, + parsed_files: &mut HashSet, +) -> Result { + let in_path = Path::new(&definitions_dir).join(definition_file); + parsed_files.insert(in_path.clone()); // Keep track of which files have been parsed - let mut profile = MavProfile { - includes: vec![], - messages: vec![], - enums: vec![], - }; + let mut stack: Vec = vec![]; + let mut profile = MavProfile::default(); let mut field = MavField::default(); let mut message = MavMessage::default(); let mut mavenum = MavEnum::default(); let mut entry = MavEnumEntry::default(); - let mut include = String::new(); + let mut include = PathBuf::new(); let mut paramid: Option = None; let mut xml_filter = MavXmlFilter::default(); - let mut parser: Vec> = - EventReader::new(file).into_iter().collect(); - xml_filter.filter(&mut parser); + let mut events: Vec> = Vec::new(); + let file = File::open(&in_path).map_err(|e| BindGenError::CouldNotReadDefinitionFile { + source: e, + path: in_path.clone(), + })?; + let mut reader = Reader::from_reader(BufReader::new(file)); + reader.config_mut().trim_text(true); + + let mut buf = Vec::new(); + loop { + match reader.read_event_into(&mut buf) { + Ok(Event::Eof) => { + events.push(Ok(Event::Eof)); + break; + } + Ok(event) => events.push(Ok(event.into_owned())), + Err(why) => events.push(Err(why)), + } + buf.clear(); + } + xml_filter.filter(&mut events); let mut is_in_extension = false; - for e in parser { + for e in events { match e { - Ok(XmlEvent::StartElement { - name, - attributes: attrs, - .. - }) => { - let id = match identify_element(&name.to_string()) { - None => { - panic!("unexpected element {:?}", name); - } - Some(kind) => kind, + Ok(Event::Start(bytes)) => { + let Some(id) = identify_element(bytes.name().into_inner()) else { + panic!( + "unexpected element {:?}", + String::from_utf8_lossy(bytes.name().into_inner()) + ); }; - if !is_valid_parent( - match stack.last().clone() { - Some(arg) => Some(arg.clone()), - None => None, - }, - id.clone(), - ) { - panic!("not valid parent {:?} of {:?}", stack.last(), id); - } + assert!( + is_valid_parent(stack.last().copied(), id), + "not valid parent {:?} of {id:?}", + stack.last(), + ); match id { MavXmlElement::Extensions => { is_in_extension = true; } MavXmlElement::Message => { - message = Default::default(); + message = MavMessage::default(); } MavXmlElement::Field => { - field = Default::default(); + field = MavField::default(); field.is_extension = is_in_extension; } MavXmlElement::Enum => { - mavenum = Default::default(); + mavenum = MavEnum::default(); } MavXmlElement::Entry => { - entry = Default::default(); + entry = MavEnumEntry::default(); } MavXmlElement::Include => { - include = Default::default(); + include = PathBuf::default(); } MavXmlElement::Param => { paramid = None; @@ -1129,51 +1129,35 @@ pub fn parse_profile(file: &mut dyn Read) -> MavProfile { stack.push(id); - for attr in attrs { + for attr in bytes.attributes() { + let attr = attr.unwrap(); match stack.last() { - Some(&MavXmlElement::Enum) => match attr.name.local_name.clone().as_ref() { - "name" => { - mavenum.name = attr - .value - .clone() - .split("_") - .map(|x| x.to_lowercase()) - .map(|x| { - let mut v: Vec = x.chars().collect(); - v[0] = v[0].to_uppercase().nth(0).unwrap(); - v.into_iter().collect() - }) - .collect::>() - .join(""); + Some(&MavXmlElement::Enum) => { + if attr.key.into_inner() == b"name" { + mavenum.name = to_pascal_case(attr.value); //mavenum.name = attr.value.clone(); } - _ => (), - }, + } Some(&MavXmlElement::Entry) => { - match attr.name.local_name.clone().as_ref() { - "name" => { - entry.name = attr.value.clone(); + match attr.key.into_inner() { + b"name" => { + entry.name = String::from_utf8_lossy(&attr.value).to_string(); } - "value" => { + b"value" => { + let value = String::from_utf8_lossy(&attr.value); // Deal with hexadecimal numbers - if attr.value.starts_with("0x") { - entry.value = Some( - u32::from_str_radix( - attr.value.trim_start_matches("0x"), - 16, - ) - .unwrap(), - ); - } else { - entry.value = Some(attr.value.parse::().unwrap()); - } + let (src, radix) = value + .strip_prefix("0x") + .map(|value| (value, 16)) + .unwrap_or((value.as_ref(), 10)); + entry.value = u32::from_str_radix(src, radix).ok(); } _ => (), } } Some(&MavXmlElement::Message) => { - match attr.name.local_name.clone().as_ref() { - "name" => { + match attr.key.into_inner() { + b"name" => { /*message.name = attr .value .clone() @@ -1187,108 +1171,124 @@ pub fn parse_profile(file: &mut dyn Read) -> MavProfile { .collect::>() .join(""); */ - message.name = attr.value.clone(); + message.name = String::from_utf8_lossy(&attr.value).to_string(); } - "id" => { - //message.id = attr.value.parse::().unwrap(); - message.id = attr.value.parse::().unwrap(); + b"id" => { + message.id = + String::from_utf8_lossy(&attr.value).parse().unwrap(); } _ => (), } } Some(&MavXmlElement::Field) => { - match attr.name.local_name.clone().as_ref() { - "name" => { - field.name = attr.value.clone(); - if field.name == "type" { - field.name = "mavtype".to_string(); - } + match attr.key.into_inner() { + b"name" => { + let name = String::from_utf8_lossy(&attr.value); + field.name = if name == "type" { + "mavtype".to_string() + } else { + name.to_string() + }; } - "type" => { - field.mavtype = MavType::parse_type(&attr.value).unwrap(); + b"type" => { + let r#type = String::from_utf8_lossy(&attr.value); + field.mavtype = MavType::parse_type(&r#type).unwrap(); } - "enum" => { - field.enumtype = Some( - attr.value - .clone() - .split("_") - .map(|x| x.to_lowercase()) - .map(|x| { - let mut v: Vec = x.chars().collect(); - v[0] = v[0].to_uppercase().nth(0).unwrap(); - v.into_iter().collect() - }) - .collect::>() - .join(""), - ); + b"enum" => { + field.enumtype = Some(to_pascal_case(attr.value)); //field.enumtype = Some(attr.value.clone()); } - "display" => { - field.display = Some(attr.value); + b"display" => { + field.display = + Some(String::from_utf8_lossy(&attr.value).to_string()); } _ => (), } } Some(&MavXmlElement::Param) => { - if let None = entry.params { + if entry.params.is_none() { entry.params = Some(vec![]); } - match attr.name.local_name.clone().as_ref() { - "index" => { - paramid = Some(attr.value.parse::().unwrap()); - } - _ => (), + if attr.key.into_inner() == b"index" { + paramid = + Some(String::from_utf8_lossy(&attr.value).parse().unwrap()); } } _ => (), } } } - Ok(XmlEvent::Characters(s)) => { + Ok(Event::Empty(bytes)) => match bytes.name().into_inner() { + b"extensions" => { + is_in_extension = true; + } + b"entry" => { + entry = MavEnumEntry::default(); + for attr in bytes.attributes() { + let attr = attr.unwrap(); + match attr.key.into_inner() { + b"name" => { + entry.name = String::from_utf8_lossy(&attr.value).to_string(); + } + b"value" => { + entry.value = + Some(String::from_utf8_lossy(&attr.value).parse().unwrap()); + } + _ => (), + } + } + mavenum.entries.push(entry.clone()); + } + _ => (), + }, + Ok(Event::Text(bytes)) => { + let s = String::from_utf8_lossy(&bytes).to_string(); + use self::MavXmlElement::*; match (stack.last(), stack.get(stack.len() - 2)) { (Some(&Description), Some(&Message)) => { - message.description = Some(s.replace("\n", " ")); + message.description = Some(s.replace('\n', " ")); } (Some(&Field), Some(&Message)) => { - field.description = Some(s.replace("\n", " ")); + field.description = Some(s.replace('\n', " ")); } (Some(&Description), Some(&Enum)) => { - mavenum.description = Some(s.replace("\n", " ")); + mavenum.description = Some(s.replace('\n', " ")); } (Some(&Description), Some(&Entry)) => { - entry.description = Some(s.replace("\n", " ")); + entry.description = Some(s.replace('\n', " ")); } (Some(&Param), Some(&Entry)) => { - if let Some(ref mut params) = entry.params { + if let Some(params) = entry.params.as_mut() { // Some messages can jump between values, like: // 0, 1, 2, 7 - if params.len() < paramid.unwrap() { - for index in params.len()..paramid.unwrap() { + let paramid = paramid.unwrap(); + if params.len() < paramid { + for index in params.len()..paramid { params.insert(index, String::from("The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).")); } } - params[paramid.unwrap() - 1] = s; + params[paramid - 1] = s; } } (Some(&Include), Some(&Mavlink)) => { - include = s.replace("\n", ""); + include = PathBuf::from(s.replace('\n', "")); } (Some(&Version), Some(&Mavlink)) => { - eprintln!("TODO: version {:?}", s); + eprintln!("TODO: version {s:?}"); } (Some(&Dialect), Some(&Mavlink)) => { - eprintln!("TODO: dialect {:?}", s); + eprintln!("TODO: dialect {s:?}"); } (Some(Deprecated), _) => { - eprintln!("TODO: deprecated {:?}", s); + eprintln!("TODO: deprecated {s:?}"); } data => { - panic!("unexpected text data {:?} reading {:?}", data, s); + panic!("unexpected text data {data:?} reading {s:?}"); } } } - Ok(XmlEvent::EndElement { .. }) => { + Ok(Event::End(_)) => { match stack.last() { Some(&MavXmlElement::Field) => message.fields.push(field.clone()), Some(&MavXmlElement::Entry) => { @@ -1312,13 +1312,23 @@ pub fn parse_profile(file: &mut dyn Read) -> MavProfile { msg.fields.extend(not_extension_fields); msg.fields.extend(extension_fields); - profile.messages.push(msg); + profile.add_message(&msg); } Some(&MavXmlElement::Enum) => { - profile.enums.push(mavenum.clone()); + profile.add_enum(&mavenum); } Some(&MavXmlElement::Include) => { - profile.includes.push(include.clone()); + let include_file = Path::new(&definitions_dir).join(include.clone()); + if !parsed_files.contains(&include_file) { + let included_profile = + parse_profile(definitions_dir, &include, parsed_files)?; + for message in included_profile.messages.values() { + profile.add_message(message); + } + for enm in included_profile.enums.values() { + profile.add_enum(enm); + } + } } _ => (), } @@ -1326,7 +1336,7 @@ pub fn parse_profile(file: &mut dyn Read) -> MavProfile { // println!("{}-{}", indent(depth), name); } Err(e) => { - eprintln!("Error: {}", e); + eprintln!("Error: {e}"); break; } _ => {} @@ -1334,17 +1344,24 @@ pub fn parse_profile(file: &mut dyn Read) -> MavProfile { } //let profile = profile.update_messages(); //TODO verify no longer needed - profile.update_enums() + Ok(profile.update_enums()) } /// Generate protobuf represenation of mavlink message set /// Generate rust representation of mavlink message set with appropriate conversion methods -pub fn generate(input: &mut R, output_rust: &mut W) { - let profile = parse_profile(input); +pub fn generate( + definitions_dir: &Path, + definition_file: &Path, + output_rust: &mut W, +) -> Result<(), BindGenError> { + let mut parsed_files: HashSet = HashSet::new(); + let profile = parse_profile(definitions_dir, definition_file, &mut parsed_files)?; // rust file let rust_tokens = profile.emit_rust(); - writeln!(output_rust, "{}", rust_tokens).unwrap(); + writeln!(output_rust, "{rust_tokens}").unwrap(); + + Ok(()) } /// CRC operates over names of the message and names of its fields @@ -1358,7 +1375,7 @@ pub fn extra_crc(msg: &MavMessage) -> u8 { let mut crc = CRCu16::crc16mcrf4cc(); crc.digest(msg.name.as_bytes()); - crc.digest(" ".as_bytes()); + crc.digest(b" "); let mut f = msg.fields.clone(); // only mavlink 1 fields should be part of the extra_crc @@ -1366,13 +1383,13 @@ pub fn extra_crc(msg: &MavMessage) -> u8 { f.sort_by(|a, b| a.mavtype.compare(&b.mavtype)); for field in &f { crc.digest(field.mavtype.primitive_type().as_bytes()); - crc.digest(" ".as_bytes()); + crc.digest(b" "); if field.name == "mavtype" { - crc.digest("type".as_bytes()); + crc.digest(b"type"); } else { crc.digest(field.name.as_bytes()); } - crc.digest(" ".as_bytes()); + crc.digest(b" "); if let MavType::Array(_, size) = field.mavtype { crc.digest(&[size as u8]); } @@ -1393,8 +1410,8 @@ struct MavXmlFilter { } impl Default for MavXmlFilter { - fn default() -> MavXmlFilter { - MavXmlFilter { + fn default() -> Self { + Self { #[cfg(not(feature = "emit-extensions"))] extension_filter: ExtensionFilter { is_in: false }, } @@ -1402,62 +1419,66 @@ impl Default for MavXmlFilter { } impl MavXmlFilter { - pub fn filter(&mut self, elements: &mut Vec>) { + pub fn filter(&mut self, elements: &mut Vec>) { // List of filters elements.retain(|x| self.filter_extension(x)); } #[cfg(feature = "emit-extensions")] - pub fn filter_extension( - &mut self, - _element: &Result, - ) -> bool { - return true; + pub fn filter_extension(&mut self, _element: &Result) -> bool { + true } /// Ignore extension fields #[cfg(not(feature = "emit-extensions"))] - pub fn filter_extension( - &mut self, - element: &Result, - ) -> bool { + pub fn filter_extension(&mut self, element: &Result) -> bool { match element { Ok(content) => { match content { - XmlEvent::StartElement { name, .. } => { - let id = match identify_element(&name.to_string()) { - None => { - panic!("unexpected element {:?}", name); - } - Some(kind) => kind, + Event::Start(bytes) | Event::Empty(bytes) => { + let Some(id) = identify_element(bytes.name().into_inner()) else { + panic!( + "unexpected element {:?}", + String::from_utf8_lossy(bytes.name().into_inner()) + ); }; - match id { - MavXmlElement::Extensions => { - self.extension_filter.is_in = true; - } - _ => {} + if id == MavXmlElement::Extensions { + self.extension_filter.is_in = true; } } - XmlEvent::EndElement { name } => { - let id = match identify_element(&name.to_string()) { - None => { - panic!("unexpected element {:?}", name); - } - Some(kind) => kind, + Event::End(bytes) => { + let Some(id) = identify_element(bytes.name().into_inner()) else { + panic!( + "unexpected element {:?}", + String::from_utf8_lossy(bytes.name().into_inner()) + ); }; - match id { - MavXmlElement::Message => { - self.extension_filter.is_in = false; - } - _ => {} + if id == MavXmlElement::Message { + self.extension_filter.is_in = false; } } _ => {} } - return !self.extension_filter.is_in; + !self.extension_filter.is_in } - Err(error) => panic!("Failed to filter XML: {}", error), + Err(error) => panic!("Failed to filter XML: {error}"), } } } + +fn to_pascal_case(text: impl AsRef<[u8]>) -> String { + text.as_ref() + .split(|c| *c == b'_') + .map(String::from_utf8_lossy) + .map(capitalize_word) + .collect() +} + +fn capitalize_word(text: impl AsRef) -> String { + let mut chars = text.as_ref().chars(); + match chars.next() { + None => String::new(), + Some(char) => char.to_uppercase().to_string() + &chars.as_str().to_ascii_lowercase(), + } +} diff --git a/build/util.rs b/mavlink-bindgen/src/util.rs similarity index 100% rename from build/util.rs rename to mavlink-bindgen/src/util.rs diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml new file mode 100644 index 0000000000..413572b61f --- /dev/null +++ b/mavlink-core/Cargo.toml @@ -0,0 +1,50 @@ +[package] +name = "mavlink-core" +version = "0.14.0" +authors = [ + "Todd Stellanova", + "Michal Podhradsky", + "Kevin Mehall", + "Tim Ryan", + "Patrick José Pereira", + "Ibiyemi Abiodun", +] +description = "Implements the MAVLink data interchange format for UAVs." +readme = "../README.md" +license = "MIT/Apache-2.0" +repository = "https://github.com/mavlink/rust-mavlink" +edition.workspace = true +rust-version.workspace = true + +[dependencies] +crc-any = { workspace = true, default-features = false } +byteorder = { workspace = true, default-features = false } +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 } +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"] +"udp" = [] +"tcp" = [] +"direct-serial" = ["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" = ["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", "dep:tokio-serial"] +"signing" = ["dep:sha2"] +default = ["std", "tcp", "udp", "direct-serial", "serde"] + +[dev-dependencies] +tokio = { version = "1.0", default-features = false, features = ["io-util", "net", "sync", "fs", "macros", "rt"] } \ No newline at end of file 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..d9b2fe1de2 --- /dev/null +++ b/mavlink-core/src/async_connection/direct_serial.rs @@ -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 { + 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/file.rs b/mavlink-core/src/async_connection/file.rs new file mode 100644 index 0000000000..5dae56d99e --- /dev/null +++ b/mavlink-core/src/async_connection/file.rs @@ -0,0 +1,83 @@ +//! Async File MAVLINK connection + +use core::ops::DerefMut; + +use super::AsyncMavConnection; +use crate::error::{MessageReadError, MessageWriteError}; + +use crate::{async_peek_reader::AsyncPeekReader, MavHeader, MavlinkVersion, Message}; + +use tokio::fs::File; +use tokio::io; +use tokio::sync::Mutex; + +#[cfg(not(feature = "signing"))] +use crate::read_versioned_msg_async; + +#[cfg(feature = "signing")] +use crate::{read_versioned_msg_async_signed, SigningConfig, SigningData}; + +pub async fn open(file_path: &str) -> io::Result { + let file = File::open(file_path).await?; + Ok(AsyncFileConnection { + file: Mutex::new(AsyncPeekReader::new(file)), + protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, + }) +} + +pub struct AsyncFileConnection { + file: Mutex>, + protocol_version: MavlinkVersion, + + #[cfg(feature = "signing")] + signing_data: Option, +} + +#[async_trait::async_trait] +impl AsyncMavConnection for AsyncFileConnection { + async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { + let mut file = self.file.lock().await; + + loop { + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg_async(file.deref_mut(), self.protocol_version).await; + #[cfg(feature = "signing")] + let result = read_versioned_msg_async_signed( + file.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ) + .await; + match result { + ok @ Ok(..) => { + return ok; + } + Err(MessageReadError::Io(e)) => { + if e.kind() == io::ErrorKind::UnexpectedEof { + return Err(MessageReadError::Io(e)); + } + } + _ => {} + } + } + } + + async fn send(&self, _header: &MavHeader, _data: &M) -> Result { + Ok(0) + } + + 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 new file mode 100644 index 0000000000..b2eb5c6757 --- /dev/null +++ b/mavlink-core/src/async_connection/mod.rs @@ -0,0 +1,137 @@ +use tokio::io; + +use crate::{MavFrame, MavHeader, MavlinkVersion, Message}; + +#[cfg(feature = "tcp")] +mod tcp; + +#[cfg(feature = "udp")] +mod udp; + +#[cfg(feature = "direct-serial")] +mod direct_serial; + +mod file; + +#[cfg(feature = "signing")] +use crate::SigningConfig; + +/// An async MAVLink connection +#[async_trait::async_trait] +pub trait AsyncMavConnection { + /// Receive a mavlink message. + /// + /// Yield until a valid frame is received, ignoring invalid messages. + async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError>; + + /// Send a mavlink message + async fn send( + &self, + header: &MavHeader, + data: &M, + ) -> Result; + + fn set_protocol_version(&mut self, version: MavlinkVersion); + fn get_protocol_version(&self) -> MavlinkVersion; + + /// Write whole frame + async fn send_frame( + &self, + frame: &MavFrame, + ) -> Result { + self.send(&frame.header, &frame.msg).await + } + + /// Read whole frame + async fn recv_frame(&self) -> Result, crate::error::MessageReadError> { + let (header, msg) = self.recv().await?; + let protocol_version = self.get_protocol_version(); + Ok(MavFrame { + header, + msg, + protocol_version, + }) + } + + /// Send a message with default header + async fn send_default(&self, data: &M) -> Result { + let header = MavHeader::default(); + self.send(&header, data).await + } + + /// Setup secret key used for message signing, or disable message signing + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option); +} + +/// Connect asynchronously to a MAVLink node by address string. +/// +/// The address must be in one of the following formats: +/// +/// * `tcpin::` to create a TCP server, listening for incoming connections +/// * `tcpout::` to create a TCP client +/// * `udpin::` to create a UDP server, listening for incoming packets +/// * `udpout::` to create a UDP client +/// * `udpbcast::` to create a UDP broadcast +/// * `serial::` to create a serial connection +/// * `file:` to extract file data +/// +/// The type of the connection is determined at runtime based on the address type, so the +/// connection is returned as a trait object. +pub async fn connect_async( + address: &str, +) -> io::Result + Sync + Send>> { + let protocol_err = Err(io::Error::new( + io::ErrorKind::AddrNotAvailable, + "Protocol unsupported", + )); + + if cfg!(feature = "tcp") && address.starts_with("tcp") { + #[cfg(feature = "tcp")] + { + tcp::select_protocol(address).await + } + #[cfg(not(feature = "tcp"))] + { + protocol_err + } + } else if cfg!(feature = "udp") && address.starts_with("udp") { + #[cfg(feature = "udp")] + { + udp::select_protocol(address).await + } + #[cfg(not(feature = "udp"))] + { + 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 { + protocol_err + } +} + +/// Returns the socket address for the given address. +pub(crate) fn get_socket_addr( + address: T, +) -> Result { + let addr = match address.to_socket_addrs()?.next() { + Some(addr) => addr, + None => { + return Err(io::Error::new( + io::ErrorKind::Other, + "Host address lookup failed", + )); + } + }; + Ok(addr) +} diff --git a/mavlink-core/src/async_connection/tcp.rs b/mavlink-core/src/async_connection/tcp.rs new file mode 100644 index 0000000000..4cd1f9ac9a --- /dev/null +++ b/mavlink-core/src/async_connection/tcp.rs @@ -0,0 +1,156 @@ +//! Async TCP MAVLink connection + +use super::{get_socket_addr, AsyncMavConnection}; +use crate::async_peek_reader::AsyncPeekReader; +use crate::{MavHeader, MavlinkVersion, Message}; + +use core::ops::DerefMut; +use tokio::io; +use tokio::net::tcp::{OwnedReadHalf, OwnedWriteHalf}; +use tokio::net::{TcpListener, TcpStream}; +use tokio::sync::Mutex; + +#[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, +}; + +pub async fn select_protocol( + address: &str, +) -> io::Result + Sync + Send>> { + let connection = if let Some(address) = address.strip_prefix("tcpout:") { + tcpout(address).await + } else if let Some(address) = address.strip_prefix("tcpin:") { + tcpin(address).await + } else { + Err(io::Error::new( + io::ErrorKind::AddrNotAvailable, + "Protocol unsupported", + )) + }; + + Ok(Box::new(connection?)) +} + +pub async fn tcpout(address: T) -> io::Result { + let addr = get_socket_addr(address)?; + + let socket = TcpStream::connect(addr).await?; + + let (reader, writer) = socket.into_split(); + + Ok(AsyncTcpConnection { + reader: Mutex::new(AsyncPeekReader::new(reader)), + writer: Mutex::new(TcpWrite { + socket: writer, + sequence: 0, + }), + protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, + }) +} + +pub async fn tcpin(address: T) -> io::Result { + let addr = get_socket_addr(address)?; + let listener = TcpListener::bind(addr).await?; + + //For now we only accept one incoming stream: this yields until we get one + match listener.accept().await { + Ok((socket, _)) => { + let (reader, writer) = socket.into_split(); + return Ok(AsyncTcpConnection { + reader: Mutex::new(AsyncPeekReader::new(reader)), + writer: Mutex::new(TcpWrite { + socket: writer, + sequence: 0, + }), + protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, + }); + } + Err(e) => { + //TODO don't println in lib + println!("listener err: {e}"); + } + } + Err(io::Error::new( + io::ErrorKind::NotConnected, + "No incoming connections!", + )) +} + +pub struct AsyncTcpConnection { + reader: Mutex>, + writer: Mutex, + protocol_version: MavlinkVersion, + #[cfg(feature = "signing")] + signing_data: Option, +} + +struct TcpWrite { + socket: OwnedWriteHalf, + sequence: u8, +} + +#[async_trait::async_trait] +impl AsyncMavConnection for AsyncTcpConnection { + async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { + let mut reader = self.reader.lock().await; + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg_async(reader.deref_mut(), self.protocol_version).await; + #[cfg(feature = "signing")] + let result = read_versioned_msg_async_signed( + reader.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ) + .await; + result + } + + async fn send( + &self, + header: &MavHeader, + data: &M, + ) -> Result { + let mut lock = self.writer.lock().await; + + let header = MavHeader { + sequence: lock.sequence, + system_id: header.system_id, + component_id: header.component_id, + }; + + lock.sequence = lock.sequence.wrapping_add(1); + #[cfg(not(feature = "signing"))] + let result = + write_versioned_msg_async(&mut lock.socket, self.protocol_version, header, data).await; + #[cfg(feature = "signing")] + let result = write_versioned_msg_async_signed( + &mut lock.socket, + 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/udp.rs b/mavlink-core/src/async_connection/udp.rs new file mode 100644 index 0000000000..3f06b83e1d --- /dev/null +++ b/mavlink-core/src/async_connection/udp.rs @@ -0,0 +1,279 @@ +//! Async UDP MAVLink connection + +use core::{ops::DerefMut, task::Poll}; +use std::{collections::VecDeque, io::Read, sync::Arc}; + +use tokio::{ + io::{self, AsyncRead, ReadBuf}, + net::UdpSocket, + sync::Mutex, +}; + +use crate::{async_peek_reader::AsyncPeekReader, MavHeader, MavlinkVersion, Message}; + +use super::{get_socket_addr, AsyncMavConnection}; + +#[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_signed, SigningConfig, SigningData, +}; + +pub async fn select_protocol( + address: &str, +) -> io::Result + Sync + Send>> { + let connection = if let Some(address) = address.strip_prefix("udpin:") { + udpin(address).await + } else if let Some(address) = address.strip_prefix("udpout:") { + udpout(address).await + } else if let Some(address) = address.strip_prefix("udpbcast:") { + udpbcast(address).await + } else { + Err(io::Error::new( + io::ErrorKind::AddrNotAvailable, + "Protocol unsupported", + )) + }; + + Ok(Box::new(connection?)) +} + +pub async fn udpbcast(address: T) -> io::Result { + let addr = get_socket_addr(address)?; + let socket = UdpSocket::bind("0.0.0.0:0").await?; + socket + .set_broadcast(true) + .expect("Couldn't bind to broadcast address."); + AsyncUdpConnection::new(socket, false, Some(addr)) +} + +pub async fn udpout(address: T) -> io::Result { + let addr = get_socket_addr(address)?; + let socket = UdpSocket::bind("0.0.0.0:0").await?; + AsyncUdpConnection::new(socket, false, Some(addr)) +} + +pub async fn udpin(address: T) -> io::Result { + let addr = address + .to_socket_addrs() + .unwrap() + .next() + .expect("Invalid address"); + let socket = UdpSocket::bind(addr).await?; + AsyncUdpConnection::new(socket, true, None) +} + +struct UdpRead { + socket: Arc, + buffer: VecDeque, + last_recv_address: Option, +} + +const MTU_SIZE: usize = 1500; +impl AsyncRead for UdpRead { + fn poll_read( + mut self: core::pin::Pin<&mut Self>, + cx: &mut core::task::Context<'_>, + buf: &mut io::ReadBuf<'_>, + ) -> Poll> { + if self.buffer.is_empty() { + let mut read_buffer = [0u8; MTU_SIZE]; + let mut read_buffer = ReadBuf::new(&mut read_buffer); + + match self.socket.poll_recv_from(cx, &mut read_buffer) { + Poll::Ready(Ok(address)) => { + let n_buffer = read_buffer.filled().len(); + + let n = (&read_buffer.filled()[0..n_buffer]).read(buf.initialize_unfilled())?; + buf.advance(n); + + self.buffer.extend(&read_buffer.filled()[n..n_buffer]); + self.last_recv_address = Some(address); + Poll::Ready(Ok(())) + } + Poll::Ready(Err(err)) => Poll::Ready(Err(err)), + Poll::Pending => Poll::Pending, + } + } else { + let read_result = self.buffer.read(buf.initialize_unfilled()); + let result = match read_result { + Ok(n) => { + buf.advance(n); + Ok(()) + } + Err(err) => Err(err), + }; + Poll::Ready(result) + } + } +} + +struct UdpWrite { + socket: Arc, + dest: Option, + sequence: u8, +} + +pub struct AsyncUdpConnection { + reader: Mutex>, + writer: Mutex, + protocol_version: MavlinkVersion, + server: bool, + #[cfg(feature = "signing")] + signing_data: Option, +} + +impl AsyncUdpConnection { + fn new( + socket: UdpSocket, + server: bool, + dest: Option, + ) -> io::Result { + let socket = Arc::new(socket); + Ok(Self { + server, + reader: Mutex::new(AsyncPeekReader::new(UdpRead { + socket: socket.clone(), + buffer: VecDeque::new(), + last_recv_address: None, + })), + writer: Mutex::new(UdpWrite { + socket, + dest, + sequence: 0, + }), + protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, + }) + } +} + +#[async_trait::async_trait] +impl AsyncMavConnection for AsyncUdpConnection { + async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { + let mut reader = self.reader.lock().await; + + loop { + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg_async(reader.deref_mut(), self.protocol_version).await; + #[cfg(feature = "signing")] + let result = read_versioned_msg_async_signed( + reader.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ) + .await; + if self.server { + if let addr @ Some(_) = reader.reader_ref().last_recv_address { + self.writer.lock().await.dest = addr; + } + } + if let ok @ Ok(..) = result { + return ok; + } + } + } + + async fn send( + &self, + header: &MavHeader, + data: &M, + ) -> Result { + let mut guard = self.writer.lock().await; + let state = &mut *guard; + + let header = MavHeader { + sequence: state.sequence, + system_id: header.system_id, + component_id: header.component_id, + }; + + state.sequence = state.sequence.wrapping_add(1); + + let len = if let Some(addr) = state.dest { + let mut buf = Vec::new(); + #[cfg(not(feature = "signing"))] + write_versioned_msg_async( + &mut buf, + self.protocol_version, + header, + data, + #[cfg(feature = "signing")] + self.signing_data.as_ref(), + ) + .await?; + #[cfg(feature = "signing")] + write_versioned_msg_signed( + &mut buf, + self.protocol_version, + header, + data, + #[cfg(feature = "signing")] + self.signing_data.as_ref(), + )?; + state.socket.send_to(&buf, addr).await? + } else { + 0 + }; + + Ok(len) + } + + 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) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use io::AsyncReadExt; + + #[tokio::test] + async fn test_datagram_buffering() { + let receiver_socket = Arc::new(UdpSocket::bind("127.0.0.1:5001").await.unwrap()); + let mut udp_reader = UdpRead { + socket: receiver_socket.clone(), + buffer: VecDeque::new(), + last_recv_address: None, + }; + let sender_socket = UdpSocket::bind("0.0.0.0:0").await.unwrap(); + sender_socket.connect("127.0.0.1:5001").await.unwrap(); + + let datagram: Vec = (0..50).collect::>(); + + let mut n_sent = sender_socket.send(&datagram).await.unwrap(); + assert_eq!(n_sent, datagram.len()); + n_sent = sender_socket.send(&datagram).await.unwrap(); + assert_eq!(n_sent, datagram.len()); + + let mut buf = [0u8; 30]; + + let mut n_read = udp_reader.read(&mut buf).await.unwrap(); + assert_eq!(n_read, 30); + assert_eq!(&buf[0..n_read], (0..30).collect::>().as_slice()); + + n_read = udp_reader.read(&mut buf).await.unwrap(); + assert_eq!(n_read, 20); + assert_eq!(&buf[0..n_read], (30..50).collect::>().as_slice()); + + n_read = udp_reader.read(&mut buf).await.unwrap(); + assert_eq!(n_read, 30); + assert_eq!(&buf[0..n_read], (0..30).collect::>().as_slice()); + + n_read = udp_reader.read(&mut buf).await.unwrap(); + assert_eq!(n_read, 20); + assert_eq!(&buf[0..n_read], (30..50).collect::>().as_slice()); + } +} diff --git a/mavlink-core/src/async_peek_reader.rs b/mavlink-core/src/async_peek_reader.rs new file mode 100644 index 0000000000..1d6ada2c4b --- /dev/null +++ b/mavlink-core/src/async_peek_reader.rs @@ -0,0 +1,147 @@ +//! This module implements a buffered/peekable reader using async I/O. +//! +//! The purpose of the buffered/peekable reader is to allow for backtracking parsers. +//! +//! This is the async version of [`crate::peek_reader::PeekReader`]. +//! A reader implementing the tokio library's [`tokio::io::AsyncBufRead`]/[`tokio::io::AsyncBufReadExt`] traits seems like a good fit, but +//! it does not allow for peeking a specific number of bytes, so it provides no way to request +//! more data from the underlying reader without consuming the existing data. +//! +//! This API still tries to adhere to the [`tokio::io::AsyncBufRead`]'s trait philosophy. +//! +//! The main type [`AsyncPeekReader`] does not implement [`tokio::io::AsyncBufReadExt`] itself, as there is no added benefit +//! in doing so. +//! +use tokio::io::AsyncReadExt; + +use crate::error::MessageReadError; + +/// A buffered/peekable reader +/// +/// This reader wraps a type implementing [`tokio::io::AsyncRead`] and adds buffering via an internal buffer. +/// +/// It allows the user to `peek` a specified number of bytes (without consuming them), +/// to `read` bytes (consuming them), or to `consume` them after `peek`ing. +/// +/// NOTE: This reader is generic over the size of the buffer, defaulting to MAVLink's current largest +/// possible message size of 280 bytes +/// +pub struct AsyncPeekReader { + // Internal buffer + buffer: [u8; BUFFER_SIZE], + // The position of the next byte to read from the buffer. + cursor: usize, + // The position of the next byte to read into the buffer. + top: usize, + // The wrapped reader. + reader: R, +} + +impl AsyncPeekReader { + /// Instantiates a new [`AsyncPeekReader`], wrapping the provided [`tokio::io::AsyncReadExt`] and using the default chunk size + pub fn new(reader: R) -> Self { + Self { + buffer: [0; BUFFER_SIZE], + cursor: 0, + top: 0, + reader, + } + } + + /// Peeks an exact amount of bytes from the internal buffer + /// + /// If the internal buffer does not contain enough data, this function will read + /// from the underlying [`tokio::io::AsyncReadExt`] until it does, an error occurs or no more data can be read (EOF). + /// + /// If an EOF occurs and the specified amount could not be read, this function will return an [`ErrorKind::UnexpectedEof`]. + /// + /// This function does not consume data from the buffer, so subsequent calls to `peek` or `read` functions + /// will still return the peeked data. + /// + pub async fn peek_exact(&mut self, amount: usize) -> Result<&[u8], MessageReadError> { + self.fetch(amount, false).await + } + + /// Reads a specified amount of bytes from the internal buffer + /// + /// If the internal buffer does not contain enough data, this function will read + /// from the underlying [`tokio::io::AsyncReadExt`] until it does, an error occurs or no more data can be read (EOF). + /// + /// If an EOF occurs and the specified amount could not be read, this function will return an [`ErrorKind::UnexpectedEof`]. + /// + /// This function consumes the data from the buffer, unless an error occurs, in which case no data is consumed. + /// + pub async fn read_exact(&mut self, amount: usize) -> Result<&[u8], MessageReadError> { + self.fetch(amount, true).await + } + + /// Reads a byte from the internal buffer + /// + /// If the internal buffer does not contain enough data, this function will read + /// from the underlying [`tokio::io::AsyncReadExt`] until it does, an error occurs or no more data can be read (EOF). + /// + /// If an EOF occurs and the specified amount could not be read, this function will return an [`ErrorKind::UnexpectedEof`]. + /// + /// This function consumes the data from the buffer, unless an error occurs, in which case no data is consumed. + /// + pub async fn read_u8(&mut self) -> Result { + let buf = self.read_exact(1).await?; + Ok(buf[0]) + } + + /// Consumes a specified amount of bytes from the buffer + /// + /// If the internal buffer does not contain enough data, this function will consume as much data as is buffered. + /// + pub fn consume(&mut self, amount: usize) -> usize { + let amount = amount.min(self.top - self.cursor); + self.cursor += amount; + amount + } + + /// Returns an immutable reference to the underlying [`tokio::io::AsyncRead`] + /// + /// Reading directly from the underlying stream will cause data loss + pub fn reader_ref(&mut self) -> &R { + &self.reader + } + + /// Returns a mutable reference to the underlying [`tokio::io::AsyncRead`] + /// + /// Reading directly from the underlying stream will cause data loss + pub fn reader_mut(&mut self) -> &mut R { + &mut self.reader + } + + /// Internal function to fetch data from the internal buffer and/or reader + async fn fetch(&mut self, amount: usize, consume: bool) -> Result<&[u8], MessageReadError> { + let buffered = self.top - self.cursor; + + // the caller requested more bytes than we have buffered, fetch them from the reader + if buffered < amount { + let bytes_read = amount - buffered; + assert!(bytes_read < BUFFER_SIZE); + let mut buf = [0u8; BUFFER_SIZE]; + + // read needed bytes from reader + self.reader.read_exact(&mut buf[..bytes_read]).await?; + + // if some bytes were read, add them to the buffer + + if self.buffer.len() - self.top < bytes_read { + // reallocate + self.buffer.copy_within(self.cursor..self.top, 0); + self.cursor = 0; + self.top = buffered; + } + self.buffer[self.top..self.top + bytes_read].copy_from_slice(&buf[..bytes_read]); + self.top += bytes_read; + } + + let result = &self.buffer[self.cursor..self.cursor + amount]; + if consume { + self.cursor += amount; + } + Ok(result) + } +} diff --git a/mavlink-core/src/bytes.rs b/mavlink-core/src/bytes.rs new file mode 100644 index 0000000000..e1b11ff433 --- /dev/null +++ b/mavlink-core/src/bytes.rs @@ -0,0 +1,134 @@ +pub struct Bytes<'a> { + data: &'a [u8], + pos: usize, +} + +impl<'a> Bytes<'a> { + pub fn new(data: &'a [u8]) -> Self { + Self { data, pos: 0 } + } + + #[inline] + fn remaining(&self) -> usize { + self.data.len() - self.pos + } + + #[inline] + pub fn remaining_bytes(&self) -> &'a [u8] { + &self.data[self.pos..] + } + + #[inline] + fn check_remaining(&self, count: usize) { + assert!( + self.remaining() >= count, + "read buffer exhausted; remaining {} bytes, try read {count} bytes", + self.remaining(), + ); + } + + #[inline] + pub fn get_bytes(&mut self, count: usize) -> &[u8] { + self.check_remaining(count); + + let bytes = &self.data[self.pos..(self.pos + count)]; + self.pos += count; + bytes + } + + #[inline] + pub fn get_array(&mut self) -> [u8; SIZE] { + let bytes = self.get_bytes(SIZE); + let mut arr = [0u8; SIZE]; + + arr.copy_from_slice(bytes); + + debug_assert_eq!(arr.as_slice(), bytes); + + arr + } + + #[inline] + pub fn get_u8(&mut self) -> u8 { + self.check_remaining(1); + + let val = self.data[self.pos]; + self.pos += 1; + val + } + + #[inline] + pub fn get_i8(&mut self) -> i8 { + self.check_remaining(1); + + let val = self.data[self.pos] as i8; + self.pos += 1; + val + } + + #[inline] + pub fn get_u16_le(&mut self) -> u16 { + u16::from_le_bytes(self.get_array()) + } + + #[inline] + pub fn get_i16_le(&mut self) -> i16 { + i16::from_le_bytes(self.get_array()) + } + + #[inline] + pub fn get_u24_le(&mut self) -> u32 { + const SIZE: usize = 3; + self.check_remaining(SIZE); + + let mut val = [0u8; SIZE + 1]; + val[..3].copy_from_slice(&self.data[self.pos..self.pos + SIZE]); + self.pos += SIZE; + + debug_assert_eq!(val[3], 0); + u32::from_le_bytes(val) + } + + #[inline] + pub fn get_i24_le(&mut self) -> i32 { + const SIZE: usize = 3; + self.check_remaining(SIZE); + + let mut val = [0u8; SIZE + 1]; + val[..3].copy_from_slice(&self.data[self.pos..self.pos + SIZE]); + self.pos += SIZE; + + debug_assert_eq!(val[3], 0); + i32::from_le_bytes(val) + } + + #[inline] + pub fn get_u32_le(&mut self) -> u32 { + u32::from_le_bytes(self.get_array()) + } + + #[inline] + pub fn get_i32_le(&mut self) -> i32 { + i32::from_le_bytes(self.get_array()) + } + + #[inline] + pub fn get_u64_le(&mut self) -> u64 { + u64::from_le_bytes(self.get_array()) + } + + #[inline] + pub fn get_i64_le(&mut self) -> i64 { + i64::from_le_bytes(self.get_array()) + } + + #[inline] + pub fn get_f32_le(&mut self) -> f32 { + f32::from_le_bytes(self.get_array()) + } + + #[inline] + pub fn get_f64_le(&mut self) -> f64 { + f64::from_le_bytes(self.get_array()) + } +} diff --git a/mavlink-core/src/bytes_mut.rs b/mavlink-core/src/bytes_mut.rs new file mode 100644 index 0000000000..df607c83c8 --- /dev/null +++ b/mavlink-core/src/bytes_mut.rs @@ -0,0 +1,178 @@ +pub struct BytesMut<'a> { + data: &'a mut [u8], + len: usize, +} + +impl<'a> BytesMut<'a> { + pub fn new(data: &'a mut [u8]) -> Self { + Self { data, len: 0 } + } + + #[inline] + pub fn len(&self) -> usize { + self.len + } + + #[inline] + pub fn is_empty(&self) -> bool { + self.len() == 0 + } + + #[inline] + pub fn remaining(&self) -> usize { + self.data.len() - self.len + } + + #[inline] + fn check_remaining(&self, count: usize) { + assert!( + self.remaining() >= count, + "write buffer overflow; remaining {} bytes, try add {count} bytes", + self.remaining(), + ); + } + + #[inline] + pub fn put_slice(&mut self, src: &[u8]) { + self.check_remaining(src.len()); + + unsafe { + core::ptr::copy_nonoverlapping(src.as_ptr(), &mut self.data[self.len], src.len()); + } + self.len += src.len(); + } + + #[inline] + pub fn put_u8(&mut self, val: u8) { + self.check_remaining(1); + + self.data[self.len] = val; + self.len += 1; + } + + #[inline] + pub fn put_i8(&mut self, val: i8) { + self.check_remaining(1); + + self.data[self.len] = val as u8; + self.len += 1; + } + + #[inline] + pub fn put_u16_le(&mut self, val: u16) { + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let src = val.to_le_bytes(); + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; + } + + #[inline] + pub fn put_i16_le(&mut self, val: i16) { + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let src = val.to_le_bytes(); + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; + } + + #[inline] + pub fn put_u24_le(&mut self, val: u32) { + const SIZE: usize = 3; + const MAX: u32 = 2u32.pow(24) - 1; + + assert!( + val <= MAX, + "Attempted to put value that is too large for 24 bits, \ + attempted to push: {val}, max allowed: {MAX}", + ); + + let src = val.to_le_bytes(); + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..3]); + self.len += SIZE; + } + + #[inline] + pub fn put_i24_le(&mut self, val: i32) { + const SIZE: usize = 3; + const MIN: i32 = 2i32.pow(23); + const MAX: i32 = 2i32.pow(23) - 1; + + assert!( + val <= MAX, + "Attempted to put value that is too large for 24 bits, \ + attempted to push: {val}, max allowed: {MAX}", + ); + assert!( + val >= MIN, + "Attempted to put value that is too negative for 24 bits, \ + attempted to push: {val}, min allowed: {MIN}", + ); + + let src = val.to_le_bytes(); + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..3]); + self.len += SIZE; + } + + #[inline] + pub fn put_u32_le(&mut self, val: u32) { + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let src = val.to_le_bytes(); + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; + } + + #[inline] + pub fn put_i32_le(&mut self, val: i32) { + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let src = val.to_le_bytes(); + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; + } + + #[inline] + pub fn put_u64_le(&mut self, val: u64) { + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let src = val.to_le_bytes(); + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; + } + + #[inline] + pub fn put_i64_le(&mut self, val: i64) { + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let src = val.to_le_bytes(); + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; + } + + #[inline] + pub fn put_f32_le(&mut self, val: f32) { + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let src = val.to_le_bytes(); + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; + } + + #[inline] + pub fn put_f64_le(&mut self, val: f64) { + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let src = val.to_le_bytes(); + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; + } +} diff --git a/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs similarity index 54% rename from src/connection/direct_serial.rs rename to mavlink-core/src/connection/direct_serial.rs index ab20df006d..2954b98107 100644 --- a/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.rs @@ -1,18 +1,22 @@ -extern crate serial; +//! Serial MAVLINK connection use crate::connection::MavConnection; -use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; -use std::io::{self}; +use crate::peek_reader::PeekReader; +use crate::{MavHeader, MavlinkVersion, Message}; +use core::ops::DerefMut; +use std::io; use std::sync::Mutex; -//TODO why is this import so hairy? -use crate::connection::direct_serial::serial::prelude::*; use crate::error::{MessageReadError, MessageWriteError}; +use serial::{prelude::*, SystemPort}; -/// Serial MAVLINK connection +#[cfg(not(feature = "signing"))] +use crate::{read_versioned_msg, write_versioned_msg}; +#[cfg(feature = "signing")] +use crate::{read_versioned_msg_signed, write_versioned_msg_signed, SigningConfig, SigningData}; pub fn open(settings: &str) -> io::Result { - let settings_toks: Vec<&str> = settings.split(":").collect(); + let settings_toks: Vec<&str> = settings.split(':').collect(); if settings_toks.len() < 2 { return Err(io::Error::new( io::ErrorKind::AddrNotAvailable, @@ -20,15 +24,15 @@ pub fn open(settings: &str) -> io::Result { )); } - let baud_opt = settings_toks[1].parse::(); - if baud_opt.is_err() { + let Ok(baud) = settings_toks[1] + .parse::() + .map(serial::core::BaudRate::from_speed) + else { return Err(io::Error::new( io::ErrorKind::AddrNotAvailable, "Invalid baud rate", )); - } - - let baud = serial::core::BaudRate::from_speed(baud_opt.unwrap()); + }; let settings = serial::core::PortSettings { baud_rate: baud, @@ -43,24 +47,35 @@ pub fn open(settings: &str) -> io::Result { port.configure(&settings)?; Ok(SerialConnection { - port: Mutex::new(port), + port: Mutex::new(PeekReader::new(port)), sequence: Mutex::new(0), protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, }) } pub struct SerialConnection { - port: Mutex, + port: Mutex>, sequence: Mutex, protocol_version: MavlinkVersion, + #[cfg(feature = "signing")] + signing_data: Option, } impl MavConnection for SerialConnection { fn recv(&self) -> Result<(MavHeader, M), MessageReadError> { let mut port = self.port.lock().unwrap(); - loop { - match read_versioned_msg(&mut *port, self.protocol_version) { + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg(port.deref_mut(), self.protocol_version); + #[cfg(feature = "signing")] + let result = read_versioned_msg_signed( + port.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ); + match result { ok @ Ok(..) => { return ok; } @@ -86,14 +101,29 @@ impl MavConnection for SerialConnection { *sequence = sequence.wrapping_add(1); - write_versioned_msg(&mut *port, self.protocol_version, header, data) + #[cfg(not(feature = "signing"))] + let result = write_versioned_msg(port.reader_mut(), self.protocol_version, header, data); + #[cfg(feature = "signing")] + let result = write_versioned_msg_signed( + port.reader_mut(), + self.protocol_version, + header, + data, + self.signing_data.as_ref(), + ); + result } fn set_protocol_version(&mut self, version: MavlinkVersion) { self.protocol_version = version; } - fn get_protocol_version(&self) -> MavlinkVersion { + fn 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/src/connection/file.rs b/mavlink-core/src/connection/file.rs similarity index 51% rename from src/connection/file.rs rename to mavlink-core/src/connection/file.rs index 777f1b280c..7258259347 100644 --- a/src/connection/file.rs +++ b/mavlink-core/src/connection/file.rs @@ -1,27 +1,35 @@ +//! File MAVLINK connection + use crate::connection::MavConnection; use crate::error::{MessageReadError, MessageWriteError}; -use crate::{read_versioned_msg, MavHeader, MavlinkVersion, Message}; +use crate::peek_reader::PeekReader; +use crate::{MavHeader, MavlinkVersion, Message}; +use core::ops::DerefMut; use std::fs::File; -use std::io::{self}; +use std::io; use std::sync::Mutex; -/// File MAVLINK connection +#[cfg(not(feature = "signing"))] +use crate::read_versioned_msg; +#[cfg(feature = "signing")] +use crate::{read_versioned_msg_signed, SigningConfig, SigningData}; pub fn open(file_path: &str) -> io::Result { - let file = match File::open(&file_path) { - Err(e) => return Err(e), - Ok(file) => file, - }; + let file = File::open(file_path)?; Ok(FileConnection { - file: Mutex::new(file), + file: Mutex::new(PeekReader::new(file)), protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, }) } pub struct FileConnection { - file: Mutex, + file: Mutex>, protocol_version: MavlinkVersion, + #[cfg(feature = "signing")] + signing_data: Option, } impl MavConnection for FileConnection { @@ -31,7 +39,15 @@ impl MavConnection for FileConnection { let mut file = self.file.lock().unwrap(); loop { - match read_versioned_msg(&mut *file, self.protocol_version) { + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg(file.deref_mut(), self.protocol_version); + #[cfg(feature = "signing")] + let result = read_versioned_msg_signed( + file.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ); + match result { ok @ Ok(..) => { return ok; } @@ -53,7 +69,12 @@ impl MavConnection for FileConnection { self.protocol_version = version; } - fn get_protocol_version(&self) -> MavlinkVersion { + fn 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/src/connection/mod.rs b/mavlink-core/src/connection/mod.rs similarity index 82% rename from src/connection/mod.rs rename to mavlink-core/src/connection/mod.rs index 10dacbe976..e60386a6aa 100644 --- a/src/connection/mod.rs +++ b/mavlink-core/src/connection/mod.rs @@ -11,6 +11,9 @@ mod udp; #[cfg(feature = "direct-serial")] mod direct_serial; +#[cfg(feature = "signing")] +use crate::SigningConfig; + mod file; /// A MAVLink connection @@ -24,7 +27,7 @@ pub trait MavConnection { fn send(&self, header: &MavHeader, data: &M) -> Result; fn set_protocol_version(&mut self, version: MavlinkVersion); - fn get_protocol_version(&self) -> MavlinkVersion; + fn protocol_version(&self) -> MavlinkVersion; /// Write whole frame fn send_frame(&self, frame: &MavFrame) -> Result { @@ -34,7 +37,7 @@ pub trait MavConnection { /// Read whole frame fn recv_frame(&self) -> Result, crate::error::MessageReadError> { let (header, msg) = self.recv()?; - let protocol_version = self.get_protocol_version(); + let protocol_version = self.protocol_version(); Ok(MavFrame { header, msg, @@ -47,6 +50,10 @@ pub trait MavConnection { let header = MavHeader::default(); self.send(&header, data) } + + /// Setup secret key used for message signing, or disable message signing + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option); } /// Connect to a MAVLink node by address string. @@ -102,3 +109,13 @@ pub fn connect(address: &str) -> io::Result protocol_err } } + +/// Returns the socket address for the given address. +pub(crate) fn get_socket_addr( + address: T, +) -> Result { + address.to_socket_addrs()?.next().ok_or(io::Error::new( + io::ErrorKind::Other, + "Host address lookup failed", + )) +} diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs new file mode 100644 index 0000000000..d362a5664a --- /dev/null +++ b/mavlink-core/src/connection/tcp.rs @@ -0,0 +1,149 @@ +//! TCP MAVLink connection + +use crate::connection::MavConnection; +use crate::peek_reader::PeekReader; +use crate::{MavHeader, MavlinkVersion, Message}; +use core::ops::DerefMut; +use std::io; +use std::net::ToSocketAddrs; +use std::net::{TcpListener, TcpStream}; +use std::sync::Mutex; +use std::time::Duration; + +use super::get_socket_addr; + +#[cfg(not(feature = "signing"))] +use crate::{read_versioned_msg, write_versioned_msg}; + +#[cfg(feature = "signing")] +use crate::{read_versioned_msg_signed, write_versioned_msg_signed, SigningConfig, SigningData}; + +pub fn select_protocol( + address: &str, +) -> io::Result + Sync + Send>> { + let connection = if let Some(address) = address.strip_prefix("tcpout:") { + tcpout(address) + } else if let Some(address) = address.strip_prefix("tcpin:") { + tcpin(address) + } else { + Err(io::Error::new( + io::ErrorKind::AddrNotAvailable, + "Protocol unsupported", + )) + }; + + Ok(Box::new(connection?)) +} + +pub fn tcpout(address: T) -> io::Result { + let addr = get_socket_addr(address)?; + + let socket = TcpStream::connect(addr)?; + socket.set_read_timeout(Some(Duration::from_millis(100)))?; + + Ok(TcpConnection { + reader: Mutex::new(PeekReader::new(socket.try_clone()?)), + writer: Mutex::new(TcpWrite { + socket, + sequence: 0, + }), + protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, + }) +} + +pub fn tcpin(address: T) -> io::Result { + let addr = get_socket_addr(address)?; + let listener = TcpListener::bind(addr)?; + + //For now we only accept one incoming stream: this blocks until we get one + for incoming in listener.incoming() { + match incoming { + Ok(socket) => { + return Ok(TcpConnection { + reader: Mutex::new(PeekReader::new(socket.try_clone()?)), + writer: Mutex::new(TcpWrite { + socket, + sequence: 0, + }), + protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, + }) + } + Err(e) => { + //TODO don't println in lib + println!("listener err: {e}"); + } + } + } + Err(io::Error::new( + io::ErrorKind::NotConnected, + "No incoming connections!", + )) +} + +pub struct TcpConnection { + reader: Mutex>, + writer: Mutex, + protocol_version: MavlinkVersion, + #[cfg(feature = "signing")] + signing_data: Option, +} + +struct TcpWrite { + socket: TcpStream, + sequence: u8, +} + +impl MavConnection for TcpConnection { + fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { + let mut reader = self.reader.lock().unwrap(); + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg(reader.deref_mut(), self.protocol_version); + #[cfg(feature = "signing")] + let result = read_versioned_msg_signed( + reader.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ); + result + } + + fn send(&self, header: &MavHeader, data: &M) -> Result { + let mut lock = self.writer.lock().unwrap(); + + let header = MavHeader { + sequence: lock.sequence, + system_id: header.system_id, + component_id: header.component_id, + }; + + lock.sequence = lock.sequence.wrapping_add(1); + #[cfg(not(feature = "signing"))] + let result = write_versioned_msg(&mut lock.socket, self.protocol_version, header, data); + #[cfg(feature = "signing")] + let result = write_versioned_msg_signed( + &mut lock.socket, + self.protocol_version, + header, + data, + self.signing_data.as_ref(), + ); + result + } + + fn set_protocol_version(&mut self, version: MavlinkVersion) { + self.protocol_version = version; + } + + fn 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/connection/udp.rs b/mavlink-core/src/connection/udp.rs new file mode 100644 index 0000000000..75e5be3c48 --- /dev/null +++ b/mavlink-core/src/connection/udp.rs @@ -0,0 +1,235 @@ +//! UDP MAVLink connection + +use std::collections::VecDeque; + +use crate::connection::MavConnection; +use crate::peek_reader::PeekReader; +use crate::{MavHeader, MavlinkVersion, Message}; +use core::ops::DerefMut; +use std::io::{self, Read}; +use std::net::ToSocketAddrs; +use std::net::{SocketAddr, UdpSocket}; +use std::sync::Mutex; + +use super::get_socket_addr; + +#[cfg(not(feature = "signing"))] +use crate::{read_versioned_msg, write_versioned_msg}; + +#[cfg(feature = "signing")] +use crate::{read_versioned_msg_signed, write_versioned_msg_signed, SigningConfig, SigningData}; + +pub fn select_protocol( + address: &str, +) -> io::Result + Sync + Send>> { + let connection = if let Some(address) = address.strip_prefix("udpin:") { + udpin(address) + } else if let Some(address) = address.strip_prefix("udpout:") { + udpout(address) + } else if let Some(address) = address.strip_prefix("udpbcast:") { + udpbcast(address) + } else { + Err(io::Error::new( + io::ErrorKind::AddrNotAvailable, + "Protocol unsupported", + )) + }; + + Ok(Box::new(connection?)) +} + +pub fn udpbcast(address: T) -> io::Result { + let addr = get_socket_addr(address)?; + let socket = UdpSocket::bind("0.0.0.0:0").unwrap(); + socket + .set_broadcast(true) + .expect("Couldn't bind to broadcast address."); + UdpConnection::new(socket, false, Some(addr)) +} + +pub fn udpout(address: T) -> io::Result { + let addr = get_socket_addr(address)?; + let socket = UdpSocket::bind("0.0.0.0:0")?; + UdpConnection::new(socket, false, Some(addr)) +} + +pub fn udpin(address: T) -> io::Result { + let addr = address + .to_socket_addrs() + .unwrap() + .next() + .expect("Invalid address"); + let socket = UdpSocket::bind(addr)?; + UdpConnection::new(socket, true, None) +} + +struct UdpRead { + socket: UdpSocket, + buffer: VecDeque, + last_recv_address: Option, +} + +const MTU_SIZE: usize = 1500; +impl Read for UdpRead { + fn read(&mut self, buf: &mut [u8]) -> io::Result { + if !self.buffer.is_empty() { + self.buffer.read(buf) + } else { + let mut read_buffer = [0u8; MTU_SIZE]; + let (n_buffer, address) = self.socket.recv_from(&mut read_buffer)?; + let n = (&read_buffer[0..n_buffer]).read(buf)?; + self.buffer.extend(&read_buffer[n..n_buffer]); + + self.last_recv_address = Some(address); + Ok(n) + } + } +} + +struct UdpWrite { + socket: UdpSocket, + dest: Option, + sequence: u8, +} + +pub struct UdpConnection { + reader: Mutex>, + writer: Mutex, + protocol_version: MavlinkVersion, + server: bool, + #[cfg(feature = "signing")] + signing_data: Option, +} + +impl UdpConnection { + fn new(socket: UdpSocket, server: bool, dest: Option) -> io::Result { + Ok(Self { + server, + reader: Mutex::new(PeekReader::new(UdpRead { + socket: socket.try_clone()?, + buffer: VecDeque::new(), + last_recv_address: None, + })), + writer: Mutex::new(UdpWrite { + socket, + dest, + sequence: 0, + }), + protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, + }) + } +} + +impl MavConnection for UdpConnection { + fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { + let mut reader = self.reader.lock().unwrap(); + + loop { + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg(reader.deref_mut(), self.protocol_version); + #[cfg(feature = "signing")] + let result = read_versioned_msg_signed( + reader.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ); + if self.server { + if let addr @ Some(_) = reader.reader_ref().last_recv_address { + self.writer.lock().unwrap().dest = addr; + } + } + if let ok @ Ok(..) = result { + return ok; + } + } + } + + fn send(&self, header: &MavHeader, data: &M) -> Result { + let mut guard = self.writer.lock().unwrap(); + let state = &mut *guard; + + let header = MavHeader { + sequence: state.sequence, + system_id: header.system_id, + component_id: header.component_id, + }; + + state.sequence = state.sequence.wrapping_add(1); + + let len = if let Some(addr) = state.dest { + let mut buf = Vec::new(); + #[cfg(not(feature = "signing"))] + write_versioned_msg(&mut buf, self.protocol_version, header, data)?; + #[cfg(feature = "signing")] + write_versioned_msg_signed( + &mut buf, + self.protocol_version, + header, + data, + self.signing_data.as_ref(), + )?; + state.socket.send_to(&buf, addr)? + } else { + 0 + }; + + Ok(len) + } + + fn set_protocol_version(&mut self, version: MavlinkVersion) { + self.protocol_version = version; + } + + fn 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) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_datagram_buffering() { + let receiver_socket = UdpSocket::bind("127.0.0.1:5000").unwrap(); + let mut udp_reader = UdpRead { + socket: receiver_socket.try_clone().unwrap(), + buffer: VecDeque::new(), + last_recv_address: None, + }; + let sender_socket = UdpSocket::bind("0.0.0.0:0").unwrap(); + sender_socket.connect("127.0.0.1:5000").unwrap(); + + let datagram: Vec = (0..50).collect::>(); + + let mut n_sent = sender_socket.send(&datagram).unwrap(); + assert_eq!(n_sent, datagram.len()); + n_sent = sender_socket.send(&datagram).unwrap(); + assert_eq!(n_sent, datagram.len()); + + let mut buf = [0u8; 30]; + + let mut n_read = udp_reader.read(&mut buf).unwrap(); + assert_eq!(n_read, 30); + assert_eq!(&buf[0..n_read], (0..30).collect::>().as_slice()); + + n_read = udp_reader.read(&mut buf).unwrap(); + assert_eq!(n_read, 20); + assert_eq!(&buf[0..n_read], (30..50).collect::>().as_slice()); + + n_read = udp_reader.read(&mut buf).unwrap(); + assert_eq!(n_read, 30); + assert_eq!(&buf[0..n_read], (0..30).collect::>().as_slice()); + + n_read = udp_reader.read(&mut buf).unwrap(); + assert_eq!(n_read, 20); + assert_eq!(&buf[0..n_read], (30..50).collect::>().as_slice()); + } +} diff --git a/mavlink-core/src/embedded.rs b/mavlink-core/src/embedded.rs new file mode 100644 index 0000000000..96917e506c --- /dev/null +++ b/mavlink-core/src/embedded.rs @@ -0,0 +1,50 @@ +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_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 = nb::block!(self.read()).map_err(|_| MessageReadError::Io)?; + } + + Ok(()) + } +} + +/// Replacement for std::io::Write + byteorder::WriteBytesExt in no_std envs +pub trait Write { + fn write_all(&mut self, buf: &[u8]) -> Result<(), MessageWriteError>; +} + +#[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)?; + } + + Ok(()) + } +} diff --git a/src/error.rs b/mavlink-core/src/error.rs similarity index 67% rename from src/error.rs rename to mavlink-core/src/error.rs index 3aece59b30..49394f8d64 100644 --- a/src/error.rs +++ b/mavlink-core/src/error.rs @@ -2,30 +2,25 @@ use core::fmt::{Display, Formatter}; #[cfg(feature = "std")] use std::error::Error; -#[cfg(not(feature = "std"))] -use alloc::string::String; - #[derive(Debug)] pub enum ParserError { - InvalidFlag { flag_type: String, value: u32 }, - InvalidEnum { enum_type: String, value: u32 }, + InvalidFlag { flag_type: &'static str, value: u32 }, + InvalidEnum { enum_type: &'static str, value: u32 }, UnknownMessage { id: u32 }, } impl Display for ParserError { fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { match self { - ParserError::InvalidFlag { flag_type, value } => write!( + Self::InvalidFlag { flag_type, value } => write!( f, - "Invalid flag value for flag type {:?}, got {:?}", - flag_type, value + "Invalid flag value for flag type {flag_type:?}, got {value:?}" ), - ParserError::InvalidEnum { enum_type, value } => write!( + Self::InvalidEnum { enum_type, value } => write!( f, - "Invalid enum value for enum type {:?}, got {:?}", - enum_type, value + "Invalid enum value for enum type {enum_type:?}, got {value:?}" ), - ParserError::UnknownMessage { id } => write!(f, "Unknown message with ID {:?}", id), + Self::UnknownMessage { id } => write!(f, "Unknown message with ID {id:?}"), } } } @@ -37,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), } @@ -46,10 +41,10 @@ impl Display for MessageReadError { fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { match self { #[cfg(feature = "std")] - Self::Io(e) => write!(f, "Failed to read message: {:#?}", e), - #[cfg(feature = "embedded")] + Self::Io(e) => write!(f, "Failed to read message: {e:#?}"), + #[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), + Self::Parse(e) => write!(f, "Failed to read message: {e:#?}"), } } } @@ -74,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, } @@ -82,8 +77,8 @@ impl Display for MessageWriteError { fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { match self { #[cfg(feature = "std")] - Self::Io(e) => write!(f, "Failed to write message: {:#?}", e), - #[cfg(feature = "embedded")] + Self::Io(e) => write!(f, "Failed to write message: {e:#?}"), + #[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 new file mode 100644 index 0000000000..2b5626a263 --- /dev/null +++ b/mavlink-core/src/lib.rs @@ -0,0 +1,1424 @@ +//! The MAVLink message set. +//! +//! # Message sets and the `Message` trait +//! Each message set has its own module with corresponding data types, including a `MavMessage` enum +//! that represents all possible messages in that message set. The [`Message`] trait is used to +//! represent messages in an abstract way, and each `MavMessage` enum implements this trait (for +//! example, [`ardupilotmega::MavMessage`]). This is then monomorphized to the specific message +//! set you are using in your application at compile-time via type parameters. If you expect +//! ArduPilotMega-flavored messages, then you will need a `MavConnection` +//! and you will receive `ardupilotmega::MavMessage`s from it. +//! +//! Some message sets include others. For example, all message sets except `common` include the +//! common message set. This is represented with extra values in the `MavMessage` enum: a message +//! in the common message set received on an ArduPilotMega connection will be an +//! `ardupilotmega::MavMessage::common(common::MavMessage)`. +//! +//! Please note that if you want to enable a given message set, you must also enable the +//! feature for the message sets that it includes. For example, you cannot use the `ardupilotmega` +//! feature without also using the `uavionix` and `icarous` features. +//! +#![cfg_attr(not(feature = "std"), no_std)] +#![cfg_attr(all(any(docsrs, doc), not(doctest)), feature(doc_auto_cfg))] +#![deny(clippy::all)] +#![warn(clippy::use_self)] + +use core::result::Result; + +#[cfg(feature = "std")] +use std::io::{Read, Write}; + +pub mod utils; +#[allow(unused_imports)] +use utils::{remove_trailing_zeroes, RustDefault}; + +#[cfg(feature = "serde")] +use serde::{Deserialize, Serialize}; + +pub mod peek_reader; +use peek_reader::PeekReader; + +use crate::{bytes::Bytes, error::ParserError}; + +use crc_any::CRCu16; + +pub mod bytes; +pub mod bytes_mut; +#[cfg(feature = "std")] +mod connection; +pub mod error; +#[cfg(feature = "std")] +pub use self::connection::{connect, MavConnection}; + +#[cfg(feature = "tokio-1")] +mod async_connection; +#[cfg(feature = "tokio-1")] +pub use self::async_connection::{connect_async, AsyncMavConnection}; + +#[cfg(feature = "tokio-1")] +pub mod async_peek_reader; +#[cfg(feature = "tokio-1")] +use async_peek_reader::AsyncPeekReader; + +#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] +pub mod embedded; +#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] +use embedded::{Read, Write}; + +#[cfg(not(feature = "signing"))] +type SigningData = (); +#[cfg(feature = "signing")] +mod signing; +#[cfg(feature = "signing")] +pub use self::signing::{SigningConfig, SigningData}; +#[cfg(feature = "signing")] +use sha2::{Digest, Sha256}; + +pub const MAX_FRAME_SIZE: usize = 280; + +pub trait Message +where + Self: Sized, +{ + fn message_id(&self) -> u32; + fn message_name(&self) -> &'static str; + + /// Serialize **Message** into byte slice and return count of bytes written + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize; + + fn parse( + version: MavlinkVersion, + msgid: u32, + payload: &[u8], + ) -> Result; + + fn message_id_from_name(name: &str) -> Result; + fn default_message_from_id(id: u32) -> Result; + fn extra_crc(id: u32) -> u8; +} + +pub trait MessageData: Sized { + type Message: Message; + + const ID: u32; + const NAME: &'static str; + const EXTRA_CRC: u8; + const ENCODED_LEN: usize; + + fn ser(&self, version: MavlinkVersion, payload: &mut [u8]) -> usize; + fn deser(version: MavlinkVersion, payload: &[u8]) -> Result; +} + +/// Metadata from a MAVLink packet header +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub struct MavHeader { + pub system_id: u8, + pub component_id: u8, + pub sequence: u8, +} + +/// Versions of the Mavlink protocol that we support +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "serde", derive(Serialize))] +#[cfg_attr(feature = "serde", serde(tag = "type"))] +pub enum MavlinkVersion { + V1, + V2, +} + +/// Message framing marker for mavlink v1 +pub const MAV_STX: u8 = 0xFE; + +/// Message framing marker for mavlink v2 +pub const MAV_STX_V2: u8 = 0xFD; + +/// Return a default GCS header, seq is replaced by the connector +/// so it can be ignored. Set `component_id` to your desired component ID. +impl Default for MavHeader { + fn default() -> Self { + Self { + system_id: 255, + component_id: 0, + sequence: 0, + } + } +} + +/// Encapsulation of the Mavlink message and the header, +/// important to preserve information about the sender system +/// and component id. +#[derive(Debug, Clone)] +#[cfg_attr(feature = "serde", derive(Serialize))] +pub struct MavFrame { + pub header: MavHeader, + pub msg: M, + pub protocol_version: MavlinkVersion, +} + +impl MavFrame { + /// Create a new frame with given message + // pub fn new(msg: MavMessage) -> MavFrame { + // MavFrame { + // header: MavHeader::get_default_header(), + // msg + // } + // } + + /// Serialize MavFrame into a vector, so it can be sent over a socket, for example. + /// The resulting buffer will start with the sequence field of the Mavlink frame + /// and will not include the initial packet marker, length field, and flags. + pub fn ser(&self, buf: &mut [u8]) -> usize { + let mut buf = bytes_mut::BytesMut::new(buf); + + // serialize message + let mut payload_buf = [0u8; 255]; + let payload_len = self.msg.ser(self.protocol_version, &mut payload_buf); + + // Currently expects a buffer with the sequence field at the start. + // If this is updated to include the initial packet marker, length field, and flags, + // uncomment. + // + // match self.protocol_version { + // MavlinkVersion::V2 => { + // buf.put_u8(MAV_STX_V2); + // buf.put_u8(payload_len as u8); + // but.put_u8(0); // incompatibility flags + // buf.put_u8(0); // compatibility flags + // } + // MavlinkVersion::V1 => { + // buf.put_u8(MAV_STX); + // buf.put_u8(payload_len as u8); + // } + // } + + // serialize header + buf.put_u8(self.header.sequence); + buf.put_u8(self.header.system_id); + buf.put_u8(self.header.component_id); + + // message id + match self.protocol_version { + MavlinkVersion::V2 => { + let bytes: [u8; 4] = self.msg.message_id().to_le_bytes(); + buf.put_slice(&bytes[..3]); + } + MavlinkVersion::V1 => { + buf.put_u8(self.msg.message_id() as u8); //TODO check + } + } + + buf.put_slice(&payload_buf[..payload_len]); + buf.len() + } + + /// Deserialize MavFrame from a slice that has been received from, for example, a socket. + /// The input buffer should start with the sequence field of the Mavlink frame. The + /// initial packet marker, length field, and flag fields should be excluded. + pub fn deser(version: MavlinkVersion, input: &[u8]) -> Result { + let mut buf = Bytes::new(input); + + // Currently expects a buffer with the sequence field at the start. + // If this is updated to include the initial packet marker, length field, and flags, + // uncomment. + // + // match version { + // MavlinkVersion::V2 => buf.get_u32_le(), + // MavlinkVersion::V1 => buf.get_u16_le().into(), + // }; + + let sequence = buf.get_u8(); + let system_id = buf.get_u8(); + let component_id = buf.get_u8(); + let header = MavHeader { + system_id, + component_id, + sequence, + }; + + let msg_id = match version { + MavlinkVersion::V2 => buf.get_u24_le(), + MavlinkVersion::V1 => buf.get_u8().into(), + }; + + M::parse(version, msg_id, buf.remaining_bytes()).map(|msg| Self { + header, + msg, + protocol_version: version, + }) + } + + /// Return the frame header + pub fn header(&self) -> MavHeader { + self.header + } +} + +pub fn calculate_crc(data: &[u8], extra_crc: u8) -> u16 { + let mut crc_calculator = CRCu16::crc16mcrf4cc(); + crc_calculator.digest(data); + + crc_calculator.digest(&[extra_crc]); + crc_calculator.get_crc() +} + +pub fn read_versioned_msg( + r: &mut PeekReader, + version: MavlinkVersion, +) -> Result<(MavHeader, M), error::MessageReadError> { + match version { + MavlinkVersion::V2 => read_v2_msg(r), + MavlinkVersion::V1 => read_v1_msg(r), + } +} + +#[cfg(feature = "tokio-1")] +pub async fn read_versioned_msg_async( + r: &mut AsyncPeekReader, + version: MavlinkVersion, +) -> Result<(MavHeader, M), error::MessageReadError> { + match version { + MavlinkVersion::V2 => read_v2_msg_async(r).await, + MavlinkVersion::V1 => read_v1_msg_async(r).await, + } +} + +#[cfg(feature = "signing")] +pub fn read_versioned_msg_signed( + r: &mut PeekReader, + version: MavlinkVersion, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + match version { + MavlinkVersion::V2 => read_v2_msg_inner(r, signing_data), + MavlinkVersion::V1 => read_v1_msg(r), + } +} + +#[cfg(all(feature = "tokio-1", feature = "signing"))] +pub async fn read_versioned_msg_async_signed( + r: &mut AsyncPeekReader, + version: MavlinkVersion, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + match version { + MavlinkVersion::V2 => read_v2_msg_async_inner(r, signing_data).await, + MavlinkVersion::V1 => read_v1_msg_async(r).await, + } +} + +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +// Follow protocol definition: `` +pub struct MAVLinkV1MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2]); + +impl Default for MAVLinkV1MessageRaw { + fn default() -> Self { + Self::new() + } +} + +impl MAVLinkV1MessageRaw { + const HEADER_SIZE: usize = 5; + + pub const fn new() -> Self { + Self([0; 1 + Self::HEADER_SIZE + 255 + 2]) + } + + #[inline] + pub fn header(&self) -> &[u8] { + &self.0[1..=Self::HEADER_SIZE] + } + + #[inline] + fn mut_header(&mut self) -> &mut [u8] { + &mut self.0[1..=Self::HEADER_SIZE] + } + + #[inline] + pub fn payload_length(&self) -> u8 { + self.0[1] + } + + #[inline] + pub fn sequence(&self) -> u8 { + self.0[2] + } + + #[inline] + pub fn system_id(&self) -> u8 { + self.0[3] + } + + #[inline] + pub fn component_id(&self) -> u8 { + self.0[4] + } + + #[inline] + pub fn message_id(&self) -> u8 { + self.0[5] + } + + #[inline] + pub fn payload(&self) -> &[u8] { + let payload_length: usize = self.payload_length().into(); + &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)] + } + + #[inline] + pub fn checksum(&self) -> u16 { + let payload_length: usize = self.payload_length().into(); + u16::from_le_bytes([ + self.0[1 + Self::HEADER_SIZE + payload_length], + self.0[1 + Self::HEADER_SIZE + payload_length + 1], + ]) + } + + #[inline] + fn mut_payload_and_checksum(&mut self) -> &mut [u8] { + let payload_length: usize = self.payload_length().into(); + &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + 2)] + } + + #[inline] + pub fn has_valid_crc(&self) -> bool { + let payload_length: usize = self.payload_length().into(); + self.checksum() + == calculate_crc( + &self.0[1..(1 + Self::HEADER_SIZE + payload_length)], + M::extra_crc(self.message_id().into()), + ) + } + + pub fn raw_bytes(&self) -> &[u8] { + let payload_length = self.payload_length() as usize; + &self.0[..(1 + Self::HEADER_SIZE + payload_length + 2)] + } + + fn serialize_stx_and_header_and_crc( + &mut self, + header: MavHeader, + msgid: u32, + payload_length: usize, + extra_crc: u8, + ) { + self.0[0] = MAV_STX; + + let header_buf = self.mut_header(); + header_buf.copy_from_slice(&[ + payload_length as u8, + header.sequence, + header.system_id, + header.component_id, + msgid as u8, + ]); + + let crc = calculate_crc( + &self.0[1..(1 + Self::HEADER_SIZE + payload_length)], + extra_crc, + ); + self.0[(1 + Self::HEADER_SIZE + payload_length) + ..(1 + Self::HEADER_SIZE + payload_length + 2)] + .copy_from_slice(&crc.to_le_bytes()); + } + + pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message.ser(MavlinkVersion::V1, payload_buf); + + let message_id = message.message_id(); + self.serialize_stx_and_header_and_crc( + header, + message_id, + payload_length, + M::extra_crc(message_id), + ); + } + + pub fn serialize_message_data(&mut self, header: MavHeader, message_data: &D) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message_data.ser(MavlinkVersion::V1, payload_buf); + + self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC); + } +} + +/// Return a raw buffer with the mavlink message +/// V1 maximum size is 263 bytes: `` +pub fn read_v1_raw_message( + reader: &mut PeekReader, +) -> Result { + loop { + loop { + // search for the magic framing value indicating start of mavlink message + if reader.read_u8()? == MAV_STX { + break; + } + } + + let mut message = MAVLinkV1MessageRaw::new(); + + message.0[0] = MAV_STX; + let header = &reader.peek_exact(MAVLinkV1MessageRaw::HEADER_SIZE)? + [..MAVLinkV1MessageRaw::HEADER_SIZE]; + message.mut_header().copy_from_slice(header); + let packet_length = message.raw_bytes().len() - 1; + let payload_and_checksum = + &reader.peek_exact(packet_length)?[MAVLinkV1MessageRaw::HEADER_SIZE..packet_length]; + message + .mut_payload_and_checksum() + .copy_from_slice(payload_and_checksum); + + // retry if CRC failed after previous STX + // (an STX byte may appear in the middle of a message) + if message.has_valid_crc::() { + reader.consume(message.raw_bytes().len() - 1); + return Ok(message); + } + } +} + +/// Return a raw buffer with the mavlink message +/// V1 maximum size is 263 bytes: `` +#[cfg(feature = "tokio-1")] +pub async fn read_v1_raw_message_async( + reader: &mut AsyncPeekReader, +) -> Result { + loop { + loop { + // search for the magic framing value indicating start of mavlink message + if reader.read_u8().await? == MAV_STX { + break; + } + } + + let mut message = MAVLinkV1MessageRaw::new(); + + message.0[0] = MAV_STX; + let header = &reader.peek_exact(MAVLinkV1MessageRaw::HEADER_SIZE).await? + [..MAVLinkV1MessageRaw::HEADER_SIZE]; + message.mut_header().copy_from_slice(header); + let packet_length = message.raw_bytes().len() - 1; + let payload_and_checksum = &reader.peek_exact(packet_length).await? + [MAVLinkV1MessageRaw::HEADER_SIZE..packet_length]; + message + .mut_payload_and_checksum() + .copy_from_slice(payload_and_checksum); + + // retry if CRC failed after previous STX + // (an STX byte may appear in the middle of a message) + if message.has_valid_crc::() { + reader.consume(message.raw_bytes().len() - 1); + return Ok(message); + } + } +} + +/// 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> { + let message = read_v1_raw_message::(r)?; + + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse( + MavlinkVersion::V1, + u32::from(message.message_id()), + message.payload(), + )?, + )) +} + +/// Read a MAVLink v1 message from a Read stream. +#[cfg(feature = "tokio-1")] +pub async fn read_v1_msg_async( + r: &mut AsyncPeekReader, +) -> Result<(MavHeader, M), error::MessageReadError> { + let message = read_v1_raw_message_async::(r).await?; + + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse( + MavlinkVersion::V1, + u32::from(message.message_id()), + message.payload(), + )?, + )) +} + +/// 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?; + + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse( + MavlinkVersion::V1, + u32::from(message.message_id()), + message.payload(), + )?, + )) +} + +const MAVLINK_IFLAG_SIGNED: u8 = 0x01; +const MAVLINK_SUPPORTED_IFLAGS: u8 = MAVLINK_IFLAG_SIGNED; + +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +// Follow protocol definition: `` +pub struct MAVLinkV2MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE]); + +impl Default for MAVLinkV2MessageRaw { + fn default() -> Self { + Self::new() + } +} + +impl MAVLinkV2MessageRaw { + const HEADER_SIZE: usize = 9; + const SIGNATURE_SIZE: usize = 13; + + pub const fn new() -> Self { + Self([0; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE]) + } + + #[inline] + pub fn header(&self) -> &[u8] { + &self.0[1..=Self::HEADER_SIZE] + } + + #[inline] + fn mut_header(&mut self) -> &mut [u8] { + &mut self.0[1..=Self::HEADER_SIZE] + } + + #[inline] + pub fn payload_length(&self) -> u8 { + self.0[1] + } + + #[inline] + pub fn incompatibility_flags(&self) -> u8 { + self.0[2] + } + + #[inline] + pub fn incompatibility_flags_mut(&mut self) -> &mut u8 { + &mut self.0[2] + } + + #[inline] + pub fn compatibility_flags(&self) -> u8 { + self.0[3] + } + + #[inline] + pub fn sequence(&self) -> u8 { + self.0[4] + } + + #[inline] + pub fn system_id(&self) -> u8 { + self.0[5] + } + + #[inline] + pub fn component_id(&self) -> u8 { + self.0[6] + } + + #[inline] + pub fn message_id(&self) -> u32 { + u32::from_le_bytes([self.0[7], self.0[8], self.0[9], 0]) + } + + #[inline] + pub fn payload(&self) -> &[u8] { + let payload_length: usize = self.payload_length().into(); + &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)] + } + + #[inline] + pub fn checksum(&self) -> u16 { + let payload_length: usize = self.payload_length().into(); + u16::from_le_bytes([ + self.0[1 + Self::HEADER_SIZE + payload_length], + self.0[1 + Self::HEADER_SIZE + payload_length + 1], + ]) + } + + #[cfg(feature = "signing")] + #[inline] + pub fn checksum_bytes(&self) -> &[u8] { + let checksum_offset = 1 + Self::HEADER_SIZE + self.payload_length() as usize; + &self.0[checksum_offset..(checksum_offset + 2)] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_link_id(&self) -> u8 { + let payload_length: usize = self.payload_length().into(); + self.0[1 + Self::HEADER_SIZE + payload_length + 2] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_link_id_mut(&mut self) -> &mut u8 { + let payload_length: usize = self.payload_length().into(); + &mut self.0[1 + Self::HEADER_SIZE + payload_length + 2] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_timestamp_bytes(&self) -> &[u8] { + let payload_length: usize = self.payload_length().into(); + let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3; + &self.0[timestamp_start..(timestamp_start + 6)] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_timestamp_bytes_mut(&mut self) -> &mut [u8] { + let payload_length: usize = self.payload_length().into(); + let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3; + &mut self.0[timestamp_start..(timestamp_start + 6)] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_timestamp(&self) -> u64 { + let mut timestamp_bytes = [0u8; 8]; + timestamp_bytes[0..6].copy_from_slice(self.signature_timestamp_bytes()); + u64::from_le_bytes(timestamp_bytes) + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_value(&self) -> &[u8] { + let payload_length: usize = self.payload_length().into(); + let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6; + &self.0[signature_start..(signature_start + 6)] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_value_mut(&mut self) -> &mut [u8] { + let payload_length: usize = self.payload_length().into(); + let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6; + &mut self.0[signature_start..(signature_start + 6)] + } + + fn mut_payload_and_checksum_and_sign(&mut self) -> &mut [u8] { + let payload_length: usize = self.payload_length().into(); + + // Signature to ensure the link is tamper-proof. + let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) == 0 { + 0 + } else { + Self::SIGNATURE_SIZE + }; + + &mut self.0 + [(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)] + } + + #[inline] + pub fn has_valid_crc(&self) -> bool { + let payload_length: usize = self.payload_length().into(); + self.checksum() + == calculate_crc( + &self.0[1..(1 + Self::HEADER_SIZE + payload_length)], + M::extra_crc(self.message_id()), + ) + } + + #[cfg(feature = "signing")] + pub fn calculate_signature(&self, secret_key: &[u8], target_buffer: &mut [u8; 6]) { + let mut hasher = Sha256::new(); + hasher.update(secret_key); + hasher.update([MAV_STX_V2]); + hasher.update(self.header()); + hasher.update(self.payload()); + hasher.update(self.checksum_bytes()); + hasher.update([self.signature_link_id()]); + hasher.update(self.signature_timestamp_bytes()); + target_buffer.copy_from_slice(&hasher.finalize()[0..6]); + } + + pub fn raw_bytes(&self) -> &[u8] { + let payload_length = self.payload_length() as usize; + + let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) == 0 { + 0 + } else { + Self::SIGNATURE_SIZE + }; + + &self.0[..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)] + } + + fn serialize_stx_and_header_and_crc( + &mut self, + header: MavHeader, + msgid: u32, + payload_length: usize, + extra_crc: u8, + incompat_flags: u8, + ) { + self.0[0] = MAV_STX_V2; + let msgid_bytes = msgid.to_le_bytes(); + + let header_buf = self.mut_header(); + header_buf.copy_from_slice(&[ + payload_length as u8, + incompat_flags, + 0, //compat_flags + header.sequence, + header.system_id, + header.component_id, + msgid_bytes[0], + msgid_bytes[1], + msgid_bytes[2], + ]); + + let crc = calculate_crc( + &self.0[1..(1 + Self::HEADER_SIZE + payload_length)], + extra_crc, + ); + self.0[(1 + Self::HEADER_SIZE + payload_length) + ..(1 + Self::HEADER_SIZE + payload_length + 2)] + .copy_from_slice(&crc.to_le_bytes()); + } + + pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message.ser(MavlinkVersion::V2, payload_buf); + + let message_id = message.message_id(); + self.serialize_stx_and_header_and_crc( + header, + message_id, + payload_length, + M::extra_crc(message_id), + 0, + ); + } + + pub fn serialize_message_for_signing(&mut self, header: MavHeader, message: &M) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message.ser(MavlinkVersion::V2, payload_buf); + + let message_id = message.message_id(); + self.serialize_stx_and_header_and_crc( + header, + message_id, + payload_length, + M::extra_crc(message_id), + 0x01, + ); + } + + pub fn serialize_message_data(&mut self, header: MavHeader, message_data: &D) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message_data.ser(MavlinkVersion::V2, payload_buf); + + self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC, 0); + } +} + +/// Return a raw buffer with the mavlink message +/// +/// V2 maximum size is 280 bytes: `` +#[inline] +pub fn read_v2_raw_message( + reader: &mut PeekReader, +) -> Result { + read_v2_raw_message_inner::(reader, None) +} + +/// Return a raw buffer with the mavlink message with signing support +/// +/// V2 maximum size is 280 bytes: `` +#[cfg(feature = "signing")] +#[inline] +pub fn read_v2_raw_message_signed( + reader: &mut PeekReader, + signing_data: Option<&SigningData>, +) -> Result { + read_v2_raw_message_inner::(reader, signing_data) +} + +fn read_v2_raw_message_inner( + reader: &mut PeekReader, + signing_data: Option<&SigningData>, +) -> Result { + loop { + loop { + // search for the magic framing value indicating start of mavlink message + if reader.read_u8()? == MAV_STX_V2 { + break; + } + } + + let mut message = MAVLinkV2MessageRaw::new(); + + message.0[0] = MAV_STX_V2; + let header = &reader.peek_exact(MAVLinkV2MessageRaw::HEADER_SIZE)? + [..MAVLinkV2MessageRaw::HEADER_SIZE]; + message.mut_header().copy_from_slice(header); + + if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 { + // if there are incompatibility flags set that we do not know discard the message + continue; + } + + let packet_length = message.raw_bytes().len() - 1; + let payload_and_checksum_and_sign = + &reader.peek_exact(packet_length)?[MAVLinkV2MessageRaw::HEADER_SIZE..packet_length]; + message + .mut_payload_and_checksum_and_sign() + .copy_from_slice(payload_and_checksum_and_sign); + + if message.has_valid_crc::() { + // even if the signature turn out to be invalid the valid crc shows that the received data presents a valid message as opposed to random bytes + reader.consume(message.raw_bytes().len() - 1); + } else { + continue; + } + + #[cfg(feature = "signing")] + if let Some(signing_data) = signing_data { + if !signing_data.verify_signature(&message) { + continue; + } + } + + return Ok(message); + } +} + +/// Async read a raw buffer with the mavlink message +/// V2 maximum size is 280 bytes: `` +#[cfg(feature = "tokio-1")] +pub async fn read_v2_raw_message_async( + reader: &mut AsyncPeekReader, +) -> Result { + read_v2_raw_message_async_inner::(reader, None).await +} + +/// Async read a raw buffer with the mavlink message +/// V2 maximum size is 280 bytes: `` +#[cfg(feature = "tokio-1")] +async fn read_v2_raw_message_async_inner( + reader: &mut AsyncPeekReader, + signing_data: Option<&SigningData>, +) -> Result { + loop { + loop { + // search for the magic framing value indicating start of mavlink message + if reader.read_u8().await? == MAV_STX_V2 { + break; + } + } + + let mut message = MAVLinkV2MessageRaw::new(); + + message.0[0] = MAV_STX_V2; + let header = &reader.peek_exact(MAVLinkV2MessageRaw::HEADER_SIZE).await? + [..MAVLinkV2MessageRaw::HEADER_SIZE]; + message.mut_header().copy_from_slice(header); + + if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 { + // if there are incompatibility flags set that we do not know discard the message + continue; + } + + let packet_length = message.raw_bytes().len() - 1; + let payload_and_checksum_and_sign = &reader.peek_exact(packet_length).await? + [MAVLinkV2MessageRaw::HEADER_SIZE..packet_length]; + message + .mut_payload_and_checksum_and_sign() + .copy_from_slice(payload_and_checksum_and_sign); + + if message.has_valid_crc::() { + // even if the signature turn out to be invalid the valid crc shows that the received data presents a valid message as opposed to random bytes + reader.consume(message.raw_bytes().len() - 1); + } else { + continue; + } + + #[cfg(feature = "signing")] + if let Some(signing_data) = signing_data { + if !signing_data.verify_signature(&message) { + continue; + } + } + + return Ok(message); + } +} + +/// Async read a raw buffer with the mavlink message with signing support +/// V2 maximum size is 280 bytes: `` +#[cfg(all(feature = "tokio-1", feature = "signing"))] +pub async fn read_v2_raw_message_async_signed( + reader: &mut AsyncPeekReader, + signing_data: Option<&SigningData>, +) -> Result { + read_v2_raw_message_async_inner::(reader, signing_data).await +} + +/// 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_V2; + reader + .read_exact(message.mut_header()) + .await + .map_err(|_| error::MessageReadError::Io)?; + + if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 { + // if there are incompatibility flags set that we do not know discard the message + continue; + } + + 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. +#[inline] +pub fn read_v2_msg( + read: &mut PeekReader, +) -> Result<(MavHeader, M), error::MessageReadError> { + read_v2_msg_inner(read, None) +} + +/// Read a MAVLink v2 message from a Read stream. +#[cfg(feature = "signing")] +#[inline] +pub fn read_v2_msg_signed( + read: &mut PeekReader, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + read_v2_msg_inner(read, signing_data) +} + +fn read_v2_msg_inner( + read: &mut PeekReader, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + let message = read_v2_raw_message_inner::(read, signing_data)?; + + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?, + )) +} + +/// Async read a MAVLink v2 message from a Read stream. +#[cfg(feature = "tokio-1")] +pub async fn read_v2_msg_async( + read: &mut AsyncPeekReader, +) -> Result<(MavHeader, M), error::MessageReadError> { + read_v2_msg_async_inner(read, None).await +} + +/// Async read a MAVLink v2 message from a Read stream. +#[cfg(all(feature = "tokio-1", feature = "signing"))] +pub async fn read_v2_msg_async_signed( + read: &mut AsyncPeekReader, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + read_v2_msg_async_inner(read, signing_data).await +} + +#[cfg(feature = "tokio-1")] +async fn read_v2_msg_async_inner( + read: &mut AsyncPeekReader, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + let message = read_v2_raw_message_async_inner::(read, signing_data).await?; + + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?, + )) +} + +/// 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?; + + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse( + MavlinkVersion::V2, + u32::from(message.message_id()), + message.payload(), + )?, + )) +} + +/// Write a message using the given mavlink version +pub fn write_versioned_msg( + w: &mut W, + version: MavlinkVersion, + header: MavHeader, + data: &M, +) -> Result { + match version { + MavlinkVersion::V2 => write_v2_msg(w, header, data), + MavlinkVersion::V1 => write_v1_msg(w, header, data), + } +} + +/// Write a message with signing support using the given mavlink version +#[cfg(feature = "signing")] +pub fn write_versioned_msg_signed( + w: &mut W, + version: MavlinkVersion, + header: MavHeader, + data: &M, + signing_data: Option<&SigningData>, +) -> Result { + match version { + MavlinkVersion::V2 => write_v2_msg_signed(w, header, data, signing_data), + MavlinkVersion::V1 => write_v1_msg(w, header, data), + } +} + +/// Async write a message using the given mavlink version +#[cfg(feature = "tokio-1")] +pub async fn write_versioned_msg_async( + w: &mut W, + 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, + } +} + +/// Async write a message with signing support using the given mavlink version +#[cfg(all(feature = "tokio-1", feature = "signing"))] +pub async fn write_versioned_msg_async_signed( + w: &mut W, + version: MavlinkVersion, + header: MavHeader, + data: &M, + signing_data: Option<&SigningData>, +) -> Result { + match version { + MavlinkVersion::V2 => write_v2_msg_async_signed(w, header, data, signing_data).await, + MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await, + } +} + +/// 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, + 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])?; + + Ok(len) +} + +/// Write a MAVLink v2 message to a Write stream with signing support. +#[cfg(feature = "signing")] +pub fn write_v2_msg_signed( + w: &mut W, + header: MavHeader, + data: &M, + signing_data: Option<&SigningData>, +) -> Result { + let mut message_raw = MAVLinkV2MessageRaw::new(); + + let signature_len = if let Some(signing_data) = signing_data { + if signing_data.config.sign_outgoing { + message_raw.serialize_message_for_signing(header, data); + signing_data.sign_message(&mut message_raw); + MAVLinkV2MessageRaw::SIGNATURE_SIZE + } else { + message_raw.serialize_message(header, data); + 0 + } + } else { + message_raw.serialize_message(header, data); + 0 + }; + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2 + signature_len; + + w.write_all(&message_raw.0[..len])?; + + Ok(len) +} + +/// Async write a MAVLink v2 message to a Write stream. +#[cfg(feature = "tokio-1")] +pub async fn write_v2_msg_async( + w: &mut W, + 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?; + + Ok(len) +} + +/// Write a MAVLink v2 message to a Write stream with signing support. +#[cfg(feature = "signing")] +#[cfg(feature = "tokio-1")] +pub async fn write_v2_msg_async_signed( + w: &mut W, + header: MavHeader, + data: &M, + signing_data: Option<&SigningData>, +) -> Result { + let mut message_raw = MAVLinkV2MessageRaw::new(); + + let signature_len = if let Some(signing_data) = signing_data { + if signing_data.config.sign_outgoing { + message_raw.serialize_message_for_signing(header, data); + signing_data.sign_message(&mut message_raw); + MAVLinkV2MessageRaw::SIGNATURE_SIZE + } else { + message_raw.serialize_message(header, data); + 0 + } + } else { + message_raw.serialize_message(header, data); + 0 + }; + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2 + signature_len; + + w.write_all(&message_raw.0[..len]).await?; + + 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, + 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])?; + + Ok(len) +} + +/// Async write a MAVLink v1 message to a Write stream. +#[cfg(feature = "tokio-1")] +pub async fn write_v1_msg_async( + w: &mut W, + 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?; + + 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 new file mode 100644 index 0000000000..5a5a8a11c5 --- /dev/null +++ b/mavlink-core/src/peek_reader.rs @@ -0,0 +1,151 @@ +//! This module implements a buffered/peekable reader. +//! +//! The purpose of the buffered/peekable reader is to allow for backtracking parsers. +//! +//! A reader implementing the standard library's [`std::io::BufRead`] trait seems like a good fit, but +//! it does not allow for peeking a specific number of bytes, so it provides no way to request +//! more data from the underlying reader without consuming the existing data. +//! +//! This API still tries to adhere to the [`std::io::BufRead`]'s trait philosophy. +//! +//! The main type `PeekReader`does not implement [`std::io::Read`] itself, as there is no added benefit +//! in doing so. +//! +#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] +use crate::embedded::Read; + +#[cfg(feature = "std")] +use std::io::Read; + +use crate::error::MessageReadError; + +/// A buffered/peekable reader +/// +/// This reader wraps a type implementing [`std::io::Read`] and adds buffering via an internal buffer. +/// +/// It allows the user to `peek` a specified number of bytes (without consuming them), +/// to `read` bytes (consuming them), or to `consume` them after `peek`ing. +/// +/// NOTE: This reader is generic over the size of the buffer, defaulting to MAVLink's current largest +/// possible message size of 280 bytes +/// +pub struct PeekReader { + // Internal buffer + buffer: [u8; BUFFER_SIZE], + // The position of the next byte to read from the buffer. + cursor: usize, + // The position of the next byte to read into the buffer. + top: usize, + // The wrapped reader. + reader: R, +} + +impl PeekReader { + /// Instantiates a new [`PeekReader`], wrapping the provided [`std::io::Read`]er and using the default chunk size + pub fn new(reader: R) -> Self { + Self { + buffer: [0; BUFFER_SIZE], + cursor: 0, + top: 0, + reader, + } + } + + /// Peeks an exact amount of bytes from the internal buffer + /// + /// If the internal buffer does not contain enough data, this function will read + /// from the underlying [`std::io::Read`]er until it does, an error occurs or no more data can be read (EOF). + /// + /// If an EOF occurs and the specified amount could not be read, this function will return an [`ErrorKind::UnexpectedEof`]. + /// + /// This function does not consume data from the buffer, so subsequent calls to `peek` or `read` functions + /// will still return the peeked data. + /// + pub fn peek_exact(&mut self, amount: usize) -> Result<&[u8], MessageReadError> { + let result = self.fetch(amount, false); + result + } + + /// Reads a specified amount of bytes from the internal buffer + /// + /// If the internal buffer does not contain enough data, this function will read + /// from the underlying [`std::io::Read`]er until it does, an error occurs or no more data can be read (EOF). + /// + /// If an EOF occurs and the specified amount could not be read, this function will return an [`ErrorKind::UnexpectedEof`]. + /// + /// This function consumes the data from the buffer, unless an error occurs, in which case no data is consumed. + /// + pub fn read_exact(&mut self, amount: usize) -> Result<&[u8], MessageReadError> { + self.fetch(amount, true) + } + + /// Reads a byte from the internal buffer + /// + /// If the internal buffer does not contain enough data, this function will read + /// from the underlying [`std::io::Read`]er until it does, an error occurs or no more data can be read (EOF). + /// + /// If an EOF occurs and the specified amount could not be read, this function will return an [`ErrorKind::UnexpectedEof`]. + /// + /// This function consumes the data from the buffer, unless an error occurs, in which case no data is consumed. + /// + pub fn read_u8(&mut self) -> Result { + let buf = self.read_exact(1)?; + Ok(buf[0]) + } + + /// Consumes a specified amount of bytes from the buffer + /// + /// If the internal buffer does not contain enough data, this function will consume as much data as is buffered. + /// + pub fn consume(&mut self, amount: usize) -> usize { + let amount = amount.min(self.top - self.cursor); + self.cursor += amount; + amount + } + + /// Returns an immutable reference to the underlying [`std::io::Read`]er + /// + /// Reading directly from the underlying stream will cause data loss + pub fn reader_ref(&mut self) -> &R { + &self.reader + } + + /// Returns a mutable reference to the underlying [`std::io::Read`]er + /// + /// Reading directly from the underlying stream will cause data loss + pub fn reader_mut(&mut self) -> &mut R { + &mut self.reader + } + + /// Internal function to fetch data from the internal buffer and/or reader + fn fetch(&mut self, amount: usize, consume: bool) -> Result<&[u8], MessageReadError> { + let buffered = self.top - self.cursor; + + // the caller requested more bytes than we have buffered, fetch them from the reader + if buffered < amount { + let bytes_read = amount - buffered; + assert!(bytes_read < BUFFER_SIZE); + let mut buf = [0u8; BUFFER_SIZE]; + + // read needed bytes from reader + self.reader.read_exact(&mut buf[..bytes_read])?; + + // if some bytes were read, add them to the buffer + + if self.buffer.len() - self.top < bytes_read { + // reallocate + self.buffer.copy_within(self.cursor..self.top, 0); + self.cursor = 0; + self.top = buffered; + } + self.buffer[self.top..self.top + bytes_read].copy_from_slice(&buf[..bytes_read]); + self.top += bytes_read; + } + + let result = &self.buffer[self.cursor..self.cursor + amount]; + if consume { + self.cursor += amount; + } + Ok(result) + } +} diff --git a/mavlink-core/src/signing.rs b/mavlink-core/src/signing.rs new file mode 100644 index 0000000000..f1b129037c --- /dev/null +++ b/mavlink-core/src/signing.rs @@ -0,0 +1,139 @@ +use crate::MAVLinkV2MessageRaw; + +use std::time::SystemTime; +use std::{collections::HashMap, sync::Mutex}; + +use crate::MAVLINK_IFLAG_SIGNED; + +/// Configuration used for MAVLink 2 messages signing as defined in . +#[derive(Debug, Clone)] +pub struct SigningConfig { + secret_key: [u8; 32], + link_id: u8, + pub(crate) sign_outgoing: bool, + allow_unsigned: bool, +} + +// mutable state of signing per connection +pub(crate) struct SigningState { + timestamp: u64, + stream_timestamps: HashMap<(u8, u8, u8), u64>, +} + +/// MAVLink 2 message signing data. +pub struct SigningData { + pub(crate) config: SigningConfig, + pub(crate) state: Mutex, +} + +impl SigningConfig { + pub fn new( + secret_key: [u8; 32], + link_id: u8, + sign_outgoing: bool, + allow_unsigned: bool, + ) -> Self { + Self { + secret_key, + link_id, + sign_outgoing, + allow_unsigned, + } + } +} + +impl SigningData { + pub fn from_config(config: SigningConfig) -> Self { + Self { + config, + state: Mutex::new(SigningState { + timestamp: 0, + stream_timestamps: HashMap::new(), + }), + } + } + + /// Verify the signature of a MAVLink 2 message. + pub fn verify_signature(&self, message: &MAVLinkV2MessageRaw) -> bool { + // The code that holds the mutex lock is not expected to panic, therefore the expect is justified. + // The only issue that might cause a panic, presuming the opertions on the message buffer are sound, + // is the `SystemTime::now()` call in `get_current_timestamp()`. + let mut state = self + .state + .lock() + .expect("Code holding MutexGuard should not panic."); + if message.incompatibility_flags() & MAVLINK_IFLAG_SIGNED > 0 { + state.timestamp = u64::max(state.timestamp, Self::get_current_timestamp()); + let timestamp = message.signature_timestamp(); + let src_system = message.system_id(); + let src_component = message.component_id(); + let stream_key = (message.signature_link_id(), src_system, src_component); + match state.stream_timestamps.get(&stream_key) { + Some(stream_timestamp) => { + if timestamp <= *stream_timestamp { + // reject old timestamp + return false; + } + } + None => { + if timestamp + 60 * 1000 * 100 < state.timestamp { + // bad new stream, more then a minute older the the last one + return false; + } + } + } + + let mut signature_buffer = [0u8; 6]; + message.calculate_signature(&self.config.secret_key, &mut signature_buffer); + let result = signature_buffer == message.signature_value(); + if result { + // if signature is valid update timestamps + state.stream_timestamps.insert(stream_key, timestamp); + state.timestamp = u64::max(state.timestamp, timestamp) + } + result + } else { + self.config.allow_unsigned + } + } + + /// Sign a MAVLink 2 message if its incompatibility flag is set accordingly. + pub fn sign_message(&self, message: &mut MAVLinkV2MessageRaw) { + if message.incompatibility_flags() & MAVLINK_IFLAG_SIGNED > 0 { + // The code that holds the mutex lock is not expected to panic, therefore the expect is justified. + // The only issue that might cause a panic, presuming the opertions on the message buffer are sound, + // is the `SystemTime::now()` call in `get_current_timestamp()`. + let mut state = self + .state + .lock() + .expect("Code holding MutexGuard should not panic."); + state.timestamp = u64::max(state.timestamp, Self::get_current_timestamp()); + let ts_bytes = u64::to_le_bytes(state.timestamp); + message + .signature_timestamp_bytes_mut() + .copy_from_slice(&ts_bytes[0..6]); + *message.signature_link_id_mut() = self.config.link_id; + + let mut signature_buffer = [0u8; 6]; + message.calculate_signature(&self.config.secret_key, &mut signature_buffer); + + message + .signature_value_mut() + .copy_from_slice(&signature_buffer); + state.timestamp += 1; + } + } + + fn get_current_timestamp() -> u64 { + // fallback to 0 if the system time appears to be before epoch + let now = SystemTime::now() + .duration_since(SystemTime::UNIX_EPOCH) + .map(|n| n.as_micros()) + .unwrap_or(0); + // use 1st January 2015 GMT as offset, fallback to 0 if before that date, the used 48bit of this will overflow in 2104 + ((now + .checked_sub(1420070400u128 * 1000000u128) + .unwrap_or_default()) + / 10u128) as u64 + } +} diff --git a/mavlink-core/src/utils.rs b/mavlink-core/src/utils.rs new file mode 100644 index 0000000000..b8126360ec --- /dev/null +++ b/mavlink-core/src/utils.rs @@ -0,0 +1,114 @@ +/// Removes the trailing zeroes in the payload +/// +/// # Note: +/// +/// There must always be at least one remaining byte even if it is a +/// zero byte. +pub fn remove_trailing_zeroes(data: &[u8]) -> usize { + let mut len = data.len(); + + for b in data[1..].iter().rev() { + if *b != 0 { + break; + } + + len -= 1; + } + + len +} + +/// A trait very similar to `Default` but is only implemented for the equivalent Rust types to +/// `MavType`s. +/// +/// This is only needed because rust doesn't currently implement `Default` for arrays +/// of all sizes. In particular this trait is only ever used when the "serde" feature is enabled. +/// For more information, check out [this issue](https://users.rust-lang.org/t/issue-for-derives-for-arrays-greater-than-size-32/59055/3). +pub trait RustDefault: Copy { + fn rust_default() -> Self; +} + +impl RustDefault for [T; N] { + #[inline(always)] + fn rust_default() -> Self { + let val: T = RustDefault::rust_default(); + [val; N] + } +} + +impl RustDefault for u8 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for i8 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for u16 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for i16 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for u32 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for i32 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for u64 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for i64 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for char { + #[inline(always)] + fn rust_default() -> Self { + '\0' + } +} + +impl RustDefault for f32 { + #[inline(always)] + fn rust_default() -> Self { + 0.0 + } +} + +impl RustDefault for f64 { + #[inline(always)] + fn rust_default() -> Self { + 0.0 + } +} diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml new file mode 100644 index 0000000000..669d9b0752 --- /dev/null +++ b/mavlink/Cargo.toml @@ -0,0 +1,120 @@ + +[package] +name = "mavlink" +version = "0.13.2" +authors = [ + "Todd Stellanova", + "Michal Podhradsky", + "Kevin Mehall", + "Tim Ryan", + "Patrick José Pereira", + "Ibiyemi Abiodun", +] +build = "build/main.rs" +description = "Implements the MAVLink data interchange format for UAVs." +readme = "../README.md" +license = "MIT/Apache-2.0" +repository = "https://github.com/mavlink/rust-mavlink" +edition.workspace = true +rust-version.workspace = true + +[build-dependencies] +mavlink-bindgen = { path = "../mavlink-bindgen", default-features = false } + +[[example]] +name = "mavlink-dump" +path = "examples/mavlink-dump/src/main.rs" +required-features = ["ardupilotmega"] + +[dependencies] +mavlink-core = { path = "../mavlink-core", default-features = false } +num-traits = { workspace = true, default-features = false } +num-derive = { workspace = true } +bitflags = { workspace = true } +serde = { version = "1.0.115", optional = true, features = ["derive"] } +serde_arrays = { version = "0.1.0", optional = true } + +[features] +"all" = [ + "ardupilotmega", + "asluav", + "common", + "development", + "icarous", + "minimal", + "python_array_test", + "standard", + "test", + "ualberta", + "uavionix", + "avssuas", + "cubepilot", +] +"ardupilotmega" = ["common", "icarous", "uavionix"] +"asluav" = ["common"] +"avssuas" = ["common"] +"development" = ["common"] +"matrixpilot" = ["common"] +"minimal" = [] +"paparazzi" = ["common"] +"python_array_test" = ["common"] +"slugs" = ["common"] +"standard" = ["common"] +"test" = [] +"ualberta" = ["common"] +"uavionix" = ["common"] +"icarous" = [] +"common" = [] +"cubepilot" = ["common"] + +"all-dialects" = [ + "ardupilotmega", + "asluav", + "avssuas", + "development", + "matrixpilot", + "minimal", + "paparazzi", + "python_array_test", + "slugs", + "standard", + "test", + "ualberta", + "uavionix", + "icarous", + "common", + "cubepilot", +] + +"format-generated-code" = [] +"emit-description" = ["mavlink-bindgen/emit-description"] +"emit-extensions" = ["mavlink-bindgen/emit-extensions"] +"std" = ["mavlink-core/std"] +"udp" = ["mavlink-core/udp"] +"tcp" = ["mavlink-core/tcp"] +"signing" = ["mavlink-core/signing"] +"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"] +"tokio-1" = ["mavlink-core/tokio-1"] +default = ["std", "tcp", "udp", "direct-serial", "serde", "ardupilotmega"] + +# build with all features on docs.rs so that users viewing documentation +# can see everything +[package.metadata.docs.rs] +features = [ + "default", + "all-dialects", + "emit-description", + "emit-extensions", + "format-generated-code", + "tokio-1", + "signing" +] + +[dev-dependencies] +tokio = { version = "1.0", default-features = false, features = ["macros", "rt", "time" ] } diff --git a/mavlink/build/main.rs b/mavlink/build/main.rs new file mode 100644 index 0000000000..64f3aea0ab --- /dev/null +++ b/mavlink/build/main.rs @@ -0,0 +1,59 @@ +#![recursion_limit = "256"] + +use std::env; +use std::fs::read_dir; +use std::path::Path; +use std::process::{Command, ExitCode}; + +fn main() -> ExitCode { + let src_dir = Path::new(env!("CARGO_MANIFEST_DIR")); + + // Update and init submodule + if let Err(error) = Command::new("git") + .arg("submodule") + .arg("update") + .arg("--init") + .current_dir(src_dir) + .status() + { + eprintln!("{error}"); + return ExitCode::FAILURE; + } + + // find & apply patches to XML definitions to avoid crashes + let patch_dir = src_dir.join("build/patches"); + let mavlink_dir = src_dir.join("mavlink"); + + if let Ok(dir) = read_dir(patch_dir) { + for entry in dir.flatten() { + if let Err(error) = Command::new("git") + .arg("apply") + .arg(entry.path().as_os_str()) + .current_dir(&mavlink_dir) + .status() + { + eprintln!("{error}"); + return ExitCode::FAILURE; + } + } + } + + let definitions_dir = src_dir.join("mavlink/message_definitions/v1.0"); + + let out_dir = env::var("OUT_DIR").unwrap(); + + let result = match mavlink_bindgen::generate(definitions_dir, out_dir) { + Ok(r) => r, + Err(e) => { + eprintln!("{e}"); + return ExitCode::FAILURE; + } + }; + + #[cfg(feature = "format-generated-code")] + mavlink_bindgen::format_generated_code(&result); + + mavlink_bindgen::emit_cargo_build_messages(&result); + + ExitCode::SUCCESS +} diff --git a/examples/embedded/.cargo/config b/mavlink/examples/embedded-async-read/.cargo/config.toml similarity index 100% rename from examples/embedded/.cargo/config rename to mavlink/examples/embedded-async-read/.cargo/config.toml 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..7f53335de3 --- /dev/null +++ b/mavlink/examples/embedded-async-read/src/main.rs @@ -0,0 +1,90 @@ +//! 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()); + + if 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/config.toml b/mavlink/examples/embedded/.cargo/config.toml new file mode 100644 index 0000000000..2c0f3e9d15 --- /dev/null +++ b/mavlink/examples/embedded/.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/examples/embedded/Cargo.toml b/mavlink/examples/embedded/Cargo.toml similarity index 71% rename from examples/embedded/Cargo.toml rename to mavlink/examples/embedded/Cargo.toml index 1f894587a3..4b0801d66d 100644 --- a/examples/embedded/Cargo.toml +++ b/mavlink/examples/embedded/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "mavlink-embedded" -edition = "2018" +edition = "2021" authors = [ "Patrick José Pereira ", ] @@ -12,12 +12,13 @@ lto = true # Performs "fat" LTO which attempts to perform optimizations acr [dependencies] cortex-m = "0.7" # Low level access to Cortex-M processors -cortex-m-rt = "0.6" # Startup code and minimal runtime for Cortex-M microcontrollers +cortex-m-rt = "0.7" # Startup code and minimal runtime for Cortex-M microcontrollers panic-halt = "0.2" # Panic handler -static-alloc = "0.2" # Small general purpose global allocator -stm32f3xx-hal = { version = "0.6", features = ["stm32f303xe"] } # HAL for stm32f303xe +stm32f3xx-hal = { version = "0.9", features = ["stm32f303xe"] } # HAL for stm32f303xe [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 diff --git a/examples/embedded/README.md b/mavlink/examples/embedded/README.md similarity index 50% rename from examples/embedded/README.md rename to mavlink/examples/embedded/README.md index 83fb338847..78c9fd8538 100644 --- a/examples/embedded/README.md +++ b/mavlink/examples/embedded/README.md @@ -2,10 +2,10 @@ ### How to run: - Install cargo flash: - cargo install cargo-flash -- Install toolchain - - rustup target add thumbv7em-none-eabihf --toolchain nightly +- Install target + - rustup target add thumbv7em-none-eabihf - Check if we can build the project - - cargo +nightly build + - cargo build - Connect your STM32f303Xe board - Flash it! - - cargo +nightly flash --chip stm32f303RETx --release --log info + - cargo flash --chip stm32f303RETx --release --log info diff --git a/examples/embedded/memory.x b/mavlink/examples/embedded/memory.x similarity index 100% rename from examples/embedded/memory.x rename to mavlink/examples/embedded/memory.x diff --git a/examples/embedded/src/main.rs b/mavlink/examples/embedded/src/main.rs similarity index 81% rename from examples/embedded/src/main.rs rename to mavlink/examples/embedded/src/main.rs index b6131b8eee..4a71ca8fcf 100644 --- a/examples/embedded/src/main.rs +++ b/mavlink/examples/embedded/src/main.rs @@ -1,6 +1,5 @@ //! Target board: stm32f303RETx (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 -#![feature(alloc_error_handler)] #![no_main] #![no_std] @@ -11,12 +10,8 @@ use cortex_m_rt::entry; use hal::pac; use hal::prelude::*; use mavlink; -use static_alloc::Bump; use stm32f3xx_hal as hal; -#[global_allocator] // 1KB allocator -static mut ALLOCATOR: Bump<[u8; 1 << 10]> = Bump::uninit(); - #[entry] fn main() -> ! { // Peripherals access @@ -49,14 +44,14 @@ fn main() -> ! { // We don't need the datasheet to check which alternative function to use // https://docs.rs/stm32f3xx-hal/0.6.1/stm32f3xx_hal/gpio/gpioa/struct.PA2.html#impl-TxPin%3CUSART2%3E // The documentation provide the necessary information about each possible hardware configuration - let pin_tx = gpioa.pa2.into_af7(&mut gpioa.moder, &mut gpioa.afrl); - let pin_rx = gpioa.pa3.into_af7(&mut gpioa.moder, &mut gpioa.afrl); + let pin_tx = gpioa.pa2.into_af_push_pull(&mut gpioa.moder, &mut gpioa.otyper, &mut gpioa.afrl); + let pin_rx = gpioa.pa3.into_af_push_pull(&mut gpioa.moder, &mut gpioa.otyper, &mut gpioa.afrl); // Create an interface USART2 with 115200 baudrate - let serial = hal::serial::Serial::usart2( + let serial = hal::serial::Serial::new( dp.USART2, (pin_tx, pin_rx), - 115_200.bps(), + 115_200.Bd(), clocks, &mut rcc.apb1, ); @@ -73,12 +68,6 @@ fn main() -> ! { // Main loop loop { - // Clear allocator before restarting our loop (cheap allocator) - // unnecessary for alloc_cortex_m::CortexMHeap - unsafe { - ALLOCATOR.reset(); - }; - // Write the mavlink message via serial mavlink::write_versioned_msg(&mut tx, mavlink::MavlinkVersion::V2, header, &heartbeat) .unwrap(); @@ -109,10 +98,3 @@ pub fn mavlink_heartbeat_message() -> mavlink::common::MavMessage { mavlink_version: 0x3, }) } - -#[alloc_error_handler] -fn alloc_error(_layout: core::alloc::Layout) -> ! { - // Init debug state - cortex_m::asm::bkpt(); - loop {} -} diff --git a/mavlink/examples/mavlink-dump/Cargo.toml b/mavlink/examples/mavlink-dump/Cargo.toml new file mode 100644 index 0000000000..284ab5c890 --- /dev/null +++ b/mavlink/examples/mavlink-dump/Cargo.toml @@ -0,0 +1,15 @@ +[package] +name = "mavlink-dump" +authors = [ + "Patrick José Pereira ", +] +license = "MIT/Apache-2.0" +edition = "2018" +version = "0.1.0" + +[profile.release] +opt-level = 3 +lto = true # Performs "fat" LTO which attempts to perform optimizations across all crates within the dependency graph + +[dependencies.mavlink] # MAVLink library +path = "../../" diff --git a/mavlink/examples/mavlink-dump/src/main.rs b/mavlink/examples/mavlink-dump/src/main.rs new file mode 100644 index 0000000000..f2f1043504 --- /dev/null +++ b/mavlink/examples/mavlink-dump/src/main.rs @@ -0,0 +1,95 @@ +use mavlink::error::MessageReadError; +use std::{env, sync::Arc, thread, time::Duration}; + +fn main() { + let args: Vec<_> = env::args().collect(); + + if args.len() < 2 { + println!( + "Usage: mavlink-dump (tcpout|tcpin|udpout|udpin|udpbcast|serial|file):(ip|dev|path):(port|baud)" + ); + return; + } + + // It's possible to change the mavlink dialect to be used in the connect call + let mut mavconn = mavlink::connect::(&args[1]).unwrap(); + + // here as an example we force the protocol version to mavlink V1: + // the default for this library is mavlink V2 + mavconn.set_protocol_version(mavlink::MavlinkVersion::V1); + + let vehicle = Arc::new(mavconn); + vehicle + .send(&mavlink::MavHeader::default(), &request_parameters()) + .unwrap(); + vehicle + .send(&mavlink::MavHeader::default(), &request_stream()) + .unwrap(); + + thread::spawn({ + let vehicle = vehicle.clone(); + move || loop { + let res = vehicle.send_default(&heartbeat_message()); + if res.is_ok() { + thread::sleep(Duration::from_secs(1)); + } else { + println!("send failed: {res:?}"); + } + } + }); + + loop { + match vehicle.recv() { + Ok((_header, msg)) => { + println!("received: {msg:?}"); + } + Err(MessageReadError::Io(e)) => { + if e.kind() == std::io::ErrorKind::WouldBlock { + //no messages currently available to receive -- wait a while + thread::sleep(Duration::from_secs(1)); + continue; + } else { + println!("recv error: {e:?}"); + break; + } + } + // messages that didn't get through due to parser errors are ignored + _ => {} + } + } +} + +/// Create a heartbeat message using 'ardupilotmega' dialect +pub fn heartbeat_message() -> mavlink::ardupilotmega::MavMessage { + mavlink::ardupilotmega::MavMessage::HEARTBEAT(mavlink::ardupilotmega::HEARTBEAT_DATA { + custom_mode: 0, + mavtype: mavlink::ardupilotmega::MavType::MAV_TYPE_QUADROTOR, + autopilot: mavlink::ardupilotmega::MavAutopilot::MAV_AUTOPILOT_ARDUPILOTMEGA, + base_mode: mavlink::ardupilotmega::MavModeFlag::empty(), + system_status: mavlink::ardupilotmega::MavState::MAV_STATE_STANDBY, + mavlink_version: 0x3, + }) +} + +/// Create a message requesting the parameters list +pub fn request_parameters() -> mavlink::ardupilotmega::MavMessage { + mavlink::ardupilotmega::MavMessage::PARAM_REQUEST_LIST( + mavlink::ardupilotmega::PARAM_REQUEST_LIST_DATA { + target_system: 0, + target_component: 0, + }, + ) +} + +/// Create a message enabling data streaming +pub fn request_stream() -> mavlink::ardupilotmega::MavMessage { + mavlink::ardupilotmega::MavMessage::REQUEST_DATA_STREAM( + mavlink::ardupilotmega::REQUEST_DATA_STREAM_DATA { + target_system: 0, + target_component: 0, + req_stream_id: 0, + req_message_rate: 10, + start_stop: 1, + }, + ) +} diff --git a/mavlink/mavlink b/mavlink/mavlink new file mode 160000 index 0000000000..d6d86d3f0c --- /dev/null +++ b/mavlink/mavlink @@ -0,0 +1 @@ +Subproject commit d6d86d3f0c90a67c275d6833931f7c712b858dba diff --git a/rustfmt.toml b/mavlink/rustfmt.toml similarity index 100% rename from rustfmt.toml rename to mavlink/rustfmt.toml diff --git a/mavlink/src/lib.rs b/mavlink/src/lib.rs new file mode 100644 index 0000000000..bfdcbeafb2 --- /dev/null +++ b/mavlink/src/lib.rs @@ -0,0 +1,10 @@ +#![cfg_attr(not(feature = "std"), no_std)] +#![cfg_attr(all(any(docsrs, doc), not(doctest)), feature(doc_auto_cfg))] +// include generate definitions +include!(concat!(env!("OUT_DIR"), "/mod.rs")); + +pub use mavlink_core::*; + +#[cfg(feature = "emit-extensions")] +#[allow(unused_imports)] +pub(crate) use mavlink_core::utils::RustDefault; diff --git a/tests/direct_serial_tests.rs b/mavlink/tests/direct_serial_tests.rs similarity index 95% rename from tests/direct_serial_tests.rs rename to mavlink/tests/direct_serial_tests.rs index 98aa617508..f52a25bf8d 100644 --- a/tests/direct_serial_tests.rs +++ b/mavlink/tests/direct_serial_tests.rs @@ -1,6 +1,3 @@ -extern crate mavlink; - -#[cfg(test)] #[cfg(all(feature = "std", feature = "direct-serial", feature = "common"))] mod test_direct_serial { use mavlink::common::MavMessage; diff --git a/tests/encode_decode_tests.rs b/mavlink/tests/encode_decode_tests.rs similarity index 74% rename from tests/encode_decode_tests.rs rename to mavlink/tests/encode_decode_tests.rs index 81adb22286..0cc290eb34 100644 --- a/tests/encode_decode_tests.rs +++ b/mavlink/tests/encode_decode_tests.rs @@ -1,11 +1,9 @@ -extern crate mavlink; - mod test_shared; -#[cfg(test)] #[cfg(feature = "common")] mod test_encode_decode { use mavlink::{common, Message}; + use mavlink_core::peek_reader::PeekReader; #[test] pub fn test_echo_heartbeat() { @@ -15,11 +13,11 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &common::MavMessage::HEARTBEAT(send_msg.clone()), + &common::MavMessage::HEARTBEAT(send_msg), ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg): (mavlink::MavHeader, common::MavMessage) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); assert_eq!(recv_msg.message_id(), 0); @@ -33,11 +31,11 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &mavlink::common::MavMessage::COMMAND_INT(send_msg.clone()), + &mavlink::common::MavMessage::COMMAND_INT(send_msg), ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); if let common::MavMessage::COMMAND_INT(recv_msg) = recv_msg { @@ -55,11 +53,11 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &mavlink::common::MavMessage::HIL_ACTUATOR_CONTROLS(send_msg.clone()), + &mavlink::common::MavMessage::HIL_ACTUATOR_CONTROLS(send_msg), ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); if let mavlink::common::MavMessage::HIL_ACTUATOR_CONTROLS(recv_msg) = recv_msg { assert_eq!( @@ -84,22 +82,18 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &ardupilotmega::MavMessage::common(common::MavMessage::HEARTBEAT(send_msg.clone())), + &mavlink::common::MavMessage::HEARTBEAT(send_msg), ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); - if let ardupilotmega::MavMessage::common(recv_msg) = recv_msg { - match &recv_msg { - common::MavMessage::HEARTBEAT(data) => { - assert_eq!(recv_msg.message_id(), 0); - } - _ => panic!("Decoded wrong message type"), + match &recv_msg { + ardupilotmega::MavMessage::HEARTBEAT(_data) => { + assert_eq!(recv_msg.message_id(), 0); } - } else { - panic!("Decoded wrong message type") + _ => panic!("Decoded wrong message type"), } } @@ -117,11 +111,11 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &ardupilotmega::MavMessage::MOUNT_STATUS(send_msg.clone()), + &ardupilotmega::MavMessage::MOUNT_STATUS(send_msg), ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); if let ardupilotmega::MavMessage::MOUNT_STATUS(recv_msg) = recv_msg { assert_eq!(4, recv_msg.pointing_b); @@ -141,22 +135,18 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &ardupilotmega::MavMessage::common(common::MavMessage::COMMAND_INT(send_msg.clone())), + &common::MavMessage::COMMAND_INT(send_msg), ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); - if let ardupilotmega::MavMessage::common(recv_msg) = recv_msg { - match &recv_msg { - common::MavMessage::COMMAND_INT(data) => { - assert_eq!(data.command, common::MavCmd::MAV_CMD_NAV_TAKEOFF); - } - _ => panic!("Decoded wrong message type"), + match &recv_msg { + ardupilotmega::MavMessage::COMMAND_INT(data) => { + assert_eq!(data.command, ardupilotmega::MavCmd::MAV_CMD_NAV_TAKEOFF); } - } else { - panic!("Decoded wrong message type") + _ => panic!("Decoded wrong message type"), } } } diff --git a/tests/helper_tests.rs b/mavlink/tests/helper_tests.rs similarity index 73% rename from tests/helper_tests.rs rename to mavlink/tests/helper_tests.rs index e44de326de..2a36c6abca 100644 --- a/tests/helper_tests.rs +++ b/mavlink/tests/helper_tests.rs @@ -1,9 +1,6 @@ -extern crate mavlink; - -#[cfg(test)] #[cfg(all(feature = "std", feature = "common"))] mod helper_tests { - use crate::mavlink::{common::MavMessage, Message}; + use mavlink::{common::MavMessage, Message}; #[test] fn test_get_default_message_from_id() { @@ -13,9 +10,8 @@ mod helper_tests { let id = id.unwrap(); assert!(id == 4, "Invalid id for message name: PING"); let message = MavMessage::default_message_from_id(id); - match message { - Ok(MavMessage::PING(_)) => {} - _ => unreachable!("Invalid message type."), + if !matches!(message, Ok(MavMessage::PING(_))) { + unreachable!("Invalid message type.") } assert!( message.unwrap().message_name() == message_name, diff --git a/tests/log.tlog b/mavlink/tests/log.tlog similarity index 100% rename from tests/log.tlog rename to mavlink/tests/log.tlog diff --git a/mavlink/tests/mav_frame_tests.rs b/mavlink/tests/mav_frame_tests.rs new file mode 100644 index 0000000000..1c225efd09 --- /dev/null +++ b/mavlink/tests/mav_frame_tests.rs @@ -0,0 +1,107 @@ +pub mod test_shared; + +mod mav_frame_tests { + use mavlink::ardupilotmega::MavMessage; + use mavlink::MavFrame; + use mavlink::MavHeader; + + // NOTE: No STX, length, or flag fields in the header + pub const HEARTBEAT_V2: &[u8] = &[ + // Currently [`MavFrame::deser`] and [`MavFrame::ser`] does not account for the first four fields. + // 0xfd, // STX V2 + // 0x09, // len + // 0x00, // incompat_flags + // 0x00, // compat_flags + crate::test_shared::COMMON_MSG_HEADER.sequence, + crate::test_shared::COMMON_MSG_HEADER.system_id, + crate::test_shared::COMMON_MSG_HEADER.component_id, + 0x00, // msg ID + 0x00, + 0x00, + 0x05, // payload + 0x00, + 0x00, + 0x00, + 0x02, + 0x03, + 0x59, + 0x03, + 0x03, + 0x10, // checksum + 0xf0, + ]; + + #[test] + pub fn test_deser_ser() { + use mavlink::{common::MavMessage, MavFrame, MavlinkVersion}; + let frame = MavFrame::::deser(MavlinkVersion::V2, HEARTBEAT_V2) + .expect("failed to parse message"); + + assert_eq!(frame.header, crate::test_shared::COMMON_MSG_HEADER); + let heartbeat_msg = crate::test_shared::get_heartbeat_msg(); + + let mut buffer = [0u8; HEARTBEAT_V2.len()]; + frame.ser(&mut buffer); + assert_eq!(buffer[..buffer.len() - 2], HEARTBEAT_V2[..buffer.len() - 2]); + + let msg = match frame.msg { + MavMessage::HEARTBEAT(msg) => msg, + _ => panic!("Decoded wrong message type"), + }; + assert_eq!(msg.custom_mode, heartbeat_msg.custom_mode); + assert_eq!(msg.mavtype, heartbeat_msg.mavtype); + assert_eq!(msg.autopilot, heartbeat_msg.autopilot); + assert_eq!(msg.base_mode, heartbeat_msg.base_mode); + assert_eq!(msg.system_status, heartbeat_msg.system_status); + assert_eq!(msg.mavlink_version, heartbeat_msg.mavlink_version); + } + + #[test] + pub fn test_deser_ser_message() { + let buf: &mut [u8; 255] = &mut [0; 255]; + + let mavlink_message = mavlink_message(); + let mavlink_frame = new(mavlink_message); + + let _len = mavlink_frame.ser(buf); + + let parsed_mavlink_frame = + MavFrame::::deser(mavlink::MavlinkVersion::V2, buf) + .unwrap(); + + assert_eq!( + format!("{mavlink_frame:?}"), + format!("{parsed_mavlink_frame:?}") + ); + } + + fn mavlink_message() -> mavlink::ardupilotmega::MavMessage { + mavlink::ardupilotmega::MavMessage::LINK_NODE_STATUS( + mavlink::ardupilotmega::LINK_NODE_STATUS_DATA { + timestamp: 92197916, + tx_rate: 0x11223344, + rx_rate: 0x55667788, + messages_sent: 0x99001122, + messages_received: 0x33445566, + messages_lost: 0x77889900, + rx_parse_err: 0x1122, + tx_overflows: 0x3355, + rx_overflows: 0x5566, + tx_buf: 0xff, + rx_buf: 0x11, + }, + ) + } + + fn new(msg: MavMessage) -> MavFrame { + MavFrame { + header: MavHeader { + system_id: 1, + component_id: 2, + sequence: 84, + }, + msg, + protocol_version: mavlink::MavlinkVersion::V2, + } + } +} diff --git a/tests/process_log_files.rs b/mavlink/tests/process_log_files.rs similarity index 68% rename from tests/process_log_files.rs rename to mavlink/tests/process_log_files.rs index 876762b4b8..5d60db9e38 100644 --- a/tests/process_log_files.rs +++ b/mavlink/tests/process_log_files.rs @@ -1,6 +1,3 @@ -extern crate mavlink; - -#[cfg(test)] #[cfg(all(feature = "default", feature = "ardupilotmega"))] mod process_files { use mavlink::ardupilotmega::MavMessage; @@ -20,15 +17,15 @@ mod process_files { let filename = filename.to_str().unwrap(); dbg!(filename); - println!("Processing file: {}", &filename); - let connection_string = format!("file:{}", &filename); + println!("Processing file: {filename}"); + let connection_string = format!("file:{filename}"); // Process file process_file(&connection_string); } pub fn process_file(connection_string: &str) { - let vehicle = mavlink::connect::(&connection_string); + let vehicle = mavlink::connect::(connection_string); assert!(vehicle.is_ok(), "Incomplete address should error"); let vehicle = vehicle.unwrap(); @@ -38,22 +35,21 @@ mod process_files { Ok((_header, _msg)) => { counter += 1; } - Err(MessageReadError::Io(e)) => match e.kind() { - std::io::ErrorKind::WouldBlock => { + Err(MessageReadError::Io(e)) => { + if e.kind() == std::io::ErrorKind::WouldBlock { continue; - } - _ => { - println!("recv error: {:?}", e); + } else { + println!("recv error: {e:?}"); break; } - }, + } _ => {} } } - println!("Number of parsed messages: {}", counter); + println!("Number of parsed messages: {counter}"); assert!( - counter == 1374, + counter == 1426, "Unable to hit the necessary amount of matches" ); } diff --git a/mavlink/tests/signing.rs b/mavlink/tests/signing.rs new file mode 100644 index 0000000000..52ca63cd46 --- /dev/null +++ b/mavlink/tests/signing.rs @@ -0,0 +1,100 @@ +mod test_shared; + +#[cfg(feature = "signing")] +mod signing { + use mavlink::{ + common::HEARTBEAT_DATA, peek_reader::PeekReader, read_v2_raw_message, MAVLinkV2MessageRaw, + MavHeader, SigningConfig, SigningData, MAV_STX_V2, + }; + + use crate::test_shared::SECRET_KEY; + + const HEARTBEAT_SIGNED: &[u8] = &[ + MAV_STX_V2, + 0x09, + 0x01, // MAVLINK_IFLAG_SIGNED + 0x00, + crate::test_shared::COMMON_MSG_HEADER.sequence, + crate::test_shared::COMMON_MSG_HEADER.system_id, + crate::test_shared::COMMON_MSG_HEADER.component_id, + 0x00, // msg ID + 0x00, + 0x00, + 0x05, // payload + 0x00, + 0x00, + 0x00, + 0x02, + 0x03, + 0x59, + 0x03, + 0x03, + 0xc9, // checksum + 0x8b, + 0x00, // link_id + 0xff, // use max timestamp to ensure test will never fail against current time + 0xff, + 0xff, + 0xff, + 0xff, + 0xff, + 0x27, // signature + 0x18, + 0xb1, + 0x68, + 0xcc, + 0xf5, + ]; + + #[test] + pub fn test_verify() { + let signing_cfg = SigningConfig::new(SECRET_KEY, 0, true, false); + let signing_data = SigningData::from_config(signing_cfg); + let mut r = PeekReader::new(HEARTBEAT_SIGNED); + let msg = read_v2_raw_message::(&mut r).unwrap(); + assert!( + signing_data.verify_signature(&msg), + "Message verification failed" + ); + } + + #[test] + pub fn test_invalid_ts() { + let signing_cfg = SigningConfig::new(SECRET_KEY, 0, true, false); + let signing_data = SigningData::from_config(signing_cfg); + let mut r = PeekReader::new(HEARTBEAT_SIGNED); + let mut msg = read_v2_raw_message::(&mut r).unwrap(); + msg.signature_timestamp_bytes_mut() + .copy_from_slice(&[0, 0, 0, 0, 0, 0]); // set timestamp to min causing the timestamp test to fail + assert!( + !signing_data.verify_signature(&msg), + "Invalid message verified" + ); + } + + #[test] + pub fn test_sign_verify() { + use mavlink::common::MavMessage; + let heartbeat_message = MavMessage::HEARTBEAT(HEARTBEAT_DATA::default()); + let mut message = MAVLinkV2MessageRaw::new(); + let header = MavHeader { + system_id: 4, + component_id: 3, + sequence: 42, + }; + message.serialize_message_for_signing(header, &heartbeat_message); + + let signing_cfg = SigningConfig::new(SECRET_KEY, 0, true, false); + let signing_data = SigningData::from_config(signing_cfg); + signing_data.sign_message(&mut message); + assert!( + signing_data.verify_signature(&message), + "Message verification failed" + ); + // the same message must not be allowed to be verified again + assert!( + !signing_data.verify_signature(&message), + "Invalid message verified" + ); + } +} diff --git a/mavlink/tests/tcp_loopback_async_tests.rs b/mavlink/tests/tcp_loopback_async_tests.rs new file mode 100644 index 0000000000..860ce0b10e --- /dev/null +++ b/mavlink/tests/tcp_loopback_async_tests.rs @@ -0,0 +1,71 @@ +mod test_shared; + +#[cfg(all(feature = "tokio-1", feature = "tcp", feature = "common"))] +mod test_tcp_connections { + #[cfg(feature = "signing")] + use crate::test_shared; + #[cfg(feature = "signing")] + use mavlink::SigningConfig; + + /// Test whether we can send a message via TCP and receive it OK using async_connect. + /// This also test signing as a property of a MavConnection if the signing feature is enabled. + #[tokio::test] + pub async fn test_tcp_loopback() { + const RECEIVE_CHECK_COUNT: i32 = 5; + + #[cfg(feature = "signing")] + let singing_cfg_server = SigningConfig::new(test_shared::SECRET_KEY, 0, true, false); + #[cfg(feature = "signing")] + let singing_cfg_client = singing_cfg_server.clone(); + + let server_thread = tokio::spawn(async move { + //TODO consider using get_available_port to use a random port + let mut server = mavlink::connect_async("tcpin:0.0.0.0:14551") + .await + .expect("Couldn't create server"); + + #[cfg(feature = "signing")] + server.setup_signing(Some(singing_cfg_server)); + + let mut recv_count = 0; + for _i in 0..RECEIVE_CHECK_COUNT { + match server.recv().await { + Ok((_header, msg)) => { + if let mavlink::common::MavMessage::HEARTBEAT(_heartbeat_msg) = msg { + recv_count += 1; + } else { + // one message parse failure fails the test + break; + } + } + Err(..) => { + // one message read failure fails the test + break; + } + } + } + assert_eq!(recv_count, RECEIVE_CHECK_COUNT); + }); + + // Give some time for the server to connect + tokio::time::sleep(std::time::Duration::from_millis(100)).await; + + // have the client send a few hearbeats + tokio::spawn(async move { + let msg = + mavlink::common::MavMessage::HEARTBEAT(crate::test_shared::get_heartbeat_msg()); + let mut client = mavlink::connect_async("tcpout:127.0.0.1:14551") + .await + .expect("Couldn't create client"); + + #[cfg(feature = "signing")] + client.setup_signing(Some(singing_cfg_client)); + + for _i in 0..RECEIVE_CHECK_COUNT { + client.send_default(&msg).await.ok(); + } + }); + + server_thread.await.unwrap(); + } +} diff --git a/tests/tcp_loopback_tests.rs b/mavlink/tests/tcp_loopback_tests.rs similarity index 57% rename from tests/tcp_loopback_tests.rs rename to mavlink/tests/tcp_loopback_tests.rs index cf4d92942f..170189cb97 100644 --- a/tests/tcp_loopback_tests.rs +++ b/mavlink/tests/tcp_loopback_tests.rs @@ -1,33 +1,41 @@ -extern crate mavlink; - mod test_shared; -#[cfg(test)] #[cfg(all(feature = "std", feature = "tcp", feature = "common"))] mod test_tcp_connections { use std::thread; - /// Test whether we can send a message via TCP and receive it OK + #[cfg(feature = "signing")] + use crate::test_shared; + #[cfg(feature = "signing")] + use mavlink::SigningConfig; + + /// Test whether we can send a message via TCP and receive it OK. This also test signing as a property of a MavConnection if the signing feature is enabled. #[test] pub fn test_tcp_loopback() { const RECEIVE_CHECK_COUNT: i32 = 5; + #[cfg(feature = "signing")] + let singing_cfg_server = SigningConfig::new(test_shared::SECRET_KEY, 0, true, false); + #[cfg(feature = "signing")] + let singing_cfg_client = singing_cfg_server.clone(); + let server_thread = thread::spawn(move || { //TODO consider using get_available_port to use a random port - let server = mavlink::connect("tcpin:0.0.0.0:14550").expect("Couldn't create server"); + let mut server = + mavlink::connect("tcpin:0.0.0.0:14550").expect("Couldn't create server"); + + #[cfg(feature = "signing")] + server.setup_signing(Some(singing_cfg_server)); let mut recv_count = 0; for _i in 0..RECEIVE_CHECK_COUNT { match server.recv() { Ok((_header, msg)) => { - match msg { - mavlink::common::MavMessage::HEARTBEAT(_heartbeat_msg) => { - recv_count += 1; - } - _ => { - // one message parse failure fails the test - break; - } + if let mavlink::common::MavMessage::HEARTBEAT(_heartbeat_msg) = msg { + recv_count += 1; + } else { + // one message parse failure fails the test + break; } } Err(..) => { @@ -46,8 +54,12 @@ mod test_tcp_connections { thread::spawn(move || { let msg = mavlink::common::MavMessage::HEARTBEAT(crate::test_shared::get_heartbeat_msg()); - let client = + let mut client = mavlink::connect("tcpout:127.0.0.1:14550").expect("Couldn't create client"); + + #[cfg(feature = "signing")] + client.setup_signing(Some(singing_cfg_client)); + for _i in 0..RECEIVE_CHECK_COUNT { client.send_default(&msg).ok(); } diff --git a/tests/test_shared/mod.rs b/mavlink/tests/test_shared/mod.rs similarity index 68% rename from tests/test_shared/mod.rs rename to mavlink/tests/test_shared/mod.rs index 35ba39a52e..90f019cebd 100644 --- a/tests/test_shared/mod.rs +++ b/mavlink/tests/test_shared/mod.rs @@ -1,11 +1,17 @@ -extern crate mavlink; +#![allow(unused)] pub const COMMON_MSG_HEADER: mavlink::MavHeader = mavlink::MavHeader { sequence: 239, system_id: 1, - component_id: 1, + component_id: 2, }; +#[cfg(feature = "signing")] +pub const SECRET_KEY: [u8; 32] = [ + 0x00, 0x01, 0xf2, 0xe3, 0xd4, 0xc5, 0xb6, 0xa7, 0x98, 0x00, 0x70, 0x76, 0x34, 0x32, 0x00, 0x16, + 0x22, 0x42, 0x00, 0xcc, 0xff, 0x7a, 0x00, 0x52, 0x75, 0x73, 0x74, 0x00, 0x4d, 0x41, 0x56, 0xb3, +]; + #[cfg(feature = "common")] pub fn get_heartbeat_msg() -> mavlink::common::HEARTBEAT_DATA { mavlink::common::HEARTBEAT_DATA { @@ -43,8 +49,8 @@ pub fn get_cmd_nav_takeoff_msg() -> mavlink::common::COMMAND_INT_DATA { #[cfg(feature = "common")] pub fn get_hil_actuator_controls_msg() -> mavlink::common::HIL_ACTUATOR_CONTROLS_DATA { mavlink::common::HIL_ACTUATOR_CONTROLS_DATA { - time_usec: 1234567 as u64, - flags: 0 as u64, + time_usec: 1234567_u64, + flags: 0_u64, controls: [ 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0, ], @@ -57,40 +63,40 @@ pub fn get_hil_actuator_controls_msg() -> mavlink::common::HIL_ACTUATOR_CONTROLS #[cfg(all(feature = "common", not(feature = "emit-extensions")))] pub fn get_servo_output_raw_v1() -> mavlink::common::SERVO_OUTPUT_RAW_DATA { mavlink::common::SERVO_OUTPUT_RAW_DATA { - time_usec: 1234567 as u32, - servo1_raw: 1100 as u16, - servo2_raw: 1200 as u16, - servo3_raw: 1300 as u16, - servo4_raw: 1400 as u16, - servo5_raw: 1500 as u16, - servo6_raw: 1600 as u16, - servo7_raw: 1700 as u16, - servo8_raw: 1800 as u16, - port: 123 as u8, + time_usec: 1234567_u32, + servo1_raw: 1100_u16, + servo2_raw: 1200_u16, + servo3_raw: 1300_u16, + servo4_raw: 1400_u16, + servo5_raw: 1500_u16, + servo6_raw: 1600_u16, + servo7_raw: 1700_u16, + servo8_raw: 1800_u16, + port: 123_u8, } } #[cfg(all(feature = "common", feature = "emit-extensions"))] pub fn get_servo_output_raw_v2() -> mavlink::common::SERVO_OUTPUT_RAW_DATA { mavlink::common::SERVO_OUTPUT_RAW_DATA { - time_usec: 1234567 as u32, - servo1_raw: 1100 as u16, - servo2_raw: 1200 as u16, - servo3_raw: 1300 as u16, - servo4_raw: 1400 as u16, - servo5_raw: 1500 as u16, - servo6_raw: 1600 as u16, - servo7_raw: 1700 as u16, - servo8_raw: 1800 as u16, - port: 123 as u8, - servo9_raw: 1110 as u16, - servo10_raw: 1220 as u16, - servo11_raw: 1330 as u16, - servo12_raw: 1440 as u16, - servo13_raw: 1550 as u16, - servo14_raw: 1660 as u16, - servo15_raw: 1770 as u16, - servo16_raw: 1880 as u16, + time_usec: 1234567_u32, + servo1_raw: 1100_u16, + servo2_raw: 1200_u16, + servo3_raw: 1300_u16, + servo4_raw: 1400_u16, + servo5_raw: 1500_u16, + servo6_raw: 1600_u16, + servo7_raw: 1700_u16, + servo8_raw: 1800_u16, + port: 123_u8, + servo9_raw: 1110_u16, + servo10_raw: 1220_u16, + servo11_raw: 1330_u16, + servo12_raw: 1440_u16, + servo13_raw: 1550_u16, + servo14_raw: 1660_u16, + servo15_raw: 1770_u16, + servo16_raw: 1880_u16, } } diff --git a/mavlink/tests/udp_loopback_async_tests.rs b/mavlink/tests/udp_loopback_async_tests.rs new file mode 100644 index 0000000000..4b3934523e --- /dev/null +++ b/mavlink/tests/udp_loopback_async_tests.rs @@ -0,0 +1,49 @@ +mod test_shared; + +#[cfg(all(feature = "tokio-1", feature = "udp", feature = "common"))] +mod test_udp_connections { + + /// Test whether we can send a message via UDP and receive it OK using async_connect + #[tokio::test] + pub async fn test_udp_loopback() { + const RECEIVE_CHECK_COUNT: i32 = 3; + + let server = mavlink::connect_async("udpin:0.0.0.0:14552") + .await + .expect("Couldn't create server"); + + // have the client send one heartbeat per second + tokio::spawn({ + async move { + let msg = + mavlink::common::MavMessage::HEARTBEAT(crate::test_shared::get_heartbeat_msg()); + let client = mavlink::connect_async("udpout:127.0.0.1:14552") + .await + .expect("Couldn't create client"); + loop { + client.send_default(&msg).await.ok(); + } + } + }); + + //TODO use std::sync::WaitTimeoutResult to timeout ourselves if recv fails? + let mut recv_count = 0; + for _i in 0..RECEIVE_CHECK_COUNT { + match server.recv().await { + Ok((_header, msg)) => { + if let mavlink::common::MavMessage::HEARTBEAT(_heartbeat_msg) = msg { + recv_count += 1; + } else { + // one message parse failure fails the test + break; + } + } + Err(..) => { + // one message read failure fails the test + break; + } + } + } + assert_eq!(recv_count, RECEIVE_CHECK_COUNT); + } +} diff --git a/tests/udp_loopback_tests.rs b/mavlink/tests/udp_loopback_tests.rs similarity index 77% rename from tests/udp_loopback_tests.rs rename to mavlink/tests/udp_loopback_tests.rs index 0d1b191cfa..fe366fcb4e 100644 --- a/tests/udp_loopback_tests.rs +++ b/mavlink/tests/udp_loopback_tests.rs @@ -1,8 +1,5 @@ -extern crate mavlink; - mod test_shared; -#[cfg(test)] #[cfg(all(feature = "std", feature = "udp", feature = "common"))] mod test_udp_connections { use std::thread; @@ -32,14 +29,11 @@ mod test_udp_connections { for _i in 0..RECEIVE_CHECK_COUNT { match server.recv() { Ok((_header, msg)) => { - match msg { - mavlink::common::MavMessage::HEARTBEAT(_heartbeat_msg) => { - recv_count += 1; - } - _ => { - // one message parse failure fails the test - break; - } + if let mavlink::common::MavMessage::HEARTBEAT(_heartbeat_msg) = msg { + recv_count += 1; + } else { + // one message parse failure fails the test + break; } } Err(..) => { diff --git a/tests/v1_encode_decode_tests.rs b/mavlink/tests/v1_encode_decode_tests.rs similarity index 71% rename from tests/v1_encode_decode_tests.rs rename to mavlink/tests/v1_encode_decode_tests.rs index ad3addc167..048e97c8e3 100644 --- a/tests/v1_encode_decode_tests.rs +++ b/mavlink/tests/v1_encode_decode_tests.rs @@ -1,17 +1,15 @@ -extern crate mavlink; - pub mod test_shared; -#[cfg(test)] #[cfg(all(feature = "std", feature = "common"))] mod test_v1_encode_decode { + use mavlink_core::peek_reader::PeekReader; - pub const HEARTBEAT_V1: &'static [u8] = &[ + pub const HEARTBEAT_V1: &[u8] = &[ mavlink::MAV_STX, 0x09, - 0xef, - 0x01, - 0x01, + crate::test_shared::COMMON_MSG_HEADER.sequence, + crate::test_shared::COMMON_MSG_HEADER.system_id, + crate::test_shared::COMMON_MSG_HEADER.component_id, 0x00, 0x05, 0x00, @@ -22,13 +20,13 @@ mod test_v1_encode_decode { 0x59, 0x03, 0x03, - 0xf1, - 0xd7, + 0x1f, + 0x50, ]; #[test] pub fn test_read_heartbeat() { - let mut r = HEARTBEAT_V1; + let mut r = PeekReader::new(HEARTBEAT_V1); let (header, msg) = mavlink::read_v1_msg(&mut r).expect("Failed to parse message"); //println!("{:?}, {:?}", header, msg); @@ -54,7 +52,7 @@ mod test_v1_encode_decode { mavlink::write_v1_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &mavlink::common::MavMessage::HEARTBEAT(heartbeat_msg.clone()), + &mavlink::common::MavMessage::HEARTBEAT(heartbeat_msg), ) .expect("Failed to write message"); @@ -64,7 +62,7 @@ mod test_v1_encode_decode { #[test] #[cfg(not(feature = "emit-extensions"))] pub fn test_echo_servo_output_raw() { - use mavlink::{common, Message}; + use mavlink::Message; let mut v = vec![]; let send_msg = crate::test_shared::get_servo_output_raw_v1(); @@ -72,24 +70,35 @@ mod test_v1_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &mavlink::common::MavMessage::SERVO_OUTPUT_RAW(send_msg.clone()), + &mavlink::common::MavMessage::SERVO_OUTPUT_RAW(send_msg), ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg): (mavlink::MavHeader, mavlink::common::MavMessage) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); assert_eq!( mavlink::common::MavMessage::extra_crc(recv_msg.message_id()), - 222 as u8 + 222_u8 ); if let mavlink::common::MavMessage::SERVO_OUTPUT_RAW(recv_msg) = recv_msg { - assert_eq!(recv_msg.port, 123 as u8); - assert_eq!(recv_msg.servo4_raw, 1400 as u16); + assert_eq!(recv_msg.port, 123_u8); + assert_eq!(recv_msg.servo4_raw, 1400_u16); } else { panic!("Decoded wrong message type") } } + + #[test] + pub fn test_serialize_to_raw() { + let heartbeat_msg = crate::test_shared::get_heartbeat_msg(); + let mut raw_msg = mavlink::MAVLinkV1MessageRaw::new(); + + raw_msg.serialize_message_data(crate::test_shared::COMMON_MSG_HEADER, &heartbeat_msg); + + assert_eq!(raw_msg.raw_bytes(), HEARTBEAT_V1); + assert!(raw_msg.has_valid_crc::()); + } } diff --git a/tests/v2_encode_decode_tests.rs b/mavlink/tests/v2_encode_decode_tests.rs similarity index 80% rename from tests/v2_encode_decode_tests.rs rename to mavlink/tests/v2_encode_decode_tests.rs index b6cfd8b2fa..f83eb127a1 100644 --- a/tests/v2_encode_decode_tests.rs +++ b/mavlink/tests/v2_encode_decode_tests.rs @@ -1,18 +1,17 @@ -extern crate mavlink; - mod test_shared; -#[cfg(test)] #[cfg(all(feature = "std", feature = "common"))] mod test_v2_encode_decode { - pub const HEARTBEAT_V2: &'static [u8] = &[ + use mavlink_core::peek_reader::PeekReader; + + pub const HEARTBEAT_V2: &[u8] = &[ mavlink::MAV_STX_V2, //magic 0x09, //payload len 0, //incompat flags 0, //compat flags - 0xef, //seq 239 - 0x01, //sys ID - 0x01, //comp ID + crate::test_shared::COMMON_MSG_HEADER.sequence, + crate::test_shared::COMMON_MSG_HEADER.system_id, + crate::test_shared::COMMON_MSG_HEADER.component_id, 0x00, 0x00, 0x00, //msg ID @@ -25,13 +24,13 @@ mod test_v2_encode_decode { 0x59, 0x03, 0x03, //payload - 16, - 240, //checksum + 46, + 115, //checksum ]; #[test] pub fn test_read_v2_heartbeat() { - let mut r = HEARTBEAT_V2; + let mut r = PeekReader::new(HEARTBEAT_V2); let (header, msg) = mavlink::read_v2_msg(&mut r).expect("Failed to parse message"); assert_eq!(header, crate::test_shared::COMMON_MSG_HEADER); @@ -56,7 +55,7 @@ mod test_v2_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &mavlink::common::MavMessage::HEARTBEAT(heartbeat_msg.clone()), + &mavlink::common::MavMessage::HEARTBEAT(heartbeat_msg), ) .expect("Failed to write message"); @@ -64,7 +63,7 @@ mod test_v2_encode_decode { } /// A COMMAND_LONG message with a truncated payload (allowed for empty fields) - pub const COMMAND_LONG_TRUNCATED_V2: &'static [u8] = &[ + pub const COMMAND_LONG_TRUNCATED_V2: &[u8] = &[ mavlink::MAV_STX_V2, 30, 0, @@ -113,7 +112,7 @@ mod test_v2_encode_decode { #[test] pub fn test_read_truncated_command_long() { - let mut r = COMMAND_LONG_TRUNCATED_V2; + let mut r = PeekReader::new(COMMAND_LONG_TRUNCATED_V2); let (_header, recv_msg) = mavlink::read_v2_msg(&mut r).expect("Failed to parse COMMAND_LONG_TRUNCATED_V2"); @@ -142,7 +141,7 @@ mod test_v2_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg): (mavlink::MavHeader, mavlink::common::MavMessage) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); @@ -159,4 +158,15 @@ mod test_v2_encode_decode { panic!("Decoded wrong message type") } } + + #[test] + pub fn test_serialize_to_raw() { + let heartbeat_msg = crate::test_shared::get_heartbeat_msg(); + let mut raw_msg = mavlink::MAVLinkV2MessageRaw::new(); + + raw_msg.serialize_message_data(crate::test_shared::COMMON_MSG_HEADER, &heartbeat_msg); + + assert_eq!(raw_msg.raw_bytes(), HEARTBEAT_V2); + assert!(raw_msg.has_valid_crc::()); + } } diff --git a/src/bin/mavlink-dump.rs b/src/bin/mavlink-dump.rs deleted file mode 100644 index badb18b1bd..0000000000 --- a/src/bin/mavlink-dump.rs +++ /dev/null @@ -1,110 +0,0 @@ -use mavlink::error::MessageReadError; -#[cfg(feature = "std")] -use std::env; -#[cfg(feature = "std")] -use std::sync::Arc; -#[cfg(feature = "std")] -use std::thread; -#[cfg(feature = "std")] -use std::time::Duration; - -#[cfg(not(feature = "std"))] -fn main() {} - -#[cfg(feature = "std")] -fn main() { - let args: Vec<_> = env::args().collect(); - - if args.len() < 2 { - println!( - "Usage: mavlink-dump (tcpout|tcpin|udpout|udpin|udpbcast|serial|file):(ip|dev|path):(port|baud)" - ); - return; - } - - // It's possible to change the mavlink dialect to be used in the connect call - let mut mavconn = mavlink::connect::(&args[1]).unwrap(); - - // here as an example we force the protocol version to mavlink V1: - // the default for this library is mavlink V2 - mavconn.set_protocol_version(mavlink::MavlinkVersion::V1); - - let vehicle = Arc::new(mavconn); - vehicle - .send(&mavlink::MavHeader::default(), &request_parameters().into()) - .unwrap(); - vehicle - .send(&mavlink::MavHeader::default(), &request_stream().into()) - .unwrap(); - - thread::spawn({ - let vehicle = vehicle.clone(); - move || loop { - let res = vehicle.send_default(&heartbeat_message().into()); - if res.is_ok() { - thread::sleep(Duration::from_secs(1)); - } else { - println!("send failed: {:?}", res); - } - } - }); - - loop { - match vehicle.recv() { - Ok((_header, msg)) => { - println!("received: {:?}", msg); - } - Err(MessageReadError::Io(e)) => { - match e.kind() { - std::io::ErrorKind::WouldBlock => { - //no messages currently available to receive -- wait a while - thread::sleep(Duration::from_secs(1)); - continue; - } - _ => { - println!("recv error: {:?}", e); - break; - } - } - } - // messages that didn't get through due to parser errors are ignored - _ => {} - } - } -} - -/// Create a heartbeat message using ardupilotmega dialect -/// If only common dialect is used, the `ardupilotmega::MavMessage::common` is not necessary, -/// and the function could return only a simple `mavlink::common::MavMessage` type -#[cfg(feature = "std")] -pub fn heartbeat_message() -> mavlink::common::MavMessage { - mavlink::common::MavMessage::HEARTBEAT(mavlink::common::HEARTBEAT_DATA { - custom_mode: 0, - mavtype: mavlink::common::MavType::MAV_TYPE_QUADROTOR, - autopilot: mavlink::common::MavAutopilot::MAV_AUTOPILOT_ARDUPILOTMEGA, - base_mode: mavlink::common::MavModeFlag::empty(), - system_status: mavlink::common::MavState::MAV_STATE_STANDBY, - mavlink_version: 0x3, - }) -} - -/// Create a message requesting the parameters list -#[cfg(feature = "std")] -pub fn request_parameters() -> mavlink::common::MavMessage { - mavlink::common::MavMessage::PARAM_REQUEST_LIST(mavlink::common::PARAM_REQUEST_LIST_DATA { - target_system: 0, - target_component: 0, - }) -} - -/// Create a message enabling data streaming -#[cfg(feature = "std")] -pub fn request_stream() -> mavlink::common::MavMessage { - mavlink::common::MavMessage::REQUEST_DATA_STREAM(mavlink::common::REQUEST_DATA_STREAM_DATA { - target_system: 0, - target_component: 0, - req_stream_id: 0, - req_message_rate: 10, - start_stop: 1, - }) -} diff --git a/src/connection/tcp.rs b/src/connection/tcp.rs deleted file mode 100644 index 89a109e86e..0000000000 --- a/src/connection/tcp.rs +++ /dev/null @@ -1,115 +0,0 @@ -use crate::connection::MavConnection; -use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; -use std::io::{self}; -use std::net::ToSocketAddrs; -use std::net::{TcpListener, TcpStream}; -use std::sync::Mutex; -use std::time::Duration; - -/// TCP MAVLink connection - -pub fn select_protocol( - address: &str, -) -> io::Result + Sync + Send>> { - if address.starts_with("tcpout:") { - Ok(Box::new(tcpout(&address["tcpout:".len()..])?)) - } else if address.starts_with("tcpin:") { - Ok(Box::new(tcpin(&address["tcpin:".len()..])?)) - } else { - Err(io::Error::new( - io::ErrorKind::AddrNotAvailable, - "Protocol unsupported", - )) - } -} - -pub fn tcpout(address: T) -> io::Result { - let addr = address - .to_socket_addrs() - .unwrap() - .next() - .expect("Host address lookup failed."); - let socket = TcpStream::connect(&addr)?; - socket.set_read_timeout(Some(Duration::from_millis(100)))?; - - Ok(TcpConnection { - reader: Mutex::new(socket.try_clone()?), - writer: Mutex::new(TcpWrite { - socket: socket, - sequence: 0, - }), - protocol_version: MavlinkVersion::V2, - }) -} - -pub fn tcpin(address: T) -> io::Result { - let addr = address - .to_socket_addrs() - .unwrap() - .next() - .expect("Invalid address"); - let listener = TcpListener::bind(&addr)?; - - //For now we only accept one incoming stream: this blocks until we get one - for incoming in listener.incoming() { - match incoming { - Ok(socket) => { - return Ok(TcpConnection { - reader: Mutex::new(socket.try_clone()?), - writer: Mutex::new(TcpWrite { - socket: socket, - sequence: 0, - }), - protocol_version: MavlinkVersion::V2, - }) - } - Err(e) => { - //TODO don't println in lib - println!("listener err: {}", e); - } - } - } - Err(io::Error::new( - io::ErrorKind::NotConnected, - "No incoming connections!", - )) -} - -pub struct TcpConnection { - reader: Mutex, - writer: Mutex, - protocol_version: MavlinkVersion, -} - -struct TcpWrite { - socket: TcpStream, - sequence: u8, -} - -impl MavConnection for TcpConnection { - fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { - let mut lock = self.reader.lock().expect("tcp read failure"); - read_versioned_msg(&mut *lock, self.protocol_version) - } - - fn send(&self, header: &MavHeader, data: &M) -> Result { - let mut lock = self.writer.lock().unwrap(); - - let header = MavHeader { - sequence: lock.sequence, - system_id: header.system_id, - component_id: header.component_id, - }; - - lock.sequence = lock.sequence.wrapping_add(1); - write_versioned_msg(&mut lock.socket, self.protocol_version, header, data) - } - - fn set_protocol_version(&mut self, version: MavlinkVersion) { - self.protocol_version = version; - } - - fn get_protocol_version(&self) -> MavlinkVersion { - self.protocol_version - } -} diff --git a/src/connection/udp.rs b/src/connection/udp.rs deleted file mode 100644 index e9418e9a06..0000000000 --- a/src/connection/udp.rs +++ /dev/null @@ -1,193 +0,0 @@ -use crate::connection::MavConnection; -use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; -use std::io::Read; -use std::io::{self}; -use std::net::ToSocketAddrs; -use std::net::{SocketAddr, UdpSocket}; -use std::str::FromStr; -use std::sync::Mutex; - -/// UDP MAVLink connection - -pub fn select_protocol( - address: &str, -) -> io::Result + Sync + Send>> { - if address.starts_with("udpin:") { - Ok(Box::new(udpin(&address["udpin:".len()..])?)) - } else if address.starts_with("udpout:") { - Ok(Box::new(udpout(&address["udpout:".len()..])?)) - } else if address.starts_with("udpbcast:") { - Ok(Box::new(udpbcast(&address["udpbcast:".len()..])?)) - } else { - Err(io::Error::new( - io::ErrorKind::AddrNotAvailable, - "Protocol unsupported", - )) - } -} - -pub fn udpbcast(address: T) -> io::Result { - let addr = address - .to_socket_addrs() - .unwrap() - .next() - .expect("Invalid address"); - let socket = UdpSocket::bind(&SocketAddr::from_str("0.0.0.0:0").unwrap()).unwrap(); - socket - .set_broadcast(true) - .expect("Couldn't bind to broadcast address."); - UdpConnection::new(socket, false, Some(addr)) -} - -pub fn udpout(address: T) -> io::Result { - let addr = address - .to_socket_addrs() - .unwrap() - .next() - .expect("Invalid address"); - let socket = UdpSocket::bind(&SocketAddr::from_str("0.0.0.0:0").unwrap())?; - UdpConnection::new(socket, false, Some(addr)) -} - -pub fn udpin(address: T) -> io::Result { - let addr = address - .to_socket_addrs() - .unwrap() - .next() - .expect("Invalid address"); - let socket = UdpSocket::bind(&addr)?; - UdpConnection::new(socket, true, None) -} - -struct UdpWrite { - socket: UdpSocket, - dest: Option, - sequence: u8, -} - -struct PacketBuf { - buf: Vec, - start: usize, - end: usize, -} - -impl PacketBuf { - pub fn new() -> PacketBuf { - let mut v = Vec::new(); - v.resize(65536, 0); - PacketBuf { - buf: v, - start: 0, - end: 0, - } - } - - pub fn reset(&mut self) -> &mut [u8] { - self.start = 0; - self.end = 0; - &mut self.buf - } - - pub fn set_len(&mut self, size: usize) { - self.end = size; - } - - pub fn slice(&self) -> &[u8] { - &self.buf[self.start..self.end] - } - - pub fn len(&self) -> usize { - self.slice().len() - } -} - -impl Read for PacketBuf { - fn read(&mut self, buf: &mut [u8]) -> io::Result { - let n = Read::read(&mut self.slice(), buf)?; - self.start += n; - Ok(n) - } -} - -struct UdpRead { - socket: UdpSocket, - recv_buf: PacketBuf, -} - -pub struct UdpConnection { - reader: Mutex, - writer: Mutex, - protocol_version: MavlinkVersion, - server: bool, -} - -impl UdpConnection { - fn new(socket: UdpSocket, server: bool, dest: Option) -> io::Result { - Ok(UdpConnection { - server: server, - reader: Mutex::new(UdpRead { - socket: socket.try_clone()?, - recv_buf: PacketBuf::new(), - }), - writer: Mutex::new(UdpWrite { - socket: socket, - dest: dest, - sequence: 0, - }), - protocol_version: MavlinkVersion::V2, - }) - } -} - -impl MavConnection for UdpConnection { - fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { - let mut guard = self.reader.lock().unwrap(); - let state = &mut *guard; - loop { - if state.recv_buf.len() == 0 { - let (len, src) = state.socket.recv_from(state.recv_buf.reset())?; - state.recv_buf.set_len(len); - - if self.server { - self.writer.lock().unwrap().dest = Some(src); - } - } - - match read_versioned_msg(&mut state.recv_buf, self.protocol_version) { - ok @ Ok(..) => return ok, - _ => (), - } - } - } - - fn send(&self, header: &MavHeader, data: &M) -> Result { - let mut guard = self.writer.lock().unwrap(); - let state = &mut *guard; - - let header = MavHeader { - sequence: state.sequence, - system_id: header.system_id, - component_id: header.component_id, - }; - - state.sequence = state.sequence.wrapping_add(1); - - let len = if let Some(addr) = state.dest { - let mut buf = Vec::new(); - write_versioned_msg(&mut buf, self.protocol_version, header, data)?; - state.socket.send_to(&buf, addr)? - } else { - 0 - }; - - Ok(len) - } - - fn set_protocol_version(&mut self, version: MavlinkVersion) { - self.protocol_version = version; - } - - fn get_protocol_version(&self) -> MavlinkVersion { - self.protocol_version - } -} diff --git a/src/embedded.rs b/src/embedded.rs deleted file mode 100644 index 3c27b8f1bb..0000000000 --- a/src/embedded.rs +++ /dev/null @@ -1,49 +0,0 @@ -use byteorder::ByteOrder; - -use crate::error::*; - -/// Replacement for std::io::Read + byteorder::ReadBytesExt in no_std envs -pub trait Read { - fn read_u8(&mut self) -> Result; - - fn read_u16(&mut self) -> Result { - let mut buffer = [0; 2]; - self.read_exact(&mut buffer)?; - Ok(B::read_u16(&buffer)) - } - - fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), MessageReadError> { - for i in 0..buf.len() { - buf[i] = self.read_u8()?; - } - - 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>; - - fn write_u16(&mut self, n: u16) -> Result<(), MessageWriteError> { - let mut buffer = [0; 2]; - B::write_u16(&mut buffer, n); - self.write_all(&buffer) - } -} - -impl> Write for W { - fn write_all(&mut self, buf: &[u8]) -> Result<(), MessageWriteError> { - for i in 0..buf.len() { - nb::block!(self.write(buf[i])).map_err(|_| MessageWriteError::Io)?; - } - - Ok(()) - } -} diff --git a/src/lib.rs b/src/lib.rs deleted file mode 100644 index 55e6095662..0000000000 --- a/src/lib.rs +++ /dev/null @@ -1,441 +0,0 @@ -//! The MAVLink message set. -//! -//! # Message sets and the `Message` trait -//! Each message set has its own module with corresponding data types, including a `MavMessage` enum -//! that represents all possible messages in that message set. The [`Message`] trait is used to -//! represent messages in an abstract way, and each `MavMessage` enum implements this trait (for -//! example, [`ardupilotmega::MavMessage`]). This is then monomorphized to the specific message -//! set you are using in your application at compile-time via type parameters. If you expect -//! ArduPilotMega-flavored messages, then you will need a `MavConnection` -//! and you will receive `ardupilotmega::MavMessage`s from it. -//! -//! Some message sets include others. For example, all message sets except `common` include the -//! common message set. This is represented with extra values in the `MavMessage` enum: a message -//! in the common message set received on an ArduPilotMega connection will be an -//! `ardupilotmega::MavMessage::common(common::MavMessage)`. -//! -//! Please note that if you want to enable a given message set, you must also enable the -//! feature for the message sets that it includes. For example, you cannot use the `ardupilotmega` -//! feature without also using the `uavionix` and `icarous` features. -//! -#![cfg_attr(not(feature = "std"), no_std)] -#[cfg(not(feature = "std"))] -extern crate alloc; - -#[cfg(not(feature = "std"))] -use alloc::vec; -#[cfg(not(feature = "std"))] -use alloc::vec::Vec; - -use core::result::Result; - -#[cfg(feature = "std")] -use std::io::{Read, Write}; - -extern crate byteorder; -use byteorder::LittleEndian; -#[cfg(feature = "std")] -use byteorder::{ReadBytesExt, WriteBytesExt}; - -#[cfg(feature = "std")] -mod connection; -#[cfg(feature = "std")] -pub use self::connection::{connect, MavConnection}; - -#[cfg(feature = "serde")] -use serde::{Deserialize, Serialize}; - -extern crate bytes; -use crate::error::ParserError; -use bytes::{Buf, BytesMut}; - -extern crate bitflags; -extern crate num_derive; -extern crate num_traits; - -use crc_any::CRCu16; - -// include generate definitions -include!(concat!(env!("OUT_DIR"), "/mod.rs")); - -pub mod error; - -#[cfg(feature = "embedded")] -mod embedded; -#[cfg(feature = "embedded")] -use embedded::{Read, Write}; - -pub trait Message -where - Self: Sized, -{ - fn message_id(&self) -> u32; - fn message_name(&self) -> &'static str; - fn ser(&self) -> Vec; - - fn parse( - version: MavlinkVersion, - msgid: u32, - payload: &[u8], - ) -> Result; - - fn message_id_from_name(name: &str) -> Result; - fn default_message_from_id(id: u32) -> Result; - fn extra_crc(id: u32) -> u8; -} - -/// Metadata from a MAVLink packet header -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub struct MavHeader { - pub system_id: u8, - pub component_id: u8, - pub sequence: u8, -} - -/// Versions of the Mavlink protocol that we support -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -#[cfg_attr(feature = "serde", derive(Serialize))] -#[cfg_attr(feature = "serde", serde(tag = "type"))] -pub enum MavlinkVersion { - V1, - V2, -} - -/// Message framing marker for mavlink v1 -pub const MAV_STX: u8 = 0xFE; - -/// Message framing marker for mavlink v2 -pub const MAV_STX_V2: u8 = 0xFD; - -/// Return a default GCS header, seq is replaced by the connector -/// so it can be ignored. Set `component_id` to your desired component ID. -impl Default for MavHeader { - fn default() -> Self { - Self { - system_id: 255, - component_id: 0, - sequence: 0, - } - } -} - -/// Encapsulation of the Mavlink message and the header, -/// important to preserve information about the sender system -/// and component id -#[derive(Debug, Clone)] -#[cfg_attr(feature = "serde", derive(Serialize))] -pub struct MavFrame { - pub header: MavHeader, - pub msg: M, - pub protocol_version: MavlinkVersion, -} - -impl MavFrame { - /// Create a new frame with given message - // pub fn new(msg: MavMessage) -> MavFrame { - // MavFrame { - // header: MavHeader::get_default_header(), - // msg - // } - // } - - /// Serialize MavFrame into a vector, so it can be sent over a socket, for example. - pub fn ser(&self) -> Vec { - let mut v = vec![]; - - // serialize header - v.push(self.header.system_id); - v.push(self.header.component_id); - v.push(self.header.sequence); - - // message id - match self.protocol_version { - MavlinkVersion::V2 => { - let bytes: [u8; 4] = self.msg.message_id().to_le_bytes(); - v.extend_from_slice(&bytes); - } - MavlinkVersion::V1 => { - v.push(self.msg.message_id() as u8); //TODO check - } - } - - // serialize message - v.append(&mut self.msg.ser()); - - v - } - - /// Deserialize MavFrame from a slice that has been received from, for example, a socket. - pub fn deser(version: MavlinkVersion, input: &[u8]) -> Result { - let mut buf = BytesMut::from(input); - - let system_id = buf.get_u8(); - let component_id = buf.get_u8(); - let sequence = buf.get_u8(); - let header = MavHeader { - system_id, - component_id, - sequence, - }; - - let msg_id = match version { - MavlinkVersion::V2 => buf.get_u32_le(), - MavlinkVersion::V1 => buf.get_u8() as u32, - }; - - match M::parse(version, msg_id, &buf.into_iter().collect::>()) { - Ok(msg) => Ok(MavFrame { - header, - msg, - protocol_version: version, - }), - Err(err) => Err(err), - } - } - - /// Return the frame header - pub fn header(&self) -> MavHeader { - self.header - } -} - -pub fn read_versioned_msg( - r: &mut R, - version: MavlinkVersion, -) -> Result<(MavHeader, M), error::MessageReadError> { - match version { - MavlinkVersion::V2 => read_v2_msg(r), - MavlinkVersion::V1 => read_v1_msg(r), - } -} - -/// Read a MAVLink v1 message from a Read stream. -pub fn read_v1_msg( - r: &mut R, -) -> Result<(MavHeader, M), error::MessageReadError> { - loop { - if r.read_u8()? != MAV_STX { - continue; - } - let len = r.read_u8()? as usize; - let seq = r.read_u8()?; - let sysid = r.read_u8()?; - let compid = r.read_u8()?; - let msgid = r.read_u8()?; - - let mut payload_buf = [0; 255]; - let payload = &mut payload_buf[..len]; - - r.read_exact(payload)?; - - let crc = r.read_u16::()?; - - let mut crc_calc = CRCu16::crc16mcrf4cc(); - crc_calc.digest(&[len as u8, seq, sysid, compid, msgid]); - crc_calc.digest(payload); - crc_calc.digest(&[M::extra_crc(msgid.into())]); - let recvd_crc = crc_calc.get_crc(); - if recvd_crc != crc { - // bad crc: ignore message - //println!("msg id {} len {} , crc got {} expected {}", msgid, len, crc, recvd_crc ); - continue; - } - - return M::parse(MavlinkVersion::V1, msgid as u32, payload) - .map(|msg| { - ( - MavHeader { - sequence: seq, - system_id: sysid, - component_id: compid, - }, - msg, - ) - }) - .map_err(|err| err.into()); - } -} - -const MAVLINK_IFLAG_SIGNED: u8 = 0x01; - -/// Read a MAVLink v2 message from a Read stream. -pub fn read_v2_msg( - r: &mut R, -) -> Result<(MavHeader, M), error::MessageReadError> { - loop { - // search for the magic framing value indicating start of mavlink message - if r.read_u8()? != MAV_STX_V2 { - continue; - } - - // println!("Got STX2"); - let payload_len = r.read_u8()? as usize; - // println!("Got payload_len: {}", payload_len); - let incompat_flags = r.read_u8()?; - // println!("Got incompat flags: {}", incompat_flags); - let compat_flags = r.read_u8()?; - // println!("Got compat flags: {}", compat_flags); - - let seq = r.read_u8()?; - // println!("Got seq: {}", seq); - - let sysid = r.read_u8()?; - // println!("Got sysid: {}", sysid); - - let compid = r.read_u8()?; - // println!("Got compid: {}", compid); - - let mut msgid_buf = [0; 4]; - msgid_buf[0] = r.read_u8()?; - msgid_buf[1] = r.read_u8()?; - msgid_buf[2] = r.read_u8()?; - - let header_buf = &[ - payload_len as u8, - incompat_flags, - compat_flags, - seq, - sysid, - compid, - msgid_buf[0], - msgid_buf[1], - msgid_buf[2], - ]; - - let msgid: u32 = u32::from_le_bytes(msgid_buf); - // println!("Got msgid: {}", msgid); - - //provide a buffer that is the maximum payload size - let mut payload_buf = [0; 255]; - let payload = &mut payload_buf[..payload_len]; - - r.read_exact(payload)?; - - let crc = r.read_u16::()?; - - if (incompat_flags & 0x01) == MAVLINK_IFLAG_SIGNED { - let mut sign = [0; 13]; - r.read_exact(&mut sign)?; - } - - let mut crc_calc = CRCu16::crc16mcrf4cc(); - crc_calc.digest(header_buf); - crc_calc.digest(payload); - let extra_crc = M::extra_crc(msgid); - - crc_calc.digest(&[extra_crc]); - let recvd_crc = crc_calc.get_crc(); - if recvd_crc != crc { - // bad crc: ignore message - // println!("msg id {} payload_len {} , crc got {} expected {}", msgid, payload_len, crc, recvd_crc ); - continue; - } - - return M::parse(MavlinkVersion::V2, msgid, payload) - .map(|msg| { - ( - MavHeader { - sequence: seq, - system_id: sysid, - component_id: compid, - }, - msg, - ) - }) - .map_err(|err| err.into()); - } -} - -/// Write a message using the given mavlink version -pub fn write_versioned_msg( - w: &mut W, - version: MavlinkVersion, - header: MavHeader, - data: &M, -) -> Result { - match version { - MavlinkVersion::V2 => write_v2_msg(w, header, data), - MavlinkVersion::V1 => write_v1_msg(w, header, data), - } -} - -/// Write a MAVLink v2 message to a Write stream. -pub fn write_v2_msg( - w: &mut W, - header: MavHeader, - data: &M, -) -> Result { - let msgid = data.message_id(); - let payload = data.ser(); - // println!("write payload_len : {}", payload.len()); - - let header = &[ - MAV_STX_V2, - payload.len() as u8, - 0, //incompat_flags - 0, //compat_flags - header.sequence, - header.system_id, - header.component_id, - (msgid & 0x0000FF) as u8, - ((msgid & 0x00FF00) >> 8) as u8, - ((msgid & 0xFF0000) >> 16) as u8, - ]; - - // println!("write H: {:?}",header ); - - let mut crc = CRCu16::crc16mcrf4cc(); - crc.digest(&header[1..]); - // let header_crc = crc.get_crc(); - crc.digest(&payload[..]); - // let base_crc = crc.get_crc(); - let extra_crc = M::extra_crc(msgid); - // println!("write header_crc: {} base_crc: {} extra_crc: {}", - // header_crc, base_crc, extra_crc); - crc.digest(&[extra_crc]); - - let crc_bytes = crc.get_crc().to_le_bytes(); - - let len = payload.len() + header.len() + crc_bytes.len(); - - w.write_all(header)?; - w.write_all(&payload[..])?; - w.write_all(&crc_bytes)?; - - Ok(len) -} - -/// Write a MAVLink v1 message to a Write stream. -pub fn write_v1_msg( - w: &mut W, - header: MavHeader, - data: &M, -) -> Result { - let msgid = data.message_id(); - let payload = data.ser(); - - let header = &[ - MAV_STX, - payload.len() as u8, - header.sequence, - header.system_id, - header.component_id, - msgid as u8, - ]; - - let mut crc = CRCu16::crc16mcrf4cc(); - crc.digest(&header[1..]); - crc.digest(&payload[..]); - crc.digest(&[M::extra_crc(msgid)]); - - let crc_bytes = crc.get_crc().to_le_bytes(); - - let len = payload.len() + header.len() + crc_bytes.len(); - - w.write_all(header)?; - w.write_all(&payload[..])?; - w.write_all(&crc_bytes)?; - - Ok(len) -}