mavlink_core/connection/
mod.rs

1use crate::{connectable::ConnectionAddress, MavFrame, MavHeader, MavlinkVersion, Message};
2
3use core::fmt::Display;
4use std::io::{self};
5
6#[cfg(feature = "tcp")]
7mod tcp;
8
9#[cfg(feature = "udp")]
10mod udp;
11
12#[cfg(feature = "direct-serial")]
13mod direct_serial;
14
15#[cfg(feature = "signing")]
16use crate::SigningConfig;
17
18mod file;
19
20/// A MAVLink connection
21pub trait MavConnection<M: Message> {
22    /// Receive a mavlink message.
23    ///
24    /// Blocks until a valid frame is received, ignoring invalid messages.
25    fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError>;
26
27    /// Send a mavlink message
28    fn send(&self, header: &MavHeader, data: &M) -> Result<usize, crate::error::MessageWriteError>;
29
30    fn set_protocol_version(&mut self, version: MavlinkVersion);
31    fn protocol_version(&self) -> MavlinkVersion;
32
33    /// Write whole frame
34    fn send_frame(&self, frame: &MavFrame<M>) -> Result<usize, crate::error::MessageWriteError> {
35        self.send(&frame.header, &frame.msg)
36    }
37
38    /// Read whole frame
39    fn recv_frame(&self) -> Result<MavFrame<M>, crate::error::MessageReadError> {
40        let (header, msg) = self.recv()?;
41        let protocol_version = self.protocol_version();
42        Ok(MavFrame {
43            header,
44            msg,
45            protocol_version,
46        })
47    }
48
49    /// Send a message with default header
50    fn send_default(&self, data: &M) -> Result<usize, crate::error::MessageWriteError> {
51        let header = MavHeader::default();
52        self.send(&header, data)
53    }
54
55    /// Setup secret key used for message signing, or disable message signing
56    #[cfg(feature = "signing")]
57    fn setup_signing(&mut self, signing_data: Option<SigningConfig>);
58}
59
60/// Connect to a MAVLink node by address string.
61///
62/// The address must be in one of the following formats:
63///
64///  * `tcpin:<addr>:<port>` to create a TCP server, listening for incoming connections
65///  * `tcpout:<addr>:<port>` to create a TCP client
66///  * `udpin:<addr>:<port>` to create a UDP server, listening for incoming packets
67///  * `udpout:<addr>:<port>` to create a UDP client
68///  * `udpbcast:<addr>:<port>` to create a UDP broadcast
69///  * `serial:<port>:<baudrate>` to create a serial connection
70///  * `file:<path>` to extract file data
71///
72/// The type of the connection is determined at runtime based on the address type, so the
73/// connection is returned as a trait object.
74pub fn connect<M: Message + Sync + Send>(
75    address: &str,
76) -> io::Result<Box<dyn MavConnection<M> + Sync + Send>> {
77    ConnectionAddress::parse_address(address)?.connect::<M>()
78}
79
80/// Returns the socket address for the given address.
81pub(crate) fn get_socket_addr<T: std::net::ToSocketAddrs>(
82    address: &T,
83) -> Result<std::net::SocketAddr, io::Error> {
84    address.to_socket_addrs()?.next().ok_or(io::Error::new(
85        io::ErrorKind::Other,
86        "Host address lookup failed",
87    ))
88}
89
90pub trait Connectable: Display {
91    fn connect<M: Message>(&self) -> io::Result<Box<dyn MavConnection<M> + Sync + Send>>;
92}
93
94impl Connectable for ConnectionAddress {
95    fn connect<M>(&self) -> std::io::Result<Box<dyn crate::MavConnection<M> + Sync + Send>>
96    where
97        M: Message,
98    {
99        match self {
100            Self::Tcp(connectable) => connectable.connect::<M>(),
101            Self::Udp(connectable) => connectable.connect::<M>(),
102            Self::Serial(connectable) => connectable.connect::<M>(),
103            Self::File(connectable) => connectable.connect::<M>(),
104        }
105    }
106}