From 8fe0541b9cd78dc9cda1b6bfb951c9d18984774b Mon Sep 17 00:00:00 2001 From: CK-Explorer <121228064+CK-Explorer@users.noreply.github.com> Date: Sun, 2 Apr 2023 16:24:47 +0800 Subject: [PATCH 1/9] Big modifications Added NED, ENU local reference frame supports. Added supports of passing different gyroscope reading's unit (rad or deg) based on user preference into Madgwick::update() and Madgwick::updateIMU(). Added non-constant IMU's update frequency support by allowing users to use elapsed time, while constant frequency still supports. All the above changes are achieved using template and functions overloading. Added Madgwick::updateCore() and Madgwick::updateCoreIMU() to avoid some code duplications for constant and non-constant frequency supports in Madgwick::update() and Madgwick::updateIMU() respectively. Added a getter function to obtain quaternion, and setter functions for filter parameter (beta) and IMU's frequency separately. Added constructors overloading to initialize filter parameter (beta) and IMU's frequency. Added inline wrapper functions for degree to radians and vice-versa conversion for readability purpose. Rearranged some previous codes for readability purpose. Added comments. --- src/MadgwickAHRS.cpp | 340 ++++++++++++++++++++++++++++++++++++------- src/MadgwickAHRS.h | 152 +++++++++++++------ 2 files changed, 391 insertions(+), 101 deletions(-) diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp index ef2bbce..fe09c7a 100644 --- a/src/MadgwickAHRS.cpp +++ b/src/MadgwickAHRS.cpp @@ -20,21 +20,11 @@ // Header files #include "MadgwickAHRS.h" -#include - -//------------------------------------------------------------------------------------------- -// Definitions - -#define sampleFreqDef 512.0f // sample frequency in Hz -#define betaDef 0.1f // 2 * proportional gain - //============================================================================================ // Functions //------------------------------------------------------------------------------------------- -// AHRS algorithm update - Madgwick::Madgwick() { beta = betaDef; q0 = 1.0f; @@ -45,23 +35,144 @@ Madgwick::Madgwick() { anglesComputed = 0; } +Madgwick::Madgwick(float beta) { + this->beta = beta; + q0 = 1.0f; + q1 = 0.0f; + q2 = 0.0f; + q3 = 0.0f; + invSampleFreq = 1.0f / sampleFreqDef; + anglesComputed = 0; +} + +Madgwick::Madgwick(float beta, float sampleFrequency) { + this->beta = beta; + q0 = 1.0f; + q1 = 0.0f; + q2 = 0.0f; + q3 = 0.0f; + invSampleFreq = 1.0f / sampleFrequency; + anglesComputed = 0; +} + +/*Set filter parameter (beta)*/ +void Madgwick::setBeta(float beta) { + this->beta = beta; +} + +/*Set IMU's update frequency (sampleFrequency)*/ +void Madgwick::setFrequency(float sampleFrequency) { + invSampleFreq = 1.0f / sampleFrequency; +} + +/*Getter function to obtain quaternion of body orientation and store it in quat*/ +void Madgwick::getQuaternion(float quat[4]) { + quat[0] = q0; + quat[1] = q1; + quat[2] = q2; + quat[3] = q3; +} + +/*AHRS algorithm update with constant update frequency, which fuses gyroscope, accelerometer and magnetometer +* Template parameters: +* type = 0: NWU +* type = 1: NED +* type = 2: ENU +* angle = D : Degree per second (Gyroscope's reading unit) +* angle = R : Radian per second (Gyroscope's reading unit) +*/ +template void Madgwick::update<0,'D'>(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); +template void Madgwick::update<1,'D'>(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); +template void Madgwick::update<2,'D'>(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); +template void Madgwick::update<0,'R'>(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); +template void Madgwick::update<1,'R'>(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); +template void Madgwick::update<2,'R'>(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); + +/*AHRS algorithm update with varying update frequency, which fuses gyroscope, accelerometer and magnetometer +* Template parameters: +* type = 0: NWU +* type = 1: NED +* type = 2: ENU +* angle = D : Degree per second (Gyroscope's reading unit) +* angle = R : Radian per second (Gyroscope's reading unit) +* +* Inputs: timeDiff = elapsed time between present and previous IMU's reading +*/ +template void Madgwick::update<0,'D'>(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float timeDiff); +template void Madgwick::update<1,'D'>(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float timeDiff); +template void Madgwick::update<2,'D'>(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float timeDiff); +template void Madgwick::update<0,'R'>(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float timeDiff); +template void Madgwick::update<1,'R'>(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float timeDiff); +template void Madgwick::update<2,'R'>(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float timeDiff); + +template void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { - float recipNorm; - float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; - float hx, hy; - float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) + if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + updateIMU(gx, gy, gz, ax, ay, az); + return; + } + updateCore(gx, gy, gz, ax, ay, az, mx, my, mz, qDot1, qDot2, qDot3, qDot4); + + // Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * invSampleFreq; + q1 += qDot2 * invSampleFreq; + q2 += qDot3 * invSampleFreq; + q3 += qDot4 * invSampleFreq; + + // Normalise quaternion + float recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + anglesComputed = 0; +} + +template +void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float timeDiff) { + + float qDot1, qDot2, qDot3, qDot4; // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { - updateIMU(gx, gy, gz, ax, ay, az); + updateIMU(gx, gy, gz, ax, ay, az, timeDiff); return; } - // Convert gyroscope degrees/sec to radians/sec - gx *= 0.0174533f; - gy *= 0.0174533f; - gz *= 0.0174533f; + updateCore(gx, gy, gz, ax, ay, az, mx, my, mz, qDot1, qDot2, qDot3, qDot4); + + // Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * timeDiff; + q1 += qDot2 * timeDiff; + q2 += qDot3 * timeDiff; + q3 += qDot4 * timeDiff; + + // Normalise quaternion + float recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + anglesComputed = 0; +} + +/*Part of the Madgwick::update() function to avoid code duplication*/ +template +inline void Madgwick::updateCore(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float& qDot1, float& qDot2, float& qDot3, float& qDot4) { + float recipNorm; + float s0, s1, s2, s3; + float hx, hy; + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + + if constexpr (angle == 'D' && angle != 'R'){ + // Convert gyroscope degrees/sec to radians/sec + gx = deg2rad(gx); + gy = deg2rad(gy); + gz = deg2rad(gz); + } // Rate of change of quaternion from gyroscope qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); @@ -114,11 +225,28 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az _4bx = 2.0f * _2bx; _4bz = 2.0f * _2bz; - // Gradient decent algorithm corrective step - s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + if constexpr (type == 0) { + // Gradient decent algorithm corrective step (NWU Frame) + s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + } + else if constexpr (type == 1) { + // Gradient decent algorithm corrective step (NED Frame) + s0 = _2q2 * (- 2.0f * q1q3 + _2q0q2 - ax) - _2q1 * (- 2.0f * q0q1 - _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = -_2q3 * (- 2.0f * q1q3 + _2q0q2 - ax) - _2q0 * (- 2.0f * q0q1 - _2q2q3 - ay) + 4.0f * q1 * (-1 + 2.0f * q1q1 + 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = _2q0 * (- 2.0f * q1q3 + _2q0q2 - ax) - _2q3 * (- 2.0f * q0q1 - _2q2q3 - ay) + 4.0f * q2 * (-1 + 2.0f * q1q1 + 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = -_2q1 * (- 2.0f * q1q3 + _2q0q2 - ax) - _2q2 * (- 2.0f * q0q1 - _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + } + else if constexpr (type == 2) { + // Gradient decent algorithm corrective step (ENU Frame) + s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) + (_2bx * q3 - _2bz * q2) * (_2bx * (q1q2 + q0q3) + _2bz * (q1q3 - q0q2) - mx) + _2bz * q1 * (_2bx * (0.5f - q1q1 - q3q3) + _2bz * (q0q1 + q2q3) - my) - _2bx * q1 * (_2bx * (q2q3 - q0q1) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (_2bx * q2 + _2bz * q3) * (_2bx * (q1q2 + q0q3) + _2bz * (q1q3 - q0q2) - mx) + (-_4bx * q1 + _2bz * q0) * (_2bx * (0.5f - q1q1 - q3q3) + _2bz * (q0q1 + q2q3) - my) + (-_2bx * q0 - _4bz * q1) * (_2bx * (q2q3 - q0q1) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (_2bx * q1 - _2bz * q0) * (_2bx * (q1q2 + q0q3) + _2bz * (q1q3 - q0q2) - mx) + _2bz * q3 * (_2bx * (0.5f - q1q1 - q3q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q2) * (_2bx * (q2q3 - q0q1) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (_2bx * q0 + _2bz * q1) * (_2bx * (q1q2 + q0q3) + _2bz * (q1q3 - q0q2) - mx) + (-_4bx * q3 + _2bz * q2) * (_2bx * (0.5f - q1q1 - q3q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q2q3 - q0q1) + _2bz * (0.5f - q1q1 - q2q2) - mz); + } + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude s0 *= recipNorm; s1 *= recipNorm; @@ -131,6 +259,47 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az qDot3 -= beta * s2; qDot4 -= beta * s3; } +} + +//------------------------------------------------------------------------------------------- +/*IMU algorithm update with constant update frequency, which fuses gyroscope and accelerometer +* Template parameters: +* type = 0: NWU +* type = 1: NED +* type = 2: ENU +* angle = D : Degree per second (Gyroscope's reading unit) +* angle = R : Radian per second (Gyroscope's reading unit) +*/ +template void Madgwick::updateIMU<0,'D'>(float gx, float gy, float gz, float ax, float ay, float az); +template void Madgwick::updateIMU<1,'D'>(float gx, float gy, float gz, float ax, float ay, float az); +template void Madgwick::updateIMU<2,'D'>(float gx, float gy, float gz, float ax, float ay, float az); +template void Madgwick::updateIMU<0,'R'>(float gx, float gy, float gz, float ax, float ay, float az); +template void Madgwick::updateIMU<1,'R'>(float gx, float gy, float gz, float ax, float ay, float az); +template void Madgwick::updateIMU<2,'R'>(float gx, float gy, float gz, float ax, float ay, float az); + +/*IMU algorithm update with varying update frequency, which fuses gyroscope and accelerometer +* Template parameters: +* type = 0: NWU +* type = 1: NED +* type = 2: ENU +* angle = D : Degree per second (Gyroscope's reading unit) +* angle = R : Radian per second (Gyroscope's reading unit) +* +* Inputs: timeDiff = elapsed time between present and previous IMU's reading +*/ +template void Madgwick::updateIMU<0,'D'>(float gx, float gy, float gz, float ax, float ay, float az, float timeDiff); +template void Madgwick::updateIMU<1,'D'>(float gx, float gy, float gz, float ax, float ay, float az, float timeDiff); +template void Madgwick::updateIMU<2,'D'>(float gx, float gy, float gz, float ax, float ay, float az, float timeDiff); +template void Madgwick::updateIMU<0,'R'>(float gx, float gy, float gz, float ax, float ay, float az, float timeDiff); +template void Madgwick::updateIMU<1,'R'>(float gx, float gy, float gz, float ax, float ay, float az, float timeDiff); +template void Madgwick::updateIMU<2,'R'>(float gx, float gy, float gz, float ax, float ay, float az, float timeDiff); + +template +void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) { + + float qDot1, qDot2, qDot3, qDot4; + + updateIMUCore(gx, gy, gz, ax, ay, az, qDot1, qDot2, qDot3, qDot4); // Integrate rate of change of quaternion to yield quaternion q0 += qDot1 * invSampleFreq; @@ -139,7 +308,7 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az q3 += qDot4 * invSampleFreq; // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + float recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; @@ -147,19 +316,41 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az anglesComputed = 0; } -//------------------------------------------------------------------------------------------- -// IMU algorithm update +template +void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az, float timeDiff) { + + float qDot1, qDot2, qDot3, qDot4; -void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) { + updateIMUCore(gx, gy, gz, ax, ay, az, qDot1, qDot2, qDot3, qDot4); + + // Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * timeDiff; + q1 += qDot2 * timeDiff; + q2 += qDot3 * timeDiff; + q3 += qDot4 * timeDiff; + + // Normalise quaternion + float recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + anglesComputed = 0; +} + +/*Part of the Madgwick::updateIMU() function to avoid code duplication*/ +template +inline void Madgwick::updateIMUCore(float gx, float gy, float gz, float ax, float ay, float az, float& qDot1, float& qDot2, float& qDot3, float& qDot4) { float recipNorm; float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; - // Convert gyroscope degrees/sec to radians/sec - gx *= 0.0174533f; - gy *= 0.0174533f; - gz *= 0.0174533f; + if constexpr (angle == 'D' && angle != 'R'){ + // Convert gyroscope degrees/sec to radians/sec + gx = deg2rad(gx); + gy = deg2rad(gy); + gz = deg2rad(gz); + } // Rate of change of quaternion from gyroscope qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); @@ -191,11 +382,21 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float q2q2 = q2 * q2; q3q3 = q3 * q3; - // Gradient decent algorithm corrective step - s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; - s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; - s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; - s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; + if constexpr (type == 0 || type == 2) { + // Gradient decent algorithm corrective step (NWU / ENU) + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; + } + else if constexpr (type == 1) { + // Gradient decent algorithm corrective step (NED) + s0 = _4q0 * q2q2 - _2q2 * ax + _4q0 * q1q1 + _2q1 * ay; + s1 = _4q1 * q3q3 + _2q3 * ax + 4.0f * q0q0 * q1 + _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 - _4q1 * az; + s2 = 4.0f * q0q0 * q2 - _2q0 * ax + _4q2 * q3q3 + _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 - _4q2 * az; + s3 = 4.0f * q1q1 * q3 + _2q1 * ax + 4.0f * q2q2 * q3 + _2q2 * ay; + } + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude s0 *= recipNorm; s1 *= recipNorm; @@ -208,20 +409,6 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float qDot3 -= beta * s2; qDot4 -= beta * s3; } - - // Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * invSampleFreq; - q1 += qDot2 * invSampleFreq; - q2 += qDot3 * invSampleFreq; - q3 += qDot4 * invSampleFreq; - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - anglesComputed = 0; } //------------------------------------------------------------------------------------------- @@ -241,11 +428,56 @@ float Madgwick::invSqrt(float x) { //------------------------------------------------------------------------------------------- -void Madgwick::computeAngles() -{ +/*Compute Tait-Bryan angles in ZYX convention*/ +void Madgwick::computeAngles() { roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2); pitch = asinf(-2.0f * (q1*q3 - q0*q2)); yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); anglesComputed = 1; } +/*Get roll angle in degree (Tait-Bryan in ZYX convention)*/ +float Madgwick::getRollDegree() { + if (!anglesComputed) computeAngles(); + return rad2deg(roll); +} + +/*Get pitch angle in degree (Tait-Bryan in ZYX convention)*/ +float Madgwick::getPitchDegree() { + if (!anglesComputed) computeAngles(); + return rad2deg(pitch); +} + +/*Get yaw angle in degree (Tait-Bryan in ZYX convention)*/ +float Madgwick::getYawDegree() { + if (!anglesComputed) computeAngles(); + return rad2deg(yaw); +} + +/*Get roll angle in radians (Tait-Bryan in ZYX convention)*/ +float Madgwick::getRollRadians() { + if (!anglesComputed) computeAngles(); + return roll; +} + +/*Get pitch angle in radians (Tait-Bryan in ZYX convention)*/ +float Madgwick::getPitchRadians() { + if (!anglesComputed) computeAngles(); + return pitch; +} + +/*Get yaw angle in radians (Tait-Bryan in ZYX convention)*/ +float Madgwick::getYawRadians() { + if (!anglesComputed) computeAngles(); + return yaw; +} + +/*Convert degree to radian*/ +inline float Madgwick::deg2rad(float value) { + return value * M_PI / 180.0f; +} + +/*Convert radian to degree*/ +inline float Madgwick::rad2deg(float value) { + return value * 180.0f / M_PI; +} diff --git a/src/MadgwickAHRS.h b/src/MadgwickAHRS.h index 7689523..8104e0d 100644 --- a/src/MadgwickAHRS.h +++ b/src/MadgwickAHRS.h @@ -16,59 +16,117 @@ //============================================================================================= #ifndef MadgwickAHRS_h #define MadgwickAHRS_h -#include -//-------------------------------------------------------------------------------------------- -// Variable declaration -class Madgwick{ -private: - static float invSqrt(float x); - float beta; // algorithm gain - float q0; - float q1; - float q2; - float q3; // quaternion of sensor frame relative to auxiliary frame - float invSampleFreq; - float roll; - float pitch; - float yaw; - char anglesComputed; - void computeAngles(); +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#include //------------------------------------------------------------------------------------------- -// Function declarations +// Definitions + +#define sampleFreqDef 512.0f // sample frequency in Hz +#define betaDef 0.1f // 2 * proportional gain + +class Madgwick{ public: - Madgwick(void); - void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; } + /*Initialize filter parameter (beta) = 0.1 and IMU's update frequency (sampleFrequency) = 512Hz*/ + Madgwick(); + /*Initialize filter parameter (beta)*/ + Madgwick(float beta); + /*Initialize filter parameter (beta) and IMU's update frequency (sampleFrequency)*/ + Madgwick(float beta, float sampleFrequency); + + /*Set filter parameter (beta)*/ + void setBeta(float beta); + /*Set IMU's update frequency (sampleFrequency)*/ + void setFrequency(float sampleFrequency); + /*Getter function to obtain quaternion of body orientation and store it in quat*/ + void getQuaternion(float quat[4]); + + /*AHRS algorithm update with constant update frequency, which fuses gyroscope, accelerometer and magnetometer + * Template parameters: + * type = 0: NWU + * type = 1: NED + * type = 2: ENU + * angle = D : Degree per second (Gyroscope's reading unit) + * angle = R : Radian per second (Gyroscope's reading unit) + */ + template void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); + /*AHRS algorithm update with varying update frequency, which fuses gyroscope, accelerometer and magnetometer + * Template parameters: + * type = 0: NWU + * type = 1: NED + * type = 2: ENU + * angle = D : Degree per second (Gyroscope's reading unit) + * angle = R : Radian per second (Gyroscope's reading unit) + * + * Inputs: timeDiff = elapsed time between present and previous IMU's reading + */ + template + void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float timeDiff); + /*IMU algorithm update with constant update frequency, which fuses gyroscope and accelerometer + * Template parameters: + * type = 0: NWU + * type = 1: NED + * type = 2: ENU + * angle = D : Degree per second (Gyroscope's reading unit) + * angle = R : Radian per second (Gyroscope's reading unit) + */ + template void updateIMU(float gx, float gy, float gz, float ax, float ay, float az); - //float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);}; - //float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);}; - //float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);}; - float getRoll() { - if (!anglesComputed) computeAngles(); - return roll * 57.29578f; - } - float getPitch() { - if (!anglesComputed) computeAngles(); - return pitch * 57.29578f; - } - float getYaw() { - if (!anglesComputed) computeAngles(); - return yaw * 57.29578f + 180.0f; - } - float getRollRadians() { - if (!anglesComputed) computeAngles(); - return roll; - } - float getPitchRadians() { - if (!anglesComputed) computeAngles(); - return pitch; - } - float getYawRadians() { - if (!anglesComputed) computeAngles(); - return yaw; - } + /*IMU algorithm update with varying update frequency, which fuses gyroscope and accelerometer + * Template parameters: + * type = 0: NWU + * type = 1: NED + * type = 2: ENU + * angle = D : Degree per second (Gyroscope's reading unit) + * angle = R : Radian per second (Gyroscope's reading unit) + * + * Inputs: timeDiff = elapsed time between present and previous IMU's reading + */ + template + void updateIMU(float gx, float gy, float gz, float ax, float ay, float az, float timeDiff); + + /*Get roll angle in degree (Tait-Bryan in ZYX convention)*/ + float getRollDegree(); + /*Get pitch angle in degree (Tait-Bryan in ZYX convention)*/ + float getPitchDegree(); + /*Get yaw angle in degree (Tait-Bryan in ZYX convention)*/ + float getYawDegree(); + /*Get roll angle in radians (Tait-Bryan in ZYX convention)*/ + float getRollRadians(); + /*Get pitch angle in radians (Tait-Bryan in ZYX convention)*/ + float getPitchRadians(); + /*Get yaw angle in radians (Tait-Bryan in ZYX convention)*/ + float getYawRadians(); + +private: + float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame (q0 q1 q2 q3) + float beta; // algorithm gain or filter parameter + float invSampleFreq; // reciprocal of IMU's update frequency + float roll; // roll angle (rad) + float pitch; // pitch angle (rad) + float yaw; // yaw angle (rad) + char anglesComputed; // variable to store if angles has been computed; 1 = computed, 0 = not computed + +private: + /*Part of the update() function to avoid code duplication*/ + template + inline void updateCore(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float& qDot1, float& qDot2, float& qDot3, float& qDot4); + /*Part of the updateIMU() function to avoid code duplication*/ + template + inline void updateIMUCore(float gx, float gy, float gz, float ax, float ay, float az, float& qDot1, float& qDot2, float& qDot3, float& qDot4); + /*Fast inverse square-root*/ + static float invSqrt(float x); + /*Compute Tait-Bryan angles in ZYX convention*/ + void computeAngles(); + /*Convert degree to radian*/ + inline static float deg2rad(float value); + /*Convert radian to degree*/ + inline static float rad2deg(float value); }; + #endif From fc23b3ec8bc166fc3afd8762562723869d35b912 Mon Sep 17 00:00:00 2001 From: CK-Explorer <121228064+CK-Explorer@users.noreply.github.com> Date: Sun, 2 Apr 2023 17:08:16 +0800 Subject: [PATCH 2/9] Update Visualize101.ino --- examples/Visualize101/Visualize101.ino | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/examples/Visualize101/Visualize101.ino b/examples/Visualize101/Visualize101.ino index b3f8c79..dc53f69 100644 --- a/examples/Visualize101/Visualize101.ino +++ b/examples/Visualize101/Visualize101.ino @@ -8,11 +8,15 @@ float accelScale, gyroScale; void setup() { Serial.begin(9600); - // start the IMU and filter + // start the IMU CurieIMU.begin(); CurieIMU.setGyroRate(25); CurieIMU.setAccelerometerRate(25); - filter.begin(25); + + //Setting the Madgwick's filter parameter (beta) + filter.setBeta(0.1); + //Setting the IMU update frequency in Hz + filter.setFrequency(25); // Set the accelerometer range to 2 g CurieIMU.setAccelerometerRange(2); @@ -48,12 +52,16 @@ void loop() { gz = convertRawGyro(giz); // update the filter, which computes orientation - filter.updateIMU(gx, gy, gz, ax, ay, az); + // the first template parameter is the local reference frame + // NWU = 0, NED = 1, ENU = 2 + // the second template parameter is the measurement unit of the gyroscope reading + // 'D' = degree per second, 'R' = radians per second + filter.updateIMU<0,'D'>(gx, gy, gz, ax, ay, az); - // print the heading, pitch and roll - roll = filter.getRoll(); - pitch = filter.getPitch(); - heading = filter.getYaw(); + // print the heading, pitch and roll (Tait-Bryan angle in ZYX convention) + roll = filter.getRollDegree(); + pitch = filter.getPitchDegree(); + heading = filter.getYawDegree(); Serial.print("Orientation: "); Serial.print(heading); Serial.print(" "); From 1c24886de0941bbbe18f0d94a7f50b88dc781597 Mon Sep 17 00:00:00 2001 From: CK-Explorer <121228064+CK-Explorer@users.noreply.github.com> Date: Sun, 2 Apr 2023 17:28:21 +0800 Subject: [PATCH 3/9] Added C++ version check --- src/MadgwickAHRS.cpp | 49 +++++++++++++++++++++++++++++++++++++------- 1 file changed, 42 insertions(+), 7 deletions(-) diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp index fe09c7a..ed53f52 100644 --- a/src/MadgwickAHRS.cpp +++ b/src/MadgwickAHRS.cpp @@ -167,7 +167,12 @@ inline void Madgwick::updateCore(float gx, float gy, float gz, float ax, float a float hx, hy; float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; - if constexpr (angle == 'D' && angle != 'R'){ +#if __cplusplus < 201703L + if (angle == 'D' && angle != 'R') +#else + if constexpr (angle == 'D' && angle != 'R') +#endif + { // Convert gyroscope degrees/sec to radians/sec gx = deg2rad(gx); gy = deg2rad(gy); @@ -225,21 +230,36 @@ inline void Madgwick::updateCore(float gx, float gy, float gz, float ax, float a _4bx = 2.0f * _2bx; _4bz = 2.0f * _2bz; - if constexpr (type == 0) { +#if __cplusplus < 201703L + if (type == 0) +#else + if constexpr (type == 0) +#endif + { // Gradient decent algorithm corrective step (NWU Frame) s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); } - else if constexpr (type == 1) { +#if __cplusplus < 201703L + else if (type == 1) +#else + else if constexpr (type == 1) +#endif + { // Gradient decent algorithm corrective step (NED Frame) s0 = _2q2 * (- 2.0f * q1q3 + _2q0q2 - ax) - _2q1 * (- 2.0f * q0q1 - _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s1 = -_2q3 * (- 2.0f * q1q3 + _2q0q2 - ax) - _2q0 * (- 2.0f * q0q1 - _2q2q3 - ay) + 4.0f * q1 * (-1 + 2.0f * q1q1 + 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s2 = _2q0 * (- 2.0f * q1q3 + _2q0q2 - ax) - _2q3 * (- 2.0f * q0q1 - _2q2q3 - ay) + 4.0f * q2 * (-1 + 2.0f * q1q1 + 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s3 = -_2q1 * (- 2.0f * q1q3 + _2q0q2 - ax) - _2q2 * (- 2.0f * q0q1 - _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); } - else if constexpr (type == 2) { +#if __cplusplus < 201703L + else if (type == 2) +#else + else if constexpr (type == 2) +#endif + { // Gradient decent algorithm corrective step (ENU Frame) s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) + (_2bx * q3 - _2bz * q2) * (_2bx * (q1q2 + q0q3) + _2bz * (q1q3 - q0q2) - mx) + _2bz * q1 * (_2bx * (0.5f - q1q1 - q3q3) + _2bz * (q0q1 + q2q3) - my) - _2bx * q1 * (_2bx * (q2q3 - q0q1) + _2bz * (0.5f - q1q1 - q2q2) - mz); s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (_2bx * q2 + _2bz * q3) * (_2bx * (q1q2 + q0q3) + _2bz * (q1q3 - q0q2) - mx) + (-_4bx * q1 + _2bz * q0) * (_2bx * (0.5f - q1q1 - q3q3) + _2bz * (q0q1 + q2q3) - my) + (-_2bx * q0 - _4bz * q1) * (_2bx * (q2q3 - q0q1) + _2bz * (0.5f - q1q1 - q2q2) - mz); @@ -345,7 +365,12 @@ inline void Madgwick::updateIMUCore(float gx, float gy, float gz, float ax, floa float s0, s1, s2, s3; float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; - if constexpr (angle == 'D' && angle != 'R'){ +#if __cplusplus < 201703L + if (angle == 'D' && angle != 'R') +#else + if constexpr (angle == 'D' && angle != 'R') +#endif + { // Convert gyroscope degrees/sec to radians/sec gx = deg2rad(gx); gy = deg2rad(gy); @@ -382,14 +407,24 @@ inline void Madgwick::updateIMUCore(float gx, float gy, float gz, float ax, floa q2q2 = q2 * q2; q3q3 = q3 * q3; - if constexpr (type == 0 || type == 2) { +#if __cplusplus < 201703L + if (type == 0 || type == 2) +#else + if constexpr (type == 0 || type == 2) +#endif + { // Gradient decent algorithm corrective step (NWU / ENU) s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; } - else if constexpr (type == 1) { +#if __cplusplus < 201703L + else if (type == 1) +#else + else if constexpr (type == 1) +#endif + { // Gradient decent algorithm corrective step (NED) s0 = _4q0 * q2q2 - _2q2 * ax + _4q0 * q1q1 + _2q1 * ay; s1 = _4q1 * q3q3 + _2q3 * ax + 4.0f * q0q0 * q1 + _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 - _4q1 * az; From 0f1a83975708b67b51983ebc57b159b6880bff1e Mon Sep 17 00:00:00 2001 From: CK-Explorer <121228064+CK-Explorer@users.noreply.github.com> Date: Sun, 2 Apr 2023 17:36:27 +0800 Subject: [PATCH 4/9] Update PI constant Using PI constant from Arduino.h instead. --- src/MadgwickAHRS.cpp | 4 ++-- src/MadgwickAHRS.h | 4 +--- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp index ed53f52..5c74549 100644 --- a/src/MadgwickAHRS.cpp +++ b/src/MadgwickAHRS.cpp @@ -509,10 +509,10 @@ float Madgwick::getYawRadians() { /*Convert degree to radian*/ inline float Madgwick::deg2rad(float value) { - return value * M_PI / 180.0f; + return value * PI / 180.0f; } /*Convert radian to degree*/ inline float Madgwick::rad2deg(float value) { - return value * 180.0f / M_PI; + return value * 180.0f / PI; } diff --git a/src/MadgwickAHRS.h b/src/MadgwickAHRS.h index 8104e0d..094b5be 100644 --- a/src/MadgwickAHRS.h +++ b/src/MadgwickAHRS.h @@ -17,9 +17,7 @@ #ifndef MadgwickAHRS_h #define MadgwickAHRS_h -#ifndef _USE_MATH_DEFINES -#define _USE_MATH_DEFINES -#endif +#include #include //------------------------------------------------------------------------------------------- From cddf13825c8c7def5250cf8eaf2d55a17c8921a0 Mon Sep 17 00:00:00 2001 From: CK-Explorer <121228064+CK-Explorer@users.noreply.github.com> Date: Sun, 2 Apr 2023 17:48:42 +0800 Subject: [PATCH 5/9] Update Visualize101.ino --- examples/Visualize101/Visualize101.ino | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/examples/Visualize101/Visualize101.ino b/examples/Visualize101/Visualize101.ino index dc53f69..f94468b 100644 --- a/examples/Visualize101/Visualize101.ino +++ b/examples/Visualize101/Visualize101.ino @@ -1,4 +1,5 @@ #include +// for better performance, compile this code using at least C++ 17. #include Madgwick filter; @@ -13,9 +14,9 @@ void setup() { CurieIMU.setGyroRate(25); CurieIMU.setAccelerometerRate(25); - //Setting the Madgwick's filter parameter (beta) + // set the Madgwick's filter parameter (beta) filter.setBeta(0.1); - //Setting the IMU update frequency in Hz + // set the IMU update frequency in Hz filter.setFrequency(25); // Set the accelerometer range to 2 g @@ -34,6 +35,7 @@ void loop() { float ax, ay, az; float gx, gy, gz; float roll, pitch, heading; + float q[4]; unsigned long microsNow; // check if it's time to read data and update the filter @@ -69,6 +71,17 @@ void loop() { Serial.print(" "); Serial.println(roll); + // get and print the quaternion + filter.getQuaternion(q); + Serial.print("Quaternion: "); + Serial.print(q[0]); + Serial.print(" "); + Serial.print(q[1]); + Serial.print(" "); + Serial.print(q[2]); + Serial.print(" "); + Serial.println(q[3]); + // increment previous time, so we keep proper pace microsPrevious = microsPrevious + microsPerReading; } From 3432d10ad2a194c214887b61a382ccc3d49295de Mon Sep 17 00:00:00 2001 From: CK-Explorer <121228064+CK-Explorer@users.noreply.github.com> Date: Sun, 2 Apr 2023 17:52:08 +0800 Subject: [PATCH 6/9] Remove some extra condition check --- src/MadgwickAHRS.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp index 5c74549..0b1086e 100644 --- a/src/MadgwickAHRS.cpp +++ b/src/MadgwickAHRS.cpp @@ -168,7 +168,7 @@ inline void Madgwick::updateCore(float gx, float gy, float gz, float ax, float a float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; #if __cplusplus < 201703L - if (angle == 'D' && angle != 'R') + if (angle == 'D') #else if constexpr (angle == 'D' && angle != 'R') #endif @@ -255,7 +255,7 @@ inline void Madgwick::updateCore(float gx, float gy, float gz, float ax, float a s3 = -_2q1 * (- 2.0f * q1q3 + _2q0q2 - ax) - _2q2 * (- 2.0f * q0q1 - _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); } #if __cplusplus < 201703L - else if (type == 2) + else #else else if constexpr (type == 2) #endif @@ -366,7 +366,7 @@ inline void Madgwick::updateIMUCore(float gx, float gy, float gz, float ax, floa float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; #if __cplusplus < 201703L - if (angle == 'D' && angle != 'R') + if (angle == 'D') #else if constexpr (angle == 'D' && angle != 'R') #endif @@ -420,7 +420,7 @@ inline void Madgwick::updateIMUCore(float gx, float gy, float gz, float ax, floa s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; } #if __cplusplus < 201703L - else if (type == 1) + else #else else if constexpr (type == 1) #endif From 39572c7b8e7e19578e4f41ee1af3cbd5a17ee33f Mon Sep 17 00:00:00 2001 From: CK-Explorer <121228064+CK-Explorer@users.noreply.github.com> Date: Sun, 2 Apr 2023 17:53:57 +0800 Subject: [PATCH 7/9] Added NED and ENU to the ignore word list --- .codespellrc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.codespellrc b/.codespellrc index 101edae..cd432a7 100644 --- a/.codespellrc +++ b/.codespellrc @@ -1,7 +1,7 @@ # See: https://github.com/codespell-project/codespell#using-a-config-file [codespell] # In the event of a false positive, add the problematic word, in all lowercase, to a comma-separated list here: -ignore-words-list = , +ignore-words-list = NED, ENU check-filenames = check-hidden = skip = ./.git From 2b00014f958abf2b0d1348c0d34246316099fa91 Mon Sep 17 00:00:00 2001 From: CK-Explorer <121228064+CK-Explorer@users.noreply.github.com> Date: Sun, 2 Apr 2023 17:57:18 +0800 Subject: [PATCH 8/9] Update keywords.txt --- keywords.txt | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/keywords.txt b/keywords.txt index 9ad144c..cd78cbb 100644 --- a/keywords.txt +++ b/keywords.txt @@ -12,11 +12,17 @@ Madgwick KEYWORD1 # Methods and Functions (KEYWORD2) ####################################### +setBeta KEYWORD2 +setFrequency KEYWORD2 +getQuaternion KEYWORD2 update KEYWORD2 updateIMU KEYWORD2 -getPitch KEYWORD2 -getYaw KEYWORD2 -getRoll KEYWORD2 +getPitchDegree KEYWORD2 +getYawDegree KEYWORD2 +getRollDegree KEYWORD2 +getPitchRadians KEYWORD2 +getYawRadians KEYWORD2 +getRollRadians KEYWORD2 ####################################### From 1ebe2f0900d912bf1032d63610ec832aaf95951f Mon Sep 17 00:00:00 2001 From: CK-Explorer <121228064+CK-Explorer@users.noreply.github.com> Date: Sun, 2 Apr 2023 18:02:09 +0800 Subject: [PATCH 9/9] Update .codespellrc --- .codespellrc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.codespellrc b/.codespellrc index cd432a7..4bf8d40 100644 --- a/.codespellrc +++ b/.codespellrc @@ -1,7 +1,7 @@ # See: https://github.com/codespell-project/codespell#using-a-config-file [codespell] # In the event of a false positive, add the problematic word, in all lowercase, to a comma-separated list here: -ignore-words-list = NED, ENU +ignore-words-list = ned, enu check-filenames = check-hidden = skip = ./.git