Skip to content

Commit

Permalink
remove heapless dependency
Browse files Browse the repository at this point in the history
  • Loading branch information
GabrielDertoni committed Mar 21, 2023
1 parent 6dfd5f7 commit af417e2
Show file tree
Hide file tree
Showing 3 changed files with 101 additions and 51 deletions.
6 changes: 3 additions & 3 deletions Cargo.toml
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ serde = { version = "1.0.115", optional = true, features = ["derive"] }
byteorder = { version = "1.3.4", default-features = false }
embedded-hal = { version = "0.2", optional = true }
nb = { version = "1.0", optional = true }
heapless = "0.7"
serde_arrays = { version = "0.1.0", optional = true }

[features]
"ardupilotmega" = ["common", "icarous", "uavionix"]
Expand Down Expand Up @@ -75,8 +75,8 @@ heapless = "0.7"
"tcp" = []
"direct-serial" = []
"embedded" = ["embedded-hal", "nb"]
"serde" = ["dep:serde", "heapless/serde"]
default= ["std", "tcp", "udp", "direct-serial", "serial", "serde", "ardupilotmega"]
"serde" = ["dep:serde", "dep:serde_arrays"]
default = ["std", "tcp", "udp", "direct-serial", "serial", "serde", "ardupilotmega"]

# build with all features on docs.rs so that users viewing documentation
# can see everything
Expand Down
122 changes: 88 additions & 34 deletions build/parser.rs
Original file line number Diff line number Diff line change
Expand Up @@ -178,8 +178,6 @@ impl MavProfile {
use num_traits::ToPrimitive;
#[allow(unused_imports)]
use bitflags::bitflags;
#[allow(unused_imports)]
use heapless::Vec;

use crate::{Message, error::*, bytes::Bytes, bytes_mut::BytesMut};

Expand Down Expand Up @@ -410,10 +408,15 @@ impl MavEnum {
quote!(#name)
}

fn emit_const_default(&self) -> TokenStream {
let default = format_ident!("{}", self.entries[0].name);
quote!(pub const DEFAULT: Self = Self::#default;)
}

fn emit_rust(&self) -> TokenStream {
let defs = self.emit_defs();
let default = format_ident!("{}", self.entries[0].name);
let enum_name = self.emit_name();
let const_default = self.emit_const_default();

#[cfg(feature = "emit-description")]
let description = if let Some(description) = self.description.as_ref() {
Expand Down Expand Up @@ -450,15 +453,19 @@ impl MavEnum {
};
}

dbg!(quote! {
quote! {
#enum_def

impl #enum_name {
#const_default
}

impl Default for #enum_name {
fn default() -> Self {
Self::#default
Self::DEFAULT
}
}
})
}
}
}

Expand Down Expand Up @@ -512,9 +519,16 @@ impl MavMessage {
quote!()
};

let serde_with_attr = if matches!(field.mavtype, MavType::Array(_, _)) {
quote!(#[cfg_attr(feature = "serde", serde(with = "serde_arrays"))])
} else {
quote!()
};

quote! {
#description
#serde_default
#serde_with_attr
#nametype
}
})
Expand Down Expand Up @@ -582,12 +596,33 @@ impl MavMessage {
}
}

fn emit_default_impl(&self) -> TokenStream {
let msg_name = self.emit_struct_name();
quote! {
impl Default for #msg_name {
fn default() -> Self {
Self::DEFAULT.clone()
}
}
}
}

fn emit_const_default(&self) -> TokenStream {
let initializers = self
.fields
.iter()
.map(|field| field.emit_default_initializer());
quote!(pub const DEFAULT: Self = Self { #(#initializers)* };)
}

fn emit_rust(&self) -> TokenStream {
let msg_name = self.emit_struct_name();
let (name_types, msg_encoded_len) = self.emit_name_types();

let deser_vars = self.emit_deserialize_vars();
let serialize_vars = self.emit_serialize_vars();
let const_default = self.emit_const_default();
let default_impl = self.emit_default_impl();

#[cfg(feature = "emit-description")]
let description = self.emit_description();
Expand All @@ -597,14 +632,15 @@ impl MavMessage {

quote! {
#description
#[derive(Debug, Clone, PartialEq, Default)]
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct #msg_name {
#(#name_types)*
}

impl #msg_name {
pub const ENCODED_LEN: usize = #msg_encoded_len;
#const_default

pub fn deser(_version: MavlinkVersion, _input: &[u8]) -> Result<Self, ParserError> {
#deser_vars
Expand All @@ -614,6 +650,8 @@ impl MavMessage {
#serialize_vars
}
}

#default_impl
}
}
}
Expand Down Expand Up @@ -733,6 +771,21 @@ impl MavField {
self.mavtype.rust_reader(&name, buf)
}
}

