From 908e6547c0283028a4784d12368f112a5e036e86 Mon Sep 17 00:00:00 2001 From: danieleades <33452915+danieleades@users.noreply.github.com> Date: Sun, 25 Aug 2024 16:44:43 +0100 Subject: [PATCH] Minor refactoring --- mavlink-bindgen/src/lib.rs | 7 +- mavlink-bindgen/src/parser.rs | 12 +- mavlink-core/src/connection/direct_serial.rs | 16 +- mavlink-core/src/connection/file.rs | 6 +- mavlink-core/src/connection/mod.rs | 18 +-- mavlink-core/src/connection/tcp.rs | 6 +- mavlink-core/src/connection/udp.rs | 6 +- mavlink-core/src/lib.rs | 137 ++++++++---------- .../examples/embedded-async-read/src/main.rs | 9 +- 9 files changed, 89 insertions(+), 128 deletions(-) diff --git a/mavlink-bindgen/src/lib.rs b/mavlink-bindgen/src/lib.rs index 22f1efa2ce..23aae4b1d0 100644 --- a/mavlink-bindgen/src/lib.rs +++ b/mavlink-bindgen/src/lib.rs @@ -39,7 +39,7 @@ fn _generate( ) -> Result { let mut bindings = vec![]; - for entry_maybe in read_dir(&definitions_dir).map_err(|source| { + for entry_maybe in read_dir(definitions_dir).map_err(|source| { BindGenError::CouldNotReadDefinitionsDirectory { source, path: definitions_dir.to_path_buf(), @@ -55,8 +55,7 @@ fn _generate( let definition_file = PathBuf::from(entry.file_name()); let module_name = util::to_module_name(&definition_file); - let mut definition_rs = PathBuf::from(&module_name); - definition_rs.set_extension("rs"); + let definition_rs = PathBuf::from(&module_name).with_extension("rs"); let dest_path = destination_dir.join(definition_rs); let mut outf = BufWriter::new(File::create(&dest_path).map_err(|source| { @@ -67,7 +66,7 @@ fn _generate( })?); // generate code - parser::generate(&definitions_dir, &definition_file, &mut outf)?; + parser::generate(definitions_dir, &definition_file, &mut outf)?; bindings.push(GeneratedBinding { module_name, diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index 2b45e5d17b..dd8e389ea1 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -7,7 +7,6 @@ use std::fs::File; use std::io::{BufReader, Write}; use std::path::{Path, PathBuf}; use std::str::FromStr; -use std::u32; use quick_xml::{events::Event, Reader}; @@ -330,7 +329,7 @@ impl MavEnum { value = quote!(#cnt); } else { let tmp_value = enum_entry.value.unwrap(); - cnt = cnt.max(tmp_value as u32); + cnt = cnt.max(tmp_value); let tmp = TokenStream::from_str(&tmp_value.to_string()).unwrap(); value = quote!(#tmp); }; @@ -774,10 +773,11 @@ impl MavField { } } -#[derive(Debug, PartialEq, Clone)] +#[derive(Debug, PartialEq, Clone, Default)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub enum MavType { UInt8MavlinkVersion, + #[default] UInt8, UInt16, UInt32, @@ -792,12 +792,6 @@ pub enum MavType { Array(Box, usize), } -impl Default for MavType { - fn default() -> Self { - Self::UInt8 - } -} - impl MavType { fn parse_type(s: &str) -> Option { use self::MavType::*; diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs index 341d075ef2..a3a33a0fa6 100644 --- a/mavlink-core/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.rs @@ -1,3 +1,5 @@ +//! Serial MAVLINK connection + use crate::connection::MavConnection; use crate::peek_reader::PeekReader; use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; @@ -8,8 +10,6 @@ use std::sync::Mutex; use crate::error::{MessageReadError, MessageWriteError}; use serial::{prelude::*, SystemPort}; -/// Serial MAVLINK connection - pub fn open(settings: &str) -> io::Result { let settings_toks: Vec<&str> = settings.split(':').collect(); if settings_toks.len() < 2 { @@ -19,15 +19,15 @@ pub fn open(settings: &str) -> io::Result { )); } - let baud_opt = settings_toks[1].parse::(); - if baud_opt.is_err() { + let Ok(baud) = settings_toks[1] + .parse::() + .map(serial::core::BaudRate::from_speed) + else { return Err(io::Error::new( io::ErrorKind::AddrNotAvailable, "Invalid baud rate", )); - } - - let baud = serial::core::BaudRate::from_speed(baud_opt.unwrap()); + }; let settings = serial::core::PortSettings { baud_rate: baud, @@ -91,7 +91,7 @@ impl MavConnection for SerialConnection { self.protocol_version = version; } - fn get_protocol_version(&self) -> MavlinkVersion { + fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } } diff --git a/mavlink-core/src/connection/file.rs b/mavlink-core/src/connection/file.rs index d0fcdc9875..85444bb6ae 100644 --- a/mavlink-core/src/connection/file.rs +++ b/mavlink-core/src/connection/file.rs @@ -1,3 +1,5 @@ +//! File MAVLINK connection + use crate::connection::MavConnection; use crate::error::{MessageReadError, MessageWriteError}; use crate::peek_reader::PeekReader; @@ -7,8 +9,6 @@ use std::fs::File; use std::io; use std::sync::Mutex; -/// File MAVLINK connection - pub fn open(file_path: &str) -> io::Result { let file = File::open(file_path)?; @@ -52,7 +52,7 @@ impl MavConnection for FileConnection { self.protocol_version = version; } - fn get_protocol_version(&self) -> MavlinkVersion { + fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } } diff --git a/mavlink-core/src/connection/mod.rs b/mavlink-core/src/connection/mod.rs index 7aa4ab78ec..035f63eb28 100644 --- a/mavlink-core/src/connection/mod.rs +++ b/mavlink-core/src/connection/mod.rs @@ -24,7 +24,7 @@ pub trait MavConnection { fn send(&self, header: &MavHeader, data: &M) -> Result; fn set_protocol_version(&mut self, version: MavlinkVersion); - fn get_protocol_version(&self) -> MavlinkVersion; + fn protocol_version(&self) -> MavlinkVersion; /// Write whole frame fn send_frame(&self, frame: &MavFrame) -> Result { @@ -34,7 +34,7 @@ pub trait MavConnection { /// Read whole frame fn recv_frame(&self) -> Result, crate::error::MessageReadError> { let (header, msg) = self.recv()?; - let protocol_version = self.get_protocol_version(); + let protocol_version = self.protocol_version(); Ok(MavFrame { header, msg, @@ -107,14 +107,8 @@ pub fn connect(address: &str) -> io::Result pub(crate) fn get_socket_addr( address: T, ) -> Result { - let addr = match address.to_socket_addrs()?.next() { - Some(addr) => addr, - None => { - return Err(io::Error::new( - io::ErrorKind::Other, - "Host address lookup failed", - )); - } - }; - Ok(addr) + address.to_socket_addrs()?.next().ok_or(io::Error::new( + io::ErrorKind::Other, + "Host address lookup failed", + )) } diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs index 44d3778777..35b7f09084 100644 --- a/mavlink-core/src/connection/tcp.rs +++ b/mavlink-core/src/connection/tcp.rs @@ -1,3 +1,5 @@ +//! TCP MAVLink connection + use crate::connection::MavConnection; use crate::peek_reader::PeekReader; use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; @@ -10,8 +12,6 @@ use std::time::Duration; use super::get_socket_addr; -/// TCP MAVLink connection - pub fn select_protocol( address: &str, ) -> io::Result + Sync + Send>> { @@ -108,7 +108,7 @@ impl MavConnection for TcpConnection { self.protocol_version = version; } - fn get_protocol_version(&self) -> MavlinkVersion { + fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } } diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index b353ea1ba1..3ec739d3f5 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -1,3 +1,5 @@ +//! UDP MAVLink connection + use std::collections::VecDeque; use crate::connection::MavConnection; @@ -11,8 +13,6 @@ use std::sync::Mutex; use super::get_socket_addr; -/// UDP MAVLink connection - pub fn select_protocol( address: &str, ) -> io::Result + Sync + Send>> { @@ -156,7 +156,7 @@ impl MavConnection for UdpConnection { self.protocol_version = version; } - fn get_protocol_version(&self) -> MavlinkVersion { + fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } } diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 29e0d20345..e6e7aec97a 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -221,14 +221,11 @@ impl MavFrame { MavlinkVersion::V1 => buf.get_u8().into(), }; - match M::parse(version, msg_id, buf.remaining_bytes()) { - Ok(msg) => Ok(Self { - header, - msg, - protocol_version: version, - }), - Err(err) => Err(err), - } + M::parse(version, msg_id, buf.remaining_bytes()).map(|msg| Self { + header, + msg, + protocol_version: version, + }) } /// Return the frame header @@ -474,22 +471,18 @@ pub fn read_v1_msg( ) -> Result<(MavHeader, M), error::MessageReadError> { let message = read_v1_raw_message::(r)?; - M::parse( - MavlinkVersion::V1, - u32::from(message.message_id()), - message.payload(), - ) - .map(|msg| { - ( - MavHeader { - sequence: message.sequence(), - system_id: message.system_id(), - component_id: message.component_id(), - }, - msg, - ) - }) - .map_err(|err| err.into()) + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse( + MavlinkVersion::V1, + u32::from(message.message_id()), + message.payload(), + )?, + )) } /// Async read a MAVLink v1 message from a Read stream. @@ -502,22 +495,18 @@ pub async fn read_v1_msg_async( ) -> Result<(MavHeader, M), error::MessageReadError> { let message = read_v1_raw_message_async::(r).await?; - M::parse( - MavlinkVersion::V1, - u32::from(message.message_id()), - message.payload(), - ) - .map(|msg| { - ( - MavHeader { - sequence: message.sequence(), - system_id: message.system_id(), - component_id: message.component_id(), - }, - msg, - ) - }) - .map_err(|err| err.into()) + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse( + MavlinkVersion::V1, + u32::from(message.message_id()), + message.payload(), + )?, + )) } const MAVLINK_IFLAG_SIGNED: u8 = 0x01; @@ -818,18 +807,14 @@ pub fn read_v2_msg( ) -> Result<(MavHeader, M), error::MessageReadError> { let message = read_v2_raw_message::(read)?; - M::parse(MavlinkVersion::V2, message.message_id(), message.payload()) - .map(|msg| { - ( - MavHeader { - sequence: message.sequence(), - system_id: message.system_id(), - component_id: message.component_id(), - }, - msg, - ) - }) - .map_err(|err| err.into()) + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?, + )) } /// Async read a MAVLink v2 message from a Read stream. @@ -839,18 +824,14 @@ pub async fn read_v2_msg_async( ) -> Result<(MavHeader, M), error::MessageReadError> { let message = read_v2_raw_message_async::(read).await?; - M::parse(MavlinkVersion::V2, message.message_id(), message.payload()) - .map(|msg| { - ( - MavHeader { - sequence: message.sequence(), - system_id: message.system_id(), - component_id: message.component_id(), - }, - msg, - ) - }) - .map_err(|err| err.into()) + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?, + )) } /// Async read a MAVLink v2 message from a Read stream. @@ -863,22 +844,18 @@ pub async fn read_v2_msg_async( ) -> Result<(MavHeader, M), error::MessageReadError> { let message = read_v2_raw_message_async::(r).await?; - M::parse( - MavlinkVersion::V2, - u32::from(message.message_id()), - message.payload(), - ) - .map(|msg| { - ( - MavHeader { - sequence: message.sequence(), - system_id: message.system_id(), - component_id: message.component_id(), - }, - msg, - ) - }) - .map_err(|err| err.into()) + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse( + MavlinkVersion::V2, + u32::from(message.message_id()), + message.payload(), + )?, + )) } /// Write a message using the given mavlink version diff --git a/mavlink/examples/embedded-async-read/src/main.rs b/mavlink/examples/embedded-async-read/src/main.rs index af76a2640a..7f53335de3 100644 --- a/mavlink/examples/embedded-async-read/src/main.rs +++ b/mavlink/examples/embedded-async-read/src/main.rs @@ -82,12 +82,9 @@ pub async fn rx_task(rx: usart::UartRx<'static, Async>) { .unwrap(); rprintln!("Read raw message: msg_id={}", raw.message_id()); - match raw.message_id() { - HEARTBEAT_DATA::ID => { - let heartbeat = HEARTBEAT_DATA::deser(MavlinkVersion::V2, raw.payload()).unwrap(); - rprintln!("heartbeat: {:?}", heartbeat); - } - _ => {} + if raw.message_id() == HEARTBEAT_DATA::ID { + let heartbeat = HEARTBEAT_DATA::deser(MavlinkVersion::V2, raw.payload()).unwrap(); + rprintln!("heartbeat: {:?}", heartbeat); } } }