#![cfg_attr(not(feature = "std"), feature(alloc))]
#![cfg_attr(not(feature = "std"), no_std)]
#[cfg(not(feature = "std"))]
extern crate alloc;
#[cfg(feature = "std")]
use std::io::{Read, Result, Write};
#[cfg(feature = "std")]
extern crate byteorder;
#[cfg(feature = "std")]
use byteorder::{LittleEndian, ReadBytesExt, WriteBytesExt};
#[cfg(feature = "std")]
mod connection;
#[cfg(feature = "std")]
pub use self::connection::{connect, MavConnection};
#[cfg(feature = "serde")]
use serde::{Deserialize, Serialize};
extern crate bytes;
use bytes::{Buf, Bytes, IntoBuf};
extern crate bitflags;
extern crate num_derive;
extern crate num_traits;
include!(concat!(env!("OUT_DIR"), "/mod.rs"));
pub trait Message
where
Self: Sized,
{
fn message_id(&self) -> u32;
fn message_id_from_name(name: &str) -> std::result::Result<u32, &'static str>;
fn default_message_from_id(id: u32) -> std::result::Result<Self, &'static str>;
fn ser(&self) -> Vec<u8>;
fn extra_crc(id: u32) -> u8;
fn parse(version: MavlinkVersion, msgid: u32, payload: &[u8]) -> Option<Self>;
}
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct MavHeader {
pub system_id: u8,
pub component_id: u8,
pub sequence: u8,
}
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(Serialize))]
#[cfg_attr(feature = "serde", serde(tag = "type"))]
pub enum MavlinkVersion {
V1,
V2,
}
pub const MAV_STX: u8 = 0xFE;
pub const MAV_STX_V2: u8 = 0xFD;
impl Default for MavHeader {
fn default() -> Self {
Self {
system_id: 255,
component_id: 0,
sequence: 0,
}
}
}
#[derive(Debug, Clone)]
#[cfg_attr(feature = "serde", derive(Serialize))]
pub struct MavFrame<M: Message> {
pub header: MavHeader,
pub msg: M,
pub protocol_version: MavlinkVersion,
}
impl<M: Message> MavFrame<M> {
pub fn ser(&self) -> Vec<u8> {
let mut v = vec![];
v.push(self.header.system_id);
v.push(self.header.component_id);
v.push(self.header.sequence);
match self.protocol_version {
MavlinkVersion::V2 => {
let bytes: [u8; 4] = self.msg.message_id().to_le_bytes();
v.extend_from_slice(&bytes);
}
MavlinkVersion::V1 => {
v.push(self.msg.message_id() as u8);
}
}
v.append(&mut self.msg.ser());
v
}
pub fn deser(version: MavlinkVersion, input: &[u8]) -> Option<Self> {
let mut buf = Bytes::from(input).into_buf();
let system_id = buf.get_u8();
let component_id = buf.get_u8();
let sequence = buf.get_u8();
let header = MavHeader {
system_id,
component_id,
sequence,
};
let msg_id = match version {
MavlinkVersion::V2 => buf.get_u32_le(),
MavlinkVersion::V1 => buf.get_u8() as u32,
};
if let Some(msg) = M::parse(version, msg_id, &buf.collect::<Vec<u8>>()) {
Some(MavFrame {
header,
msg,
protocol_version: version,
})
} else {
None
}
}
pub fn header(&self) -> MavHeader {
self.header
}
}
pub fn read_versioned_msg<M: Message, R: Read>(
r: &mut R,
version: MavlinkVersion,
) -> Result<(MavHeader, M)> {
match version {
MavlinkVersion::V2 => read_v2_msg(r),
MavlinkVersion::V1 => read_v1_msg(r),
}
}
pub fn read_v1_msg<M: Message, R: Read>(r: &mut R) -> Result<(MavHeader, M)> {
loop {
if r.read_u8()? != MAV_STX {
continue;
}
let len = r.read_u8()? as usize;
let seq = r.read_u8()?;
let sysid = r.read_u8()?;
let compid = r.read_u8()?;
let msgid = r.read_u8()?;
let mut payload_buf = [0; 255];
let payload = &mut payload_buf[..len];
r.read_exact(payload)?;
let crc = r.read_u16::<LittleEndian>()?;
let mut crc_calc = crc16::State::<crc16::MCRF4XX>::new();
crc_calc.update(&[len as u8, seq, sysid, compid, msgid]);
crc_calc.update(payload);
crc_calc.update(&[M::extra_crc(msgid.into())]);
let recvd_crc = crc_calc.get();
if recvd_crc != crc {
continue;
}
if let Some(msg) = M::parse(MavlinkVersion::V1, msgid.into(), payload) {
return Ok((
MavHeader {
sequence: seq,
system_id: sysid,
component_id: compid,
},
msg,
));
}
}
}
const MAVLINK_IFLAG_SIGNED: u8 = 0x01;
pub fn read_v2_msg<M: Message, R: Read>(r: &mut R) -> Result<(MavHeader, M)> {
loop {
if r.read_u8()? != MAV_STX_V2 {
continue;
}
let payload_len = r.read_u8()? as usize;
let incompat_flags = r.read_u8()?;
let compat_flags = r.read_u8()?;
let seq = r.read_u8()?;
let sysid = r.read_u8()?;
let compid = r.read_u8()?;
let mut msgid_buf = [0; 4];
msgid_buf[0] = r.read_u8()?;
msgid_buf[1] = r.read_u8()?;
msgid_buf[2] = r.read_u8()?;
let header_buf = &[
payload_len as u8,
incompat_flags,
compat_flags,
seq,
sysid,
compid,
msgid_buf[0],
msgid_buf[1],
msgid_buf[2],
];
let msgid: u32 = u32::from_le_bytes(msgid_buf);
let mut payload_buf = [0; 255];
let payload = &mut payload_buf[..payload_len];
r.read_exact(payload)?;
let crc = r.read_u16::<LittleEndian>()?;
if (incompat_flags & 0x01) == MAVLINK_IFLAG_SIGNED {
let mut sign = [0; 13];
r.read_exact(&mut sign)?;
}
let mut crc_calc = crc16::State::<crc16::MCRF4XX>::new();
crc_calc.update(header_buf);
crc_calc.update(payload);
let extra_crc = M::extra_crc(msgid);
crc_calc.update(&[extra_crc]);
let recvd_crc = crc_calc.get();
if recvd_crc != crc {
continue;
}
if let Some(msg) = M::parse(MavlinkVersion::V2, msgid, payload) {
return Ok((
MavHeader {
sequence: seq,
system_id: sysid,
component_id: compid,
},
msg,
));
} else {
return Err(std::io::Error::new(
std::io::ErrorKind::InvalidData,
"Invalid MavMessage",
));
}
}
}
pub fn write_versioned_msg<M: Message, W: Write>(
w: &mut W,
version: MavlinkVersion,
header: MavHeader,
data: &M,
) -> Result<()> {
match version {
MavlinkVersion::V2 => write_v2_msg(w, header, data),
MavlinkVersion::V1 => write_v1_msg(w, header, data),
}
}
pub fn write_v2_msg<M: Message, W: Write>(w: &mut W, header: MavHeader, data: &M) -> Result<()> {
let msgid = data.message_id();
let payload = data.ser();
let header = &[
MAV_STX_V2,
payload.len() as u8,
0,
0,
header.sequence,
header.system_id,
header.component_id,
(msgid & 0x0000FF) as u8,
((msgid & 0x00FF00) >> 8) as u8,
((msgid & 0xFF0000) >> 16) as u8,
];
let mut crc = crc16::State::<crc16::MCRF4XX>::new();
crc.update(&header[1..]);
crc.update(&payload[..]);
let extra_crc = M::extra_crc(msgid);
crc.update(&[extra_crc]);
w.write_all(header)?;
w.write_all(&payload[..])?;
w.write_u16::<LittleEndian>(crc.get())?;
Ok(())
}
pub fn write_v1_msg<M: Message, W: Write>(w: &mut W, header: MavHeader, data: &M) -> Result<()> {
let msgid = data.message_id();
let payload = data.ser();
let header = &[
MAV_STX,
payload.len() as u8,
header.sequence,
header.system_id,
header.component_id,
msgid as u8,
];
let mut crc = crc16::State::<crc16::MCRF4XX>::new();
crc.update(&header[1..]);
crc.update(&payload[..]);
crc.update(&[M::extra_crc(msgid)]);
w.write_all(header)?;
w.write_all(&payload[..])?;
w.write_u16::<LittleEndian>(crc.get())?;
Ok(())
}