bno08x_rvc/
parser.rs

1use crate::Error;
2use crate::{BNO08X_UART_RVC_FRAME_SIZE, BNO08X_UART_RVC_HEADER, BUFFER_SIZE};
3use bbqueue::Consumer;
4use core::borrow::Borrow;
5use serde::Deserialize;
6
7#[derive(Deserialize, Debug, Copy, Clone, PartialEq)]
8pub struct Bno08xRvcRawFrame {
9    pub index: u8,
10    pub yaw: i16,
11    pub pitch: i16,
12    pub roll: i16,
13    pub x_acc: i16,
14    pub y_acc: i16,
15    pub z_acc: i16,
16    pub motion_intent: u8,
17    pub motion_request: u8,
18    pub rsvd: u8,
19    pub csum: u8,
20}
21
22const G_ACCELERATION: f32 = 9.80665;
23
24#[derive(Debug, Copy, Clone, PartialEq)]
25pub struct Bno08xRvcPrettyFrame {
26    pub index: u8,  // A monotonically increasing 8-bit count is provided (0-255) per report
27    pub yaw: f32, // The yaw is a measure of the rotation around the Z-axis since reset. The yaw has a range of +/- 180̊  and is provided in 0.01̊  increments.
28    pub pitch: f32, // The pitch is a measure of the rotation around the Y-axis. The pitch has a range of +/- 90̊  and is provided in 0.01̊  increments
29    pub roll: f32, // The roll is a measure of the rotation around the X-axis. The roll has a range of +/- 180̊  and is provided in 0.01̊  increments
30    pub x_acc: f32, // The acceleration along the X-axis, presented in m/s2
31    pub y_acc: f32, // The acceleration along the Y-axis, presented in m/s2
32    pub z_acc: f32, // The acceleration along the Z-axis, presented in m/s2
33    pub motion_intent: u8, // BNO086 only. Otherwise, reserved
34    pub motion_request: u8, // BNO086 only. Otherwise, reserved
35    pub rsvd: u8, // The message is terminated with one (BNO086) or three (otherwise) reserved bytes, currently set to zero
36}
37
38impl Bno08xRvcRawFrame {
39    fn convert(&self) -> Bno08xRvcPrettyFrame {
40        Bno08xRvcPrettyFrame {
41            index: self.index,
42            yaw: (self.yaw as f32) / 100.0,
43            pitch: (self.pitch as f32) / 100.0,
44            roll: (self.roll as f32) / 100.0,
45            x_acc: (self.x_acc as f32) * G_ACCELERATION / 1000.0,
46            y_acc: (self.y_acc as f32) * G_ACCELERATION / 1000.0,
47            z_acc: (self.z_acc as f32) * G_ACCELERATION / 1000.0,
48            motion_intent: self.motion_intent,
49            motion_request: self.motion_request,
50            rsvd: self.rsvd,
51        }
52    }
53
54    pub fn as_pretty_frame(&self) -> Bno08xRvcPrettyFrame {
55        self.convert()
56    }
57
58    pub fn as_pretty_closure<F: FnMut(&Bno08xRvcPrettyFrame)>(&self, mut f: F) {
59        f(&self.convert());
60    }
61}
62
63#[derive(PartialEq)]
64enum State {
65    LookingForFirstHeaderByte,
66    LookingForSecondHeaderByte,
67    GetFrameData,
68    GotFrame,
69}
70
71pub struct Parser {
72    pub(crate) consumer: Consumer<'static, BUFFER_SIZE>,
73    last_frame: Option<Bno08xRvcRawFrame>,
74    state: State,
75}
76
77impl Parser {
78    pub fn new(consumer: Consumer<'static, BUFFER_SIZE>) -> Parser {
79        Parser {
80            consumer,
81            last_frame: None,
82            state: State::LookingForFirstHeaderByte,
83        }
84    }
85
86    pub fn get_last_raw_frame(&self) -> Option<Bno08xRvcRawFrame> {
87        self.last_frame
88    }
89
90    pub fn worker<F: FnMut(&Bno08xRvcRawFrame)>(&mut self, mut f_opt: F) -> Result<(), Error> {
91        return match self.consumer.split_read() {
92            Err(e) => Err(Error::BbqError(e)),
93            Ok(rgr) => {
94                let (s1, s2) = rgr.bufs();
95                let mut tmp = [0u8; BUFFER_SIZE];
96                tmp[0..s1.len()].copy_from_slice(s1);
97                tmp[s1.len()..s1.len() + s2.len()].copy_from_slice(s2);
98                match self.parse(&tmp[0..(s1.len() + s2.len())]) {
99                    None => Ok(()),
100                    Some((frame_option, release_size)) => {
101                        rgr.release(release_size);
102                        match frame_option {
103                            None => Ok(()),
104                            Some(frame) => {
105                                f_opt(frame.borrow());
106                                Ok(())
107                            }
108                        }
109                    }
110                }
111            }
112        };
113    }
114
115    fn parse(&mut self, raw_bytes: &[u8]) -> Option<(Option<Bno08xRvcRawFrame>, usize)> {
116        let mut release_size = 0;
117        for (idx, iter) in raw_bytes.iter().enumerate() {
118            match self.state {
119                State::LookingForFirstHeaderByte => {
120                    if *iter == (BNO08X_UART_RVC_HEADER >> 8) as u8 {
121                        self.state = State::LookingForSecondHeaderByte;
122                    } else {
123                        self.state = State::LookingForFirstHeaderByte;
124                        release_size = idx + 1;
125                    }
126                }
127                State::LookingForSecondHeaderByte => {
128                    if *iter == BNO08X_UART_RVC_HEADER as u8 {
129                        self.state = State::GetFrameData;
130                    } else {
131                        self.state = State::LookingForFirstHeaderByte;
132                        release_size = idx + 1;
133                    }
134                }
135                State::GetFrameData => {
136                    if raw_bytes.len() - idx >= (BNO08X_UART_RVC_FRAME_SIZE - 2) {
137                        let data = &raw_bytes[idx..(idx + BNO08X_UART_RVC_FRAME_SIZE - 2)];
138                        let csum = data[0..(data.len() - 1)]
139                            .iter()
140                            .map(|v| *v as u32)
141                            .sum::<u32>() as u8;
142                        let frame_unchecked: Bno08xRvcRawFrame = postcard::from_bytes(data).ok()?;
143                        if csum == frame_unchecked.csum {
144                            self.last_frame = Some(frame_unchecked);
145                        }
146                        release_size = idx + BNO08X_UART_RVC_FRAME_SIZE - 2;
147                        self.state = State::GotFrame;
148                    } else {
149                        self.state = State::LookingForFirstHeaderByte;
150                    }
151                    break;
152                }
153                _ => {}
154            }
155        }
156        if self.state == State::GotFrame {
157            self.state = State::LookingForFirstHeaderByte;
158            Some((self.last_frame, release_size))
159        } else {
160            self.state = State::LookingForFirstHeaderByte;
161            Some((None, release_size))
162        }
163    }
164}
bno08x_rvc/parser.rs

bno08x_rvc/
parser.rs