fn emit_default_initializer(&self) -> TokenStream {
let field = self.emit_name();
// FIXME: Is this actually expected behaviour??
if matches!(self.mavtype, MavType::Array(_, _)) {
let default_value = self.mavtype.emit_default_value();
quote!(#field: #default_value,)
} else if let Some(ref enumname) = self.enumtype {
let ty = TokenStream::from_str(enumname).unwrap();
quote!(#field: #ty::DEFAULT,)
} else {
let default_value = self.mavtype.emit_default_value();
quote!(#field: #default_value,)
}
}
}

#[derive(Debug, PartialEq, Clone)]
Expand Down Expand Up @@ -805,24 +858,12 @@ impl MavType {
Int64 => quote! {#val = #buf.get_i64_le();},
Float => quote! {#val = #buf.get_f32_le();},
Double => quote! {#val = #buf.get_f64_le();},
Array(t, size) => {
if size > 32 {
// it is a vector
let r = t.rust_reader(&quote!(let val), buf);
quote! {
for _ in 0..#size {
#r
#val.push(val).unwrap();
}
}
} else {
// handle as a slice
let r = t.rust_reader(&quote!(let val), buf);
quote! {
for idx in 0..#size {
#r
#val[idx] = val;
}
Array(t, _) => {
let r = t.rust_reader(&quote!(let val), buf);
quote! {
for v in &mut #val {
#r
*v = val;
}
}
}
Expand Down Expand Up @@ -916,15 +957,28 @@ impl MavType {
UInt64 => "u64".into(),
Int64 => "i64".into(),
Double => "f64".into(),
Array(t, size) => {
// format!("[{};{}]", t.rust_type(), size)
if size > 32 {
// we have to use a vector to make our lives easier
format!("Vec<{}, {}>", t.rust_type(), size)
} else {
// we can use a slice, as Rust derives lot of things for slices <= 32 elements
format!("[{};{}]", t.rust_type(), size)
}
Array(t, size) => format!("[{};{}]", t.rust_type(), size),
}
}

pub fn emit_default_value(&self) -> TokenStream {
use self::MavType::*;

match self {
UInt8 | UInt8MavlinkVersion => quote!(0_u8),
Int8 => quote!(0_i8),
Char => quote!(0_u8),
UInt16 => quote!(0_u16),
Int16 => quote!(0_i16),
UInt32 => quote!(0_u32),
Int32 => quote!(0_i32),
Float => quote!(0.0_f32),
UInt64 => quote!(0_u64),
Int64 => quote!(0_i64),
Double => quote!(0.0_f64),
Array(ty, size) => {
let default_value = ty.emit_default_value();
quote!([#default_value; #size])
}
}
}
Expand Down
24 changes: 10 additions & 14 deletions src/lib.rs
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,6 @@ use std::io::{Read, Write};
#[cfg(feature = "std")]
use byteorder::ReadBytesExt;

use heapless::Vec;

#[cfg(feature = "std")]
mod connection;
#[cfg(feature = "std")]
Expand Down Expand Up @@ -140,32 +138,30 @@ impl<M: Message> MavFrame<M> {
// }

/// Serialize MavFrame into a vector, so it can be sent over a socket, for example.
pub fn ser(&self) -> Vec<u8, MAX_FRAME_SIZE> {
pub fn ser(&self, buf: &mut [u8]) -> usize {
let mut buf = bytes_mut::BytesMut::new(buf);

// serialize header
let mut v = Vec::from_slice(&[
self.header.system_id,
self.header.component_id,
self.header.sequence,
])
.unwrap();
buf.put_u8(self.header.system_id);
buf.put_u8(self.header.component_id);
buf.put_u8(self.header.sequence);

// message id
match self.protocol_version {
MavlinkVersion::V2 => {
let bytes: [u8; 4] = self.msg.message_id().to_le_bytes();
v.extend_from_slice(&bytes).unwrap();
buf.put_slice(&bytes);
}
MavlinkVersion::V1 => {
v.push(self.msg.message_id() as u8).unwrap(); //TODO check
buf.put_u8(self.msg.message_id() as u8); //TODO check
}
}
// serialize message
let mut payload_buf = [0u8; 255];
let payload_len = self.msg.ser(self.protocol_version, &mut payload_buf);

v.extend_from_slice(&payload_buf[..payload_len]).unwrap();

v
buf.put_slice(&payload_buf[..payload_len]);
buf.len()
}

/// Deserialize MavFrame from a slice that has been received from, for example, a socket.
Expand Down

0 comments on commit af417e2

Please sign in to comment.