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
87
88
|
#[repr(packed)]
struct Header {
secs: u32,
micros: u32,
len: u32,
}
impl Header {
fn time(&self) -> std::time::Duration {
std::time::Duration::from_micros(u64::from(
self.secs * 1_000_000 + self.micros,
))
}
fn len(&self) -> usize {
self.len as usize
}
}
pub struct Parser {
reading: std::collections::VecDeque<u8>,
read_state: Option<Header>,
}
impl Parser {
pub fn new() -> Self {
Default::default()
}
pub fn add_bytes(&mut self, bytes: &[u8]) {
self.reading.extend(bytes.iter());
}
pub fn next_frame(&mut self) -> Option<crate::frame::Frame> {
let header = if let Some(header) = &self.read_state {
header
} else {
if self.reading.len() < std::mem::size_of::<Header>() {
return None;
}
let secs1 = self.reading.pop_front().unwrap();
let secs2 = self.reading.pop_front().unwrap();
let secs3 = self.reading.pop_front().unwrap();
let secs4 = self.reading.pop_front().unwrap();
let secs = u32::from_le_bytes([secs1, secs2, secs3, secs4]);
let usecs1 = self.reading.pop_front().unwrap();
let usecs2 = self.reading.pop_front().unwrap();
let usecs3 = self.reading.pop_front().unwrap();
let usecs4 = self.reading.pop_front().unwrap();
let micros = u32::from_le_bytes([usecs1, usecs2, usecs3, usecs4]);
let len1 = self.reading.pop_front().unwrap();
let len2 = self.reading.pop_front().unwrap();
let len3 = self.reading.pop_front().unwrap();
let len4 = self.reading.pop_front().unwrap();
let len = u32::from_le_bytes([len1, len2, len3, len4]);
let header = Header { secs, micros, len };
self.read_state = Some(header);
self.read_state.as_ref().unwrap()
};
if self.reading.len() < header.len() {
return None;
}
let mut data = vec![];
for _ in 0..header.len() {
data.push(self.reading.pop_front().unwrap());
}
let time = header.time();
self.read_state = None;
Some(crate::frame::Frame { time, data })
}
}
impl Default for Parser {
fn default() -> Self {
Self {
reading: std::collections::VecDeque::new(),
read_state: None,
}
}
}
|
