-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathframe_reader.cpp
More file actions
64 lines (46 loc) · 1.55 KB
/
frame_reader.cpp
File metadata and controls
64 lines (46 loc) · 1.55 KB
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
#include <chrono>
#include <stdexcept>
#include "gnsspp/error.hpp"
#include "gnsspp/frame_reader.hpp"
namespace gnsspp {
FrameReader::FrameReader(Port& port)
: port_(port)
{
}
void FrameReader::add_parser(std::unique_ptr<Parser> parser)
{
parsers_.push_back(std::move(parser));
}
std::optional<Frame> FrameReader::read_frame(int timeout_ms)
{
using clock = std::chrono::steady_clock;
using ms = std::chrono::milliseconds;
const auto deadline = (timeout_ms < 0)
? clock::time_point::max()
: clock::now() + ms(timeout_ms);
auto ms_left = [&]() -> int {
if (timeout_ms < 0) return -1;
auto rem = std::chrono::duration_cast<ms>(deadline - clock::now()).count();
return (rem > 0) ? static_cast<int>(rem) : 0;
};
if (!port_.wait_readable(ms_left()))
return std::nullopt;
uint8_t b1 = port_.read_byte();
// Sliding-window sync search: advance one byte at a time until a parser
// recognises the (b1, b2) pair. This handles leading garbage bytes
// (e.g. a stray '\n' before a '$' in an NMEA stream) without losing sync.
for (;;) {
int left = ms_left();
if (left == 0)
return std::nullopt;
if (!port_.wait_readable(left))
return std::nullopt;
uint8_t b2 = port_.read_byte();
for (auto& parser : parsers_) {
if (parser->matches(b1, b2))
return parser->parse(port_, b1, b2);
}
b1 = b2; // slide: discard b1, try again with b2 as the new b1
}
}
} // namespace gnsspp