BNO08X Driver 0.1.0
API reference
Loading...
Searching...
No Matches
bno08x_rvc.hpp
Go to the documentation of this file.
1
7#ifndef BNO08X_RVC_HPP
8#define BNO08X_RVC_HPP
9
10#include <algorithm>
11#include <array>
12#include <cstddef>
13#include <cstdint>
14#include <span>
15
16#include "bno08x_detail.hpp"
17#include "bno08x_registers.hpp"
18#include "bno08x_shtp.hpp"
19#include "bno08x_types.hpp"
20
21namespace bno08x::detail {
22
24inline constexpr uint8_t kRvcHeader = 0xAA;
25
27[[nodiscard]] constexpr uint8_t RvcSum(std::span<const uint8_t> bytes, std::size_t first,
28 std::size_t last) noexcept {
29 uint32_t sum = 0;
30 for (std::size_t i = first; i <= last; ++i) {
31 sum += static_cast<uint32_t>(bytes[i]);
32 }
33 return static_cast<uint8_t>(sum & 0xFFU);
34}
35
37enum class RvcSumRule : uint8_t {
38 None,
39 WithMiMr,
40 WithoutMiMr,
41};
42
44[[nodiscard]] constexpr RvcSumRule RvcCheck(std::span<const uint8_t> frame) noexcept {
45 const uint8_t want = frame[18];
46 if (RvcSum(frame, 2U, 17U) == want) {
47 return RvcSumRule::WithMiMr;
48 }
49 const auto alt = static_cast<uint8_t>(
50 (static_cast<uint32_t>(RvcSum(frame, 2U, 14U)) + static_cast<uint32_t>(frame[17])) & 0xFFU);
51 return (alt == want) ? RvcSumRule::WithoutMiMr : RvcSumRule::None;
52}
53
55[[nodiscard]] constexpr RvcFrame RvcDecode(std::span<const uint8_t> frame, bool mi_mr) noexcept {
56 RvcFrame f{};
57 f.index = frame[2];
58 f.yaw = static_cast<int16_t>(S16(Le16(frame, 3U)));
59 f.pitch = static_cast<int16_t>(S16(Le16(frame, 5U)));
60 f.roll = static_cast<int16_t>(S16(Le16(frame, 7U)));
61 f.ax = static_cast<int16_t>(S16(Le16(frame, 9U)));
62 f.ay = static_cast<int16_t>(S16(Le16(frame, 11U)));
63 f.az = static_cast<int16_t>(S16(Le16(frame, 13U)));
64 f.mi_mr_valid = mi_mr;
65 if (mi_mr) {
66 f.mi = frame[15];
67 f.mr = frame[16];
68 }
69 return f;
70}
71
79class RvcParser {
80public:
82 RvcParser(Stats& stats, const Config& cfg) noexcept : stats_(stats), cfg_(cfg) {}
83
85 void Reset() noexcept {
86 count_ = 0;
87 have_index_ = false;
88 }
89
91 bool Feed(uint8_t b) noexcept {
92 if ((count_ < 2U) && (b != kRvcHeader)) {
93 count_ = 0;
94 ++stats_.rvc_banner_bytes;
95 return false;
96 }
97 buf_[count_] = b;
98 ++count_;
99 if (count_ < reg::kRvcFrameLen) {
100 return false;
101 }
102 const RvcSumRule rule = RvcCheck(buf_);
103 if (rule == RvcSumRule::None) {
104 ++stats_.rvc_bad_csum;
105 slide();
106 return false;
107 }
108 count_ = 0;
109 return accept(rule);
110 }
111
113 [[nodiscard]] const RvcFrame& Frame() const noexcept {
114 return frame_;
115 }
116
117private:
119 void slide() noexcept {
120 std::size_t p = 1;
121 while ((p + 1U) < reg::kRvcFrameLen) {
122 if ((buf_[p] == kRvcHeader) && (buf_[p + 1U] == kRvcHeader)) {
123 break;
124 }
125 ++p;
126 }
127 if ((p + 1U) >= reg::kRvcFrameLen) {
128 // no header pair left: keep a trailing 0xAA, which may start the next one
129 count_ = 0;
130 if (buf_[reg::kRvcFrameLen - 1U] == kRvcHeader) {
131 buf_[0] = kRvcHeader;
132 count_ = 1;
133 }
134 return;
135 }
136 static_cast<void>(
137 std::copy(buf_.begin() + static_cast<std::ptrdiff_t>(p), buf_.end(), buf_.begin()));
138 count_ = reg::kRvcFrameLen - p;
139 }
140
142 bool accept(RvcSumRule rule) noexcept {
143 const uint8_t index = buf_[2];
144 const bool gap =
145 have_index_ &&
146 (index != static_cast<uint8_t>((static_cast<uint32_t>(last_index_) + 1U) & 0xFFU));
147 if (gap) {
148 ++stats_.rvc_index_gaps;
149 }
150 const bool had = have_index_;
151 have_index_ = true;
152 last_index_ = index;
153 if (cfg_.rvc_strict && had && gap) {
154 return false; // one frame is lost to prove continuity again
155 }
156 if (rule == RvcSumRule::WithMiMr) {
157 ++stats_.rvc_csum_with_mi;
158 } else {
159 ++stats_.rvc_csum_without_mi;
160 }
161 frame_ = RvcDecode(buf_, cfg_.variant == Variant::Bno086);
162 const bool range = (frame_.yaw < -18000) || (frame_.yaw > 18000) || (frame_.pitch < -9000) ||
163 (frame_.pitch > 9000) || (frame_.roll < -18000) || (frame_.roll > 18000);
164 if (range) {
165 ++stats_.rvc_out_of_range;
166 }
167 ++stats_.rvc_frames;
168 return true;
169 }
170
171 Stats& stats_;
172 const Config& cfg_;
173 std::array<uint8_t, reg::kRvcFrameLen> buf_{};
174 std::size_t count_{0};
175 RvcFrame frame_{};
176 uint8_t last_index_{0};
177 bool have_index_{false};
178};
179
180} // namespace bno08x::detail
181
182#endif // BNO08X_RVC_HPP
Compile-time helpers of the BNO08X driver: typed integer literals, exact hook matching,...
BNO08x protocol constants: SHTP channels, SH-2 report, command and record identifiers,...
The SHTP frame engine: header codec, strict receive validation (rules V1 to V12), the UART framer and...
Public types of the BNO08x driver: errors, Result<T>, configuration, events and the integer unit help...
constexpr std::size_t kRvcFrameLen
UART-RVC frame length (DS p21).
Definition bno08x_registers.hpp:217
@ Bno086
BNO086: interactive calibration, RVC MI/MR bytes, zero-length I2C packets.
@ Reset
The hub restarted; detail is the reset cause when known.
uint8_t index
Frame index, increments per frame.
Definition bno08x_types.hpp:489