mavlink_core/
connectable.rs

1use core::fmt::Display;
2use std::io;
3use std::path::PathBuf;
4
5#[cfg(feature = "direct-serial")]
6use crate::connection::direct_serial::config::SerialConfig;
7use crate::connection::file::config::FileConfig;
8#[cfg(feature = "tcp")]
9use crate::connection::tcp::config::{TcpConfig, TcpMode};
10#[cfg(feature = "udp")]
11use crate::connection::udp::config::{UdpConfig, UdpMode};
12
13/// A parsed MAVLink connection address
14pub enum ConnectionAddress {
15    /// TCP client or server address
16    #[cfg(feature = "tcp")]
17    Tcp(TcpConfig),
18    /// UDP client, server or broadcast address
19    #[cfg(feature = "udp")]
20    Udp(UdpConfig),
21    /// Serial port address
22    #[cfg(feature = "direct-serial")]
23    Serial(SerialConfig),
24    /// File input address
25    File(FileConfig),
26}
27
28#[cfg(feature = "tcp")]
29impl From<TcpConfig> for ConnectionAddress {
30    fn from(value: TcpConfig) -> Self {
31        Self::Tcp(value)
32    }
33}
34
35#[cfg(feature = "udp")]
36impl From<UdpConfig> for ConnectionAddress {
37    fn from(value: UdpConfig) -> Self {
38        Self::Udp(value)
39    }
40}
41
42#[cfg(feature = "direct-serial")]
43impl From<SerialConfig> for ConnectionAddress {
44    fn from(value: SerialConfig) -> Self {
45        Self::Serial(value)
46    }
47}
48
49impl From<FileConfig> for ConnectionAddress {
50    fn from(value: FileConfig) -> Self {
51        Self::File(value)
52    }
53}
54
55impl Display for ConnectionAddress {
56    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
57        match self {
58            #[cfg(feature = "tcp")]
59            Self::Tcp(connectable) => write!(f, "{connectable}"),
60            #[cfg(feature = "udp")]
61            Self::Udp(connectable) => write!(f, "{connectable}"),
62            #[cfg(feature = "direct-serial")]
63            Self::Serial(connectable) => write!(f, "{connectable}"),
64            Self::File(connectable) => write!(f, "{connectable}"),
65        }
66    }
67}
68
69impl ConnectionAddress {
70    /// Parse a MAVLink address string.
71    ///
72    ///  The address must be in one of the following formats:
73    ///
74    ///  * `tcpin:<addr>:<port>` to create a TCP server, listening for an incoming connection
75    ///  * `tcpout:<addr>:<port>` to create a TCP client
76    ///  * `udpin:<addr>:<port>` to create a UDP server, listening for incoming packets
77    ///  * `udpout:<addr>:<port>` to create a UDP client
78    ///  * `udpbcast:<addr>:<port>` to create a UDP broadcast
79    ///  * `serial:<port>:<baudrate>` to create a serial connection
80    ///  * `file:<path>` to extract file data, writing to such a connection does nothing
81    pub fn parse_address(address: &str) -> Result<Self, io::Error> {
82        let (protocol, address) = address.split_once(':').ok_or(io::Error::new(
83            io::ErrorKind::AddrNotAvailable,
84            "Protocol unsupported",
85        ))?;
86        let conn = match protocol {
87            #[cfg(feature = "direct-serial")]
88            "serial" => {
89                let (port_name, baud) = address.split_once(':').ok_or(io::Error::new(
90                    io::ErrorKind::AddrNotAvailable,
91                    "Incomplete port settings",
92                ))?;
93                Self::Serial(SerialConfig::new(
94                    port_name.to_string(),
95                    baud.parse().map_err(|_| {
96                        io::Error::new(io::ErrorKind::AddrNotAvailable, "Invalid baud rate")
97                    })?,
98                ))
99            }
100            #[cfg(feature = "tcp")]
101            "tcpin" | "tcpout" => {
102                let mode = if protocol == "tcpout" {
103                    TcpMode::TcpOut
104                } else {
105                    TcpMode::TcpIn
106                };
107                Self::Tcp(TcpConfig::new(address.to_string(), mode))
108            }
109            #[cfg(feature = "udp")]
110            "udpin" | "udpout" | "udpcast" => Self::Udp(UdpConfig::new(
111                address.to_string(),
112                match protocol {
113                    "udpin" => UdpMode::Udpin,
114                    "udpout" => UdpMode::Udpout,
115                    "udpcast" => UdpMode::Udpcast,
116                    _ => unreachable!(),
117                },
118            )),
119            "file" => Self::File(FileConfig::new(PathBuf::from(address))),
120            _ => {
121                return Err(io::Error::new(
122                    io::ErrorKind::AddrNotAvailable,
123                    "Protocol unsupported",
124                ))
125            }
126        };
127        Ok(conn)
128    }
129}