pamoja_mavlink/drivers/
tcp.rs

1//! A TCP [`ByteLink`], the transport ArduPilot SITL exposes by default (port 5760).
2//!
3//! ArduPilot's SITL listens for a ground station on TCP, and MAVLink then rides the stream as a
4//! continuous byte flow the [`Parser`](crate::Parser) frames. [`TcpLink`] connects to such a
5//! server; [`from_stream`](TcpLink::from_stream) wraps an already-accepted stream, so a server
6//! side can use the same driver.
7
8use std::io;
9
10use tokio::io::{AsyncReadExt, AsyncWriteExt};
11use tokio::net::{TcpStream, ToSocketAddrs};
12
13use super::link_fault;
14use crate::error::Result;
15use crate::link::ByteLink;
16
17/// A TCP stream presented as a [`ByteLink`].
18pub struct TcpLink {
19    stream: TcpStream,
20}
21
22impl TcpLink {
23    /// Connects to a MAVLink TCP server, such as ArduPilot SITL on `127.0.0.1:5760`.
24    ///
25    /// Nagle's algorithm is disabled so a frame is sent without waiting to coalesce, which keeps
26    /// control latency low.
27    ///
28    /// # Arguments
29    ///
30    /// * `addr` - the server address to connect to.
31    ///
32    /// # Returns
33    ///
34    /// The connected link.
35    ///
36    /// # Errors
37    ///
38    /// Returns an [`io::Error`] if the connection cannot be made.
39    pub async fn connect(addr: impl ToSocketAddrs) -> io::Result<Self> {
40        let stream = TcpStream::connect(addr).await?;
41        let _ = stream.set_nodelay(true);
42        Ok(TcpLink { stream })
43    }
44
45    /// Wraps an already-connected stream, for the accepting side of a connection.
46    ///
47    /// # Arguments
48    ///
49    /// * `stream` - the connected TCP stream.
50    ///
51    /// # Returns
52    ///
53    /// The link over the stream.
54    pub fn from_stream(stream: TcpStream) -> Self {
55        let _ = stream.set_nodelay(true);
56        TcpLink { stream }
57    }
58}
59
60impl ByteLink for TcpLink {
61    async fn read(&mut self, buf: &mut [u8]) -> Result<usize> {
62        self.stream.read(buf).await.map_err(link_fault)
63    }
64
65    async fn write_all(&mut self, data: &[u8]) -> Result<()> {
66        self.stream.write_all(data).await.map_err(link_fault)
67    }
68}
69
70#[cfg(test)]
71mod tests {
72    use super::*;
73    use crate::dialect::{CommandAck, CommandLong, Message};
74    use crate::link::Connection;
75    use tokio::net::TcpListener;
76
77    #[tokio::test]
78    async fn a_command_round_trips_over_a_real_tcp_connection() {
79        // A listener stands in for a MAVLink TCP server (as ArduPilot SITL is); a command and
80        // its acknowledgement cross a real localhost TCP connection through the driver.
81        let listener = TcpListener::bind("127.0.0.1:0").await.unwrap();
82        let addr = listener.local_addr().unwrap();
83
84        let server = tokio::spawn(async move {
85            let (stream, _) = listener.accept().await.unwrap();
86            let mut vehicle = Connection::new(TcpLink::from_stream(stream), 1, 1);
87            let frame = vehicle.recv().await.unwrap();
88            let command = CommandLong::decode(frame.payload()).unwrap();
89            let ack = CommandAck {
90                command: command.command,
91                result: 0,
92                progress: 0,
93                result_param2: 0,
94                target_system: frame.system_id(),
95                target_component: frame.component_id(),
96            };
97            vehicle.send(&ack).await.unwrap();
98        });
99
100        let mut gcs = Connection::new(TcpLink::connect(addr).await.unwrap(), 255, 190);
101        let arm = CommandLong {
102            param1: 1.0,
103            param2: 0.0,
104            param3: 0.0,
105            param4: 0.0,
106            param5: 0.0,
107            param6: 0.0,
108            param7: 0.0,
109            command: 400,
110            target_system: 1,
111            target_component: 1,
112            confirmation: 0,
113        };
114        gcs.send(&arm).await.unwrap();
115        let frame = gcs.recv().await.unwrap();
116        assert_eq!(frame.message_id(), CommandAck::ID);
117        server.await.unwrap();
118    }
119}
pamoja_mavlink/drivers/tcp.rs

pamoja_mavlink/drivers/
tcp.rs
