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    /// Sets the MAVLink version to use for receiving (when `allow_recv_any_version()` is `false`) and sending messages.
31    fn set_protocol_version(&mut self, version: MavlinkVersion);
32    /// Gets the currently used MAVLink version
33    fn protocol_version(&self) -> MavlinkVersion;
34
35    /// Set wether MAVLink messages of either version may be received.
36    ///
37    /// If set to false only messages of the version configured with `set_protocol_version()` are received.
38    fn set_allow_recv_any_version(&mut self, allow: bool);
39    /// Wether messages of any MAVLink version may be received
40    fn allow_recv_any_version(&self) -> bool;
41
42    /// Write whole frame
43    fn send_frame(&self, frame: &MavFrame<M>) -> Result<usize, crate::error::MessageWriteError> {
44        self.send(&frame.header, &frame.msg)
45    }
46
47    /// Read whole frame
48    fn recv_frame(&self) -> Result<MavFrame<M>, crate::error::MessageReadError> {
49        let (header, msg) = self.recv()?;
50        let protocol_version = self.protocol_version();
51        Ok(MavFrame {
52            header,
53            msg,
54            protocol_version,
55        })
56    }
57
58    /// Send a message with default header
59    fn send_default(&self, data: &M) -> Result<usize, crate::error::MessageWriteError> {
60        let header = MavHeader::default();
61        self.send(&header, data)
62    }
63
64    /// Setup secret key used for message signing, or disable message signing
65    #[cfg(feature = "signing")]
66    fn setup_signing(&mut self, signing_data: Option<SigningConfig>);
67}
68
69/// Connect to a MAVLink node by address string.
70///
71/// The address must be in one of the following formats:
72///
73///  * `tcpin:<addr>:<port>` to create a TCP server, listening an incoming connection
74///  * `tcpout:<addr>:<port>` to create a TCP client
75///  * `udpin:<addr>:<port>` to create a UDP server, listening for incoming packets
76///  * `udpout:<addr>:<port>` to create a UDP client
77///  * `udpbcast:<addr>:<port>` to create a UDP broadcast
78///  * `serial:<port>:<baudrate>` to create a serial connection
79///  * `file:<path>` to extract file data, writing to such a connection does nothing
80///
81/// The type of the connection is determined at runtime based on the address type, so the
82/// connection is returned as a trait object.
83pub fn connect<M: Message + Sync + Send>(
84    address: &str,
85) -> io::Result<Box<dyn MavConnection<M> + Sync + Send>> {
86    ConnectionAddress::parse_address(address)?.connect::<M>()
87}
88
89/// Returns the socket address for the given address.
90#[cfg(any(feature = "tcp", feature = "udp"))]
91pub(crate) fn get_socket_addr<T: std::net::ToSocketAddrs>(
92    address: &T,
93) -> Result<std::net::SocketAddr, io::Error> {
94    address.to_socket_addrs()?.next().ok_or(io::Error::new(
95        io::ErrorKind::Other,
96        "Host address lookup failed",
97    ))
98}
99
100/// A MAVLink connection address that can be connected to, establishing a [`MavConnection`]
101pub trait Connectable: Display {
102    /// Attempt to establish a blocking MAVLink connection
103    fn connect<M: Message>(&self) -> io::Result<Box<dyn MavConnection<M> + Sync + Send>>;
104}
105
106impl Connectable for ConnectionAddress {
107    fn connect<M>(&self) -> std::io::Result<Box<dyn crate::MavConnection<M> + Sync + Send>>
108    where
109        M: Message,
110    {
111        match self {
112            #[cfg(feature = "tcp")]
113            Self::Tcp(connectable) => connectable.connect::<M>(),
114            #[cfg(feature = "udp")]
115            Self::Udp(connectable) => connectable.connect::<M>(),
116            #[cfg(feature = "direct-serial")]
117            Self::Serial(connectable) => connectable.connect::<M>(),
118            Self::File(connectable) => connectable.connect::<M>(),
119        }
120    }
121}