bluerobotics_ping/
message.rs

1#[cfg(feature = "serde")]
2use serde::{Deserialize, Serialize};
3use std::io::Write;
4
5pub const HEADER: [u8; 2] = ['B' as u8, 'R' as u8];
6
7#[derive(Clone, Debug, Default, PartialEq)]
8#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
9pub struct ProtocolMessage {
10    pub payload_length: u16,
11    pub message_id: u16,
12    pub src_device_id: u8,
13    pub dst_device_id: u8,
14    #[serde(with = "serde_bytes")]
15    pub payload: Vec<u8>,
16    pub checksum: u16,
17}
18
19impl ProtocolMessage {
20    /**
21     * Message Format
22     *
23     * Each message consists of a header, optional payload, and checksum. The binary format is specified as follows:
24     *
25     * | Byte        | Type | Name           | Description                                                                                               |
26     * |-------------|------|----------------|-----------------------------------------------------------------------------------------------------------|
27     * | 0           | u8   | start1         | Start frame identifier, ASCII 'B'                                                                         |
28     * | 1           | u8   | start2         | Start frame identifier, ASCII 'R'                                                                         |
29     * | 2-3         | u16  | payload_length | Number of bytes in payload.                                                                               |
30     * | 4-5         | u16  | message_id     | The message id.                                                                                           |
31     * | 6           | u8   | src_device_id  | The device ID of the device sending the message.                                                          |
32     * | 7           | u8   | dst_device_id  | The device ID of the intended recipient of the message.                                                   |
33     * | 8-n         | u8[] | payload        | The message payload.                                                                                      |
34     * | (n+1)-(n+2) | u16  | checksum       | The message checksum. The checksum is calculated as the sum of all the non-checksum bytes in the message. |
35     */
36
37    pub fn new() -> Self {
38        Default::default()
39    }
40
41    // Assuming PingMessage is a trait that your code defines
42    pub fn set_message(&mut self, message: &impl PingMessage) {
43        self.message_id = message.message_id();
44        self.payload = message.serialize(); // Assuming serialize returns Vec<u8>
45        self.payload_length = self.payload.len() as u16;
46        self.update_checksum();
47    }
48
49    #[inline]
50    pub fn set_src_device_id(&mut self, src_device_id: u8) {
51        self.src_device_id = src_device_id;
52        self.update_checksum();
53    }
54
55    #[inline]
56    pub fn dst_device_id(&self) -> u8 {
57        self.dst_device_id
58    }
59
60    #[inline]
61    pub fn set_dst_device_id(&mut self, dst_device_id: u8) {
62        self.dst_device_id = dst_device_id;
63        self.update_checksum();
64    }
65
66    #[inline]
67    pub fn payload(&self) -> &[u8] {
68        &self.payload
69    }
70
71    #[inline]
72    pub fn checksum(&self) -> u16 {
73        self.checksum
74    }
75
76    #[inline]
77    pub fn update_checksum(&mut self) {
78        self.checksum = self.calculate_crc();
79    }
80
81    pub fn calculate_crc(&self) -> u16 {
82        let mut checksum: u16 = 0;
83        checksum = checksum.wrapping_add(HEADER[0] as u16);
84        checksum = checksum.wrapping_add(HEADER[1] as u16);
85        self.payload_length
86            .to_le_bytes()
87            .iter()
88            .for_each(|byte| checksum = checksum.wrapping_add(*byte as u16));
89        self.message_id
90            .to_le_bytes()
91            .iter()
92            .for_each(|byte| checksum = checksum.wrapping_add(*byte as u16));
93        checksum = checksum.wrapping_add(self.src_device_id as u16);
94        checksum = checksum.wrapping_add(self.dst_device_id as u16);
95        for &byte in &self.payload {
96            checksum = checksum.wrapping_add(byte as u16);
97        }
98        checksum
99    }
100
101    pub fn has_valid_crc(&self) -> bool {
102        self.checksum == self.calculate_crc()
103    }
104
105    pub fn length(&self) -> usize {
106        HEADER.len() + 2 + 2 + 1 + 1 + self.payload_length as usize + 2
107    }
108
109    pub fn write(&self, writer: &mut dyn Write) -> std::io::Result<usize> {
110        let data = self.serialized();
111        writer.write_all(&data)?;
112        Ok(data.len())
113    }
114
115    pub fn serialized(&self) -> Vec<u8> {
116        let mut serialized_data = Vec::with_capacity(self.length());
117        serialized_data.extend_from_slice(&HEADER);
118        serialized_data.extend_from_slice(&self.payload_length.to_le_bytes());
119        serialized_data.extend_from_slice(&self.message_id.to_le_bytes());
120        serialized_data.push(self.src_device_id);
121        serialized_data.push(self.dst_device_id);
122        serialized_data.extend_from_slice(&self.payload);
123        serialized_data.extend_from_slice(&self.checksum.to_le_bytes());
124        serialized_data
125    }
126}
127
128// This information is only related to the message itself,
129// not the entire package with header, dst, src and etc.
130pub trait PingMessage
131where
132    Self: Sized + SerializePayload + SerializePayload,
133{
134    fn message_id(&self) -> u16;
135    fn message_name(&self) -> &'static str;
136
137    fn message_id_from_name(name: &str) -> Result<u16, String>;
138}
139
140pub trait SerializePayload {
141    fn serialize(&self) -> Vec<u8>;
142}
143
144pub trait DeserializePayload {
145    fn deserialize(payload: &[u8]) -> Self;
146}
147
148pub trait MessageInfo {
149    fn id() -> u16;
150    fn name() -> &'static str;
151}
152
153pub trait DeserializeGenericMessage
154where
155    Self: Sized,
156{
157    fn deserialize(message_id: u16, payload: &[u8]) -> Result<Self, &'static str>;
158}
bluerobotics_ping/message.rs

bluerobotics_ping/
message.rs
