mavspec_rust_spec/
payload.rs

1//! # MAVLink payload
2
3#[cfg(feature = "alloc")]
4extern crate alloc;
5
6use core::cmp::min;
7
8#[cfg(feature = "std")]
9use std::fmt::{Debug, Formatter};
10
11use crate::consts::PAYLOAD_MAX_SIZE;
12use crate::error::SpecError;
13use crate::types::{MavLinkVersion, MessageId};
14
15#[cfg(feature = "alloc")]
16type PayloadContainer = alloc::vec::Vec<u8>;
17#[cfg(not(feature = "alloc"))]
18use no_alloc_payload_container::PayloadContainer;
19
20/// MAVlink message payload.
21///
22/// Encapsulates MAVLink payload.
23/// In `no_std` non-allocating targets it uses fixed-sized
24/// arrays of bytes. Otherwise, payload is stored on heap as a dynamically sized sequence.
25#[derive(Clone)]
26#[cfg_attr(feature = "specta", derive(specta::Type))]
27#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
28#[cfg_attr(not(feature = "std"), derive(Debug))]
29pub struct Payload {
30    /// MAVLink message ID.
31    id: MessageId,
32    /// Message payload as a sequence of bytes.
33    payload: PayloadContainer,
34    /// Payload length.
35    length: usize,
36    /// MAVLink protocol version.
37    version: MavLinkVersion,
38}
39
40#[allow(clippy::derivable_impls)]
41impl Default for Payload {
42    /// Creates [`Payload`] populated with default values.
43    fn default() -> Self {
44        Self {
45            id: MessageId::default(),
46            payload: Payload::container_default(),
47            version: MavLinkVersion::default(),
48            length: PAYLOAD_MAX_SIZE,
49        }
50    }
51}
52
53#[cfg(feature = "std")]
54impl Debug for Payload {
55    /// Formats [`Payload`] with `payload` truncated up to `max_size`.
56    ///
57    /// This is important for `no_std` implementations where `payload` has fixed size of
58    /// [`PAYLOAD_MAX_SIZE`] bytes.
59    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
60        let payload = match self.version {
61            MavLinkVersion::V1 => &self.payload[0..min(self.payload.len(), self.length)],
62            MavLinkVersion::V2 => {
63                &self.payload[0..min(self.payload.len(), Self::truncated_length(&self.payload))]
64            }
65        };
66
67        f.debug_struct("payload")
68            .field("id", &self.id)
69            .field("payload", &payload)
70            .field("length", &self.length)
71            .field("version", &self.version)
72            .finish()
73    }
74}
75
76impl Payload {
77    /// Default constructor.
78    ///
79    /// Upon creation, the length of the provided payload will define
80    /// [`Payload::length`] the maximum length of the
81    /// [`Payload::bytes`].
82    ///
83    /// If `payload` is longer, than [`PAYLOAD_MAX_SIZE`], all trailing elements will be ignored.
84    ///
85    /// For MavLink v2 the trailing zeros will be truncated according to
86    /// [specification](https://mavlink.io/en/guide/serialization.html#payload_truncation).
87    pub fn new(id: MessageId, payload: &[u8], version: MavLinkVersion) -> Self {
88        let max_size = min(PAYLOAD_MAX_SIZE, payload.len());
89
90        // Define length based on MAVLink protocol version since `MAVLink 2` requires payload truncation.
91        let length = match version {
92            MavLinkVersion::V1 => max_size,
93            MavLinkVersion::V2 => Self::truncated_length(&payload[0..max_size]),
94        };
95
96        let payload = Self::container_from_slice(payload, length);
97
98        Self {
99            id,
100            payload,
101            length,
102            version,
103        }
104    }
105
106    /// MAVLink message ID.
107    pub fn id(&self) -> MessageId {
108        self.id
109    }
110
111    /// Message payload as bytes.
112    ///
113    /// For `MAVLink 2` zero trailing bytes will be truncated.
114    /// See [MAVLink 2 payload truncation](https://mavlink.io/en/guide/serialization.html#payload_truncation).
115    pub fn bytes(&self) -> &[u8] {
116        &self.payload[0..self.length]
117    }
118
119    /// Message payload as mutable byte slice.
120    ///
121    /// For `MAVLink 2` zero trailing bytes will be truncated.
122    /// See [MAVLink 2 payload truncation](https://mavlink.io/en/guide/serialization.html#payload_truncation).
123    pub fn bytes_mut(&mut self) -> &mut [u8] {
124        #[cfg(feature = "alloc")]
125        let slice = &mut self.payload.as_mut_slice()[0..self.length];
126        #[cfg(not(feature = "alloc"))]
127        let slice = &mut self.payload.content.as_mut_slice()[0..self.length];
128        slice
129    }
130
131    /// MAVLink protocol version.
132    ///
133    /// See [`MavLinkVersion`].
134    pub fn version(&self) -> MavLinkVersion {
135        self.version
136    }
137
138    /// Payload size in bytes.
139    ///
140    /// Note that for `MAVLink 2` payloads trailing zero bytes are truncated.  
141    ///
142    /// See [`Payload::bytes`].
143    pub fn length(&self) -> u8 {
144        self.length as u8
145    }
146
147    /// Upgrade payload to `MAVLink 2` protocol version in-place.
148    ///
149    /// The reverse procedure is not possible since `MAVLink 2` payload may contain extra fields
150    /// and its trailing zero bytes are truncated.
151    ///
152    /// To replace an existing payload by value, use [`Payload::upgraded`].
153    pub fn upgrade(&mut self) {
154        self.version = MavLinkVersion::V2;
155        self.length = Self::truncated_length(self.bytes());
156    }
157
158    /// Upgrade protocol version to `MAVLink 2` replacing payload by value.
159    ///
160    /// The reverse procedure is not possible since `MAVLink 2` payload may contain extra fields
161    /// and its trailing zero bytes are truncated.
162    ///
163    /// To upgrade payload in-place, use [`Payload::upgrade`].
164    pub fn upgraded(self) -> Self {
165        Self::new(self.id, self.bytes(), MavLinkVersion::V2)
166    }
167
168    fn truncated_length(slice: &[u8]) -> usize {
169        let n: usize = slice.len();
170        // Assume that all elements are zeros
171        let mut num_non_zero = 0usize;
172        // Seek from the end to start
173        for i in 1..=n {
174            // Stop when non-zero element is found
175            if slice[n - i] != 0u8 {
176                num_non_zero = n - i + 1;
177                break;
178            }
179        }
180
181        core::cmp::max(num_non_zero, 1)
182    }
183
184    fn container_from_slice(value: &[u8], max_size: usize) -> PayloadContainer {
185        // Truncate value up to maximum possible length
186        let value: &[u8] = if value.len() > max_size {
187            &value[0..max_size]
188        } else {
189            value
190        };
191
192        #[cfg(not(feature = "alloc"))]
193        let payload: PayloadContainer = PayloadContainer {
194            content: {
195                let mut no_std_payload = [0u8; PAYLOAD_MAX_SIZE];
196                let max_len = if PAYLOAD_MAX_SIZE < value.len() {
197                    PAYLOAD_MAX_SIZE
198                } else {
199                    value.len()
200                };
201                no_std_payload[0..max_len].copy_from_slice(&value[0..max_len]);
202                no_std_payload
203            },
204        };
205        #[cfg(feature = "alloc")]
206        let payload: PayloadContainer = if value.len() < max_size {
207            let mut payload = alloc::vec![0u8; max_size];
208            payload[0..value.len()].copy_from_slice(value);
209            payload
210        } else {
211            PayloadContainer::from(value)
212        };
213
214        payload
215    }
216
217    fn container_default() -> PayloadContainer {
218        PayloadContainer::default()
219    }
220}
221
222/// MAVLink message encoder.
223///
224/// Decodes MAVLink message into [`Payload`].
225pub trait IntoPayload {
226    /// Encodes message into MAVLink payload.
227    ///
228    /// # Errors
229    ///
230    /// * Returns [`SpecError::UnsupportedMavLinkVersion`] if specified
231    /// MAVLink `version` is not supported.
232    fn encode(&self, version: MavLinkVersion) -> Result<Payload, SpecError>;
233}
234
235#[cfg(not(feature = "alloc"))]
236mod no_alloc_payload_container {
237    use crate::payload::PAYLOAD_MAX_SIZE;
238
239    #[derive(Clone, Debug)]
240    #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
241    pub struct PayloadContainer {
242        #[cfg_attr(feature = "serde", serde(with = "serde_arrays"))]
243        pub(super) content: [u8; PAYLOAD_MAX_SIZE],
244    }
245
246    impl Default for PayloadContainer {
247        fn default() -> Self {
248            Self {
249                content: [0u8; PAYLOAD_MAX_SIZE],
250            }
251        }
252    }
253
254    impl<Idx> core::ops::Index<Idx> for PayloadContainer
255    where
256        Idx: core::slice::SliceIndex<[u8]>,
257    {
258        type Output = Idx::Output;
259
260        fn index(&self, index: Idx) -> &Self::Output {
261            &self.content[index]
262        }
263    }
264}
265
266#[cfg(test)]
267mod tests {
268    use super::*;
269
270    #[test]
271    fn new_payload() {
272        // Small initial payload
273        let payload = Payload::new(0, &[1, 2, 3, 4, 5, 6u8], MavLinkVersion::V1);
274        assert_eq!(payload.length(), 6);
275        assert_eq!(payload.bytes().len(), 6);
276        assert_eq!(payload.bytes(), &[1, 2, 3, 4, 5, 6u8]);
277
278        // Payload with trailing zeros V1
279        let payload = Payload::new(0, &[1, 2, 3, 4, 0, 0u8], MavLinkVersion::V1);
280        assert_eq!(payload.length(), 6);
281        assert_eq!(payload.bytes().len(), 6);
282
283        // Payload with trailing zeros V2
284        let payload = Payload::new(0, &[1, 2, 3, 4, 0, 0u8], MavLinkVersion::V2);
285        assert_eq!(payload.length(), 4);
286        assert_eq!(payload.bytes().len(), 4);
287
288        // Payload that contains only zeros V2
289        let payload = Payload::new(0, &[0, 0, 0, 0, 0, 0u8], MavLinkVersion::V2);
290        assert_eq!(payload.length(), 1);
291        assert_eq!(payload.bytes().len(), 1);
292
293        // Large initial payload
294        let payload = Payload::new(0, &[1u8; PAYLOAD_MAX_SIZE * 2], MavLinkVersion::V1);
295        assert_eq!(payload.length() as usize, PAYLOAD_MAX_SIZE);
296        assert_eq!(payload.bytes().len(), PAYLOAD_MAX_SIZE);
297    }
298
299    #[test]
300    fn truncated_length() {
301        assert_eq!(Payload::truncated_length(&[1, 2, 3, 4, 5, 6u8]), 6);
302        assert_eq!(Payload::truncated_length(&[1, 2, 3, 4, 0, 0u8]), 4);
303        assert_eq!(Payload::truncated_length(&[0, 0, 0, 0, 0, 0u8]), 1);
304    }
305}
mavspec_rust_spec/payload.rs

mavspec_rust_spec/
payload.rs
