crazyflie_link/
packet.rs

1use std::fmt;
2
3/// CRTP data packet
4#[derive(Debug, Clone)]
5pub struct Packet {
6    header: u8,
7    port: u8,
8    channel: u8,
9    data: Vec<u8>,
10}
11
12impl fmt::Display for Packet {
13    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
14        write!(
15            f,
16            "(ch: {}, port: {}, data: {})",
17            self.channel, self.port, self.data[0]
18        )
19    }
20}
21
22impl From<Vec<u8>> for Packet {
23    fn from(v: Vec<u8>) -> Self {
24        let header = v[0];
25        //
26        // A packet on the wire starts with a header byte, formatted as:
27        //
28        //   pppp00cc
29        //
30        // Where ...
31        //  ... bit 1 and 2 is the channel (c) ...
32        //  ... bit 3 and 4 are reserved for flow-control for the radio link ...
33        //  ... bit 5, 6, 7 and 8 is the port (p).
34        //
35        let channel = header & 0x03; // mask out the channel
36        let port = (header & 0xF0) >> 4; // twiddle out the port
37        let data = v[1..].to_vec(); // the rest is data!
38
39        Packet {
40            header,
41            port,
42            channel,
43            data,
44        }
45    }
46}
47
48impl From<Packet> for Vec<u8> {
49    fn from(packet: Packet) -> Self {
50        let mut vec = vec![packet.get_header()];
51        vec.append(&mut packet.get_data().to_vec());
52        vec
53    }
54}
55
56impl Packet {
57    pub fn new(port: u8, channel: u8, data: Vec<u8>) -> Self {
58        let mut header = 0;
59
60        // channel is at bit 1 to 2
61        header |= channel & 0x03; // header => 000000cc
62
63        // port is at bit 5 to 8
64        header |= (port << 4) & 0xF0; // header => pppp00cc
65
66        Packet {
67            header,
68            port,
69            channel,
70            data,
71        }
72    }
73
74    pub fn get_channel(&self) -> u8 {
75        self.channel
76    }
77
78    pub fn get_port(&self) -> u8 {
79        self.port
80    }
81
82    pub fn get_data(&self) -> &Vec<u8> {
83        &self.data
84    }
85
86    pub fn append_data(&mut self, v: &mut Vec<u8>) {
87        self.data.append(v);
88    }
89
90    pub fn get_header(&self) -> u8 {
91        //
92        // See the From trait implementation above for more details of the
93        // vector format.
94        //
95        self.header
96    }
97}
crazyflie_link/packet.rs

crazyflie_link/
packet.rs
