uf_crsf/packets/
logging.rs

1use crate::packets::{CrsfPacket, PacketType};
2use crate::CrsfParsingError;
3use core::mem::size_of;
4use heapless::Vec;
5
6/// Represents a Logging packet (frame type 0x34).
7#[derive(Clone, Debug, PartialEq)]
8pub struct Logging {
9    pub dst_addr: u8,
10    pub src_addr: u8,
11    pub logtype: u16,
12    pub timestamp: u32,
13    params: Vec<u32, 13>,
14}
15
16impl Logging {
17    /// Creates a new Logging packet.
18    pub fn new(
19        dst_addr: u8,
20        src_addr: u8,
21        logtype: u16,
22        timestamp: u32,
23        params: &[u32],
24    ) -> Result<Self, CrsfParsingError> {
25        if params.len() > 13 {
26            return Err(CrsfParsingError::InvalidPayloadLength);
27        }
28        let mut p = Vec::new();
29        p.extend_from_slice(params)
30            .map_err(|_| CrsfParsingError::InvalidPayloadLength)?;
31        Ok(Self {
32            dst_addr,
33            src_addr,
34            logtype,
35            timestamp,
36            params: p,
37        })
38    }
39
40    /// Returns the logging parameters as a slice.
41    pub fn params(&self) -> &[u32] {
42        &self.params
43    }
44}
45
46#[cfg(feature = "defmt")]
47impl defmt::Format for Logging {
48    fn format(&self, fmt: defmt::Formatter) {
49        defmt::write!(
50            fmt,
51            "Logging {{ dst_addr: {}, src_addr: {}, logtype: {}, timestamp: {}, params: {} }}",
52            self.dst_addr,
53            self.src_addr,
54            self.logtype,
55            self.timestamp,
56            self.params(),
57        )
58    }
59}
60
61impl CrsfPacket for Logging {
62    const PACKET_TYPE: PacketType = PacketType::Logging;
63    const MIN_PAYLOAD_SIZE: usize = size_of::<u8>() * 2 + size_of::<u16>() + size_of::<u32>();
64
65    fn from_bytes(data: &[u8]) -> Result<Self, CrsfParsingError> {
66        if data.len() < Self::MIN_PAYLOAD_SIZE {
67            return Err(CrsfParsingError::InvalidPayloadLength);
68        }
69
70        if !(data.len() - Self::MIN_PAYLOAD_SIZE).is_multiple_of(size_of::<u32>()) {
71            return Err(CrsfParsingError::InvalidPayloadLength);
72        }
73
74        let dst_addr = data[0];
75        let src_addr = data[1];
76        let logtype = u16::from_be_bytes(data[2..4].try_into().expect("infallible"));
77        let timestamp = u32::from_be_bytes(data[4..8].try_into().expect("infallible"));
78
79        let mut params = Vec::new();
80        for chunk in data[8..].chunks_exact(4) {
81            let param = u32::from_be_bytes(chunk.try_into().expect("infallible"));
82            if params.push(param).is_err() {
83                // This would mean we have more params than our Vec can hold.
84                return Err(CrsfParsingError::InvalidPayloadLength);
85            }
86        }
87
88        Ok(Self {
89            dst_addr,
90            src_addr,
91            logtype,
92            timestamp,
93            params,
94        })
95    }
96
97    fn to_bytes(&self, buffer: &mut [u8]) -> Result<usize, CrsfParsingError> {
98        let params_len = core::mem::size_of_val(self.params());
99        let payload_len = Self::MIN_PAYLOAD_SIZE + params_len;
100        if buffer.len() < payload_len {
101            return Err(CrsfParsingError::BufferOverflow);
102        }
103
104        buffer[0] = self.dst_addr;
105        buffer[1] = self.src_addr;
106        buffer[2..4].copy_from_slice(&self.logtype.to_be_bytes());
107        buffer[4..8].copy_from_slice(&self.timestamp.to_be_bytes());
108
109        for (i, param) in self.params().iter().enumerate() {
110            let offset = Self::MIN_PAYLOAD_SIZE + i * size_of::<u32>();
111            buffer[offset..offset + size_of::<u32>()].copy_from_slice(&param.to_be_bytes());
112        }
113
114        Ok(payload_len)
115    }
116}
117
118#[cfg(test)]
119mod tests {
120    use super::*;
121
122    #[test]
123    fn test_logging_from_bytes_no_params() {
124        assert_eq!(Logging::MIN_PAYLOAD_SIZE, 8);
125        let data: [u8; 8] = [
126            0xEA, 0xEE, // dst_addr, src_addr
127            0x01, 0x02, // logtype
128            0x03, 0x04, 0x05, 0x06, // timestamp
129        ];
130        let packet = Logging::from_bytes(&data).unwrap();
131        assert_eq!(packet.dst_addr, 0xEA);
132        assert_eq!(packet.src_addr, 0xEE);
133        assert_eq!(packet.logtype, 0x0102);
134        assert_eq!(packet.timestamp, 0x03040506);
135        assert!(packet.params().is_empty());
136    }
137
138    #[test]
139    fn test_logging_from_bytes_with_params() {
140        let data: [u8; 16] = [
141            0xEA, 0xEE, // dst_addr, src_addr
142            0x01, 0x02, // logtype
143            0x03, 0x04, 0x05, 0x06, // timestamp
144            0x07, 0x08, 0x09, 0x0A, // param1
145            0x0B, 0x0C, 0x0D, 0x0E, // param2
146        ];
147        let packet = Logging::from_bytes(&data).unwrap();
148        assert_eq!(packet.dst_addr, 0xEA);
149        assert_eq!(packet.src_addr, 0xEE);
150        assert_eq!(packet.logtype, 0x0102);
151        assert_eq!(packet.timestamp, 0x03040506);
152        assert_eq!(packet.params(), &[0x0708090A, 0x0B0C0D0E]);
153    }
154
155    #[test]
156    fn test_logging_to_bytes_no_params() {
157        let packet = Logging::new(0xEA, 0xEE, 0x1234, 0x56789ABC, &[]).unwrap();
158        let mut buffer = [0u8; 8];
159        let len = packet.to_bytes(&mut buffer).unwrap();
160        assert_eq!(len, 8);
161        let expected: [u8; 8] = [0xEA, 0xEE, 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC];
162        assert_eq!(buffer, expected);
163    }
164
165    #[test]
166    fn test_logging_to_bytes_with_params() {
167        let params = [0x11223344, 0x55667788];
168        let packet = Logging::new(0xEA, 0xEE, 0xABCD, 0xDEADBEEF, &params).unwrap();
169        let mut buffer = [0u8; 16];
170        let len = packet.to_bytes(&mut buffer).unwrap();
171        assert_eq!(len, 16);
172        let expected: [u8; 16] = [
173            0xEA, 0xEE, // dst_addr, src_addr
174            0xAB, 0xCD, // logtype
175            0xDE, 0xAD, 0xBE, 0xEF, // timestamp
176            0x11, 0x22, 0x33, 0x44, // param1
177            0x55, 0x66, 0x77, 0x88, // param2
178        ];
179        assert_eq!(buffer, expected);
180    }
181
182    #[test]
183    fn test_logging_round_trip() {
184        let params = [1, 2, 3];
185        let packet = Logging::new(0xEA, 0xEE, 123, 456, &params).unwrap();
186        let mut buffer = [0u8; 20];
187        let len = packet.to_bytes(&mut buffer).unwrap();
188        assert_eq!(len, 20);
189        let round_trip = Logging::from_bytes(&buffer[..len]).unwrap();
190        assert_eq!(packet, round_trip);
191    }
192
193    #[test]
194    fn test_invalid_payload_length_too_short() {
195        let data = [0u8; 7];
196        assert_eq!(
197            Logging::from_bytes(&data),
198            Err(CrsfParsingError::InvalidPayloadLength)
199        );
200    }
201
202    #[test]
203    fn test_invalid_payload_length_not_multiple_of_4() {
204        let data = [0u8; 9];
205        assert_eq!(
206            Logging::from_bytes(&data),
207            Err(CrsfParsingError::InvalidPayloadLength)
208        );
209    }
210}
uf_crsf/packets/logging.rs

uf_crsf/packets/
logging.rs
