BNO08X Driver 0.1.0
API reference
Loading...
Searching...
No Matches
bno08x_euler.hpp
Go to the documentation of this file.
1
6#ifndef BNO08X_EULER_HPP
7#define BNO08X_EULER_HPP
8
9#include <array>
10#include <cstdint>
11
12#include "bno08x_types.hpp"
13
14namespace bno08x {
15
16namespace detail {
17
19inline constexpr std::array<int32_t, 14> kAtanTable{
20 {72000, 42504, 22458, 11400, 5722, 2864, 1432, 716, 358, 179, 90, 45, 22, 11}};
21
23constexpr uint32_t Isqrt(uint64_t v) noexcept {
24 uint64_t bit = uint64_t{1} << 62U;
25 uint64_t res = 0;
26 uint64_t rem = v;
27 while (bit > rem) {
28 bit >>= 2U;
29 }
30 while (bit != 0U) {
31 if (rem >= (res + bit)) {
32 rem -= res + bit;
33 res = (res >> 1U) + bit;
34 } else {
35 res >>= 1U;
36 }
37 bit >>= 2U;
38 }
39 return static_cast<uint32_t>(res);
40}
41
43constexpr int64_t Asr(int64_t v, uint32_t n) noexcept {
44 return (v >= 0) ? static_cast<int64_t>(static_cast<uint64_t>(v) >> n)
45 : -static_cast<int64_t>(static_cast<uint64_t>(-v) >> n);
46}
47
49constexpr int32_t Atan2CentiDeg(int32_t y, int32_t x) noexcept {
50 if ((x == 0) && (y == 0)) {
51 return 0;
52 }
53 int64_t cx = x;
54 int64_t cy = y;
55 int32_t base = 0;
56 if (cx < 0) {
57 base = (cy >= 0) ? 18000 : -18000;
58 cx = -cx;
59 cy = -cy;
60 }
61 int32_t angle = 0; // 1/100 degree * 16
62 for (uint32_t n = 0U; n < kAtanTable.size(); ++n) {
63 const int64_t dx = Asr(cy, n);
64 const int64_t dy = Asr(cx, n);
65 if (cy > 0) {
66 cx += dx;
67 cy -= dy;
68 angle += kAtanTable[n];
69 } else {
70 cx -= dx;
71 cy += dy;
72 angle -= kAtanTable[n];
73 }
74 }
75 return base + ((angle + ((angle >= 0) ? 8 : -8)) / 16);
76}
77
78} // namespace detail
79
88[[nodiscard]] constexpr EulerCentiDeg ToEulerCentiDeg(int32_t i, int32_t j, int32_t k,
89 int32_t real) noexcept {
90 const int64_t w = real;
91 const int64_t x = i;
92 const int64_t y = j;
93 const int64_t z = k;
94 EulerCentiDeg e{};
95 e.roll =
96 detail::Atan2CentiDeg(static_cast<int32_t>(2 * ((w * x) + (y * z)) / 16384),
97 static_cast<int32_t>(((w * w) - (x * x) - (y * y) + (z * z)) / 16384));
98 e.yaw =
99 detail::Atan2CentiDeg(static_cast<int32_t>(2 * ((w * z) + (x * y)) / 16384),
100 static_cast<int32_t>(((w * w) + (x * x) - (y * y) - (z * z)) / 16384));
101 // sin(pitch) = 2 (w y - z x); pitch = atan2(s, sqrt(1 - s^2)).
102 int64_t s = 2 * ((w * y) - (z * x)) / 16384;
103 if (s > 16384) {
104 s = 16384;
105 } else if (s < -16384) {
106 s = -16384;
107 } else {
108 // in range: nothing to clamp
109 }
110 const uint32_t c = detail::Isqrt(static_cast<uint64_t>((int64_t{16384} * 16384) - (s * s)));
111 e.pitch = detail::Atan2CentiDeg(static_cast<int32_t>(s), static_cast<int32_t>(c));
112 return e;
113}
114
115} // namespace bno08x
116
117#endif // BNO08X_EULER_HPP
Public types of the BNO08x driver: errors, Result<T>, configuration, events and the integer unit help...
The BNO08x driver: everything it declares lives here; internals in detail.
Definition bno08x.hpp:36
constexpr EulerCentiDeg ToEulerCentiDeg(int32_t i, int32_t j, int32_t k, int32_t real) noexcept
Euler angles from a Q14 unit quaternion, with integer arithmetic only.
Definition bno08x_euler.hpp:88
Yaw, pitch and roll in centidegrees.
Definition bno08x_types.hpp:613
int32_t roll
Rotation about X, -18000 to 18000.
Definition bno08x_types.hpp:616