From 142f404e5240699257c404de2e25908ee970c9cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sat, 4 Jul 2020 20:10:12 -0300 Subject: [PATCH 01/10] wip MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/lib.rs | 76 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 76 insertions(+) diff --git a/src/lib.rs b/src/lib.rs index 15cebb3673..f18de3c779 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -191,6 +191,81 @@ impl MavFrame { } } +trait MavlinkPacketFormat { + fn checksum(&mut self) -> &u16; + + fn calc_checksum(&self) -> u16; + + fn update_checksum(&mut self) { + *self.checksum() = self.calc_checksum::(); + } + + fn validate_checksum(&self) ->bool { + return *self.checksum() == self.calc_checksum::(); + } +} + +/// MAVLink V1 packet format: https://mavlink.io/en/guide/serialization.html#v1_packet_format +struct MavlinkV1PacketFormat { + pub magic: u8, + pub len: u8, + pub seq: u8, + pub sysid: u8, + pub compid: u8, + pub msgid: u8, + pub payload: Vec, + pub checksum: u16, +} + +impl Default for MavlinkV1PacketFormat { + fn default() -> Self { + MavlinkV1PacketFormat { + magic: MAV_STX, + ..Default::default() + } + } +} + +impl MavlinkPacketFormat for MavlinkV1PacketFormat { + fn checksum(&mut self) -> &u16 { + &self.checksum + } + + fn calc_checksum(&self) -> u16 { + let mut crc_calculator = CRCu16::crc16mcrf4cc(); + crc_calculator.digest(&[self.len, self.seq, self.sysid, self.compid, self.msgid]); + crc_calculator.digest(&self.payload); + crc_calculator.digest(&[M::extra_crc(self.msgid.into())]); + + return crc_calculator.get_crc(); + } +} + +/// MAVLink V2 packet format: https://mavlink.io/en/guide/serialization.html#mavlink2_packet_format +struct MavlinkV2PacketFormat { + pub magic: u8, + pub len: u8, + pub incompat_flags: u8, + pub compat_flags: u8, + pub seq: u8, + pub sysid: u8, + pub compid: u8, + pub msgid: u8, + pub payload: Vec, + pub checksum: u16, + pub signature: [u8; 13], +} + +impl Default for MavlinkV2PacketFormat { + fn default() -> Self { + MavlinkV2PacketFormat { + magic: MAV_STX_V2, + ..Default::default() + } + } +} + +/* pub fn read_versioned_msg( r: &mut R, version: MavlinkVersion, @@ -337,6 +412,7 @@ pub fn read_v2_msg( .map_err(|err| err.into()); } } +*/ /// Write a message using the given mavlink version pub fn write_versioned_msg( From 42de50dc53cd905b1abe4b8064b441987cde21cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sat, 4 Jul 2020 20:57:04 -0300 Subject: [PATCH 02/10] wip MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/lib.rs | 31 +++++++++++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index f18de3c779..000e9fe483 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -200,7 +200,7 @@ trait MavlinkPacketFormat { *self.checksum() = self.calc_checksum::(); } - fn validate_checksum(&self) ->bool { + fn validate_checksum(&self) -> bool { return *self.checksum() == self.calc_checksum::(); } } @@ -250,7 +250,7 @@ struct MavlinkV2PacketFormat { pub seq: u8, pub sysid: u8, pub compid: u8, - pub msgid: u8, + pub msgid: [u8; 3], pub payload: Vec, pub checksum: u16, pub signature: [u8; 13], @@ -265,6 +265,33 @@ impl Default for MavlinkV2PacketFormat { } } +impl MavlinkPacketFormat for MavlinkV2PacketFormat { + fn checksum(&mut self) -> &u16 { + &self.checksum + } + + fn calc_checksum(&self) -> u16 { + let mut crc_calculator = CRCu16::crc16mcrf4cc(); + crc_calculator.digest(&[ + self.len, + self.incompat_flags, + self.compat_flags, + self.seq, + self.sysid, + self.compid, + ]); + crc_calculator.digest(&self.msgid); + crc_calculator.digest(&self.payload); + + let mut msgid_buffer = [0 as u8; 4]; + msgid_buffer[..3].copy_from_slice(&self.msgid); + let msgid = u32::from_le_bytes(msgid_buffer); + + crc_calculator.digest(&[M::extra_crc(msgid)]); + return crc_calculator.get_crc(); + } +} + /* pub fn read_versioned_msg( r: &mut R, From 9904f8a33a4b62a84399fd29161bb14b3dd50e0b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sat, 4 Jul 2020 22:32:07 -0300 Subject: [PATCH 03/10] wip MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/lib.rs | 133 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 133 insertions(+) diff --git a/src/lib.rs b/src/lib.rs index 000e9fe483..fe5858294b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -292,6 +292,139 @@ impl MavlinkPacketFormat for MavlinkV2PacketFormat { } } +trait MavlinkParser { + fn version() -> MavlinkVersion; + + fn read(&mut self, reader: &mut R) + -> Result<(MavHeader, M), error::MessageReadError>; +} + +enum MavlinkV1ParserState { + Len, + Seq, + SysId, + CompId, + MsgId, + Payload, + Crc1, + Crc2, + Done, +} + +struct MavlinkV1Parser { + format: MavlinkV1PacketFormat, + state: MavlinkV1ParserState, + payload_parsed: u8, +} + +impl Default for MavlinkV1Parser { + fn default() -> Self { + MavlinkV1Parser { + format: Default::default(), + state: MavlinkV1ParserState::Len, + payload_parsed: 0, + } + } +} + +enum MavlinkV2ParserState { + Len, + IncompatFlags, + CompatFlags, + Seq, + SysId, + CompId, + MsgId1, + MsgId2, + MsgId3, + Payload, + Crc1, + Crc2, + Signature, + Done, +} + +struct MavlinkV2Parser { + format: MavlinkV2PacketFormat, + state: MavlinkV2ParserState, + payload_parsed: u8, + signature_parsed: u8, +} + +impl Default for MavlinkV2Parser { + fn default() -> Self { + MavlinkV2Parser { + format: Default::default(), + state: MavlinkV2ParserState::Len, + payload_parsed: 0, + signature_parsed: 0, + } + } +} + +impl MavlinkParser for MavlinkV1Parser { + fn version() -> MavlinkVersion { + MavlinkVersion::V1 + } + + fn read( + &mut self, + reader: &mut R, + ) -> Result<(MavHeader, M), error::MessageReadError> { + loop { + let mut buffer = [0; 1]; + let size = match reader.read(&mut buffer) { + Ok(size) => { size } + _ => { break } + }; + + // PATRICK IS MISSING MAGIC NUMBER + use MavlinkV1ParserState::*; + match self.state { + Len => { + self.format.len = buffer[0]; + self.format.payload = vec![0; self.format.len as usize]; + self.state = Seq; + } + Seq => { + self.format.seq = buffer[0]; + self.state = SysId; + } + SysId => { + self.format.sysid = buffer[0]; + self.state = CompId; + } + CompId => { + self.format.compid = buffer[0]; + self.state = MsgId; + } + MsgId => { + self.format.msgid = buffer[0]; + self.state = Payload; + } + Payload => { + self.format.payload[self.payload_parsed as usize] = buffer[0]; + self.payload_parsed += 1; + if self.payload_parsed == self.format.len { + self.state = Crc1; + } + } + Crc1 => { + self.format.msgid = buffer[0]; + self.state = Crc2; //MISSING + } + Crc2 => { + self.format.msgid = buffer[0]; + self.state = Done; // MISSING + } + Done => { + // MISSING + } + } + } + } +} + /* pub fn read_versioned_msg( r: &mut R, From ae9bbfb6c455ba5429c2a95d0c410b7eef30b2f4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sat, 4 Jul 2020 23:50:29 -0300 Subject: [PATCH 04/10] wip MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/error.rs | 6 ++++++ src/lib.rs | 49 ++++++++++++++++++++++++++++++++++++++----------- 2 files changed, 44 insertions(+), 11 deletions(-) diff --git a/src/error.rs b/src/error.rs index 63422a22c9..53288ab5a5 100644 --- a/src/error.rs +++ b/src/error.rs @@ -5,6 +5,7 @@ use std::fmt::{Display, Formatter}; pub enum ParserError { InvalidFlag { flag_type: String, value: u32 }, InvalidEnum { enum_type: String, value: u32 }, + WrongCrc { expected: u16, received: u16 }, UnknownMessage { id: u32 }, } @@ -21,6 +22,11 @@ impl Display for ParserError { "Invalid enum value for enum type {:?}, got {:?}", enum_type, value ), + ParserError::WrongCrc { expected, received } => write!( + f, + "CRC does not mache, expected {:?}, got {:?}", + expected, received + ), ParserError::UnknownMessage { id } => write!(f, "Unknown message with ID {:?}", id), } } diff --git a/src/lib.rs b/src/lib.rs index fe5858294b..b122aebe29 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -295,8 +295,10 @@ impl MavlinkPacketFormat for MavlinkV2PacketFormat { trait MavlinkParser { fn version() -> MavlinkVersion; - fn read(&mut self, reader: &mut R) - -> Result<(MavHeader, M), error::MessageReadError>; + fn read( + &mut self, + reader: &mut R, + ) -> Result<(MavHeader, M), error::MessageReadError>; } enum MavlinkV1ParserState { @@ -374,8 +376,13 @@ impl MavlinkParser for MavlinkV1Parser { loop { let mut buffer = [0; 1]; let size = match reader.read(&mut buffer) { - Ok(size) => { size } - _ => { break } + Ok(size) => size, + _ => { + return Err(error::MessageReadError::Io(std::io::Error::new( + std::io::ErrorKind::TimedOut, + "No data avaiable.", + ))); + } }; // PATRICK IS MISSING MAGIC NUMBER @@ -410,15 +417,35 @@ impl MavlinkParser for MavlinkV1Parser { } } Crc1 => { - self.format.msgid = buffer[0]; - self.state = Crc2; //MISSING + let value = self.format.checksum.to_le_bytes(); + value[0] = buffer[0]; + self.format.checksum = u16::from_le_bytes(value); + self.state = Crc2; } Crc2 => { - self.format.msgid = buffer[0]; - self.state = Done; // MISSING - } - Done => { - // MISSING + let value = self.format.checksum.to_le_bytes(); + value[1] = buffer[0]; + self.format.checksum = u16::from_le_bytes(value); + self.state = Len; + + if self.format.validate_checksum::() { + return M::parse( + MavlinkV1Parser::version(), + self.format.msgid as u32, + &self.format.payload, + ) + .map(|msg| { + ( + MavHeader { + sequence: self.format.seq, + system_id: self.format.sysid, + component_id: self.format.compid, + }, + msg, + ) + }) + .map_err(|err| err.into()); + } } } } From 88820c59635e91c42d73ccee93fe301825e6c088 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sun, 5 Jul 2020 00:04:38 -0300 Subject: [PATCH 05/10] wip MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/lib.rs | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/lib.rs b/src/lib.rs index b122aebe29..ec88b99d13 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -302,6 +302,7 @@ trait MavlinkParser { } enum MavlinkV1ParserState { + Magic, Len, Seq, SysId, @@ -330,6 +331,7 @@ impl Default for MavlinkV1Parser { } enum MavlinkV2ParserState { + Magic, Len, IncompatFlags, CompatFlags, @@ -388,6 +390,11 @@ impl MavlinkParser for MavlinkV1Parser { // PATRICK IS MISSING MAGIC NUMBER use MavlinkV1ParserState::*; match self.state { + Magic => { + if buffer[0] == self.format.magic { + self.state = Len; + } + } Len => { self.format.len = buffer[0]; self.format.payload = vec![0; self.format.len as usize]; @@ -446,6 +453,11 @@ impl MavlinkParser for MavlinkV1Parser { }) .map_err(|err| err.into()); } + + return Err(error::MessageReadError::Parse(ParserError::WrongCrc { + expected: self.format.calc_checksum::(), + received: self.format.checksum, + })); } } } From deb86fcd9ab993027a5fe0ccb94d337eec26a8b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sun, 5 Jul 2020 00:11:31 -0300 Subject: [PATCH 06/10] wip MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/lib.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index ec88b99d13..98588e2450 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -387,7 +387,6 @@ impl MavlinkParser for MavlinkV1Parser { } }; - // PATRICK IS MISSING MAGIC NUMBER use MavlinkV1ParserState::*; match self.state { Magic => { From 890254cc876144c27a6f8e12d64e8b8ff0a53efc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sun, 5 Jul 2020 18:28:42 -0300 Subject: [PATCH 07/10] wip MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- Cargo.toml | 2 +- src/connection/file.rs | 12 +++++++----- src/connection/udp.rs | 15 +++++++-------- src/lib.rs | 7 ++++--- 4 files changed, 19 insertions(+), 17 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 48ae644425..00aa2d16a9 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -54,7 +54,7 @@ byteorder = { version = "1.3.4", optional = true } "udp" = [] "tcp" = [] "direct-serial" = [] -default= ["std", "tcp", "udp", "direct-serial", "serial", "serde", "common"] +default= ["std", "udp", "serde", "common"] # build with all features on docs.rs so that users viewing documentation # can see everything diff --git a/src/connection/file.rs b/src/connection/file.rs index 5cc64b58c5..bfa84169a0 100644 --- a/src/connection/file.rs +++ b/src/connection/file.rs @@ -1,6 +1,6 @@ use crate::connection::MavConnection; use crate::error::MessageReadError; -use crate::{read_versioned_msg, MavHeader, MavlinkVersion, Message}; +use crate::{MavHeader, MavlinkVersion, Message}; use std::fs::File; use std::io::{self}; use std::sync::Mutex; @@ -15,13 +15,13 @@ pub fn open(file_path: &str) -> io::Result { Ok(FileConnection { file: Mutex::new(file), - protocol_version: MavlinkVersion::V2, + //protocol_version: MavlinkVersion::V2, }) } pub struct FileConnection { file: Mutex, - protocol_version: MavlinkVersion, + //protocol_version: MavlinkVersion, } impl MavConnection for FileConnection { @@ -31,6 +31,7 @@ impl MavConnection for FileConnection { let mut file = self.file.lock().unwrap(); loop { + /* match read_versioned_msg(&mut *file, self.protocol_version) { ok @ Ok(..) => { return ok; @@ -42,6 +43,7 @@ impl MavConnection for FileConnection { } _ => {} } + */ } } @@ -50,10 +52,10 @@ impl MavConnection for FileConnection { } fn set_protocol_version(&mut self, version: MavlinkVersion) { - self.protocol_version = version; + //self.protocol_version = version; } fn get_protocol_version(&self) -> MavlinkVersion { - self.protocol_version + //self.protocol_version } } diff --git a/src/connection/udp.rs b/src/connection/udp.rs index 87f66d6fa2..d327f84b0e 100644 --- a/src/connection/udp.rs +++ b/src/connection/udp.rs @@ -1,5 +1,5 @@ use crate::connection::MavConnection; -use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; +use crate::{MavlinkParser, MavlinkV1Parser, write_versioned_msg, MavHeader, MavlinkVersion, Message}; use std::io::Read; use std::io::{self}; use std::net::ToSocketAddrs; @@ -117,7 +117,6 @@ struct UdpRead { pub struct UdpConnection { reader: Mutex, writer: Mutex, - protocol_version: MavlinkVersion, server: bool, } @@ -134,7 +133,6 @@ impl UdpConnection { dest: dest, sequence: 0, }), - protocol_version: MavlinkVersion::V2, }) } } @@ -153,10 +151,11 @@ impl MavConnection for UdpConnection { } } - match read_versioned_msg(&mut state.recv_buf, self.protocol_version) { + /* + match self.parser.read(&mut state.recv_buf, self.protocol_version) { ok @ Ok(..) => return ok, _ => (), - } + }*/ } } @@ -174,7 +173,7 @@ impl MavConnection for UdpConnection { if let Some(addr) = state.dest { let mut buf = Vec::new(); - write_versioned_msg(&mut buf, self.protocol_version, header, data)?; + //write_versioned_msg(&mut buf, self.protocol_version, header, data)?; state.socket.send_to(&buf, addr)?; } @@ -182,10 +181,10 @@ impl MavConnection for UdpConnection { } fn set_protocol_version(&mut self, version: MavlinkVersion) { - self.protocol_version = version; + //self.protocol_version = version; } fn get_protocol_version(&self) -> MavlinkVersion { - self.protocol_version + //self.protocol_version } } diff --git a/src/lib.rs b/src/lib.rs index 98588e2450..bfcaa47f31 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -293,7 +293,7 @@ impl MavlinkPacketFormat for MavlinkV2PacketFormat { } trait MavlinkParser { - fn version() -> MavlinkVersion; + fn version(&self) -> MavlinkVersion; fn read( &mut self, @@ -320,6 +320,7 @@ struct MavlinkV1Parser { payload_parsed: u8, } +//TODO: this should be a new function and not default to not let user to change intern variables impl Default for MavlinkV1Parser { fn default() -> Self { MavlinkV1Parser { @@ -367,7 +368,7 @@ impl Default for MavlinkV2Parser { } impl MavlinkParser for MavlinkV1Parser { - fn version() -> MavlinkVersion { + fn version(&self) -> MavlinkVersion { MavlinkVersion::V1 } @@ -436,7 +437,7 @@ impl MavlinkParser for MavlinkV1Parser { if self.format.validate_checksum::() { return M::parse( - MavlinkV1Parser::version(), + self.version(), self.format.msgid as u32, &self.format.payload, ) From 74002c47ea2ead91b06eff81f7f1b89faf93aae3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sun, 5 Jul 2020 18:33:53 -0300 Subject: [PATCH 08/10] wip MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/lib.rs | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index bfcaa47f31..c1c6c37d70 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -320,9 +320,8 @@ struct MavlinkV1Parser { payload_parsed: u8, } -//TODO: this should be a new function and not default to not let user to change intern variables -impl Default for MavlinkV1Parser { - fn default() -> Self { +impl MavlinkV1Parser { + pub fn new() -> Self { MavlinkV1Parser { format: Default::default(), state: MavlinkV1ParserState::Len, From 4488270f0a038f71a83c243b1722d11b5887e41e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sat, 8 Aug 2020 14:10:34 -0300 Subject: [PATCH 09/10] compile MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/connection/file.rs | 1 + src/connection/udp.rs | 8 +++++--- src/lib.rs | 33 +++++++++++++++++++++++---------- 3 files changed, 29 insertions(+), 13 deletions(-) diff --git a/src/connection/file.rs b/src/connection/file.rs index bfa84169a0..da9fd445cc 100644 --- a/src/connection/file.rs +++ b/src/connection/file.rs @@ -57,5 +57,6 @@ impl MavConnection for FileConnection { fn get_protocol_version(&self) -> MavlinkVersion { //self.protocol_version + MavlinkVersion::V1 } } diff --git a/src/connection/udp.rs b/src/connection/udp.rs index d327f84b0e..b679ce4b1d 100644 --- a/src/connection/udp.rs +++ b/src/connection/udp.rs @@ -118,6 +118,7 @@ pub struct UdpConnection { reader: Mutex, writer: Mutex, server: bool, + protocol_version: MavlinkVersion, } impl UdpConnection { @@ -133,6 +134,7 @@ impl UdpConnection { dest: dest, sequence: 0, }), + protocol_version: MavlinkVersion::V1, }) } } @@ -181,10 +183,10 @@ impl MavConnection for UdpConnection { } fn set_protocol_version(&mut self, version: MavlinkVersion) { - //self.protocol_version = version; + self.protocol_version = version; } fn get_protocol_version(&self) -> MavlinkVersion { - //self.protocol_version + self.protocol_version } -} +} \ No newline at end of file diff --git a/src/lib.rs b/src/lib.rs index c1c6c37d70..8bb08f6299 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -192,16 +192,18 @@ impl MavFrame { } trait MavlinkPacketFormat { - fn checksum(&mut self) -> &u16; + fn checksum(&self) -> u16; + + fn set_checksum(&mut self, value: u16); fn calc_checksum(&self) -> u16; fn update_checksum(&mut self) { - *self.checksum() = self.calc_checksum::(); + self.set_checksum(self.calc_checksum::()); } fn validate_checksum(&self) -> bool { - return *self.checksum() == self.calc_checksum::(); + return self.checksum() == self.calc_checksum::(); } } @@ -227,8 +229,12 @@ impl Default for MavlinkV1PacketFormat { } impl MavlinkPacketFormat for MavlinkV1PacketFormat { - fn checksum(&mut self) -> &u16 { - &self.checksum + fn checksum(&self) -> u16 { + return self.checksum; + } + + fn set_checksum(&mut self, value: u16){ + self.checksum = value; } fn calc_checksum(&self) -> u16 { @@ -266,8 +272,12 @@ impl Default for MavlinkV2PacketFormat { } impl MavlinkPacketFormat for MavlinkV2PacketFormat { - fn checksum(&mut self) -> &u16 { - &self.checksum + fn checksum(&self) -> u16 { + return self.checksum; + } + + fn set_checksum(&mut self, value: u16){ + self.checksum = value; } fn calc_checksum(&self) -> u16 { @@ -423,16 +433,16 @@ impl MavlinkParser for MavlinkV1Parser { } } Crc1 => { - let value = self.format.checksum.to_le_bytes(); + let mut value = self.format.checksum.to_le_bytes(); value[0] = buffer[0]; self.format.checksum = u16::from_le_bytes(value); self.state = Crc2; } Crc2 => { - let value = self.format.checksum.to_le_bytes(); + let mut value = self.format.checksum.to_le_bytes(); value[1] = buffer[0]; self.format.checksum = u16::from_le_bytes(value); - self.state = Len; + self.state = Done; if self.format.validate_checksum::() { return M::parse( @@ -458,6 +468,9 @@ impl MavlinkParser for MavlinkV1Parser { received: self.format.checksum, })); } + Done => { + self.state = Magic; + } } } } From 1d532e03eff2059932c9288223c986b32e07ad51 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sun, 30 Aug 2020 20:25:54 -0300 Subject: [PATCH 10/10] wip MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/bin/mavlink-dump.rs | 4 ++-- src/connection/udp.rs | 23 ++++++++++++++++------- src/lib.rs | 20 ++++++++++++++------ 3 files changed, 32 insertions(+), 15 deletions(-) diff --git a/src/bin/mavlink-dump.rs b/src/bin/mavlink-dump.rs index efa2967c8f..a0f3cee2a1 100644 --- a/src/bin/mavlink-dump.rs +++ b/src/bin/mavlink-dump.rs @@ -27,7 +27,7 @@ fn main() { // here as an example we force the protocol version to mavlink V1: // the default for this library is mavlink V2 - mavconn.set_protocol_version(mavlink::MavlinkVersion::V1); + mavconn.set_protocol_version(mavlink::MavlinkVersion::V2); let vehicle = Arc::new(mavconn); vehicle @@ -63,7 +63,7 @@ fn main() { } _ => { println!("recv error: {:?}", e); - break; + //break; } } } diff --git a/src/connection/udp.rs b/src/connection/udp.rs index b679ce4b1d..376592eb36 100644 --- a/src/connection/udp.rs +++ b/src/connection/udp.rs @@ -13,11 +13,11 @@ pub fn select_protocol( address: &str, ) -> io::Result + Sync + Send>> { if address.starts_with("udpin:") { - Ok(Box::new(udpin(&address["udpin:".len()..])?)) + Ok(Box::new(udpin(&address["udpin:".len()..])?)) } else if address.starts_with("udpout:") { - Ok(Box::new(udpout(&address["udpout:".len()..])?)) + Ok(Box::new(udpout(&address["udpout:".len()..])?)) } else if address.starts_with("udpbcast:") { - Ok(Box::new(udpbcast(&address["udpbcast:".len()..])?)) + Ok(Box::new(udpbcast(&address["udpbcast:".len()..])?)) } else { Err(io::Error::new( io::ErrorKind::AddrNotAvailable, @@ -26,7 +26,7 @@ pub fn select_protocol( } } -pub fn udpbcast(address: T) -> io::Result { +pub fn udpbcast(address: T) -> io::Result { let addr = address .to_socket_addrs() .unwrap() @@ -36,7 +36,7 @@ pub fn udpbcast(address: T) -> io::Result { socket .set_broadcast(true) .expect("Couldn't bind to broadcast address."); - UdpConnection::new(socket, false, Some(addr)) + UdpConnection::new(socket, false, Some(addr)) } pub fn udpout(address: T) -> io::Result { @@ -114,11 +114,12 @@ struct UdpRead { recv_buf: PacketBuf, } -pub struct UdpConnection { +pub struct UdpConnection { reader: Mutex, writer: Mutex, server: bool, protocol_version: MavlinkVersion, + parser: Box, } impl UdpConnection { @@ -135,6 +136,7 @@ impl UdpConnection { sequence: 0, }), protocol_version: MavlinkVersion::V1, + parser: MavlinkV1Parser::default(), }) } } @@ -152,12 +154,19 @@ impl MavConnection for UdpConnection { self.writer.lock().unwrap().dest = Some(src); } } + println!("Received buffer: {} start {} end {}", state.recv_buf.len(), state.recv_buf.start, state.recv_buf.end); /* + return Err(crate::error::MessageReadError::Io(std::io::Error::new( + std::io::ErrorKind::TimedOut, + "No data avaiable.", + ))); + */ + match self.parser.read(&mut state.recv_buf, self.protocol_version) { ok @ Ok(..) => return ok, _ => (), - }*/ + } } } diff --git a/src/lib.rs b/src/lib.rs index 8bb08f6299..208cf3d60c 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -305,9 +305,9 @@ impl MavlinkPacketFormat for MavlinkV2PacketFormat { trait MavlinkParser { fn version(&self) -> MavlinkVersion; - fn read( + fn read( &mut self, - reader: &mut R, + buffer: &Vec ) -> Result<(MavHeader, M), error::MessageReadError>; } @@ -381,11 +381,20 @@ impl MavlinkParser for MavlinkV1Parser { MavlinkVersion::V1 } - fn read( + fn read( &mut self, - reader: &mut R, + buffer: &Vec, ) -> Result<(MavHeader, M), error::MessageReadError> { + let mut counter = 0; + //TODO: move to iterator + let mut get = || -> Option { + let value = buffer.get(counter); + counter += 1; + return value; + }; + loop { + /* let mut buffer = [0; 1]; let size = match reader.read(&mut buffer) { Ok(size) => size, @@ -395,8 +404,7 @@ impl MavlinkParser for MavlinkV1Parser { "No data avaiable.", ))); } - }; - + };*/ use MavlinkV1ParserState::*; match self.state { Magic => {