-
Notifications
You must be signed in to change notification settings - Fork 86
/
Copy pathmod.rs
126 lines (109 loc) · 3.93 KB
/
mod.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
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
use async_trait::async_trait;
use tokio::io;
use crate::{connectable::ConnectionAddress, MavFrame, MavHeader, MavlinkVersion, Message};
#[cfg(feature = "tcp")]
mod tcp;
#[cfg(feature = "udp")]
mod udp;
#[cfg(feature = "direct-serial")]
mod direct_serial;
mod file;
#[cfg(feature = "signing")]
use crate::SigningConfig;
/// An async MAVLink connection
#[async_trait::async_trait]
pub trait AsyncMavConnection<M: Message + Sync + Send> {
/// Receive a mavlink message.
///
/// Yield until a valid frame is received, ignoring invalid messages.
async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError>;
/// Send a mavlink message
async 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;
/// Write whole frame
async fn send_frame(
&self,
frame: &MavFrame<M>,
) -> Result<usize, crate::error::MessageWriteError> {
self.send(&frame.header, &frame.msg).await
}
/// Read whole frame
async fn recv_frame(&self) -> Result<MavFrame<M>, crate::error::MessageReadError> {
let (header, msg) = self.recv().await?;
let protocol_version = self.get_protocol_version();
Ok(MavFrame {
header,
msg,
protocol_version,
})
}
/// Send a message with default header
async fn send_default(&self, data: &M) -> Result<usize, crate::error::MessageWriteError> {
let header = MavHeader::default();
self.send(&header, data).await
}
/// Setup secret key used for message signing, or disable message signing
#[cfg(feature = "signing")]
fn setup_signing(&mut self, signing_data: Option<SigningConfig>);
}
/// Connect asynchronously to a MAVLink node by address string.
///
/// The address must be in one of the following formats:
///
/// * `tcpin:<addr>:<port>` to create a TCP server, listening for incoming connections
/// * `tcpout:<addr>:<port>` to create a TCP client
/// * `udpin:<addr>:<port>` to create a UDP server, listening for incoming packets
/// * `udpout:<addr>:<port>` to create a UDP client
/// * `udpbcast:<addr>:<port>` to create a UDP broadcast
/// * `serial:<port>:<baudrate>` to create a serial connection
/// * `file:<path>` to extract file data
///
/// The type of the connection is determined at runtime based on the address type, so the
/// connection is returned as a trait object.
pub async fn connect_async<M: Message + Sync + Send>(
address: &str,
) -> io::Result<Box<dyn AsyncMavConnection<M> + Sync + Send>> {
ConnectionAddress::parse_address(address)?
.connect_async::<M>()
.await
}
/// Returns the socket address for the given address.
pub(crate) fn get_socket_addr<T: std::net::ToSocketAddrs>(
address: T,
) -> Result<std::net::SocketAddr, io::Error> {
let addr = match address.to_socket_addrs()?.next() {
Some(addr) => addr,
None => {
return Err(io::Error::new(
io::ErrorKind::Other,
"Host address lookup failed",
));
}
};
Ok(addr)
}
#[async_trait]
pub trait AsyncConnectable {
async fn connect_async<M>(&self) -> io::Result<Box<dyn AsyncMavConnection<M> + Sync + Send>>
where
M: Message + Sync + Send;
}
#[async_trait]
impl AsyncConnectable for ConnectionAddress {
async fn connect_async<M>(&self) -> io::Result<Box<dyn AsyncMavConnection<M> + Sync + Send>>
where
M: Message + Sync + Send,
{
match self {
Self::Tcp(connectable) => connectable.connect_async::<M>().await,
Self::Udp(connectable) => connectable.connect_async::<M>().await,
Self::Serial(connectable) => connectable.connect_async::<M>().await,
Self::File(connectable) => connectable.connect_async::<M>().await,
}
}
}