Skip to content

Commit

Permalink
minor refactoring
Browse files Browse the repository at this point in the history
  • Loading branch information
danieleades committed Aug 25, 2024
1 parent 8fb5875 commit 25d72b6
Show file tree
Hide file tree
Showing 10 changed files with 77 additions and 105 deletions.
5 changes: 5 additions & 0 deletions Cargo.toml
Original file line number Diff line number Diff line change
Expand Up @@ -8,3 +8,8 @@ num-traits = { version = "0.2", default-features = false }
num-derive = "0.3.2"
bitflags = "1.2.1"
byteorder = { version = "1.3.4", default-features = false }

[workspace.lints.clippy]
all = "deny"
pedantic = "warn"
nursery = "warn"
7 changes: 3 additions & 4 deletions mavlink-bindgen/src/lib.rs
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ fn _generate(
) -> Result<GeneratedBindings, BindGenError> {
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(),
Expand All @@ -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| {
Expand All @@ -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,
Expand Down
12 changes: 3 additions & 9 deletions mavlink-bindgen/src/parser.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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};

Expand Down Expand Up @@ -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);
};
Expand Down Expand Up @@ -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,
Expand All @@ -792,12 +792,6 @@ pub enum MavType {
Array(Box<MavType>, usize),
}

impl Default for MavType {
fn default() -> Self {
Self::UInt8
}
}

impl MavType {
fn parse_type(s: &str) -> Option<Self> {
use self::MavType::*;
Expand Down
2 changes: 1 addition & 1 deletion mavlink-core/src/connection/direct_serial.rs
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,7 @@ impl<M: Message> MavConnection<M> for SerialConnection {
self.protocol_version = version;
}

fn get_protocol_version(&self) -> MavlinkVersion {
fn protocol_version(&self) -> MavlinkVersion {
self.protocol_version
}
}
2 changes: 1 addition & 1 deletion mavlink-core/src/connection/file.rs
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ impl<M: Message> MavConnection<M> for FileConnection {
self.protocol_version = version;
}

fn get_protocol_version(&self) -> MavlinkVersion {
fn protocol_version(&self) -> MavlinkVersion {
self.protocol_version
}
}
4 changes: 2 additions & 2 deletions mavlink-core/src/connection/mod.rs
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ pub trait MavConnection<M: Message> {
fn send(&self, header: &MavHeader, data: &M) -> Result<usize, crate::error::MessageWriteError>;

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<M>) -> Result<usize, crate::error::MessageWriteError> {
Expand All @@ -34,7 +34,7 @@ pub trait MavConnection<M: Message> {
/// Read whole frame
fn recv_frame(&self) -> Result<MavFrame<M>, 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,
Expand Down
2 changes: 1 addition & 1 deletion mavlink-core/src/connection/tcp.rs
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ impl<M: Message> MavConnection<M> for TcpConnection {
self.protocol_version = version;
}

fn get_protocol_version(&self) -> MavlinkVersion {
fn protocol_version(&self) -> MavlinkVersion {
self.protocol_version
}
}
2 changes: 1 addition & 1 deletion mavlink-core/src/connection/udp.rs
Original file line number Diff line number Diff line change
Expand Up @@ -156,7 +156,7 @@ impl<M: Message> MavConnection<M> for UdpConnection {
self.protocol_version = version;
}

fn get_protocol_version(&self) -> MavlinkVersion {
fn protocol_version(&self) -> MavlinkVersion {
self.protocol_version
}
}
Expand Down
137 changes: 57 additions & 80 deletions mavlink-core/src/lib.rs
Original file line number Diff line number Diff line change
Expand Up @@ -221,14 +221,11 @@ impl<M: Message> MavFrame<M> {
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
Expand Down Expand Up @@ -474,22 +471,18 @@ pub fn read_v1_msg<M: Message, R: Read>(
) -> Result<(MavHeader, M), error::MessageReadError> {
let message = read_v1_raw_message::<M, _>(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.
Expand All @@ -502,22 +495,18 @@ pub async fn read_v1_msg_async<M: Message>(
) -> Result<(MavHeader, M), error::MessageReadError> {
let message = read_v1_raw_message_async::<M>(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;
Expand Down Expand Up @@ -818,18 +807,14 @@ pub fn read_v2_msg<M: Message, R: Read>(
) -> Result<(MavHeader, M), error::MessageReadError> {
let message = read_v2_raw_message::<M, _>(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.
Expand All @@ -839,18 +824,14 @@ pub async fn read_v2_msg_async<M: Message, R: tokio::io::AsyncReadExt + Unpin>(
) -> Result<(MavHeader, M), error::MessageReadError> {
let message = read_v2_raw_message_async::<M, _>(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.
Expand All @@ -863,22 +844,18 @@ pub async fn read_v2_msg_async<M: Message, R: embedded_io_async::Read>(
) -> Result<(MavHeader, M), error::MessageReadError> {
let message = read_v2_raw_message_async::<M>(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
Expand Down
9 changes: 3 additions & 6 deletions mavlink/examples/embedded-async-read/src/main.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
}

0 comments on commit 25d72b6

Please sign in to comment.