6#ifndef BNO08X_EULER_HPP
7#define BNO08X_EULER_HPP
19inline constexpr std::array<int32_t, 14> kAtanTable{
20 {72000, 42504, 22458, 11400, 5722, 2864, 1432, 716, 358, 179, 90, 45, 22, 11}};
23constexpr uint32_t Isqrt(uint64_t v)
noexcept {
24 uint64_t bit = uint64_t{1} << 62U;
31 if (rem >= (res + bit)) {
33 res = (res >> 1U) + bit;
39 return static_cast<uint32_t
>(res);
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);
49constexpr int32_t Atan2CentiDeg(int32_t y, int32_t x)
noexcept {
50 if ((x == 0) && (y == 0)) {
57 base = (cy >= 0) ? 18000 : -18000;
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);
68 angle += kAtanTable[n];
72 angle -= kAtanTable[n];
75 return base + ((angle + ((angle >= 0) ? 8 : -8)) / 16);
89 int32_t real)
noexcept {
90 const int64_t w = real;
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));
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));
102 int64_t s = 2 * ((w * y) - (z * x)) / 16384;
105 }
else if (s < -16384) {
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));
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