diff --git a/.gitignore b/.gitignore index ea8c4bf..c41cc9e 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1 @@ -/target +/target \ No newline at end of file diff --git a/Cargo.lock b/Cargo.lock index 1cb4c39..bbc68c7 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -84,9 +84,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] name = "bitflags" -version = "2.6.0" +version = "2.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b048fb63fd8b5923fc5aa7b340d8e156aec7ec02f0c78fa8a6ddc2613f6f71de" +checksum = "5c8214115b7bf84099f1309324e63141d4c5d7cc26862f97a0a857dbefe165bd" [[package]] name = "bumpalo" @@ -162,6 +162,7 @@ checksum = "19bc80abd44e4bed93ca373a0704ccbd1b710dc5749406201bb018272808dc54" dependencies = [ "anstyle", "clap_lex", + "terminal_size", ] [[package]] @@ -170,6 +171,12 @@ version = "0.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1462739cb27611015575c0c11df5df7601141071f07518d56fcc1be504cbec97" +[[package]] +name = "condtype" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf0a07a401f374238ab8e2f11a104d2851bf9ce711ec69804834de8af45c7af" + [[package]] name = "crc-any" version = "2.5.0" @@ -253,12 +260,58 @@ dependencies = [ "rand", ] +[[package]] +name = "divan" +version = "0.1.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a405457ec78b8fe08b0e32b4a3570ab5dff6dd16eb9e76a5ee0a9d9cbd898933" +dependencies = [ + "cfg-if", + "clap", + "condtype", + "divan-macros", + "libc", + "regex-lite", +] + +[[package]] +name = "divan-macros" +version = "0.1.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9556bc800956545d6420a640173e5ba7dfa82f38d3ea5a167eb555bc69ac3323" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + [[package]] name = "either" version = "1.13.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "60b1af1c220855b6ceac025d3f6ecdd2b7c4894bfe9cd9bda4fbb4bc7c0d4cf0" +[[package]] +name = "enum-primitive-derive" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba7795da175654fe16979af73f81f26a8ea27638d8d9823d317016888a63dc4c" +dependencies = [ + "num-traits", + "quote", + "syn", +] + +[[package]] +name = "errno" +version = "0.3.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "976dd42dc7e85965fe702eb8164f21f450704bdde31faefd6471dba214cb594e" +dependencies = [ + "libc", + "windows-sys 0.52.0", +] + [[package]] name = "futures" version = "0.3.31" @@ -395,7 +448,7 @@ checksum = "261f68e344040fbd0edea105bef17c66edf46f984ddb1115b775ce31be948f4b" dependencies = [ "hermit-abi 0.4.0", "libc", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -434,6 +487,12 @@ version = "0.2.159" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "561d97a539a36e26a9a5fad1ea11a3039a67714694aaa379433e580854bc3dc5" +[[package]] +name = "linux-raw-sys" +version = "0.4.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d26c52dbd32dccf2d10cac7725f8eae5296885fb5703b261f7d0a0739ec807ab" + [[package]] name = "lock_api" version = "0.4.12" @@ -446,9 +505,9 @@ dependencies = [ [[package]] name = "log" -version = "0.4.22" +version = "0.4.27" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a7a70ba024b9dc04c27ea2f0c0548feb474ec5c54bba33a7f72f873a39d07b24" +checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94" [[package]] name = "mavlink" @@ -460,6 +519,8 @@ dependencies = [ "mavlink-core", "num-derive", "num-traits", + "serde", + "serde_arrays", ] [[package]] @@ -483,10 +544,17 @@ dependencies = [ "bytes", "criterion", "dev-utils", + "divan", + "enum-primitive-derive", "futures", "log", "mavlink", + "num-traits", + "phf", "rand", + "serde", + "serde_derive", + "serde_json", "thiserror 2.0.12", "tokio", "tokio-stream", @@ -502,6 +570,8 @@ dependencies = [ "async-trait", "byteorder", "crc-any", + "serde", + "serde_arrays", "tokio", ] @@ -529,7 +599,7 @@ dependencies = [ "hermit-abi 0.3.9", "libc", "wasi", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -596,6 +666,48 @@ dependencies = [ "windows-targets", ] +[[package]] +name = "phf" +version = "0.11.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd6780a80ae0c52cc120a26a1a42c1ae51b247a253e4e06113d23d2c2edd078" +dependencies = [ + "phf_macros", + "phf_shared", +] + +[[package]] +name = "phf_generator" +version = "0.11.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3c80231409c20246a13fddb31776fb942c38553c51e871f8cbd687a4cfb5843d" +dependencies = [ + "phf_shared", + "rand", +] + +[[package]] +name = "phf_macros" +version = "0.11.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f84ac04429c13a7ff43785d75ad27569f2951ce0ffd30a3321230db2fc727216" +dependencies = [ + "phf_generator", + "phf_shared", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "phf_shared" +version = "0.11.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "67eabc2ef2a60eb7faa00097bd1ffdb5bd28e62bf39990626a582201b7a754e5" +dependencies = [ + "siphasher", +] + [[package]] name = "pin-project-lite" version = "0.2.14" @@ -665,9 +777,9 @@ dependencies = [ [[package]] name = "quote" -version = "1.0.37" +version = "1.0.40" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b5b9d34b8991d19d98081b46eacdd8eb58c6f2b201139f7c5f643cc155a633af" +checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d" dependencies = [ "proc-macro2", ] @@ -728,7 +840,7 @@ version = "0.5.7" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9b6dfecf2c74bce2466cabf93f6664d6998a69eb21e39f4207930065b27b771f" dependencies = [ - "bitflags 2.6.0", + "bitflags 2.9.0", ] [[package]] @@ -754,6 +866,12 @@ dependencies = [ "regex-syntax", ] +[[package]] +name = "regex-lite" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "53a49587ad06b26609c52e423de037e7f57f20d53535d66e08c695f347df952a" + [[package]] name = "regex-syntax" version = "0.8.5" @@ -766,6 +884,19 @@ version = "0.1.24" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "719b953e2095829ee67db738b3bfa9fa368c94900df327b3f07fe6e794d2fe1f" +[[package]] +name = "rustix" +version = "0.38.37" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8acb788b847c24f28525660c4d7758620a7210875711f79e7f663cc152726811" +dependencies = [ + "bitflags 2.9.0", + "errno", + "libc", + "linux-raw-sys", + "windows-sys 0.52.0", +] + [[package]] name = "ryu" version = "1.0.18" @@ -789,18 +920,27 @@ checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" [[package]] name = "serde" -version = "1.0.210" +version = "1.0.219" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c8e3592472072e6e22e0a54d5904d9febf8508f65fb8552499a1abc7d1078c3a" +checksum = "5f0e2c6ed6606019b4e29e69dbaba95b11854410e5347d525002456dbbb786b6" dependencies = [ "serde_derive", ] +[[package]] +name = "serde_arrays" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38636132857f68ec3d5f3eb121166d2af33cb55174c4d5ff645db6165cbef0fd" +dependencies = [ + "serde", +] + [[package]] name = "serde_derive" -version = "1.0.210" +version = "1.0.219" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "243902eda00fad750862fc144cea25caca5e20d615af0a81bee94ca738f1df1f" +checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00" dependencies = [ "proc-macro2", "quote", @@ -809,9 +949,9 @@ dependencies = [ [[package]] name = "serde_json" -version = "1.0.128" +version = "1.0.140" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6ff5456707a1de34e7e37f2a6fd3d3f808c318259cbd01ab6377795054b483d8" +checksum = "20068b6e96dc6c9bd23e01df8827e6c7e1f2fddd43c21810382803c136b99373" dependencies = [ "itoa", "memchr", @@ -828,6 +968,12 @@ dependencies = [ "libc", ] +[[package]] +name = "siphasher" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "56199f7ddabf13fe5074ce809e7d3f42b42ae711800501b5b16ea82ad029c39d" + [[package]] name = "slab" version = "0.4.9" @@ -850,7 +996,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ce305eb0b4296696835b71df73eb912e0f1ffd2556a501fcede6e0c50349191c" dependencies = [ "libc", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -864,6 +1010,16 @@ dependencies = [ "unicode-ident", ] +[[package]] +name = "terminal_size" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5352447f921fda68cf61b4101566c0bdb5104eff6804d0678e5227580ab6a4e9" +dependencies = [ + "rustix", + "windows-sys 0.59.0", +] + [[package]] name = "thiserror" version = "1.0.64" @@ -929,7 +1085,7 @@ dependencies = [ "signal-hook-registry", "socket2", "tokio-macros", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -1091,7 +1247,7 @@ version = "0.1.9" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cf221c93e13a30d793f7645a0e7762c55d169dbb0a49671918a2319d289b10bb" dependencies = [ - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -1103,6 +1259,15 @@ dependencies = [ "windows-targets", ] +[[package]] +name = "windows-sys" +version = "0.59.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b" +dependencies = [ + "windows-targets", +] + [[package]] name = "windows-targets" version = "0.52.6" diff --git a/Cargo.toml b/Cargo.toml index e329310..612fe3f 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -19,26 +19,49 @@ bench = false [dependencies] bytes = "1.10" log = "0.4" -mavlink = { default-features = false, features = ["std", "ardupilotmega", "tokio-1"], git = "https://github.com/mavlink/rust-mavlink", hash = "5f2ecbe8" } +mavlink = { default-features = false, features = [ + "std", + "ardupilotmega", + "tokio-1", +], git = "https://github.com/mavlink/rust-mavlink", hash = "5f2ecbe8" } thiserror = "2.0" tokio-util = { version = "0.7", features = ["codec"] } +# Experimenting: +# classic-bitfield = { version = "0.2.3", features = ["serde"] } +# bitflags = { version = "2.9.0", features = ["serde"]} +phf = { version = "0.11", features = ["macros"] } +enum-primitive-derive = "0.3.0" +num-traits = "0.2.0" + +# TODO: Add feature "serde" +serde = "1.0.219" +serde_derive = "1.0.219" +serde_json = "1.0.140" + [features] default = ["std"] std = [] [dev-dependencies] anyhow = "1.0" -criterion = { version = "0.5", features = ["async_tokio"] } +criterion = { version = "0.5", features = ["async_tokio"] } +divan = { version = "0.1" } dev-utils = { path = "dev_utils" } futures = "0.3" -mavlink = { default-features = false, features = ["std", "ardupilotmega", "tokio-1"], git = "https://github.com/mavlink/rust-mavlink", hash = "5f2ecbe8" } +mavlink = { default-features = false, features = [ + "std", + "ardupilotmega", + "tokio-1", + "serde", +], git = "https://github.com/mavlink/rust-mavlink", hash = "5f2ecbe8" } rand = "0.8" tokio = { version = "1", features = ["full"] } tokio-stream = "0.1" tokio-util = "0.7" tracing = "0.1" + [[bench]] name = "bench" harness = false @@ -46,3 +69,15 @@ harness = false [[bench]] name = "compatibility_bench" harness = false + +[[bench]] +name = "parser_bench" +harness = false + +[profile.profiling] +inherits = "release" +debug = 1 +jemalloc = false + +[profile.bench] +inherits = "release" diff --git a/benches/parser_bench.rs b/benches/parser_bench.rs new file mode 100644 index 0000000..5985817 --- /dev/null +++ b/benches/parser_bench.rs @@ -0,0 +1,851 @@ +use criterion::{ + black_box, criterion_group, criterion_main, AxisScale, BatchSize, BenchmarkId, Criterion, + PlotConfiguration, Throughput, +}; + +use mavlink::{Message, MessageData}; +use mavlink_codec::{codec::MavlinkCodec, v2::V2Packet, Packet}; +use rand::{rngs::StdRng, seq::SliceRandom as _, SeedableRng}; +use serde_derive::{Deserialize, Serialize}; +use tokio_util::codec::Decoder; + +/// Adapted from mavlink2rest and mavlink-server +#[derive(Clone, Debug, Deserialize, Serialize)] +pub struct MAVLinkMessage { + pub header: mavlink::MavHeader, + pub message: T, +} + +fn add_random_v2_message(buf: &mut Vec, rng: &mut StdRng) { + use rand::Rng; + + let header = mavlink::MavHeader { + system_id: rng.gen_range(1..255), + component_id: rng.gen_range(1..255), + sequence: rng.gen_range(0..255), + }; + + loop { + // let message_id = rng.gen_range(0..2 ^ 24); + let message_id = *[ + mavlink::ardupilotmega::RC_CHANNELS_DATA::ID, + mavlink::ardupilotmega::HEARTBEAT_DATA::ID, + // Add more message types + ] + .choose(rng) + .unwrap(); + + if let Ok(data) = mavlink::ardupilotmega::MavMessage::default_message_from_id(message_id) { + if mavlink::write_v2_msg(buf, header, &data).is_ok() { + break; + } + }; + } +} + +fn benchmark_packet_to_mavframe(c: &mut Criterion) { + let seed = 42; + println!("Using seed {seed:?}"); + let mut rng: StdRng = SeedableRng::seed_from_u64(seed); + + let mut group = c.benchmark_group("packet_to_mavframe"); + group + .confidence_level(0.95) + .sample_size(1000) + .plot_config(PlotConfiguration::default().summary_scale(AxisScale::Logarithmic)); + + for messages_count in &vec![1, 100, 10000] { + group.throughput(Throughput::Elements(*messages_count)); + + let mut buf: Vec = + Vec::with_capacity(V2Packet::MAX_PACKET_SIZE * *messages_count as usize); + for _ in 0..*messages_count { + add_random_v2_message(&mut buf, &mut rng); + } + + let mut decoded_packets = Vec::with_capacity(*messages_count as usize); + + let rt = tokio::runtime::Runtime::new().unwrap(); + rt.block_on(async { + let mut buf = bytes::BytesMut::from(buf.as_slice()); + let mut codec = MavlinkCodec::::default(); + + for _ in 0..*messages_count { + let decodec_packet = codec.decode(&mut buf).unwrap().unwrap().unwrap(); + decoded_packets.push(decodec_packet) + } + }); + + if *messages_count == 1 { + group.bench_function(BenchmarkId::new("new_single_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets[0].clone(), + |packet| black_box(mavframe_from_packet_new(&packet)), + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_single_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets[0].clone(), + |packet| black_box(mavframe_from_packet_old(&packet)), + BatchSize::SmallInput, + ) + }); + } + + group.bench_function(BenchmarkId::new("new_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets + .iter() + .map(|packet| black_box(mavframe_from_packet_new(packet))) + .collect::>() + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets + .iter() + .map(|packet| black_box(mavframe_from_packet_old(packet))) + .collect::>() + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("new_drop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets.iter().for_each(|packet| { + let _packet = black_box(mavframe_from_packet_new(packet)); + }) + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_drop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets.iter().for_each(|packet| { + let _packet = black_box(mavframe_from_packet_old(packet)); + }) + }, + BatchSize::SmallInput, + ) + }); + } + + group.finish(); +} + +fn benchmark_packet_to_json_value(c: &mut Criterion) { + let seed = 42; + println!("Using seed {seed:?}"); + let mut rng: StdRng = SeedableRng::seed_from_u64(seed); + + let mut group = c.benchmark_group("packet_to_json_value"); + group + .confidence_level(0.95) + .sample_size(100) + .plot_config(PlotConfiguration::default().summary_scale(AxisScale::Logarithmic)); + + for messages_count in &vec![1] { + group.throughput(Throughput::Elements(*messages_count)); + + let mut buf: Vec = + Vec::with_capacity(V2Packet::MAX_PACKET_SIZE * *messages_count as usize); + for _ in 0..*messages_count { + add_random_v2_message(&mut buf, &mut rng); + } + + let mut decoded_packets = Vec::with_capacity(*messages_count as usize); + + let rt = tokio::runtime::Runtime::new().unwrap(); + rt.block_on(async { + let mut buf = bytes::BytesMut::from(buf.as_slice()); + let mut codec = MavlinkCodec::::default(); + + for _ in 0..*messages_count { + let decodec_packet = codec.decode(&mut buf).unwrap().unwrap().unwrap(); + decoded_packets.push(decodec_packet) + } + }); + + if *messages_count == 1 { + group.bench_function(BenchmarkId::new("new_single_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets[0].clone(), + |packet| { + let frame = mavframe_from_packet_new(&packet); + + black_box(serde_json::to_value(&frame).unwrap()) + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_single_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets[0].clone(), + |packet| { + let frame = mavframe_from_packet_old(&packet); + + black_box(serde_json::to_value(&frame).unwrap()) + }, + BatchSize::SmallInput, + ) + }); + } + + group.bench_function(BenchmarkId::new("new_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets + .iter() + .map(|packet| { + let frame = mavframe_from_packet_new(packet); + + black_box(serde_json::to_value(&frame).unwrap()) + }) + .collect::>() + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets + .iter() + .map(|packet| { + let frame = mavframe_from_packet_old(packet); + + black_box(serde_json::to_value(&frame).unwrap()) + }) + .collect::>() + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("new_drop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets.iter().for_each(|packet| { + let frame = mavframe_from_packet_new(packet); + + let _ret = black_box(serde_json::to_value(&frame).unwrap()); + }) + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_drop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets.iter().for_each(|packet| { + let frame = mavframe_from_packet_old(packet); + + let _ret = black_box(serde_json::to_value(&frame).unwrap()); + }) + }, + BatchSize::SmallInput, + ) + }); + } + + group.finish(); +} + +fn benchmark_get_json_fields(c: &mut Criterion) { + let seed = 42; + println!("Using seed {seed:?}"); + let mut rng: StdRng = SeedableRng::seed_from_u64(seed); + + let mut group = c.benchmark_group("get_json_fields"); + group + .confidence_level(0.95) + .sample_size(100) + .plot_config(PlotConfiguration::default().summary_scale(AxisScale::Logarithmic)); + + for messages_count in &vec![1, 10000] { + group.throughput(Throughput::Elements(*messages_count)); + + let mut buf: Vec = + Vec::with_capacity(V2Packet::MAX_PACKET_SIZE * *messages_count as usize); + for _ in 0..*messages_count { + add_random_v2_message(&mut buf, &mut rng); + } + + let mut decoded_packets = Vec::with_capacity(*messages_count as usize); + + let rt = tokio::runtime::Runtime::new().unwrap(); + rt.block_on(async { + let mut buf = bytes::BytesMut::from(buf.as_slice()); + let mut codec = MavlinkCodec::::default(); + + for _ in 0..*messages_count { + let decodec_packet = codec.decode(&mut buf).unwrap().unwrap().unwrap(); + decoded_packets.push(decodec_packet) + } + }); + + if *messages_count == 1 { + group.bench_function(BenchmarkId::new("new_single_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets[0].clone(), + |packet| { + let frame = mavframe_from_packet_new(&packet); + + black_box(get_json_fields_new(frame)) + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_single_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets[0].clone(), + |packet| { + let frame = mavframe_from_packet_old(&packet); + + black_box(get_json_fields_old(frame)) + }, + BatchSize::SmallInput, + ) + }); + } + + group.bench_function(BenchmarkId::new("new_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets + .iter() + .map(|packet| { + let frame = mavframe_from_packet_new(packet); + + black_box(get_json_fields_new(frame)) + }) + .collect::>() + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets + .iter() + .map(|packet| { + let frame = mavframe_from_packet_old(packet); + + black_box(get_json_fields_old(frame)) + }) + .collect::>() + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("new_drop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets.iter().for_each(|packet| { + let frame = mavframe_from_packet_new(packet); + + let _fields = black_box(get_json_fields_new(frame)); + }) + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_drop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets.iter().for_each(|packet| { + let frame = mavframe_from_packet_old(packet); + + let _fields = black_box(get_json_fields_old(frame)); + }) + }, + BatchSize::SmallInput, + ) + }); + } + + group.finish(); +} + +fn get_json_fields_new( + frame: mavlink_codec::mav_types::mav_frame::MavFrame, +) -> Vec<(String, String)> { + let message = frame.message(); + + let message_fields = mavlink_codec::mav_types::mav_message::MavMessageFields::fields(&message); + + message_fields + .entries() + .map(|(message_field_name, message_field_function)| { + let message_field_value = message_field_function(&message); + + ( + format!( + "{}/{}/{}/{}", + frame.header().system_id(), + frame.header().component_id(), + frame.message().id(), + message_field_name + ), + message_field_value.to_string(), + ) + }) + .collect::>() +} + +fn get_json_fields_old( + frame: MAVLinkMessage, +) -> Vec<(String, String)> { + let value = serde_json::to_value(&frame).unwrap(); + + let serde_json::Value::Object(message_fields) = value else { + unreachable!() + }; + + message_fields + .iter() + .map(|(message_field_name, message_field_value)| { + ( + format!( + "{}/{}/{}/{}", + frame.header.system_id, + frame.header.component_id, + frame.message.message_id(), + message_field_name + ), + message_field_value.to_string(), + ) + }) + .collect::>() +} + +fn benchmark_packet_to_json_string(c: &mut Criterion) { + let seed = 42; + println!("Using seed {seed:?}"); + let mut rng: StdRng = SeedableRng::seed_from_u64(seed); + + let mut group = c.benchmark_group("packet_to_json_string"); + group + .confidence_level(0.95) + .sample_size(100) + .plot_config(PlotConfiguration::default().summary_scale(AxisScale::Logarithmic)); + + for messages_count in &vec![1, 10000] { + group.throughput(Throughput::Elements(*messages_count)); + + let mut buf: Vec = + Vec::with_capacity(V2Packet::MAX_PACKET_SIZE * *messages_count as usize); + for _ in 0..*messages_count { + add_random_v2_message(&mut buf, &mut rng); + } + + let mut decoded_packets = Vec::with_capacity(*messages_count as usize); + + tokio::runtime::Runtime::new().unwrap().block_on(async { + let mut buf = bytes::BytesMut::from(buf.as_slice()); + let mut codec = MavlinkCodec::::default(); + + for _ in 0..*messages_count { + let decodec_packet = codec.decode(&mut buf).unwrap().unwrap().unwrap(); + decoded_packets.push(decodec_packet) + } + }); + + if *messages_count == 1 { + group.bench_function(BenchmarkId::new("new_single_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets[0].clone(), + |packet| { + let frame = mavframe_from_packet_new(&packet); + + black_box(serde_json::to_string_pretty(&frame).unwrap()) + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_single_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets[0].clone(), + |packet| { + let frame = mavframe_from_packet_old(&packet); + + black_box(serde_json::to_string_pretty(&frame).unwrap()) + }, + BatchSize::SmallInput, + ) + }); + } + + group.bench_function(BenchmarkId::new("new_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets + .iter() + .map(|packet| { + let frame = mavframe_from_packet_new(packet); + + black_box(serde_json::to_string_pretty(&frame).unwrap()) + }) + .collect::>() + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_nodrop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets + .iter() + .map(|packet| { + let frame = mavframe_from_packet_old(packet); + + black_box(serde_json::to_string_pretty(&frame).unwrap()) + }) + .collect::>() + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("new_drop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets.iter().for_each(|packet| { + let frame = mavframe_from_packet_new(packet); + + let _ret = black_box(serde_json::to_string_pretty(&frame).unwrap()); + }) + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_drop", messages_count), |b| { + b.iter_batched( + || decoded_packets.clone(), + |decoded_packets| { + decoded_packets.iter().for_each(|packet| { + let frame = mavframe_from_packet_old(packet); + + let _ret = black_box(serde_json::to_string_pretty(&frame).unwrap()); + }) + }, + BatchSize::SmallInput, + ) + }); + } + + group.finish(); +} + +fn benchmark_from_json_string_to_frame(c: &mut Criterion) { + let seed = 42; + println!("Using seed {seed:?}"); + let mut rng: StdRng = SeedableRng::seed_from_u64(seed); + + let mut group = c.benchmark_group("from_json_string_to_frame"); + group + .confidence_level(0.95) + .sample_size(100) + .plot_config(PlotConfiguration::default().summary_scale(AxisScale::Logarithmic)); + + for messages_count in &vec![1, 100, 10000] { + group.throughput(Throughput::Elements(*messages_count)); + + let mut json_strings: Vec = Vec::with_capacity(*messages_count as usize); + + for _ in 0..*messages_count { + use rand::Rng; + + let header = mavlink::MavHeader { + system_id: rng.gen_range(1..255), + component_id: rng.gen_range(1..255), + sequence: rng.gen_range(0..255), + }; + + loop { + // let message_id = rng.gen_range(0..2 ^ 24); + let message_id = *[ + mavlink::ardupilotmega::RC_CHANNELS_DATA::ID, + mavlink::ardupilotmega::HEARTBEAT_DATA::ID, + // Add more message types + ] + .choose(&mut rng) + .unwrap(); + + if let Ok(message) = + mavlink::ardupilotmega::MavMessage::default_message_from_id(message_id) + { + let frame = MAVLinkMessage { header, message }; + + let json_string = serde_json::to_string_pretty(&frame).unwrap(); + + json_strings.push(json_string); + break; + }; + } + } + + group.bench_function(BenchmarkId::new("new_nodrop", messages_count), |b| { + b.iter_batched( + || json_strings.clone(), + |json_strings| { + json_strings + .iter() + .map(|json_string| black_box(mavframe_from_string_new(json_string))) + .collect::>() + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_nodrop", messages_count), |b| { + b.iter_batched( + || json_strings.clone(), + |json_strings| { + json_strings + .iter() + .map(|json_string| black_box(mavframe_from_string_old(json_string))) + .collect::>() + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("new_drop", messages_count), |b| { + b.iter_batched( + || json_strings.clone(), + |json_strings| { + json_strings.iter().for_each(|json_string| { + let _ret = black_box(mavframe_from_string_new(json_string)); + }) + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_drop", messages_count), |b| { + b.iter_batched( + || json_strings.clone(), + |json_strings| { + json_strings.iter().for_each(|json_string| { + let _ret = black_box(mavframe_from_string_old(json_string)); + }) + }, + BatchSize::SmallInput, + ) + }); + } + + group.finish(); +} + +fn benchmark_from_json_string_to_packet(c: &mut Criterion) { + let seed = 42; + println!("Using seed {seed:?}"); + let mut rng: StdRng = SeedableRng::seed_from_u64(seed); + + let mut group = c.benchmark_group("from_json_string_to_packet"); + group + .confidence_level(0.95) + .sample_size(100) + .plot_config(PlotConfiguration::default().summary_scale(AxisScale::Logarithmic)); + + for messages_count in &vec![1, 100, 10000] { + group.throughput(Throughput::Elements(*messages_count)); + + let mut json_strings: Vec = Vec::with_capacity(*messages_count as usize); + + for _ in 0..*messages_count { + use rand::Rng; + + let header = mavlink::MavHeader { + system_id: rng.gen_range(1..255), + component_id: rng.gen_range(1..255), + sequence: rng.gen_range(0..255), + }; + + loop { + // let message_id = rng.gen_range(0..2 ^ 24); + let message_id = *[ + mavlink::ardupilotmega::RC_CHANNELS_DATA::ID, + mavlink::ardupilotmega::HEARTBEAT_DATA::ID, + // Add more message types + ] + .choose(&mut rng) + .unwrap(); + + if let Ok(message) = + mavlink::ardupilotmega::MavMessage::default_message_from_id(message_id) + { + let frame = MAVLinkMessage { header, message }; + + let json_string = serde_json::to_string_pretty(&frame).unwrap(); + + json_strings.push(json_string); + break; + }; + } + } + + group.bench_function(BenchmarkId::new("new_nodrop", messages_count), |b| { + b.iter_batched( + || json_strings.clone(), + |json_strings| { + json_strings + .iter() + .map(|json_string| black_box(packet_from_string_new(json_string))) + .collect::>() + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_nodrop", messages_count), |b| { + b.iter_batched( + || json_strings.clone(), + |json_strings| { + json_strings + .iter() + .map(|json_string| black_box(packet_from_string_old(json_string))) + .collect::>() + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("new_drop", messages_count), |b| { + b.iter_batched( + || json_strings.clone(), + |json_strings| { + json_strings.iter().for_each(|json_string| { + let _ret = black_box(packet_from_string_new(json_string)); + }) + }, + BatchSize::SmallInput, + ) + }); + + group.bench_function(BenchmarkId::new("old_drop", messages_count), |b| { + b.iter_batched( + || json_strings.clone(), + |json_strings| { + json_strings.iter().for_each(|json_string| { + let _ret = black_box(packet_from_string_old(json_string)); + }) + }, + BatchSize::SmallInput, + ) + }); + } + + group.finish(); +} + +#[inline(always)] +fn mavframe_from_string_new( + json_string: &str, +) -> mavlink_codec::mav_types::mav_frame::serde_impl::MavFrameSemanticModel { + serde_json::from_str(json_string).unwrap() +} + +#[inline(always)] +fn mavframe_from_string_old( + json_string: &str, +) -> MAVLinkMessage { + serde_json::from_str(json_string).unwrap() +} + +#[inline(always)] +fn packet_from_string_new(json_string: &str) -> Packet { + let frame: mavlink_codec::mav_types::mav_frame::MavFrame = + serde_json::from_str(json_string).unwrap(); + + Packet::from(frame) +} + +#[inline(always)] +fn packet_from_string_old(json_string: &str) -> Packet { + let frame: MAVLinkMessage = + serde_json::from_str(json_string).unwrap(); + + let mut message_raw = mavlink::MAVLinkV2MessageRaw::new(); + message_raw.serialize_message(frame.header, &frame.message); + + Packet::from(message_raw) +} + +#[inline(always)] +fn mavframe_from_packet_new(packet: &Packet) -> mavlink_codec::mav_types::mav_frame::MavFrame { + mavlink_codec::mav_types::mav_frame::MavFrame::from(packet) +} + +#[inline(always)] +fn mavframe_from_packet_old(packet: &Packet) -> MAVLinkMessage { + let header = mavlink::MavHeader { + sequence: packet.sequence(), + system_id: packet.system_id(), + component_id: packet.component_id(), + }; + + let version = match &packet { + Packet::V1(_) => mavlink::MavlinkVersion::V1, + Packet::V2(_) => mavlink::MavlinkVersion::V2, + }; + + let message = mavlink::ardupilotmega::MavMessage::parse( + version, + u32::from(packet.message_id()), + packet.payload(), + ) + .unwrap(); + + MAVLinkMessage { header, message } +} + +criterion_group!( + benches, + // benchmark_packet_to_mavframe, + // benchmark_packet_to_json_value, + benchmark_get_json_fields, + // benchmark_packet_to_json_string, + // benchmark_from_json_string_to_frame, + // benchmark_from_json_string_to_packet, +); +criterion_main!(benches); diff --git a/benches/parser_bench_divan.rs b/benches/parser_bench_divan.rs new file mode 100644 index 0000000..42adb31 --- /dev/null +++ b/benches/parser_bench_divan.rs @@ -0,0 +1,119 @@ +use divan::{AllocProfiler, Bencher}; +use serde::{Deserialize, Serialize}; + +const LENS: &[usize] = &[100]; + +#[global_allocator] +static ALLOC: AllocProfiler = AllocProfiler::system(); + +/// Adapted from mavlink2rest and mavlink-server +#[derive(Clone, Debug, Deserialize, Serialize)] +pub struct MAVLinkMessage { + pub header: mavlink::MavHeader, + pub message: T, +} + +#[divan::bench_group(sample_size = 100)] +mod from_json_string_to_packet { + use mavlink::{Message as _, MessageData as _}; + use mavlink_codec::Packet; + use rand::{rngs::StdRng, seq::SliceRandom, SeedableRng}; + + use super::*; + + fn prepare(messages_count: usize) -> Vec { + let seed = 42; + let mut rng: StdRng = SeedableRng::seed_from_u64(seed); + + let mut json_strings: Vec = Vec::with_capacity(messages_count as usize); + + for _ in 0..messages_count { + use rand::Rng; + + let header = mavlink::MavHeader { + system_id: rng.gen_range(1..255), + component_id: rng.gen_range(1..255), + sequence: rng.gen_range(0..255), + }; + + loop { + // let message_id = rng.gen_range(0..2 ^ 24); + let message_id = *[ + mavlink::ardupilotmega::RC_CHANNELS_DATA::ID, + mavlink::ardupilotmega::HEARTBEAT_DATA::ID, + // Add more message types + ] + .choose(&mut rng) + .unwrap(); + + if let Ok(message) = + mavlink::ardupilotmega::MavMessage::default_message_from_id(message_id) + { + let frame = MAVLinkMessage { header, message }; + + let json_string = serde_json::to_string_pretty(&frame).unwrap(); + + json_strings.push(json_string); + break; + }; + } + } + + json_strings + } + + #[inline(always)] + fn packet_from_string_new(json_string: &str) -> Packet { + let frame: mavlink_codec::mav_types::mav_frame::MavFrame = + serde_json::from_str(json_string).unwrap(); + + Packet::from(frame) + } + + #[inline(always)] + fn packet_from_string_old(json_string: &str) -> Packet { + let frame: MAVLinkMessage = + serde_json::from_str(json_string).unwrap(); + + let mut message_raw = mavlink::MAVLinkV2MessageRaw::new(); + message_raw.serialize_message(frame.header, &frame.message); + + Packet::from(message_raw) + } + + #[divan::bench(args = LENS)] + fn new(bencher: Bencher, messages_count: usize) { + let json_strings = prepare(messages_count); + let packets: Vec = Vec::with_capacity(messages_count); + + bencher + .counter(divan::counter::ItemsCount::new(messages_count)) + .with_inputs(|| (json_strings.clone(), packets.clone())) + .bench_refs(|(json_strings, packets)| { + json_strings.iter().for_each(|json_string| { + let packet = packet_from_string_new(json_string); + packets.push(packet); + }); + }); + } + + #[divan::bench(args = LENS)] + fn old(bencher: Bencher, messages_count: usize) { + let json_strings = prepare(messages_count); + let packets: Vec = Vec::with_capacity(messages_count); + + bencher + .counter(divan::counter::ItemsCount::new(messages_count)) + .with_inputs(|| (json_strings.clone(), packets.clone())) + .bench_refs(|(json_strings, packets)| { + json_strings.iter().for_each(|json_string| { + let packet = packet_from_string_old(json_string); + packets.push(packet); + }); + }); + } +} + +fn main() { + divan::main(); +} diff --git a/dev/rust-mavlink-generated-example.rs b/dev/rust-mavlink-generated-example.rs new file mode 100644 index 0000000..1861e32 --- /dev/null +++ b/dev/rust-mavlink-generated-example.rs @@ -0,0 +1,454 @@ +#![doc = "This file was automatically generated, do not edit"] +use bitflags::bitflags; +use mavlink_core::{bytes::Bytes, bytes_mut::BytesMut, MavlinkVersion, Message, MessageData}; +use num_derive::FromPrimitive; +use num_derive::ToPrimitive; +use num_traits::FromPrimitive; +use num_traits::ToPrimitive; +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct HEARTBEAT_DATA { + pub custom_mode: u32, + pub mavtype: MavType, + pub autopilot: MavAutopilot, + pub base_mode: MavModeFlag, + pub system_status: MavState, + pub mavlink_version: u8, +} + +impl HEARTBEAT_DATA { + pub const ENCODED_LEN: usize = 9usize; + pub const DEFAULT: Self = Self { + custom_mode: 0_u32, + mavtype: MavType::DEFAULT, + autopilot: MavAutopilot::DEFAULT, + base_mode: MavModeFlag::DEFAULT, + system_status: MavState::DEFAULT, + mavlink_version: 0_u8, + }; +} + +impl Default for HEARTBEAT_DATA { + fn default() -> Self { + Self::DEFAULT.clone() + } +} + +impl MessageData for HEARTBEAT_DATA { + type Message = mav_message; + const ID: u32 = 0u32; + const NAME: &'static str = "HEARTBEAT"; + const EXTRA_CRC: u8 = 50u8; + const ENCODED_LEN: usize = 9usize; + fn deser( + _version: MavlinkVersion, + __input: &[u8], + ) -> Result { + let avail_len = __input.len(); + let mut payload_buf = [0; Self::ENCODED_LEN]; + let mut buf = if avail_len < Self::ENCODED_LEN { + payload_buf[0..avail_len].copy_from_slice(__input); + Bytes::new(&payload_buf) + } else { + Bytes::new(__input) + }; + let mut __struct = Self::default(); + __struct.custom_mode = buf.get_u32_le(); + let tmp = buf.get_u8(); + __struct.mavtype = + FromPrimitive::from_u8(tmp).ok_or(::mavlink_core::error::ParserError::InvalidEnum { + enum_type: "MavType", + value: tmp as u32, + })?; + let tmp = buf.get_u8(); + __struct.autopilot = + FromPrimitive::from_u8(tmp).ok_or(::mavlink_core::error::ParserError::InvalidEnum { + enum_type: "MavAutopilot", + value: tmp as u32, + })?; + let tmp = buf.get_u8(); + __struct.base_mode = MavModeFlag::from_bits(tmp & MavModeFlag::all().bits()).ok_or( + ::mavlink_core::error::ParserError::InvalidFlag { + flag_type: "MavModeFlag", + value: tmp as u32, + }, + )?; + let tmp = buf.get_u8(); + __struct.system_status = + FromPrimitive::from_u8(tmp).ok_or(::mavlink_core::error::ParserError::InvalidEnum { + enum_type: "MavState", + value: tmp as u32, + })?; + __struct.mavlink_version = buf.get_u8(); + Ok(__struct) + } + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { + let mut __tmp = BytesMut::new(bytes); + #[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() + ) + } + __tmp.put_u32_le(self.custom_mode); + __tmp.put_u8(self.mavtype as u8); + __tmp.put_u8(self.autopilot as u8); + __tmp.put_u8(self.base_mode.bits()); + __tmp.put_u8(self.system_status as u8); + __tmp.put_u8(self.mavlink_version); + if matches!(version, MavlinkVersion::V2) { + let len = __tmp.len(); + ::mavlink_core::utils::remove_trailing_zeroes(&bytes[..len]) + } else { + __tmp.len() + } + } +} + +#[derive(Debug, Copy, Clone, PartialEq, FromPrimitive, ToPrimitive, Serialize, Deserialize)] +#[serde(tag = "type")] +#[repr(u32)] +pub enum MavType { + MAV_TYPE_GENERIC = 0, + MAV_TYPE_FIXED_WING = 1, + MAV_TYPE_QUADROTOR = 2, + MAV_TYPE_COAXIAL = 3, + MAV_TYPE_HELICOPTER = 4, + MAV_TYPE_ANTENNA_TRACKER = 5, + MAV_TYPE_GCS = 6, + MAV_TYPE_AIRSHIP = 7, + MAV_TYPE_FREE_BALLOON = 8, + MAV_TYPE_ROCKET = 9, + MAV_TYPE_GROUND_ROVER = 10, + MAV_TYPE_SURFACE_BOAT = 11, + MAV_TYPE_SUBMARINE = 12, + MAV_TYPE_HEXAROTOR = 13, + MAV_TYPE_OCTOROTOR = 14, + MAV_TYPE_TRICOPTER = 15, + MAV_TYPE_FLAPPING_WING = 16, + MAV_TYPE_KITE = 17, + MAV_TYPE_ONBOARD_CONTROLLER = 18, + MAV_TYPE_VTOL_TAILSITTER_DUOROTOR = 19, + MAV_TYPE_VTOL_TAILSITTER_QUADROTOR = 20, + MAV_TYPE_VTOL_TILTROTOR = 21, + MAV_TYPE_VTOL_FIXEDROTOR = 22, + MAV_TYPE_VTOL_TAILSITTER = 23, + MAV_TYPE_VTOL_TILTWING = 24, + MAV_TYPE_VTOL_RESERVED5 = 25, + MAV_TYPE_GIMBAL = 26, + MAV_TYPE_ADSB = 27, + MAV_TYPE_PARAFOIL = 28, + MAV_TYPE_DODECAROTOR = 29, + MAV_TYPE_CAMERA = 30, + MAV_TYPE_CHARGING_STATION = 31, + MAV_TYPE_FLARM = 32, + MAV_TYPE_SERVO = 33, + MAV_TYPE_ODID = 34, + MAV_TYPE_DECAROTOR = 35, + MAV_TYPE_BATTERY = 36, + MAV_TYPE_PARACHUTE = 37, + MAV_TYPE_LOG = 38, + MAV_TYPE_OSD = 39, + MAV_TYPE_IMU = 40, + MAV_TYPE_GPS = 41, + MAV_TYPE_WINCH = 42, +} +impl MavType { + pub const DEFAULT: Self = Self::MAV_TYPE_GENERIC; +} +impl Default for MavType { + fn default() -> Self { + Self::DEFAULT + } +} + +#[derive(Debug, Copy, Clone, PartialEq, FromPrimitive, ToPrimitive, Serialize, Deserialize)] +#[serde(tag = "type")] +#[repr(u32)] +pub enum MavAutopilot { + MAV_AUTOPILOT_GENERIC = 0, + MAV_AUTOPILOT_RESERVED = 1, + MAV_AUTOPILOT_SLUGS = 2, + MAV_AUTOPILOT_ARDUPILOTMEGA = 3, + MAV_AUTOPILOT_OPENPILOT = 4, + MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY = 5, + MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY = 6, + MAV_AUTOPILOT_GENERIC_MISSION_FULL = 7, + MAV_AUTOPILOT_INVALID = 8, + MAV_AUTOPILOT_PPZ = 9, + MAV_AUTOPILOT_UDB = 10, + MAV_AUTOPILOT_FP = 11, + MAV_AUTOPILOT_PX4 = 12, + MAV_AUTOPILOT_SMACCMPILOT = 13, + MAV_AUTOPILOT_AUTOQUAD = 14, + MAV_AUTOPILOT_ARMAZILA = 15, + MAV_AUTOPILOT_AEROB = 16, + MAV_AUTOPILOT_ASLUAV = 17, + MAV_AUTOPILOT_SMARTAP = 18, + MAV_AUTOPILOT_AIRRAILS = 19, + MAV_AUTOPILOT_REFLEX = 20, +} +impl MavAutopilot { + pub const DEFAULT: Self = Self::MAV_AUTOPILOT_GENERIC; +} +impl Default for MavAutopilot { + fn default() -> Self { + Self::DEFAULT + } +} + +bitflags! { + #[derive (Serialize , Deserialize)] + pub struct MavModeFlag : u8 { + const MAV_MODE_FLAG_SAFETY_ARMED = 128 ; + const MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64 ; + const MAV_MODE_FLAG_HIL_ENABLED = 32 ; + const MAV_MODE_FLAG_STABILIZE_ENABLED = 16 ; + const MAV_MODE_FLAG_GUIDED_ENABLED = 8 ; + const MAV_MODE_FLAG_AUTO_ENABLED = 4 ; + const MAV_MODE_FLAG_TEST_ENABLED = 2 ; + const MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 ; + } +} +impl MavModeFlag { + pub const DEFAULT: Self = Self::MAV_MODE_FLAG_SAFETY_ARMED; +} +impl Default for MavModeFlag { + fn default() -> Self { + Self::DEFAULT + } +} + +#[derive(Debug, Copy, Clone, PartialEq, FromPrimitive, ToPrimitive, Serialize, Deserialize)] +#[serde(tag = "type")] +#[repr(u32)] +pub enum MavState { + MAV_STATE_UNINIT = 0, + MAV_STATE_BOOT = 1, + MAV_STATE_CALIBRATING = 2, + MAV_STATE_STANDBY = 3, + MAV_STATE_ACTIVE = 4, + MAV_STATE_CRITICAL = 5, + MAV_STATE_EMERGENCY = 6, + MAV_STATE_POWEROFF = 7, + MAV_STATE_FLIGHT_TERMINATION = 8, +} +impl MavState { + pub const DEFAULT: Self = Self::MAV_STATE_UNINIT; +} +impl Default for MavState { + fn default() -> Self { + Self::DEFAULT + } +} + +#[derive(Clone, PartialEq, Debug, Serialize, Deserialize)] +#[serde(tag = "type")] +#[repr(u32)] +pub enum mav_message { + HEARTBEAT(HEARTBEAT_DATA), + // ... thousands of others +} + +impl Message for mav_message { + fn parse( + version: MavlinkVersion, + id: u32, + payload: &[u8], + ) -> Result { + match id { + HEARTBEAT_DATA::ID => HEARTBEAT_DATA::deser(version, payload).map(Self::HEARTBEAT), + // ... thousands of others + _ => Err(::mavlink_core::error::ParserError::UnknownMessage { id }), + } + } + + fn message_name(&self) -> &'static str { + match self { + Self::HEARTBEAT(..) => HEARTBEAT_DATA::NAME, + // ... thousands of others + } + } + + fn message_id(&self) -> u32 { + match self { + Self::HEARTBEAT(..) => HEARTBEAT_DATA::ID, + // ... thousands of others + } + } + + fn message_id_from_name(name: &str) -> Result { + match name { + HEARTBEAT_DATA::NAME => HEARTBEAT_DATA::ID, + // ... thousands of others + _ => Err(::mavlink_core::error::ParserError::UnknownMessage { name }), + } + } + fn default_message_from_id(id: u32) -> Result { + match id { + HEARTBEAT_DATA::ID => HEARTBEAT_DATA::default(), + // ... thousands of others + _ => Err(::mavlink_core::error::ParserError::UnknownMessage { id }), + } + } + + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { + match self { + HEARTBEAT_DATA::ID => HEARTBEAT_DATA::ser(), + // ... thousands of others + } + } + + fn extra_crc(id: u32) -> u8 { + match id { + HEARTBEAT_DATA::ID => HEARTBEAT_DATA::EXTRA_CRC, + // ... thousands of others + _ => Err(::mavlink_core::error::ParserError::UnknownMessage { id }), + } + } +} + +////////////////////////////////////////////////////////////////////// + +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub struct RC_CHANNELS_DATA { + pub time_boot_ms: u32, + pub chan1_raw: u16, + pub chan2_raw: u16, + pub chan3_raw: u16, + pub chan4_raw: u16, + pub chan5_raw: u16, + pub chan6_raw: u16, + pub chan7_raw: u16, + pub chan8_raw: u16, + pub chan9_raw: u16, + pub chan10_raw: u16, + pub chan11_raw: u16, + pub chan12_raw: u16, + pub chan13_raw: u16, + pub chan14_raw: u16, + pub chan15_raw: u16, + pub chan16_raw: u16, + pub chan17_raw: u16, + pub chan18_raw: u16, + pub chancount: u8, + pub rssi: u8, +} +impl RC_CHANNELS_DATA { + pub const ENCODED_LEN: usize = 42usize; + pub const DEFAULT: Self = Self { + time_boot_ms: 0_u32, + chan1_raw: 0_u16, + chan2_raw: 0_u16, + chan3_raw: 0_u16, + chan4_raw: 0_u16, + chan5_raw: 0_u16, + chan6_raw: 0_u16, + chan7_raw: 0_u16, + chan8_raw: 0_u16, + chan9_raw: 0_u16, + chan10_raw: 0_u16, + chan11_raw: 0_u16, + chan12_raw: 0_u16, + chan13_raw: 0_u16, + chan14_raw: 0_u16, + chan15_raw: 0_u16, + chan16_raw: 0_u16, + chan17_raw: 0_u16, + chan18_raw: 0_u16, + chancount: 0_u8, + rssi: 0_u8, + }; +} +impl Default for RC_CHANNELS_DATA { + fn default() -> Self { + Self::DEFAULT.clone() + } +} +impl MessageData for RC_CHANNELS_DATA { + type Message = MavMessage; + const ID: u32 = 65u32; + const NAME: &'static str = "RC_CHANNELS"; + const EXTRA_CRC: u8 = 118u8; + const ENCODED_LEN: usize = 42usize; + fn deser( + _version: MavlinkVersion, + __input: &[u8], + ) -> Result { + let avail_len = __input.len(); + let mut payload_buf = [0; Self::ENCODED_LEN]; + let mut buf = if avail_len < Self::ENCODED_LEN { + payload_buf[0..avail_len].copy_from_slice(__input); + Bytes::new(&payload_buf) + } else { + Bytes::new(__input) + }; + let mut __struct = Self::default(); + __struct.time_boot_ms = buf.get_u32_le(); + __struct.chan1_raw = buf.get_u16_le(); + __struct.chan2_raw = buf.get_u16_le(); + __struct.chan3_raw = buf.get_u16_le(); + __struct.chan4_raw = buf.get_u16_le(); + __struct.chan5_raw = buf.get_u16_le(); + __struct.chan6_raw = buf.get_u16_le(); + __struct.chan7_raw = buf.get_u16_le(); + __struct.chan8_raw = buf.get_u16_le(); + __struct.chan9_raw = buf.get_u16_le(); + __struct.chan10_raw = buf.get_u16_le(); + __struct.chan11_raw = buf.get_u16_le(); + __struct.chan12_raw = buf.get_u16_le(); + __struct.chan13_raw = buf.get_u16_le(); + __struct.chan14_raw = buf.get_u16_le(); + __struct.chan15_raw = buf.get_u16_le(); + __struct.chan16_raw = buf.get_u16_le(); + __struct.chan17_raw = buf.get_u16_le(); + __struct.chan18_raw = buf.get_u16_le(); + __struct.chancount = buf.get_u8(); + __struct.rssi = buf.get_u8(); + Ok(__struct) + } + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { + let mut __tmp = BytesMut::new(bytes); + #[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(), + ) + } + __tmp.put_u32_le(self.time_boot_ms); + __tmp.put_u16_le(self.chan1_raw); + __tmp.put_u16_le(self.chan2_raw); + __tmp.put_u16_le(self.chan3_raw); + __tmp.put_u16_le(self.chan4_raw); + __tmp.put_u16_le(self.chan5_raw); + __tmp.put_u16_le(self.chan6_raw); + __tmp.put_u16_le(self.chan7_raw); + __tmp.put_u16_le(self.chan8_raw); + __tmp.put_u16_le(self.chan9_raw); + __tmp.put_u16_le(self.chan10_raw); + __tmp.put_u16_le(self.chan11_raw); + __tmp.put_u16_le(self.chan12_raw); + __tmp.put_u16_le(self.chan13_raw); + __tmp.put_u16_le(self.chan14_raw); + __tmp.put_u16_le(self.chan15_raw); + __tmp.put_u16_le(self.chan16_raw); + __tmp.put_u16_le(self.chan17_raw); + __tmp.put_u16_le(self.chan18_raw); + __tmp.put_u8(self.chancount); + __tmp.put_u8(self.rssi); + if matches!(version, MavlinkVersion::V2) { + let len = __tmp.len(); + ::mavlink_core::utils::remove_trailing_zeroes(&bytes[..len]) + } else { + __tmp.len() + } + } +} diff --git a/src/codec.rs b/src/codec.rs index e8c8f03..db4787b 100644 --- a/src/codec.rs +++ b/src/codec.rs @@ -489,7 +489,7 @@ mod test_decode { fn test_decode_v2() { let mut codec = MavlinkCodec::::default(); - let mut buf = BytesMut::with_capacity(V1Packet::MAX_PACKET_SIZE); + let mut buf = BytesMut::with_capacity(V2Packet::MAX_PACKET_SIZE); let expected_packet = { let header = MavHeader { diff --git a/src/lib.rs b/src/lib.rs index c4c68fe..290ee3b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,5 +1,7 @@ pub mod codec; pub mod error; +pub mod mav_types; +pub mod parser; pub mod rust_mavlink_compatibility; pub mod v1; pub mod v2; @@ -16,6 +18,13 @@ pub enum Packet { V2(V2Packet) = V2_STX, } +#[derive(Debug, Clone, PartialEq, Eq, PartialOrd, Ord, Hash)] +#[repr(u8)] +pub enum PacketVersion { + V1 = V1_STX, + V2 = V2_STX, +} + impl Packet { #[inline(always)] pub fn bytes(&self) -> &Bytes { @@ -74,7 +83,7 @@ impl Packet { } #[inline(always)] - pub fn stx(&self) -> &u8 { + pub fn stx(&self) -> u8 { match self { Packet::V1(v1_packet) => v1_packet.stx(), Packet::V2(v2_packet) => v2_packet.stx(), @@ -82,7 +91,7 @@ impl Packet { } #[inline(always)] - pub fn payload_length(&self) -> &u8 { + pub fn payload_length(&self) -> u8 { match self { Packet::V1(v1_packet) => v1_packet.payload_length(), Packet::V2(v2_packet) => v2_packet.payload_length(), @@ -90,7 +99,7 @@ impl Packet { } #[inline(always)] - pub fn sequence(&self) -> &u8 { + pub fn sequence(&self) -> u8 { match self { Packet::V1(v1_packet) => v1_packet.sequence(), Packet::V2(v2_packet) => v2_packet.sequence(), @@ -98,7 +107,7 @@ impl Packet { } #[inline(always)] - pub fn system_id(&self) -> &u8 { + pub fn system_id(&self) -> u8 { match self { Packet::V1(v1_packet) => v1_packet.system_id(), Packet::V2(v2_packet) => v2_packet.system_id(), @@ -106,7 +115,7 @@ impl Packet { } #[inline(always)] - pub fn component_id(&self) -> &u8 { + pub fn component_id(&self) -> u8 { match self { Packet::V1(v1_packet) => v1_packet.component_id(), Packet::V2(v2_packet) => v2_packet.component_id(), @@ -116,10 +125,32 @@ impl Packet { #[inline(always)] pub fn message_id(&self) -> u32 { match self { - Packet::V1(v1_packet) => *v1_packet.message_id() as u32, + Packet::V1(v1_packet) => v1_packet.message_id() as u32, Packet::V2(v2_packet) => v2_packet.message_id(), } } + + fn header_bytes(&self) -> Bytes { + match self { + Packet::V1(v1_packet) => v1_packet.frame_header_bytes(), + Packet::V2(v2_packet) => v2_packet.frame_header_bytes(), + } + } + + fn payload_bytes(&self) -> Bytes { + match self { + Packet::V1(v1_packet) => v1_packet.payload_bytes(), + Packet::V2(v2_packet) => v2_packet.payload_bytes(), + } + } + + // // TODO: Maybe change this to from_frame! + // pub fn try_from_frame(frame: MavFrame, version: PacketVersion) -> Result { + // match version { + // PacketVersion::V1 => V1Packet::try_from(frame).map(Self::V1), + // PacketVersion::V2 => V2Packet::try_from(frame).map(Self::V2), + // } + // } } /// Creates a `MavlinkCodec` with compile-time configuration. diff --git a/src/mav_types/field_types.rs b/src/mav_types/field_types.rs new file mode 100644 index 0000000..584368f --- /dev/null +++ b/src/mav_types/field_types.rs @@ -0,0 +1,136 @@ +use enum_primitive_derive::Primitive; +// use bitflags::bitflags; +// use classic_bitfield::bitfield_enum; +use serde_derive::{Deserialize, Serialize}; + +#[repr(u8)] +#[derive(Debug, Default, Copy, Clone, PartialEq, Serialize, Deserialize, Primitive)] +#[serde(tag = "type", rename_all = "SCREAMING_SNAKE_CASE")] +pub enum MavType { + #[default] + MavTypeGeneric = 0, + MavTypeFixedWing = 1, + MavTypeQuadrotor = 2, + MavTypeCoaxial = 3, + MavTypeHelicopter = 4, + MavTypeAntennaTracker = 5, + MavTypeGcs = 6, + MavTypeAirship = 7, + MavTypeFreeBalloon = 8, + MavTypeRocket = 9, + MavTypeGroundRover = 10, + MavTypeSurfaceBoat = 11, + MavTypeSubmarine = 12, + MavTypeHexarotor = 13, + MavTypeOctorotor = 14, + MavTypeTricopter = 15, + MavTypeFlappingWing = 16, + MavTypeKite = 17, + MavTypeOnboardController = 18, + MavTypeVtolTailsitterDuorotor = 19, + MavTypeVtolTailsitterQuadrotor = 20, + MavTypeVtolTiltrotor = 21, + MavTypeVtolFixedrotor = 22, + MavTypeVtolTailsitter = 23, + MavTypeVtolTiltwing = 24, + MavTypeVtolReserved5 = 25, + MavTypeGimbal = 26, + MavTypeAdsb = 27, + MavTypeParafoil = 28, + MavTypeDodecarotor = 29, + MavTypeCamera = 30, + MavTypeChargingStation = 31, + MavTypeFlarm = 32, + MavTypeServo = 33, + MavTypeOdid = 34, + MavTypeDecarotor = 35, + MavTypeBattery = 36, + MavTypeParachute = 37, + MavTypeLog = 38, + MavTypeOsd = 39, + MavTypeImu = 40, + MavTypeGps = 41, + MavTypeWinch = 42, +} + +#[repr(u8)] +#[derive(Debug, Default, Copy, Clone, PartialEq, Serialize, Deserialize, Primitive)] +#[serde(tag = "type", rename_all = "SCREAMING_SNAKE_CASE")] +pub enum MavAutopilot { + #[default] + MavAutopilotGeneric = 0, + MavAutopilotReserved = 1, + MavAutopilotSlugs = 2, + MavAutopilotArdupilotmega = 3, + MavAutopilotOpenpilot = 4, + MavAutopilotGenericWaypointsOnly = 5, + MavAutopilotGenericWaypointsAndSimpleNavigationOnly = 6, + MavAutopilotGenericMissionFull = 7, + MavAutopilotInvalid = 8, + MavAutopilotPpz = 9, + MavAutopilotUdb = 10, + MavAutopilotFp = 11, + MavAutopilotPx4 = 12, + MavAutopilotSmaccmpilot = 13, + MavAutopilotAutoquad = 14, + MavAutopilotArmazila = 15, + MavAutopilotAerob = 16, + MavAutopilotAsluav = 17, + MavAutopilotSmartap = 18, + MavAutopilotAirrails = 19, + MavAutopilotReflex = 20, +} + +#[repr(u8)] +#[derive(Debug, Default, Copy, Clone, PartialEq, Serialize, Deserialize, Primitive)] +#[serde(tag = "type", rename_all = "SCREAMING_SNAKE_CASE")] +pub enum MavState { + #[default] + MavStateUninit = 0, + MavStateBoot = 1, + MavStateCalibrating = 2, + MavStateStandby = 3, + MavStateActive = 4, + MavStateCritical = 5, + MavStateEmergency = 6, + MavStatePoweroff = 7, + MavStateFlightTermination = 8, +} + +// #[bitfield_enum(as u8)] +// #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize, Primitive)] +// pub enum MavModeFlag { +// MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, +// MAV_MODE_FLAG_TEST_ENABLED, +// MAV_MODE_FLAG_AUTO_ENABLED, +// MAV_MODE_FLAG_GUIDED_ENABLED, +// MAV_MODE_FLAG_STABILIZE_ENABLED, +// MAV_MODE_FLAG_HIL_ENABLED, +// MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, +// MAV_MODE_FLAG_SAFETY_ARMED, +// } + +// impl Default for MavModeFlag { +// fn default() -> Self { +// Self(128) +// } +// } + +// bitflags! { +// #[derive (Debug, Clone, PartialEq, Serialize , Deserialize)] +// pub struct MavModeFlag : u8 { +// const MAV_MODE_FLAG_SAFETY_ARMED = 128 ; +// const MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64 ; +// const MAV_MODE_FLAG_HIL_ENABLED = 32 ; +// const MAV_MODE_FLAG_STABILIZE_ENABLED = 16 ; +// const MAV_MODE_FLAG_GUIDED_ENABLED = 8 ; +// const MAV_MODE_FLAG_AUTO_ENABLED = 4 ; +// const MAV_MODE_FLAG_TEST_ENABLED = 2 ; +// const MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 ; +// } +// } +// impl Default for MavModeFlag { +// fn default() -> Self { +// Self::MAV_MODE_FLAG_SAFETY_ARMED +// } +// } diff --git a/src/mav_types/mav_frame/mod.rs b/src/mav_types/mav_frame/mod.rs new file mode 100644 index 0000000..7a03db0 --- /dev/null +++ b/src/mav_types/mav_frame/mod.rs @@ -0,0 +1,76 @@ +pub mod serde_impl; + +use serde_impl::MavFrameSemanticModel; + +use crate::{v1::V1Packet, v2::V2Packet, Packet, PacketVersion}; + +use super::{ + mav_header::MavHeader, + mav_message::{ + heartbeat::HeartbeatMessage, rc_channels::RcChannelsMessage, MavMessage, MavMessageDef, + }, +}; + +#[derive(Debug, Clone, PartialEq)] +pub struct MavFrame { + packet: Packet, +} + +impl MavFrame { + pub fn new(model: &MavFrameSemanticModel, version: PacketVersion) -> Self { + let packet = match version { + PacketVersion::V1 => Packet::V1(V1Packet::from(model)), + PacketVersion::V2 => Packet::V2(V2Packet::from(model)), + }; + + Self { packet } + } + + pub fn header(&self) -> MavHeader { + MavHeader::new(self.packet.header_bytes()) + } + + pub fn message(&self) -> MavMessage { + match self.packet.message_id() { + HeartbeatMessage::ID => { + MavMessage::Heartbeat(HeartbeatMessage::new(self.packet.payload_bytes())) + } + RcChannelsMessage::ID => { + MavMessage::RcChannels(RcChannelsMessage::new(self.packet.payload_bytes())) + } + unknown_id => { + Err(format!("Unknown ID: {unknown_id:?}")).unwrap() // Unreachable because of the check during the MavFrame creation + } + } + } +} + +impl From<&Packet> for MavFrame { + #[inline(always)] + fn from(value: &Packet) -> Self { + Self { + packet: value.clone(), + } + } +} + +impl From<&MavFrame> for Packet { + #[inline(always)] + fn from(value: &MavFrame) -> Self { + value.packet.clone() + } +} + +impl From for MavFrame { + #[inline(always)] + fn from(value: Packet) -> Self { + Self { packet: value } + } +} + +impl From for Packet { + #[inline(always)] + fn from(value: MavFrame) -> Self { + value.packet + } +} diff --git a/src/mav_types/mav_frame/serde_impl.rs b/src/mav_types/mav_frame/serde_impl.rs new file mode 100644 index 0000000..0d518aa --- /dev/null +++ b/src/mav_types/mav_frame/serde_impl.rs @@ -0,0 +1,116 @@ +use bytes::BufMut; +use serde::de::{Deserialize, Deserializer}; +use serde::ser::{Serialize, Serializer}; +use serde_derive::{Deserialize, Serialize}; + +use crate::mav_types::mav_header::MavHeader; +use crate::mav_types::mav_message::MavMessage; +use crate::v1::{V1Packet, V1_STX}; +use crate::v2::{V2Packet, V2_STX}; + +use super::MavFrame; + +// #[repr(C, packed)] +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct MavFrameSemanticModel { + pub header: MavHeader, + pub message: MavMessage, +} + +impl Serialize for MavFrame { + fn serialize(&self, serializer: S) -> Result + where + S: Serializer, + { + MavFrameSemanticModel::from(self).serialize(serializer) + } +} + +impl<'de> Deserialize<'de> for MavFrame { + fn deserialize(deserializer: D) -> Result + where + D: Deserializer<'de>, + { + Ok(MavFrame::from(&MavFrameSemanticModel::deserialize( + deserializer, + )?)) + } +} + +impl From<&MavFrame> for MavFrameSemanticModel { + fn from(frame: &MavFrame) -> Self { + Self { + header: frame.header(), + message: frame.message(), + } + } +} + +impl From<&MavFrameSemanticModel> for MavFrame { + fn from(model: &MavFrameSemanticModel) -> Self { + MavFrame::new(model, crate::PacketVersion::V2) + } +} + +impl From<&MavFrameSemanticModel> for V1Packet { + fn from(frame: &MavFrameSemanticModel) -> Self { + let payload = frame.message.bytes(); + let payload_len = payload.len(); + + let mut buffer = bytes::BytesMut::with_capacity( + V1Packet::STX_SIZE + V1Packet::HEADER_SIZE + payload_len + V1Packet::CHECKSUM_SIZE, + ); + + buffer.extend_from_slice(&[ + V1_STX, // packet start + payload_len as u8, // payload len + 0, // packet sequence + frame.header.system_id(), // system id + frame.header.component_id(), // component id + ]); + buffer.put_u32_le( + frame + .header + .message_id() + .unwrap_or_else(|| frame.message.id()), + ); + buffer.extend_from_slice(payload); + buffer.put_u16_le( + 0, // TODO: compute CRC + ); + + V1Packet::new(buffer.freeze()) + } +} + +impl From<&MavFrameSemanticModel> for V2Packet { + fn from(frame: &MavFrameSemanticModel) -> Self { + let payload = frame.message.bytes(); + let payload_len = payload.len(); + + let mut buffer = bytes::BytesMut::with_capacity( + V2Packet::STX_SIZE + V2Packet::HEADER_SIZE + payload_len + V2Packet::CHECKSUM_SIZE, + ); + buffer.extend_from_slice(&[ + V2_STX, // packet start + payload_len as u8, // payload len + 0, // incompat flags + 0, // compat flags + 0, // packet sequence + frame.header.system_id(), // system id + frame.header.component_id(), // component id + ]); + buffer.put_u32_le( + frame + .header + .message_id() + .unwrap_or_else(|| frame.message.id()), + ); + buffer.extend_from_slice(payload); + buffer.put_u16_le( + 0, // TODO: compute CRC + ); + + V2Packet::new(buffer.freeze()) + } +} diff --git a/src/mav_types/mav_header/mod.rs b/src/mav_types/mav_header/mod.rs new file mode 100644 index 0000000..d9b3ed2 --- /dev/null +++ b/src/mav_types/mav_header/mod.rs @@ -0,0 +1,37 @@ +pub mod serde_impl; + +#[derive(Debug, Clone, PartialEq)] +pub struct MavHeader { + pub buffer: bytes::Bytes, +} + +impl MavHeader { + pub fn new(bytes: bytes::Bytes) -> Self { + Self { buffer: bytes } + } + + pub fn sequence(&self) -> u8 { + self.buffer[0] + } + + pub fn system_id(&self) -> u8 { + self.buffer[1] + } + + pub fn component_id(&self) -> u8 { + self.buffer[2] + } + + pub fn message_id(&self) -> Option { + match self.buffer.len() { + 4 => Some(u32::from_le_bytes([self.buffer[3], 0, 0, 0])), + 6 => Some(u32::from_le_bytes([ + self.buffer[3], + self.buffer[4], + self.buffer[5], + 0, + ])), + _ => None, + } + } +} diff --git a/src/mav_types/mav_header/serde_impl.rs b/src/mav_types/mav_header/serde_impl.rs new file mode 100644 index 0000000..40a0f4e --- /dev/null +++ b/src/mav_types/mav_header/serde_impl.rs @@ -0,0 +1,53 @@ +use bytes::{BufMut, BytesMut}; +use serde::de::{Deserialize, Deserializer}; +use serde::ser::{Serialize, Serializer}; +use serde_derive::{Deserialize as DeriveDeserialize, Serialize as DeriveSerialize}; + +use crate::v2::V2Packet; + +use super::MavHeader; + +#[derive(Debug, Default, Clone, PartialEq, DeriveSerialize, DeriveDeserialize)] +pub struct MavHeaderSemanticModel { + pub sequence: u8, + pub system_id: u8, + pub component_id: u8, + pub message_id: Option, +} + +impl Serialize for MavHeader { + fn serialize(&self, serializer: S) -> Result + where + S: Serializer, + { + MavHeaderSemanticModel::from(self).serialize(serializer) + } +} + +impl<'de> Deserialize<'de> for MavHeader { + fn deserialize(deserializer: D) -> Result + where + D: Deserializer<'de>, + { + let model = MavHeaderSemanticModel::deserialize(deserializer)?; + + let mut bytes = BytesMut::with_capacity(V2Packet::HEADER_SIZE); + bytes.put_u8(model.sequence); + bytes.put_u8(model.system_id); + bytes.put_u8(model.component_id); + bytes.put_u32(model.message_id.unwrap_or_default()); + + Ok(Self::new(bytes.freeze())) + } +} + +impl From<&MavHeader> for MavHeaderSemanticModel { + fn from(header: &MavHeader) -> Self { + MavHeaderSemanticModel { + sequence: header.sequence(), + system_id: header.system_id(), + component_id: header.component_id(), + message_id: header.message_id(), + } + } +} diff --git a/src/mav_types/mav_message/heartbeat/mod.rs b/src/mav_types/mav_message/heartbeat/mod.rs new file mode 100644 index 0000000..5d46697 --- /dev/null +++ b/src/mav_types/mav_message/heartbeat/mod.rs @@ -0,0 +1,193 @@ +pub mod serde_impl; + +use bytes::Bytes; + +use crate::{define_mav_message_fields, mav_types::field_types::*}; + +use super::{MavMessageDef, MavMessageFields}; + +#[derive(Default, Clone, PartialEq, Eq, PartialOrd, Ord, Hash)] +pub struct HeartbeatMessage { + pub(crate) buffer: bytes::Bytes, +} + +impl MavMessageDef for HeartbeatMessage { + const ID: u32 = 0; + const LEN: u16 = 9; + const CRC: u32 = 300; + + #[inline(always)] + fn new(bytes: Bytes) -> Self { + Self { buffer: bytes } + } + + #[inline(always)] + fn bytes(&self) -> &Bytes { + &self.buffer + } + + #[inline(always)] + fn as_slice(&self) -> &[u8] { + &self.buffer[..] + } +} + +define_mav_message_fields!(HeartbeatMessage, HEARTBEAT_MESSAGE_FIELDS, { + "custom_mode" => custom_mode() => (serde_json::Value::from), + "mav_type" => mav_type() => (|v: MavType| serde_json::Value::from(v as u8)), + "autopilot" => autopilot() => (|v: MavAutopilot| serde_json::Value::from(v as u8)), + "base_mode" => base_mode() => (serde_json::Value::from), + "system_status" => system_status() => (|v: MavState| serde_json::Value::from(v as u8)), + "mavlink_version" => mavlink_version() => (serde_json::Value::from), +}); + +impl MavMessageFields for HeartbeatMessage { + fn fields( + &self, + ) -> &'static phf::Map<&'static str, fn(&dyn MavMessageFields) -> serde_json::Value> { + &HEARTBEAT_MESSAGE_FIELDS + } + + fn as_any(&self) -> &dyn std::any::Any { + self + } +} + +impl HeartbeatMessage { + #[inline(always)] + pub fn custom_mode(&self) -> u32 { + if self.buffer.len() < 4 { + return 0; + } + + let mut bytes = [0u8; 4]; + bytes.copy_from_slice(&self.buffer[..4]); + u32::from_le_bytes(bytes) + } + + #[inline(always)] + pub fn mav_type(&self) -> MavType { + const OFFSET: usize = 4; + if self.buffer.len() <= OFFSET { + return MavType::default(); + } + + let raw = self.buffer[OFFSET]; + unsafe { std::mem::transmute(raw) } + } + + #[inline(always)] + pub fn autopilot(&self) -> MavAutopilot { + const OFFSET: usize = 5; + if self.buffer.len() <= OFFSET { + return MavAutopilot::default(); + } + + let raw = self.buffer[OFFSET]; + unsafe { std::mem::transmute(raw) } + } + + #[inline(always)] + pub fn base_mode(&self) -> u8 { + const OFFSET: usize = 6; + if self.buffer.len() <= OFFSET { + return 0; + } + + self.buffer[OFFSET] + } + + #[inline(always)] + pub fn system_status(&self) -> MavState { + const OFFSET: usize = 7; + if self.buffer.len() <= OFFSET { + return MavState::default(); + } + + let raw = self.buffer[OFFSET]; + unsafe { std::mem::transmute(raw) } + } + + #[inline(always)] + pub fn mavlink_version(&self) -> u8 { + const OFFSET: usize = 8; + if self.buffer.len() <= OFFSET { + return 0; + } + + self.buffer[OFFSET] + } +} + +impl std::fmt::Debug for HeartbeatMessage { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("HeartbeatMessage") + .field("custom_mode", &self.custom_mode()) + .field("mav_type", &self.mav_type()) + .field("autopilot", &self.autopilot()) + .field("base_mode", &self.base_mode()) + .field("system_status", &self.system_status()) + .field("mavlink_version", &self.mavlink_version()) + .finish() + } +} + +#[cfg(test)] +mod tests { + use super::*; + use bytes::Bytes; + use mavlink::Message as _; + + #[test] + fn test() { + // 1. Create a heartbeat message with All non-zero values using rust-mavlink + let original_heartbeat_message_data = mavlink::ardupilotmega::HEARTBEAT_DATA { + custom_mode: std::u32::MAX, + mavtype: mavlink::ardupilotmega::MavType::MAV_TYPE_FIXED_WING, + autopilot: mavlink::ardupilotmega::MavAutopilot::MAV_AUTOPILOT_ARDUPILOTMEGA, + base_mode: mavlink::ardupilotmega::MavModeFlag::all(), + system_status: mavlink::ardupilotmega::MavState::MAV_STATE_STANDBY, + mavlink_version: 0x03, + }; + dbg!(&original_heartbeat_message_data); + + // 2. Serialize it + let mut slice = [0u8; mavlink::ardupilotmega::HEARTBEAT_DATA::ENCODED_LEN]; + mavlink::ardupilotmega::MavMessage::ser( + &mavlink::ardupilotmega::MavMessage::HEARTBEAT(original_heartbeat_message_data.clone()), + mavlink::MavlinkVersion::V2, + &mut slice, + ); + dbg!(&slice); + + // 3. Parse it using the new implementation + let parsed_heartbeat_message_data = HeartbeatMessage::new(Bytes::copy_from_slice(&slice)); + dbg!(&parsed_heartbeat_message_data); + + // 4. Now, compare all fields + assert_eq!( + parsed_heartbeat_message_data.custom_mode(), + original_heartbeat_message_data.custom_mode + ); + assert_eq!( + parsed_heartbeat_message_data.mav_type() as u32, + original_heartbeat_message_data.mavtype as u32 + ); + assert_eq!( + parsed_heartbeat_message_data.autopilot() as u8, + original_heartbeat_message_data.autopilot as u8 + ); + assert_eq!( + parsed_heartbeat_message_data.base_mode(), + original_heartbeat_message_data.base_mode.bits() + ); + assert_eq!( + parsed_heartbeat_message_data.system_status() as u32, + original_heartbeat_message_data.system_status as u32 + ); + assert_eq!( + parsed_heartbeat_message_data.mavlink_version(), + original_heartbeat_message_data.mavlink_version + ); + } +} diff --git a/src/mav_types/mav_message/heartbeat/serde_impl.rs b/src/mav_types/mav_message/heartbeat/serde_impl.rs new file mode 100644 index 0000000..5a744b4 --- /dev/null +++ b/src/mav_types/mav_message/heartbeat/serde_impl.rs @@ -0,0 +1,134 @@ +use bytes::{BufMut, BytesMut}; +use serde::de::{Deserialize, Deserializer}; +use serde::ser::{Serialize, Serializer}; +use serde_derive::{Deserialize as DeriveDeserialize, Serialize as DeriveSerialize}; + +use crate::mav_types::field_types::*; +use crate::mav_types::mav_message::MavMessageDef; +use crate::mav_types::serde_utils::BitsField; + +use super::HeartbeatMessage; + +#[derive(Debug, Clone, PartialEq, DeriveSerialize, DeriveDeserialize)] +#[serde(rename_all = "snake_case")] +pub struct HeartbeatMessageSemanticModel { + pub autopilot: MavAutopilot, + pub base_mode: BitsField, + pub custom_mode: u32, + pub mavlink_version: u8, + pub mavtype: MavType, + pub system_status: MavState, +} + +impl From<&HeartbeatMessage> for HeartbeatMessageSemanticModel { + fn from(message: &HeartbeatMessage) -> Self { + HeartbeatMessageSemanticModel { + autopilot: message.autopilot(), + base_mode: BitsField { + bits: message.base_mode(), + }, + custom_mode: message.custom_mode(), + mavlink_version: message.mavlink_version(), + mavtype: message.mav_type(), + system_status: message.system_status(), + } + } +} + +impl From<&HeartbeatMessageSemanticModel> for HeartbeatMessage { + fn from(model: &HeartbeatMessageSemanticModel) -> Self { + let mut buffer = BytesMut::with_capacity(HeartbeatMessage::LEN as usize); + + buffer.put_u32_le(model.custom_mode); + buffer.put_u8(model.mavtype as u8); + buffer.put_u8(model.autopilot as u8); + buffer.put_u8(model.base_mode.bits); + buffer.put_u8(model.system_status as u8); + buffer.put_u8(model.mavlink_version); + + HeartbeatMessage::new(buffer.freeze()) + } +} + +impl Serialize for HeartbeatMessage { + fn serialize(&self, serializer: S) -> Result + where + S: Serializer, + { + HeartbeatMessageSemanticModel::from(self).serialize(serializer) + } +} + +impl<'de> Deserialize<'de> for HeartbeatMessage { + fn deserialize(deserializer: D) -> Result + where + D: Deserializer<'de>, + { + Ok(HeartbeatMessage::from( + &HeartbeatMessageSemanticModel::deserialize(deserializer)?, + )) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + use bytes::Bytes; + use serde_json::{from_value, json, to_value}; + + const HEARTBEAT_BYTES: &[u8] = &[ + 19, + 0, + 0, + 0, // custom_mode = 19 + MavType::MavTypeFixedWing as u8, + MavAutopilot::MavAutopilotArdupilotmega as u8, + 81, // base_mode + MavState::MavStateCritical as u8, + 3, // mavlink_version + ]; + + fn heartbeat_json() -> serde_json::Value { + json!({ + "autopilot": { "type": "MAV_AUTOPILOT_ARDUPILOTMEGA" }, + "base_mode": { "bits": 81 }, + "custom_mode": 19, + "mavlink_version": 3, + "mavtype": { "type": "MAV_TYPE_FIXED_WING" }, + "system_status": { "type": "MAV_STATE_CRITICAL" }, + }) + } + + #[test] + fn test_heartbeat_serialize() { + let expected_json = heartbeat_json(); + dbg!(&expected_json); + + let message = HeartbeatMessage { + buffer: Bytes::from(HEARTBEAT_BYTES), + }; + dbg!(&message); + + let serialized_json = to_value(&message).unwrap(); + dbg!(&serialized_json); + + assert_eq!(serialized_json, expected_json); + } + + #[test] + fn test_heartbeat_deserialize() { + let expected_message = HeartbeatMessage { + buffer: Bytes::from(HEARTBEAT_BYTES), + }; + dbg!(&expected_message); + + let json = heartbeat_json(); + dbg!(&json); + + let deserialized_message: HeartbeatMessage = from_value(json).unwrap(); + dbg!(&expected_message); + + assert_eq!(deserialized_message, expected_message); + } +} diff --git a/src/mav_types/mav_message/mod.rs b/src/mav_types/mav_message/mod.rs new file mode 100644 index 0000000..8eeefef --- /dev/null +++ b/src/mav_types/mav_message/mod.rs @@ -0,0 +1,101 @@ +pub mod heartbeat; +pub mod rc_channels; + +use std::any::Any; + +use heartbeat::{serde_impl::HeartbeatMessageSemanticModel, HeartbeatMessage}; +use rc_channels::{serde_impl::RcChannelsMessageSemanticModel, RcChannelsMessage}; +use serde_derive::{Deserialize, Serialize}; + +use crate::{error::DecoderError, Packet}; + +pub trait MavMessageDef: MavMessageFields { + const ID: u32; + const LEN: u16; + const CRC: u32; + + fn new(bytes: bytes::Bytes) -> Self; + + fn bytes(&self) -> &bytes::Bytes; + + fn as_slice(&self) -> &[u8]; +} + +pub trait MavMessageFields: Any { + fn fields( + &self, + ) -> &'static phf::Map<&'static str, fn(&dyn MavMessageFields) -> serde_json::Value>; + fn as_any(&self) -> &dyn Any; +} + +#[repr(u32)] +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +#[serde(tag = "type", rename_all = "SCREAMING_SNAKE_CASE")] +pub enum MavMessage { + Heartbeat(HeartbeatMessage) = HeartbeatMessage::ID, + RcChannels(RcChannelsMessage) = RcChannelsMessage::ID, + // ... and thousands of others +} + +impl MavMessageFields for MavMessage { + fn fields( + &self, + ) -> &'static phf::Map<&'static str, fn(&dyn MavMessageFields) -> serde_json::Value> { + match self { + MavMessage::Heartbeat(heartbeat_message) => heartbeat_message.fields(), + MavMessage::RcChannels(rc_channels_message) => rc_channels_message.fields(), + } + } + + fn as_any(&self) -> &dyn Any { + match self { + MavMessage::Heartbeat(heartbeat_message) => heartbeat_message, + MavMessage::RcChannels(rc_channels_message) => rc_channels_message, + } + } +} + +#[repr(u32)] +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +#[serde(tag = "type", rename_all = "SCREAMING_SNAKE_CASE")] +pub enum MavMessageSemanticModel { + Heartbeat(HeartbeatMessageSemanticModel) = HeartbeatMessage::ID, + RcChannels(RcChannelsMessageSemanticModel) = RcChannelsMessage::ID, +} + +impl MavMessage { + pub fn bytes(&self) -> &bytes::Bytes { + match self { + MavMessage::Heartbeat(heartbeat_message) => heartbeat_message.bytes(), + MavMessage::RcChannels(rc_channels_message) => rc_channels_message.bytes(), + } + } + + pub fn id(&self) -> u32 { + match self { + MavMessage::Heartbeat(_) => HeartbeatMessage::ID, + MavMessage::RcChannels(_) => RcChannelsMessage::ID, + } + } +} + +impl TryFrom<&Packet> for MavMessage { + type Error = DecoderError; + + fn try_from(value: &Packet) -> Result { + crate::parser::parse(value) + } +} + +impl From<&MavMessage> for MavMessageSemanticModel { + fn from(value: &MavMessage) -> Self { + match value { + MavMessage::Heartbeat(heartbeat_message) => MavMessageSemanticModel::Heartbeat( + HeartbeatMessageSemanticModel::from(heartbeat_message), + ), + MavMessage::RcChannels(rc_channels_message) => MavMessageSemanticModel::RcChannels( + RcChannelsMessageSemanticModel::from(rc_channels_message), + ), + } + } +} diff --git a/src/mav_types/mav_message/rc_channels/mod.rs b/src/mav_types/mav_message/rc_channels/mod.rs new file mode 100644 index 0000000..f5f068b --- /dev/null +++ b/src/mav_types/mav_message/rc_channels/mod.rs @@ -0,0 +1,243 @@ +use bytes::Bytes; + +use crate::{define_mav_message_fields, mav_types::mav_message::MavMessageDef}; + +use super::MavMessageFields; + +pub mod serde_impl; + +#[derive(Default, Clone, PartialEq, Eq, PartialOrd, Ord, Hash)] +pub struct RcChannelsMessage { + pub(crate) buffer: bytes::Bytes, +} + +impl MavMessageDef for RcChannelsMessage { + const ID: u32 = 65; + const LEN: u16 = 42; + // Using u32 for CRC to match the Heartbeat example's trait definition, + // even though the source example provides u8. + const CRC: u32 = 118; + + #[inline(always)] + fn new(bytes: Bytes) -> Self { + Self { buffer: bytes } + } + + #[inline(always)] + fn bytes(&self) -> &Bytes { + &self.buffer + } + + #[inline(always)] + fn as_slice(&self) -> &[u8] { + &self.buffer[..] + } +} + +// Helper macro to generate accessor methods for u16 channels +macro_rules! impl_chan_raw_accessor { + ($name:ident, $offset:expr) => { + #[inline(always)] + pub fn $name(&self) -> u16 { + const OFFSET: usize = $offset; + if self.buffer.len() < OFFSET + 2 { + return 0; + } + let mut bytes = [0u8; 2]; + bytes.copy_from_slice(&self.buffer[OFFSET..OFFSET + 2]); + u16::from_le_bytes(bytes) + } + }; +} + +define_mav_message_fields!(RcChannelsMessage, RC_CHANNELS_MESSAGE_FIELDS, { + + "time_boot_ms" => time_boot_ms() => (serde_json::Value::from), + "chan1_raw" => chan1_raw() => (serde_json::Value::from), + "chan2_raw" => chan2_raw() => (serde_json::Value::from), + "chan3_raw" => chan3_raw() => (serde_json::Value::from), + "chan4_raw" => chan4_raw() => (serde_json::Value::from), + "chan5_raw" => chan5_raw() => (serde_json::Value::from), + "chan6_raw" => chan6_raw() => (serde_json::Value::from), + "chan7_raw" => chan7_raw() => (serde_json::Value::from), + "chan8_raw" => chan8_raw() => (serde_json::Value::from), + "chan9_raw" => chan9_raw() => (serde_json::Value::from), + "chan10_raw" => chan10_raw() => (serde_json::Value::from), + "chan11_raw" => chan11_raw() => (serde_json::Value::from), + "chan12_raw" => chan12_raw() => (serde_json::Value::from), + "chan13_raw" => chan13_raw() => (serde_json::Value::from), + "chan14_raw" => chan14_raw() => (serde_json::Value::from), + "chan15_raw" => chan15_raw() => (serde_json::Value::from), + "chan16_raw" => chan16_raw() => (serde_json::Value::from), + "chan17_raw" => chan17_raw() => (serde_json::Value::from), + "chan18_raw" => chan18_raw() => (serde_json::Value::from), + "chancount" => chancount() =>(serde_json::Value::from), + "rssi" => rssi() =>(serde_json::Value::from), +}); + +impl MavMessageFields for RcChannelsMessage { + fn fields( + &self, + ) -> &'static phf::Map<&'static str, fn(&dyn MavMessageFields) -> serde_json::Value> { + &RC_CHANNELS_MESSAGE_FIELDS + } + + fn as_any(&self) -> &dyn std::any::Any { + self + } +} + +impl RcChannelsMessage { + #[inline(always)] + pub fn time_boot_ms(&self) -> u32 { + const OFFSET: usize = 0; + if self.buffer.len() < OFFSET + 4 { + return 0; + } + let mut bytes = [0u8; 4]; + bytes.copy_from_slice(&self.buffer[OFFSET..OFFSET + 4]); + u32::from_le_bytes(bytes) + } + + impl_chan_raw_accessor!(chan1_raw, 4); + impl_chan_raw_accessor!(chan2_raw, 6); + impl_chan_raw_accessor!(chan3_raw, 8); + impl_chan_raw_accessor!(chan4_raw, 10); + impl_chan_raw_accessor!(chan5_raw, 12); + impl_chan_raw_accessor!(chan6_raw, 14); + impl_chan_raw_accessor!(chan7_raw, 16); + impl_chan_raw_accessor!(chan8_raw, 18); + impl_chan_raw_accessor!(chan9_raw, 20); + impl_chan_raw_accessor!(chan10_raw, 22); + impl_chan_raw_accessor!(chan11_raw, 24); + impl_chan_raw_accessor!(chan12_raw, 26); + impl_chan_raw_accessor!(chan13_raw, 28); + impl_chan_raw_accessor!(chan14_raw, 30); + impl_chan_raw_accessor!(chan15_raw, 32); + impl_chan_raw_accessor!(chan16_raw, 34); + impl_chan_raw_accessor!(chan17_raw, 36); + impl_chan_raw_accessor!(chan18_raw, 38); + + #[inline(always)] + pub fn chancount(&self) -> u8 { + const OFFSET: usize = 40; + if self.buffer.len() <= OFFSET { + return 0; + } + self.buffer[OFFSET] + } + + #[inline(always)] + pub fn rssi(&self) -> u8 { + const OFFSET: usize = 41; + if self.buffer.len() <= OFFSET { + return 0; + } + self.buffer[OFFSET] + } +} + +impl std::fmt::Debug for RcChannelsMessage { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.debug_struct("RcChannelsMessage") + .field("time_boot_ms", &self.time_boot_ms()) + .field("chan1_raw", &self.chan1_raw()) + .field("chan2_raw", &self.chan2_raw()) + .field("chan3_raw", &self.chan3_raw()) + .field("chan4_raw", &self.chan4_raw()) + .field("chan5_raw", &self.chan5_raw()) + .field("chan6_raw", &self.chan6_raw()) + .field("chan7_raw", &self.chan7_raw()) + .field("chan8_raw", &self.chan8_raw()) + .field("chan9_raw", &self.chan9_raw()) + .field("chan10_raw", &self.chan10_raw()) + .field("chan11_raw", &self.chan11_raw()) + .field("chan12_raw", &self.chan12_raw()) + .field("chan13_raw", &self.chan13_raw()) + .field("chan14_raw", &self.chan14_raw()) + .field("chan15_raw", &self.chan15_raw()) + .field("chan16_raw", &self.chan16_raw()) + .field("chan17_raw", &self.chan17_raw()) + .field("chan18_raw", &self.chan18_raw()) + .field("chancount", &self.chancount()) + .field("rssi", &self.rssi()) + .finish() + } +} + +#[cfg(test)] +mod tests { + use super::*; + use bytes::Bytes; + use mavlink::Message as _; + + #[test] + fn test_rc_channels_parsing() { + // 1. Create an RC_CHANNELS message with non-zero values using rust-mavlink + let original_rc_data = mavlink::ardupilotmega::RC_CHANNELS_DATA { + time_boot_ms: 123456789, + chan1_raw: 1100, + chan2_raw: 1200, + chan3_raw: 1300, + chan4_raw: 1400, + chan5_raw: 1500, + chan6_raw: 1600, + chan7_raw: 1700, + chan8_raw: 1800, + chan9_raw: 1900, + chan10_raw: 1110, + chan11_raw: 1120, + chan12_raw: 1130, + chan13_raw: 1140, + chan14_raw: 1150, + chan15_raw: 1160, + chan16_raw: 1170, + chan17_raw: 1180, + chan18_raw: 1190, + chancount: 18, + rssi: 200, + }; + dbg!(&original_rc_data); + + // 2. Serialize it + let mut slice = [0u8; mavlink::ardupilotmega::RC_CHANNELS_DATA::ENCODED_LEN]; + mavlink::ardupilotmega::MavMessage::ser( + &mavlink::ardupilotmega::MavMessage::RC_CHANNELS(original_rc_data.clone()), + mavlink::MavlinkVersion::V2, // Or V1, shouldn't matter for fixed-size payload + &mut slice, + ); + dbg!(&slice); + + // 3. Parse it using the new implementation + let parsed_rc_message = RcChannelsMessage::new(Bytes::copy_from_slice(&slice)); + dbg!(&parsed_rc_message); + + // 4. Compare all fields + assert_eq!( + parsed_rc_message.time_boot_ms(), + original_rc_data.time_boot_ms + ); + assert_eq!(parsed_rc_message.chan1_raw(), original_rc_data.chan1_raw); + assert_eq!(parsed_rc_message.chan2_raw(), original_rc_data.chan2_raw); + assert_eq!(parsed_rc_message.chan3_raw(), original_rc_data.chan3_raw); + assert_eq!(parsed_rc_message.chan4_raw(), original_rc_data.chan4_raw); + assert_eq!(parsed_rc_message.chan5_raw(), original_rc_data.chan5_raw); + assert_eq!(parsed_rc_message.chan6_raw(), original_rc_data.chan6_raw); + assert_eq!(parsed_rc_message.chan7_raw(), original_rc_data.chan7_raw); + assert_eq!(parsed_rc_message.chan8_raw(), original_rc_data.chan8_raw); + assert_eq!(parsed_rc_message.chan9_raw(), original_rc_data.chan9_raw); + assert_eq!(parsed_rc_message.chan10_raw(), original_rc_data.chan10_raw); + assert_eq!(parsed_rc_message.chan11_raw(), original_rc_data.chan11_raw); + assert_eq!(parsed_rc_message.chan12_raw(), original_rc_data.chan12_raw); + assert_eq!(parsed_rc_message.chan13_raw(), original_rc_data.chan13_raw); + assert_eq!(parsed_rc_message.chan14_raw(), original_rc_data.chan14_raw); + assert_eq!(parsed_rc_message.chan15_raw(), original_rc_data.chan15_raw); + assert_eq!(parsed_rc_message.chan16_raw(), original_rc_data.chan16_raw); + // Note: rust-mavlink might pad unused channels (like 17/18 if chancount < 18) with different values (often u16::MAX or 0). + // Test according to actual observed behavior or spec if needed. Here we assume chancount=18 means all are valid. + assert_eq!(parsed_rc_message.chan17_raw(), original_rc_data.chan17_raw); + assert_eq!(parsed_rc_message.chan18_raw(), original_rc_data.chan18_raw); + assert_eq!(parsed_rc_message.chancount(), original_rc_data.chancount); + assert_eq!(parsed_rc_message.rssi(), original_rc_data.rssi); + } +} diff --git a/src/mav_types/mav_message/rc_channels/serde_impl.rs b/src/mav_types/mav_message/rc_channels/serde_impl.rs new file mode 100644 index 0000000..81b8b2c --- /dev/null +++ b/src/mav_types/mav_message/rc_channels/serde_impl.rs @@ -0,0 +1,201 @@ +use bytes::{BufMut, BytesMut}; +use serde::de::{Deserialize, Deserializer}; +use serde::ser::{Serialize, Serializer}; +use serde_derive::{Deserialize as DeriveDeserialize, Serialize as DeriveSerialize}; + +use crate::mav_types::mav_message::MavMessageDef; + +use super::RcChannelsMessage; + +#[derive(Debug, Clone, PartialEq, DeriveSerialize, DeriveDeserialize)] +#[serde(rename_all = "snake_case")] +pub struct RcChannelsMessageSemanticModel { + pub time_boot_ms: u32, + pub chan1_raw: u16, + pub chan2_raw: u16, + pub chan3_raw: u16, + pub chan4_raw: u16, + pub chan5_raw: u16, + pub chan6_raw: u16, + pub chan7_raw: u16, + pub chan8_raw: u16, + pub chan9_raw: u16, + pub chan10_raw: u16, + pub chan11_raw: u16, + pub chan12_raw: u16, + pub chan13_raw: u16, + pub chan14_raw: u16, + pub chan15_raw: u16, + pub chan16_raw: u16, + pub chan17_raw: u16, + pub chan18_raw: u16, + pub chancount: u8, + pub rssi: u8, +} + +impl From<&RcChannelsMessage> for RcChannelsMessageSemanticModel { + fn from(message: &RcChannelsMessage) -> Self { + RcChannelsMessageSemanticModel { + time_boot_ms: message.time_boot_ms(), + chan1_raw: message.chan1_raw(), + chan2_raw: message.chan2_raw(), + chan3_raw: message.chan3_raw(), + chan4_raw: message.chan4_raw(), + chan5_raw: message.chan5_raw(), + chan6_raw: message.chan6_raw(), + chan7_raw: message.chan7_raw(), + chan8_raw: message.chan8_raw(), + chan9_raw: message.chan9_raw(), + chan10_raw: message.chan10_raw(), + chan11_raw: message.chan11_raw(), + chan12_raw: message.chan12_raw(), + chan13_raw: message.chan13_raw(), + chan14_raw: message.chan14_raw(), + chan15_raw: message.chan15_raw(), + chan16_raw: message.chan16_raw(), + chan17_raw: message.chan17_raw(), + chan18_raw: message.chan18_raw(), + chancount: message.chancount(), + rssi: message.rssi(), + } + } +} + +impl From<&RcChannelsMessageSemanticModel> for RcChannelsMessage { + fn from(model: &RcChannelsMessageSemanticModel) -> Self { + let mut buffer = BytesMut::with_capacity(RcChannelsMessage::LEN as usize); + + buffer.put_u32_le(model.time_boot_ms); + buffer.put_u16_le(model.chan1_raw); + buffer.put_u16_le(model.chan2_raw); + buffer.put_u16_le(model.chan3_raw); + buffer.put_u16_le(model.chan4_raw); + buffer.put_u16_le(model.chan5_raw); + buffer.put_u16_le(model.chan6_raw); + buffer.put_u16_le(model.chan7_raw); + buffer.put_u16_le(model.chan8_raw); + buffer.put_u16_le(model.chan9_raw); + buffer.put_u16_le(model.chan10_raw); + buffer.put_u16_le(model.chan11_raw); + buffer.put_u16_le(model.chan12_raw); + buffer.put_u16_le(model.chan13_raw); + buffer.put_u16_le(model.chan14_raw); + buffer.put_u16_le(model.chan15_raw); + buffer.put_u16_le(model.chan16_raw); + buffer.put_u16_le(model.chan17_raw); + buffer.put_u16_le(model.chan18_raw); + buffer.put_u8(model.chancount); + buffer.put_u8(model.rssi); + + RcChannelsMessage::new(buffer.freeze()) + } +} + +impl Serialize for RcChannelsMessage { + fn serialize(&self, serializer: S) -> Result + where + S: Serializer, + { + RcChannelsMessageSemanticModel::from(self).serialize(serializer) + } +} + +impl<'de> Deserialize<'de> for RcChannelsMessage { + fn deserialize(deserializer: D) -> Result + where + D: Deserializer<'de>, + { + Ok(RcChannelsMessage::from( + &RcChannelsMessageSemanticModel::deserialize(deserializer)?, + )) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use bytes::Bytes; + use serde_json::{from_value, json, to_value}; + + const RC_CHANNELS_BYTES: &[u8] = &[ + 0xe8, 0x03, 0x00, 0x00, // time_boot_ms (u32_le) + 0xdc, 0x05, // chan1_raw (u16_le) + 0xdc, 0x05, // chan2_raw (u16_le) + 0xdc, 0x05, // chan3_raw (u16_le) + 0xdc, 0x05, // chan4_raw (u16_le) + 0xdc, 0x05, // chan5_raw (u16_le) + 0xdc, 0x05, // chan6_raw (u16_le) + 0xdc, 0x05, // chan7_raw (u16_le) + 0xdc, 0x05, // chan8_raw (u16_le) + 0xe8, 0x03, // chan9_raw (u16_le) + 0xe8, 0x03, // chan10_raw (u16_le) + 0xe8, 0x03, // chan11_raw (u16_le) + 0xe8, 0x03, // chan12_raw (u16_le) + 0xe8, 0x03, // chan13_raw (u16_le) + 0xe8, 0x03, // chan14_raw (u16_le) + 0xe8, 0x03, // chan15_raw (u16_le) + 0xe8, 0x03, // chan16_raw (u16_le) + 0xe8, 0x03, // chan17_raw (u16_le) + 0xe8, 0x03, // chan18_raw (u16_le) + 18, // chancount (u8) + 210, // rssi (u8) + ]; + + fn rc_channels_json() -> serde_json::Value { + json!({ + "time_boot_ms": 1000, + "chan1_raw": 1500, + "chan2_raw": 1500, + "chan3_raw": 1500, + "chan4_raw": 1500, + "chan5_raw": 1500, + "chan6_raw": 1500, + "chan7_raw": 1500, + "chan8_raw": 1500, + "chan9_raw": 1000, + "chan10_raw": 1000, + "chan11_raw": 1000, + "chan12_raw": 1000, + "chan13_raw": 1000, + "chan14_raw": 1000, + "chan15_raw": 1000, + "chan16_raw": 1000, + "chan17_raw": 1000, + "chan18_raw": 1000, + "chancount": 18, + "rssi": 210 + }) + } + + #[test] + fn test_rc_channels_serialize() { + let expected_json = rc_channels_json(); + dbg!(&expected_json); + + let message = RcChannelsMessage { + buffer: Bytes::from(RC_CHANNELS_BYTES), + }; + dbg!(&message); + + let serialized_json = to_value(&message).unwrap(); + dbg!(&serialized_json); + + assert_eq!(serialized_json, expected_json); + } + + #[test] + fn test_rc_channels_deserialize() { + let expected_message = RcChannelsMessage { + buffer: Bytes::from(RC_CHANNELS_BYTES), + }; + dbg!(&expected_message); + + let json = rc_channels_json(); + dbg!(&json); + + let deserialized_message: RcChannelsMessage = from_value(json).unwrap(); + dbg!(&deserialized_message); + + assert_eq!(deserialized_message, expected_message); + } +} diff --git a/src/mav_types/mod.rs b/src/mav_types/mod.rs new file mode 100644 index 0000000..feb73eb --- /dev/null +++ b/src/mav_types/mod.rs @@ -0,0 +1,5 @@ +pub mod field_types; +pub mod mav_frame; +pub mod mav_header; +pub mod mav_message; +pub mod serde_utils; diff --git a/src/mav_types/serde_utils.rs b/src/mav_types/serde_utils.rs new file mode 100644 index 0000000..590b6d5 --- /dev/null +++ b/src/mav_types/serde_utils.rs @@ -0,0 +1,35 @@ +use serde_derive::{Deserialize, Serialize}; + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct BitsField { + pub bits: T, +} + +#[macro_export] +macro_rules! define_mav_message_fields { + ( + $type:ty, $map_name:ident, { + $( + $key:literal => $method:ident() => $wrap:expr + ),* $(,)? + } + ) => { + pub static $map_name: ::phf::Map< + &'static str, + fn(&dyn $crate::mav_types::mav_message::MavMessageFields) -> serde_json::Value + > = ::phf::phf_map! { + $( + $key => { + fn get(msg: &dyn $crate::mav_types::mav_message::MavMessageFields) -> serde_json::Value { + let msg = msg.as_any().downcast_ref::<$type>() + .expect(concat!("downcast failed for: ", stringify!($type))); + + let value = msg.$method(); + $wrap(value) + } + get as fn(&dyn $crate::mav_types::mav_message::MavMessageFields) -> serde_json::Value + } + ),* + }; + }; +} diff --git a/src/parser.rs b/src/parser.rs new file mode 100644 index 0000000..8ac7326 --- /dev/null +++ b/src/parser.rs @@ -0,0 +1,85 @@ +use crate::{ + error::DecoderError, + mav_types::mav_message::{heartbeat::HeartbeatMessage, MavMessage, MavMessageDef}, + v2::V2Packet, + Packet, +}; + +pub fn parse(packet: &Packet) -> Result { + let payload_start = V2Packet::STX_SIZE + V2Packet::HEADER_SIZE; + let payload_end = payload_start + packet.payload_length() as usize; + + // This increases the reference counter from the original packet.buffer, so it is guarantee to exist beyond packet's life + let payload = packet.bytes().slice(payload_start..payload_end); + + let message_id = packet.message_id(); + + match message_id { + HeartbeatMessage::ID => Ok(MavMessage::Heartbeat(HeartbeatMessage { buffer: payload })), + _ => Err(DecoderError::UnknownMessageID { msgid: message_id }), + } +} + +#[cfg(test)] +mod tests { + use super::*; + use mavlink::Message as _; + + #[test] + fn test_parse() { + // 1. Creates a default Heartbeat message using rust-mavlink + let original_message = + mavlink::ardupilotmega::MavMessage::default_message_from_id(0).unwrap(); + dbg!(&original_message); + + // 2. Serialize it into a raw message + let mut raw_v2_message = mavlink::MAVLinkV2MessageRaw::new(); + raw_v2_message.serialize_message(mavlink::MavHeader::default(), &original_message); + let mavlink::ardupilotmega::MavMessage::HEARTBEAT(original_heartbeat_message_data) = + original_message + else { + unreachable!("Wrong message parsed!"); + }; + + // 2. Convert it to V2Packet, this is our decoded mavlink packet: + let packet = Packet::from(raw_v2_message); + dbg!(&packet); + + // 3. From the decoded packet, parse the mavlink message + let parsed_message = parse(&packet).unwrap(); + + drop(packet); // Just to be sure the parsed message outlives the packet it came from + + dbg!(&parsed_message); + let MavMessage::Heartbeat(parsed_heartbeat_message_data) = parsed_message else { + panic!("Wrong message parsed!"); + }; + dbg!(&parsed_heartbeat_message_data); + + // 4. Now, compare all fields + assert_eq!( + parsed_heartbeat_message_data.custom_mode(), + original_heartbeat_message_data.custom_mode + ); + assert_eq!( + parsed_heartbeat_message_data.mav_type() as u32, + original_heartbeat_message_data.mavtype as u32 + ); + assert_eq!( + parsed_heartbeat_message_data.autopilot() as u8, + original_heartbeat_message_data.autopilot as u8 + ); + assert_eq!( + parsed_heartbeat_message_data.base_mode(), + original_heartbeat_message_data.base_mode.bits() + ); + assert_eq!( + parsed_heartbeat_message_data.system_status() as u32, + original_heartbeat_message_data.system_status as u32 + ); + assert_eq!( + parsed_heartbeat_message_data.mavlink_version(), + original_heartbeat_message_data.mavlink_version + ); + } +} diff --git a/src/v1.rs b/src/v1.rs index 38e012c..378bca2 100644 --- a/src/v1.rs +++ b/src/v1.rs @@ -66,33 +66,51 @@ impl V1Packet { } #[inline(always)] - pub fn stx(&self) -> &u8 { - stx(&self.buffer) + pub fn stx(&self) -> u8 { + *stx(&self.buffer) } #[inline(always)] - pub fn payload_length(&self) -> &u8 { - len(&self.buffer) + pub fn payload_length(&self) -> u8 { + *len(&self.buffer) } #[inline(always)] - pub fn sequence(&self) -> &u8 { - seq(&self.buffer) + pub fn sequence(&self) -> u8 { + *seq(&self.buffer) } #[inline(always)] - pub fn system_id(&self) -> &u8 { - sysid(&self.buffer) + pub fn system_id(&self) -> u8 { + *sysid(&self.buffer) } #[inline(always)] - pub fn component_id(&self) -> &u8 { - compid(&self.buffer) + pub fn component_id(&self) -> u8 { + *compid(&self.buffer) } #[inline(always)] - pub fn message_id(&self) -> &u8 { - msgid(&self.buffer) + pub fn message_id(&self) -> u8 { + *msgid(&self.buffer) + } + + #[inline(always)] + pub(crate) fn frame_header_bytes(&self) -> Bytes { + const LEN_SIZE: usize = 1; + let header_start = V1Packet::STX_SIZE + LEN_SIZE; + let header_end = header_start + V1Packet::HEADER_SIZE; + + self.buffer.slice(header_start..header_end) + } + + #[inline(always)] + pub(crate) fn payload_bytes(&self) -> Bytes { + let payload_start = V1Packet::STX_SIZE + V1Packet::HEADER_SIZE; + let payload_size = *len(&self.buffer) as usize; + let payload_end = payload_start + payload_size; + + self.buffer.slice(payload_start..payload_end) } } @@ -265,12 +283,12 @@ mod test { let v1_packet = V1Packet::from(raw_v1_message); assert_eq!(v1_packet.header(), raw_v1_message.clone().header()); // Todo: remote this clone once [this PR](https://github.com/mavlink/rust-mavlink/pull/288) get merged upstream - assert_eq!(*v1_packet.stx(), raw_v1_message.raw_bytes()[0]); - assert_eq!(*v1_packet.payload_length(), raw_v1_message.payload_length()); - assert_eq!(*v1_packet.sequence(), raw_v1_message.sequence()); - assert_eq!(*v1_packet.system_id(), raw_v1_message.system_id()); - assert_eq!(*v1_packet.component_id(), raw_v1_message.component_id()); - assert_eq!(*v1_packet.message_id(), raw_v1_message.message_id()); + assert_eq!(v1_packet.stx(), raw_v1_message.raw_bytes()[0]); + assert_eq!(v1_packet.payload_length(), raw_v1_message.payload_length()); + assert_eq!(v1_packet.sequence(), raw_v1_message.sequence()); + assert_eq!(v1_packet.system_id(), raw_v1_message.system_id()); + assert_eq!(v1_packet.component_id(), raw_v1_message.component_id()); + assert_eq!(v1_packet.message_id(), raw_v1_message.message_id()); assert_eq!(v1_packet.payload(), raw_v1_message.payload()); assert_eq!(v1_packet.checksum(), raw_v1_message.checksum()); } diff --git a/src/v2.rs b/src/v2.rs index da8214d..264ede8 100644 --- a/src/v2.rs +++ b/src/v2.rs @@ -80,44 +80,62 @@ impl V2Packet { } #[inline(always)] - pub fn stx(&self) -> &u8 { - stx(&self.buffer) + pub fn stx(&self) -> u8 { + *stx(&self.buffer) } #[inline(always)] - pub fn payload_length(&self) -> &u8 { - len(&self.buffer) + pub fn payload_length(&self) -> u8 { + *len(&self.buffer) } #[inline(always)] - pub fn incompatibility_flags(&self) -> &u8 { - incompat_flags(&self.buffer) + pub fn incompatibility_flags(&self) -> u8 { + *incompat_flags(&self.buffer) } #[inline(always)] - pub fn compatibility_flags(&self) -> &u8 { - compat_flags(&self.buffer) + pub fn compatibility_flags(&self) -> u8 { + *compat_flags(&self.buffer) } #[inline(always)] - pub fn sequence(&self) -> &u8 { - seq(&self.buffer) + pub fn sequence(&self) -> u8 { + *seq(&self.buffer) } #[inline(always)] - pub fn system_id(&self) -> &u8 { - sysid(&self.buffer) + pub fn system_id(&self) -> u8 { + *sysid(&self.buffer) } #[inline(always)] - pub fn component_id(&self) -> &u8 { - compid(&self.buffer) + pub fn component_id(&self) -> u8 { + *compid(&self.buffer) } #[inline(always)] pub fn message_id(&self) -> u32 { msgid(&self.buffer) } + + #[inline(always)] + pub(crate) fn frame_header_bytes(&self) -> Bytes { + const LEN_SIZE: usize = 1; + let header_start = V2Packet::STX_SIZE + LEN_SIZE; + let header_end = header_start + V2Packet::HEADER_SIZE; + + self.buffer.slice(header_start..header_end) + } + + #[inline(always)] + pub(crate) fn payload_bytes(&self) -> Bytes { + let payload_start = V2Packet::STX_SIZE + V2Packet::HEADER_SIZE; + let payload_size = *len(&self.buffer) as usize; + let payload_end = payload_start + payload_size; + + self.buffer.slice(payload_start..payload_end) + } } #[inline(always)] @@ -357,19 +375,19 @@ mod test { let v2_packet = V2Packet::from(raw_v2_message); assert_eq!(v2_packet.header(), raw_v2_message.header()); - assert_eq!(*v2_packet.stx(), raw_v2_message.raw_bytes()[0]); - assert_eq!(*v2_packet.payload_length(), raw_v2_message.payload_length()); + assert_eq!(v2_packet.stx(), raw_v2_message.raw_bytes()[0]); + assert_eq!(v2_packet.payload_length(), raw_v2_message.payload_length()); assert_eq!( - *v2_packet.incompatibility_flags(), + v2_packet.incompatibility_flags(), raw_v2_message.incompatibility_flags() ); assert_eq!( - *v2_packet.compatibility_flags(), + v2_packet.compatibility_flags(), raw_v2_message.compatibility_flags() ); - assert_eq!(*v2_packet.sequence(), raw_v2_message.sequence()); - assert_eq!(*v2_packet.system_id(), raw_v2_message.system_id()); - assert_eq!(*v2_packet.component_id(), raw_v2_message.component_id()); + assert_eq!(v2_packet.sequence(), raw_v2_message.sequence()); + assert_eq!(v2_packet.system_id(), raw_v2_message.system_id()); + assert_eq!(v2_packet.component_id(), raw_v2_message.component_id()); assert_eq!(v2_packet.message_id(), raw_v2_message.message_id()); assert_eq!(v2_packet.payload(), raw_v2_message.payload()); assert_eq!(v2_packet.checksum(), raw_v2_message.checksum());