pamoja_mavlink/drivers/
udp.rs

1//! A UDP [`ByteLink`], the transport PX4 SITL uses and a common ground-station link.
2//!
3//! MAVLink over UDP is connectionless: a ground station binds a port and learns the vehicle's
4//! address from the first datagram it receives, then replies to it. [`UdpLink`] supports that
5//! bind-and-learn pattern with [`bind`](UdpLink::bind), and the send-first pattern (talking to a
6//! vehicle at a known address, as with PX4's offboard port) with [`connect`](UdpLink::connect).
7//! Each MAVLink frame is written as one datagram, which stays well under the MTU.
8
9use std::io;
10use std::net::SocketAddr;
11
12use tokio::net::{ToSocketAddrs, UdpSocket};
13
14use super::link_fault;
15use crate::error::{MavlinkError, Result};
16use crate::link::ByteLink;
17
18/// A UDP socket presented as a [`ByteLink`].
19///
20/// The peer is either learned from the first datagram received (in [`bind`](UdpLink::bind)
21/// mode) or set up front (in [`connect`](UdpLink::connect) mode), and every received datagram
22/// refreshes it, so a vehicle that moves ports is followed.
23pub struct UdpLink {
24    socket: UdpSocket,
25    peer: Option<SocketAddr>,
26}
27
28impl UdpLink {
29    /// Binds a local address and learns the peer from the first datagram received.
30    ///
31    /// Read before writing in this mode: a write before any datagram has arrived has no peer to
32    /// send to. This suits a vehicle configured to send its telemetry to this port.
33    ///
34    /// # Arguments
35    ///
36    /// * `local` - the local address to bind, such as `"0.0.0.0:14550"`.
37    ///
38    /// # Returns
39    ///
40    /// The bound link, with no peer yet.
41    ///
42    /// # Errors
43    ///
44    /// Returns an [`io::Error`] if the address cannot be bound.
45    pub async fn bind(local: impl ToSocketAddrs) -> io::Result<Self> {
46        let socket = UdpSocket::bind(local).await?;
47        Ok(UdpLink { socket, peer: None })
48    }
49
50    /// Binds a local address and sets a fixed peer to send to.
51    ///
52    /// This suits talking to a vehicle at a known address, such as PX4's offboard UDP port.
53    ///
54    /// # Arguments
55    ///
56    /// * `local` - the local address to bind, such as `"0.0.0.0:0"`.
57    /// * `remote` - the vehicle's address to send to.
58    ///
59    /// # Returns
60    ///
61    /// The link, ready to send.
62    ///
63    /// # Errors
64    ///
65    /// Returns an [`io::Error`] if the address cannot be bound.
66    pub async fn connect(local: impl ToSocketAddrs, remote: SocketAddr) -> io::Result<Self> {
67        let socket = UdpSocket::bind(local).await?;
68        Ok(UdpLink {
69            socket,
70            peer: Some(remote),
71        })
72    }
73
74    /// Returns the local address the socket is bound to.
75    ///
76    /// # Returns
77    ///
78    /// The bound local address.
79    ///
80    /// # Errors
81    ///
82    /// Returns an [`io::Error`] if the address cannot be read.
83    pub fn local_addr(&self) -> io::Result<SocketAddr> {
84        self.socket.local_addr()
85    }
86
87    /// Returns the peer address, once one is known.
88    ///
89    /// # Returns
90    ///
91    /// The peer address, or [`None`] before any datagram has been received in bind mode.
92    pub fn peer(&self) -> Option<SocketAddr> {
93        self.peer
94    }
95}
96
97impl ByteLink for UdpLink {
98    async fn read(&mut self, buf: &mut [u8]) -> Result<usize> {
99        let (n, from) = self.socket.recv_from(buf).await.map_err(link_fault)?;
100        self.peer = Some(from);
101        Ok(n)
102    }
103
104    async fn write_all(&mut self, data: &[u8]) -> Result<()> {
105        let peer = self.peer.ok_or(MavlinkError::Closed)?;
106        let sent = self.socket.send_to(data, peer).await.map_err(link_fault)?;
107        if sent != data.len() {
108            return Err(MavlinkError::Closed);
109        }
110        Ok(())
111    }
112}
113
114#[cfg(test)]
115mod tests {
116    use super::*;
117    use crate::dialect::{Heartbeat, Message};
118    use crate::link::Connection;
119
120    #[tokio::test]
121    async fn a_frame_crosses_a_real_udp_socket_pair() {
122        // A vehicle end binds and a ground-station end sends to it; the frame crosses a real
123        // localhost UDP socket, exercising the driver rather than an in-memory pipe.
124        let vehicle = UdpLink::bind("127.0.0.1:0").await.unwrap();
125        let vehicle_addr = vehicle.local_addr().unwrap();
126        let gcs = UdpLink::connect("127.0.0.1:0", vehicle_addr).await.unwrap();
127
128        let mut vehicle = Connection::new(vehicle, 1, 1);
129        let mut gcs = Connection::new(gcs, 255, 190);
130
131        let heartbeat = Heartbeat {
132            custom_mode: 0,
133            type_: 2,
134            autopilot: 3,
135            base_mode: 0,
136            system_status: 4,
137            mavlink_version: 3,
138        };
139        gcs.send(&heartbeat).await.unwrap();
140        let frame = vehicle.recv().await.unwrap();
141        assert_eq!(frame.message_id(), Heartbeat::ID);
142
143        // The vehicle now knows the ground station's address and can reply.
144        vehicle.send(&heartbeat).await.unwrap();
145        let reply = gcs.recv().await.unwrap();
146        assert_eq!(reply.message_id(), Heartbeat::ID);
147    }
148}
pamoja_mavlink/drivers/udp.rs

pamoja_mavlink/drivers/
udp.rs
