1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
use super::*;
use byteorder::{LittleEndian, ReadBytesExt};
use crc16::*;
use nom::*;
use std::io::Cursor;
#[derive(Clone, Debug, Default, PartialEq)]
pub struct RawIMU {
pub accel: [i16; 3],
pub gyro: [i16; 3],
pub gyro_temp: u8,
}
impl RawIMU {
pub fn new(mut msg: Vec<u8>) -> Option<Self> {
let mut data = msg.split_off(2);
let mut wrapper = Cursor::new(msg);
let check = wrapper.read_u16::<LittleEndian>().unwrap_or(0);
if check != AUX_SYNC {
println!("RawIMU: Bad sync - {:x}", check);
return None;
}
let len = data.len() - 2;
let mut crc = Cursor::new(data.split_off(len));
let crc = crc.read_u16::<LittleEndian>().unwrap_or(0);
let calc = State::<ARC>::calculate(&data);
match calc == crc {
true => {
match raw_imu(&data) {
Ok(conv) => Some(conv.1),
_ => None,
}
}
false => None,
}
}
}
named!(raw_imu(&[u8]) -> RawIMU,
do_parse!(
le_i16 >>
le_i16 >>
accel_x: le_i16 >>
accel_y: le_i16 >>
accel_z: le_i16 >>
gyro_x: le_i16 >>
gyro_y: le_i16 >>
gyro_z: le_i16 >>
gyro_temp: le_u8 >>
(RawIMU {
accel: [accel_x ,accel_y, accel_z],
gyro: [gyro_x, gyro_y, gyro_z],
gyro_temp
})
)
);