ados_protocol/
mavlink_socket.rs

1//! The MAVLink broadcast socket wire (`/run/ados/mavlink.sock`, contract
2//! `mavlink.socket`).
3//!
4//! Every record in both directions is a 4-byte big-endian length followed by
5//! the body ([`crate::frame`]). Router → client bodies (version 2) start with
6//! an 8-byte little-endian `rx_mono_ns`, the host `CLOCK_MONOTONIC` time the
7//! router read that frame off the flight-controller transport (or received it
8//! from any other source), followed by one raw MAVLink frame. Client → router
9//! bodies are one raw MAVLink frame with no header.
10//!
11//! Ungated (no MAVLink dialect dependency), so a consumer that only moves raw
12//! frames reads the socket without building the codec.
13
14use std::io;
15
16use tokio::io::AsyncReadExt;
17
18use crate::frame::{encode_frame, FrameError, MAVLINK_MAX_FRAME};
19
20/// Wire version of the router → client record; mirrors the `mavlink.socket`
21/// row in `contracts.toml`.
22pub const MAVLINK_SOCKET_VERSION: u16 = 2;
23
24/// Length of the `rx_mono_ns` header that leads every router → client body.
25pub const MAVLINK_SOCKET_HEADER_LEN: usize = 8;
26
27/// Largest router → client body: the frame cap plus the header.
28pub const MAVLINK_SOCKET_MAX_BODY: usize = MAVLINK_MAX_FRAME + MAVLINK_SOCKET_HEADER_LEN;
29
30/// One frame as a client receives it.
31#[derive(Debug, Clone, PartialEq, Eq)]
32pub struct SocketFrame {
33    /// Host monotonic ns at which the router received the frame.
34    pub rx_mono_ns: u64,
35    /// The raw MAVLink frame, byte for byte.
36    pub frame: Vec<u8>,
37}
38
39/// The router → client body: header then frame (no length prefix).
40pub fn encode_body(rx_mono_ns: u64, frame: &[u8]) -> Vec<u8> {
41    let mut body = Vec::with_capacity(MAVLINK_SOCKET_HEADER_LEN + frame.len());
42    body.extend_from_slice(&rx_mono_ns.to_le_bytes());
43    body.extend_from_slice(frame);
44    body
45}
46
47/// The complete router → client record: length prefix, header, frame.
48pub fn encode_record(rx_mono_ns: u64, frame: &[u8]) -> Result<Vec<u8>, FrameError> {
49    encode_frame(&encode_body(rx_mono_ns, frame), MAVLINK_SOCKET_MAX_BODY)
50}
51
52/// Split a router → client body into `(rx_mono_ns, frame)`. `None` when the
53/// body is too short to carry the header.
54pub fn decode_body(body: &[u8]) -> Option<(u64, &[u8])> {
55    let (head, frame) = body.split_at_checked(MAVLINK_SOCKET_HEADER_LEN)?;
56    let ns = u64::from_le_bytes(head.try_into().ok()?);
57    Some((ns, frame))
58}
59
60/// Read one router → client record. `Ok(None)` on clean EOF; a body shorter
61/// than the header is `InvalidData`.
62pub async fn read_frame<R: AsyncReadExt + Unpin>(
63    reader: &mut R,
64) -> io::Result<Option<SocketFrame>> {
65    let Some(mut body) =
66        crate::ipc::read_length_prefixed(reader, MAVLINK_SOCKET_MAX_BODY, false).await?
67    else {
68        return Ok(None);
69    };
70    let Some((rx_mono_ns, _)) = decode_body(&body) else {
71        return Err(io::Error::new(
72            io::ErrorKind::InvalidData,
73            "mavlink socket record shorter than its header",
74        ));
75    };
76    body.drain(..MAVLINK_SOCKET_HEADER_LEN);
77    Ok(Some(SocketFrame {
78        rx_mono_ns,
79        frame: body,
80    }))
81}
82
83#[cfg(test)]
84mod tests {
85    use super::*;
86
87    #[test]
88    fn body_round_trips() {
89        let frame = [0xFD, 9, 0, 0, 5, 1, 1, 0, 0, 0];
90        let body = encode_body(0x0102_0304_0506_0708, &frame);
91        assert_eq!(&body[..8], &[8, 7, 6, 5, 4, 3, 2, 1]);
92        assert_eq!(
93            decode_body(&body),
94            Some((0x0102_0304_0506_0708, &frame[..]))
95        );
96        assert_eq!(decode_body(&body[..7]), None);
97        assert_eq!(
98            decode_body(&body[..8]),
99            Some((0x0102_0304_0506_0708, &[][..]))
100        );
101    }
102
103    #[tokio::test]
104    async fn record_reads_back_through_the_length_prefix() {
105        let mut wire = encode_record(42, b"\xfeframe-a").unwrap();
106        assert_eq!(&wire[..4], &(8u32 + 8).to_be_bytes());
107        wire.extend(encode_record(43, b"\xfdframe-bb").unwrap());
108        let mut r = &wire[..];
109        let a = read_frame(&mut r).await.unwrap().unwrap();
110        assert_eq!(a.rx_mono_ns, 42);
111        assert_eq!(a.frame, b"\xfeframe-a");
112        let b = read_frame(&mut r).await.unwrap().unwrap();
113        assert_eq!(
114            (b.rx_mono_ns, b.frame.as_slice()),
115            (43, &b"\xfdframe-bb"[..])
116        );
117        assert_eq!(read_frame(&mut r).await.unwrap(), None);
118    }
119
120    #[tokio::test]
121    async fn short_body_is_invalid_data() {
122        let wire = encode_frame(&[1, 2, 3], MAVLINK_SOCKET_MAX_BODY).unwrap();
123        let err = read_frame(&mut &wire[..]).await.unwrap_err();
124        assert_eq!(err.kind(), io::ErrorKind::InvalidData);
125    }
126}
ados_protocol/mavlink_socket.rs

ados_protocol/
mavlink_socket.rs
