1#![no_std]
2#![deny(warnings)]
3
4use bbqueue::BBBuffer;
5
6pub mod parser;
7pub mod processor;
8
9const BNO08X_UART_RVC_FRAME_SIZE: usize = 19;
10const BNO08X_UART_RVC_HEADER: u16 = 0xAAAA;
11const MAX_AMOUNT_OF_FRAMES: usize = 3;
12
13pub const BNO08X_UART_RVC_BAUD_RATE: u32 = 115_200;
14pub const BUFFER_SIZE: usize = BNO08X_UART_RVC_FRAME_SIZE * MAX_AMOUNT_OF_FRAMES;
15
16#[derive(Debug)]
17pub enum Error {
18 BbqError(bbqueue::Error),
19}
20
21pub fn create(
22 bbuffer: &'static BBBuffer<BUFFER_SIZE>,
23) -> Result<(processor::Processor, parser::Parser), Error> {
24 match bbuffer.try_split() {
25 Ok((prod, cons)) => Ok((processor::Processor::new(prod), parser::Parser::new(cons))),
26 Err(e) => Err(Error::BbqError(e)),
27 }
28}
29
30#[cfg(test)]
31mod tests {
32 use super::*;
33 use crate::parser::{Bno08xRvcPrettyFrame, Bno08xRvcRawFrame};
34 use core::borrow::Borrow;
35
36 const TEST_FRAME: Bno08xRvcRawFrame = Bno08xRvcRawFrame {
37 index: 0xDE,
38 yaw: 1i16,
39 pitch: -110i16,
40 roll: 2085i16,
41 x_acc: -371i16,
42 y_acc: -20i16,
43 z_acc: 977i16,
44 motion_intent: 0,
45 motion_request: 0,
46 rsvd: 0,
47 csum: 0xE7,
48 };
49
50 #[test]
51 fn parse_buffer_with_only_one_valid_frame() {
52 let test_data: [u8; BNO08X_UART_RVC_FRAME_SIZE] = [
53 0xAA, 0xAA, 0xDE, 0x01, 0x00, 0x92, 0xFF, 0x25, 0x08, 0x8D, 0xFE, 0xEC, 0xFF, 0xD1,
54 0x03, 0x00, 0x00, 0x00, 0xE7,
55 ];
56 static BB: BBBuffer<{ BUFFER_SIZE }> = BBBuffer::new();
57 let create_option = match create(BB.borrow()) {
58 Ok((proc, pars)) => Some((proc, pars)),
59 Err(_) => None,
60 };
61 assert_eq!(matches!(create_option, None), false);
62 let (mut processor, mut parser) = create_option.unwrap();
63 let processor_result = processor.process_slice(&test_data);
64 assert_eq!(matches!(processor_result, Ok(())), true);
65 let worker_result = parser.worker(|frame| {
66 assert_eq!(*frame, TEST_FRAME);
67 });
68 assert_eq!(matches!(worker_result, Ok(())), true);
69 assert_eq!(parser.get_last_raw_frame(), Some(TEST_FRAME));
70 let parser_result = parser.consumer.read();
71 assert_eq!(
72 matches!(parser_result, Err(bbqueue::Error::InsufficientSize)),
73 true
74 );
75 }
76
77 #[test]
78 fn parse_buffer_with_invalid_header() {
79 let test_data: [u8; BNO08X_UART_RVC_FRAME_SIZE] = [
80 0xAA, 0xBB, 0xDE, 0x01, 0x00, 0x92, 0xFF, 0x25, 0x08, 0x8D, 0xFE, 0xEC, 0xFF, 0xD1,
81 0x03, 0x00, 0x00, 0x00, 0xE7,
82 ];
83 static BB: BBBuffer<{ BUFFER_SIZE }> = BBBuffer::new();
84 let create_option = match create(BB.borrow()) {
85 Ok((proc, pars)) => Some((proc, pars)),
86 Err(_) => None,
87 };
88 assert_eq!(matches!(create_option, None), false);
89 let (mut processor, mut parser) = create_option.unwrap();
90 let processor_result = processor.process_slice(&test_data);
91 assert_eq!(matches!(processor_result, Ok(())), true);
92 let worker_result = parser.worker(|_| {
93 panic!();
94 });
95 assert_eq!(matches!(worker_result, Ok(())), true);
96 assert_eq!(parser.get_last_raw_frame(), None);
97 let parser_result = parser.consumer.read();
98 assert_eq!(
99 matches!(parser_result, Err(bbqueue::Error::InsufficientSize)),
100 true
101 );
102 }
103
104 #[test]
105 fn parse_buffer_with_invalid_check_sum() {
106 let test_data: [u8; BNO08X_UART_RVC_FRAME_SIZE] = [
107 0xAA, 0xAA, 0xDE, 0x01, 0x00, 0x92, 0xFF, 0x25, 0x08, 0x8D, 0xFE, 0xEC, 0xFF, 0xD1,
108 0x03, 0x00, 0x00, 0x00, 0xE9,
109 ];
110 static BB: BBBuffer<{ BUFFER_SIZE }> = BBBuffer::new();
111 let create_option = match create(BB.borrow()) {
112 Ok((proc, pars)) => Some((proc, pars)),
113 Err(_) => None,
114 };
115 assert_eq!(matches!(create_option, None), false);
116 let (mut processor, mut parser) = create_option.unwrap();
117 let processor_result = processor.process_slice(&test_data);
118 assert_eq!(matches!(processor_result, Ok(())), true);
119 let worker_result = parser.worker(|_| {
120 panic!();
121 });
122 assert_eq!(matches!(worker_result, Ok(())), true);
123 assert_eq!(parser.get_last_raw_frame(), None);
124 let parser_result = parser.consumer.read();
125 assert_eq!(
126 matches!(parser_result, Err(bbqueue::Error::InsufficientSize)),
127 true
128 );
129 }
130
131 #[test]
132 fn parse_buffer_with_smaller_len() {
133 let test_data: [u8; BNO08X_UART_RVC_FRAME_SIZE - 5] = [
134 0xAA, 0xAA, 0xDE, 0x01, 0x00, 0x92, 0xFF, 0x25, 0x08, 0x8D, 0xFE, 0xEC, 0xFF, 0xD1,
135 ];
136 static BB: BBBuffer<{ BUFFER_SIZE }> = BBBuffer::new();
137 let create_option = match create(BB.borrow()) {
138 Ok((proc, pars)) => Some((proc, pars)),
139 Err(_) => None,
140 };
141 assert_eq!(matches!(create_option, None), false);
142 let (mut processor, mut parser) = create_option.unwrap();
143 let processor_result = processor.process_slice(&test_data);
144 assert_eq!(matches!(processor_result, Ok(())), true);
145 let worker_result = parser.worker(|_| {
146 panic!();
147 });
148 assert_eq!(matches!(worker_result, Ok(())), true);
149 assert_eq!(parser.get_last_raw_frame(), None);
150 let parser_result = parser.consumer.read();
151 assert_eq!(
152 matches!(parser_result, Err(bbqueue::Error::InsufficientSize)),
153 false
154 );
155 assert_eq!(
156 matches!(parser_result, Err(bbqueue::Error::GrantInProgress)),
157 false
158 );
159 assert_eq!(parser_result.unwrap().len(), BNO08X_UART_RVC_FRAME_SIZE - 5);
160 }
161
162 #[test]
163 fn parse_buffer_with_bigger_len() {
164 let test_data: [u8; BNO08X_UART_RVC_FRAME_SIZE + 5] = [
165 0xAA, 0xAA, 0xDE, 0x01, 0x00, 0x92, 0xFF, 0x25, 0x08, 0x8D, 0xFE, 0xEC, 0xFF, 0xD1,
166 0x03, 0x00, 0x00, 0x00, 0xE7, 0x01, 0x02, 0x03, 0x04, 0x05,
167 ];
168 static BB: BBBuffer<{ BUFFER_SIZE }> = BBBuffer::new();
169 let create_option = match create(BB.borrow()) {
170 Ok((proc, pars)) => Some((proc, pars)),
171 Err(_) => None,
172 };
173 assert_eq!(matches!(create_option, None), false);
174 let (mut processor, mut parser) = create_option.unwrap();
175 let processor_result = processor.process_slice(&test_data);
176 assert_eq!(matches!(processor_result, Ok(())), true);
177 let worker_result = parser.worker(|frame| {
178 assert_eq!(*frame, TEST_FRAME);
179 });
180 assert_eq!(matches!(worker_result, Ok(())), true);
181 assert_eq!(parser.get_last_raw_frame(), Some(TEST_FRAME));
182 let parser_result = parser.consumer.read();
183 assert_eq!(
184 matches!(parser_result, Err(bbqueue::Error::InsufficientSize)),
185 false
186 );
187 assert_eq!(
188 matches!(parser_result, Err(bbqueue::Error::GrantInProgress)),
189 false
190 );
191 assert_eq!(parser_result.unwrap().len(), 5);
192 }
193
194 #[test]
195 fn parse_buffer_with_no_valid_frame() {
196 let test_data: [u8; BNO08X_UART_RVC_FRAME_SIZE + 5] = [
197 0xDD, 0xCA, 0xDE, 0x01, 0x00, 0x92, 0xFF, 0x25, 0x08, 0x8D, 0xFE, 0xEC, 0xFF, 0xD1,
198 0x03, 0x00, 0x00, 0x00, 0xE7, 0x01, 0x02, 0x03, 0x04, 0x05,
199 ];
200 static BB: BBBuffer<{ BUFFER_SIZE }> = BBBuffer::new();
201 let create_option = match create(BB.borrow()) {
202 Ok((proc, pars)) => Some((proc, pars)),
203 Err(_) => None,
204 };
205 assert_eq!(matches!(create_option, None), false);
206 let (mut processor, mut parser) = create_option.unwrap();
207 let processor_result = processor.process_slice(&test_data);
208 assert_eq!(matches!(processor_result, Ok(())), true);
209 let worker_result = parser.worker(|_| {
210 panic!();
211 });
212 assert_eq!(matches!(worker_result, Ok(())), true);
213 assert_eq!(parser.get_last_raw_frame(), None);
214 let parser_result = parser.consumer.read();
215 assert_eq!(
216 matches!(parser_result, Err(bbqueue::Error::InsufficientSize)),
217 true
218 );
219 }
220
221 #[test]
222 fn parse_buffer_with_valid_frame_inside_garbage() {
223 let test_data: [u8; BNO08X_UART_RVC_FRAME_SIZE + 10] = [
224 0x01, 0x02, 0x03, 0x04, 0x05, 0xAA, 0xAA, 0xDE, 0x01, 0x00, 0x92, 0xFF, 0x25, 0x08,
225 0x8D, 0xFE, 0xEC, 0xFF, 0xD1, 0x03, 0x00, 0x00, 0x00, 0xE7, 0x01, 0x02, 0x03, 0x04,
226 0x05,
227 ];
228 static BB: BBBuffer<{ BUFFER_SIZE }> = BBBuffer::new();
229 let create_option = match create(BB.borrow()) {
230 Ok((proc, pars)) => Some((proc, pars)),
231 Err(_) => None,
232 };
233 assert_eq!(matches!(create_option, None), false);
234 let (mut processor, mut parser) = create_option.unwrap();
235 let processor_result = processor.process_slice(&test_data);
236 assert_eq!(matches!(processor_result, Ok(())), true);
237 let worker_result = parser.worker(|frame| {
238 assert_eq!(*frame, TEST_FRAME);
239 });
240 assert_eq!(matches!(worker_result, Ok(())), true);
241 assert_eq!(parser.get_last_raw_frame(), Some(TEST_FRAME));
242 let parser_result = parser.consumer.read();
243 assert_eq!(
244 matches!(parser_result, Err(bbqueue::Error::InsufficientSize)),
245 false
246 );
247 assert_eq!(
248 matches!(parser_result, Err(bbqueue::Error::GrantInProgress)),
249 false
250 );
251 assert_eq!(parser_result.unwrap().len(), 5);
252 }
253
254 #[test]
255 fn try_to_process_big_slice() {
256 let test_data: [u8; BUFFER_SIZE + 10] = [0xFF; BUFFER_SIZE + 10];
257 static BB: BBBuffer<{ BUFFER_SIZE }> = BBBuffer::new();
258 let create_option = match create(BB.borrow()) {
259 Ok((proc, pars)) => Some((proc, pars)),
260 Err(_) => None,
261 };
262 assert_eq!(matches!(create_option, None), false);
263 let (mut processor, mut parser) = create_option.unwrap();
264 let processor_result = processor.process_slice(&test_data);
265 assert_eq!(
266 matches!(
267 processor_result,
268 Err(Error::BbqError(bbqueue::Error::InsufficientSize))
269 ),
270 true
271 );
272 let worker_result = parser.worker(|_| {
273 panic!();
274 });
275 assert_eq!(
276 matches!(
277 worker_result,
278 Err(Error::BbqError(bbqueue::Error::InsufficientSize))
279 ),
280 true
281 );
282 assert_eq!(parser.get_last_raw_frame(), None);
283 let parser_result = parser.consumer.read();
284 assert_eq!(
285 matches!(parser_result, Err(bbqueue::Error::InsufficientSize)),
286 true
287 );
288 }
289
290 #[test]
291 fn try_to_process_invalid_frames_by_byte() {
292 let test_data: [u8; BUFFER_SIZE] = [0xFF; BUFFER_SIZE];
293 static BB: BBBuffer<{ BUFFER_SIZE }> = BBBuffer::new();
294 let create_option = match create(BB.borrow()) {
295 Ok((proc, pars)) => Some((proc, pars)),
296 Err(_) => None,
297 };
298 assert_eq!(matches!(create_option, None), false);
299 let (mut processor, mut parser) = create_option.unwrap();
300 for (idx, iter) in test_data.iter().enumerate() {
301 let byte = *iter;
302 let processor_result = processor.process_slice(&[byte]);
303 assert_eq!(
304 matches!(
305 processor_result,
306 Err(Error::BbqError(bbqueue::Error::InsufficientSize))
307 ),
308 false
309 );
310 let worker_result = parser.worker(|_| {
311 panic!();
312 });
313 assert_eq!(
314 matches!(
315 worker_result,
316 Err(Error::BbqError(bbqueue::Error::InsufficientSize))
317 ),
318 false
319 );
320 assert_eq!(parser.get_last_raw_frame(), None);
321 match parser.consumer.read() {
322 Ok(_) => {
323 panic!("{:}", idx);
324 }
325 Err(e) => {
326 assert_eq!(matches!(e, bbqueue::Error::InsufficientSize), true);
327 }
328 }
329 }
330 }
331
332 #[test]
333 fn try_to_process_valid_frames_by_byte() {
334 let test_data: [u8; BUFFER_SIZE] = [
335 0xAA, 0xAA, 0xDE, 0x01, 0x00, 0x92, 0xFF, 0x25, 0x08, 0x8D, 0xFE, 0xEC, 0xFF, 0xD1,
336 0x03, 0x00, 0x00, 0x00, 0xE7, 0xAA, 0xAA, 0xDE, 0x01, 0x00, 0x92, 0xFF, 0x25, 0x08,
337 0x8D, 0xFE, 0xEC, 0xFF, 0xD1, 0x03, 0x00, 0x00, 0x00, 0xE7, 0xAA, 0xAA, 0xDE, 0x01,
338 0x00, 0x92, 0xFF, 0x25, 0x08, 0x8D, 0xFE, 0xEC, 0xFF, 0xD1, 0x03, 0x00, 0x00, 0x00,
339 0xE7,
340 ];
341 static BB: BBBuffer<{ BUFFER_SIZE }> = BBBuffer::new();
342 let create_option = match create(BB.borrow()) {
343 Ok((proc, pars)) => Some((proc, pars)),
344 Err(_) => None,
345 };
346 assert_eq!(matches!(create_option, None), false);
347 let (mut processor, mut parser) = create_option.unwrap();
348 let mut flag_in_worker = 0;
349 for iter in test_data.iter() {
350 let byte = *iter;
351 let processor_result = processor.process_slice(&[byte]);
352 assert_eq!(
353 matches!(
354 processor_result,
355 Err(Error::BbqError(bbqueue::Error::InsufficientSize))
356 ),
357 false
358 );
359 parser
360 .worker(|frame| {
361 assert_eq!(*frame, TEST_FRAME);
362 flag_in_worker += 1;
363 })
364 .unwrap();
365 }
366 assert_eq!(flag_in_worker, 3);
367 match parser.consumer.read() {
368 Ok(_) => {
369 panic!("Rgr is not empty!");
370 }
371 Err(e) => {
372 assert_eq!(matches!(e, bbqueue::Error::InsufficientSize), true);
373 }
374 }
375 }
376
377 #[test]
378 fn try_to_process_valid_and_invalid_frames_by_byte() {
379 let test_data: [u8; BUFFER_SIZE] = [
380 0xAA, 0xBB, 0xDE, 0x01, 0x00, 0x92, 0xFF, 0x25, 0x08, 0x8D, 0xFE, 0xEC, 0xFF, 0xD1,
381 0x03, 0x00, 0x00, 0x00, 0xE7, 0xAA, 0xAA, 0xDE, 0x01, 0x00, 0x92, 0xFF, 0x25, 0x08,
382 0x8D, 0xFE, 0xEC, 0xFF, 0xD1, 0x03, 0x00, 0x00, 0x00, 0xE7, 0xFF, 0xAA, 0xDE, 0x01,
383 0x00, 0x92, 0xFF, 0x25, 0x08, 0x8D, 0xFE, 0xEC, 0xFF, 0xD1, 0x03, 0x00, 0x00, 0x00,
384 0xE7,
385 ];
386 static BB: BBBuffer<{ BUFFER_SIZE }> = BBBuffer::new();
387 let create_option = match create(BB.borrow()) {
388 Ok((proc, pars)) => Some((proc, pars)),
389 Err(_) => None,
390 };
391 assert_eq!(matches!(create_option, None), false);
392 let (mut processor, mut parser) = create_option.unwrap();
393 let mut flag_in_worker = 0;
394 for iter in test_data.iter() {
395 let byte = *iter;
396 let processor_result = processor.process_slice(&[byte]);
397 assert_eq!(
398 matches!(
399 processor_result,
400 Err(Error::BbqError(bbqueue::Error::InsufficientSize))
401 ),
402 false
403 );
404 parser
405 .worker(|frame| {
406 assert_eq!(*frame, TEST_FRAME);
407 flag_in_worker += 1;
408 })
409 .unwrap();
410 }
411 assert_eq!(flag_in_worker, 1);
412 match parser.consumer.read() {
413 Ok(_) => {
414 panic!("Rgr is not empty!");
415 }
416 Err(e) => {
417 assert_eq!(matches!(e, bbqueue::Error::InsufficientSize), true);
418 }
419 }
420 }
421
422 #[test]
423 fn pretty_output_test() {
424 const P_FRAME: Bno08xRvcPrettyFrame = Bno08xRvcPrettyFrame {
425 index: 222,
426 yaw: 0.01,
427 pitch: -1.10,
428 roll: 20.85,
429 x_acc: -3.638,
430 y_acc: -0.196,
431 z_acc: 9.581,
432 motion_intent: 0,
433 motion_request: 0,
434 rsvd: 0,
435 };
436
437 let frame = TEST_FRAME.as_pretty_frame();
438 assert_eq!(frame.index, P_FRAME.index);
439 assert!(
440 (frame.yaw - P_FRAME.yaw).abs() < 0.001,
441 "a = {}, b = {}",
442 frame.yaw,
443 P_FRAME.yaw
444 );
445 assert!(
446 (frame.pitch - P_FRAME.pitch).abs() < 0.001,
447 "a = {}, b = {}",
448 frame.pitch,
449 P_FRAME.pitch
450 );
451 assert!(
452 (frame.roll - P_FRAME.roll).abs() < 0.001,
453 "a = {}, b = {}",
454 frame.roll,
455 P_FRAME.roll
456 );
457 assert!(
458 (frame.x_acc - P_FRAME.x_acc).abs() < 0.001,
459 "a = {}, b = {}",
460 frame.x_acc,
461 P_FRAME.x_acc
462 );
463 assert!(
464 (frame.y_acc - P_FRAME.y_acc).abs() < 0.001,
465 "a = {}, b = {}",
466 frame.y_acc,
467 P_FRAME.y_acc
468 );
469 assert!(
470 (frame.z_acc - P_FRAME.z_acc).abs() < 0.001,
471 "a = {}, b = {}",
472 frame.z_acc,
473 P_FRAME.z_acc
474 );
475 assert_eq!(frame.motion_intent, P_FRAME.motion_intent);
476 assert_eq!(frame.motion_request, P_FRAME.motion_request);
477 assert_eq!(frame.rsvd, P_FRAME.rsvd);
478 }
479}