forked from mavlink/rust-mavlink
/
direct_serial.rs
97 lines (81 loc) · 2.84 KB
/
direct_serial.rs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
use crate::connection::MavConnection;
use crate::peek_reader::PeekReader;
use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message};
use core::ops::DerefMut;
use std::io;
use std::sync::Mutex;
use crate::error::{MessageReadError, MessageWriteError};
use serial::{prelude::*, SystemPort};
/// Serial MAVLINK connection
pub fn open(settings: &str) -> io::Result<SerialConnection> {
let settings_toks: Vec<&str> = settings.split(':').collect();
if settings_toks.len() < 2 {
return Err(io::Error::new(
io::ErrorKind::AddrNotAvailable,
"Incomplete port settings",
));
}
let baud_opt = settings_toks[1].parse::<usize>();
if baud_opt.is_err() {
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,
char_size: serial::Bits8,
parity: serial::ParityNone,
stop_bits: serial::Stop1,
flow_control: serial::FlowNone,
};
let port_name = settings_toks[0];
let mut port = serial::open(port_name)?;
port.configure(&settings)?;
Ok(SerialConnection {
port: Mutex::new(PeekReader::new(port)),
sequence: Mutex::new(0),
protocol_version: MavlinkVersion::V2,
})
}
pub struct SerialConnection {
port: Mutex<PeekReader<SystemPort>>,
sequence: Mutex<u8>,
protocol_version: MavlinkVersion,
}
impl<M: Message> MavConnection<M> for SerialConnection {
fn recv(&self) -> Result<(MavHeader, M), MessageReadError> {
let mut port = self.port.lock().unwrap();
loop {
match read_versioned_msg(port.deref_mut(), self.protocol_version) {
ok @ Ok(..) => {
return ok;
}
Err(MessageReadError::Io(e)) => {
if e.kind() == io::ErrorKind::UnexpectedEof {
return Err(MessageReadError::Io(e));
}
}
_ => {}
}
}
}
fn send(&self, header: &MavHeader, data: &M) -> Result<usize, MessageWriteError> {
let mut port = self.port.lock().unwrap();
let mut sequence = self.sequence.lock().unwrap();
let header = MavHeader {
sequence: *sequence,
system_id: header.system_id,
component_id: header.component_id,
};
*sequence = sequence.wrapping_add(1);
write_versioned_msg(port.reader_mut(), self.protocol_version, header, data)
}
fn set_protocol_version(&mut self, version: MavlinkVersion) {
self.protocol_version = version;
}
fn get_protocol_version(&self) -> MavlinkVersion {
self.protocol_version
}
}