diff --git a/.codespellrc b/.codespellrc index 101edae..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 = , +ignore-words-list = ned, enu check-filenames = check-hidden = skip = ./.git diff --git a/examples/Visualize101/Visualize101.ino b/examples/Visualize101/Visualize101.ino index b3f8c79..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; @@ -8,11 +9,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); + + // set the Madgwick's filter parameter (beta) + filter.setBeta(0.1); + // set the IMU update frequency in Hz + filter.setFrequency(25); // Set the accelerometer range to 2 g CurieIMU.setAccelerometerRange(2); @@ -30,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 @@ -48,12 +54,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(" "); @@ -61,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; } 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 ####################################### diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp index ef2bbce..0b1086e 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,149 @@ 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 __cplusplus < 201703L + if (angle == 'D') +#else + if constexpr (angle == 'D' && angle != 'R') +#endif + { + // 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 +230,43 @@ 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 __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); + } +#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); + } +#if __cplusplus < 201703L + else +#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); + 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 +279,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 +328,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 +336,46 @@ 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 __cplusplus < 201703L + if (angle == 'D') +#else + if constexpr (angle == 'D' && angle != 'R') +#endif + { + // 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 +407,31 @@ 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 __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; + } +#if __cplusplus < 201703L + else +#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; + 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 +444,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 +463,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 * PI / 180.0f; +} + +/*Convert radian to degree*/ +inline float Madgwick::rad2deg(float value) { + return value * 180.0f / PI; +} diff --git a/src/MadgwickAHRS.h b/src/MadgwickAHRS.h index 7689523..094b5be 100644 --- a/src/MadgwickAHRS.h +++ b/src/MadgwickAHRS.h @@ -16,59 +16,115 @@ //============================================================================================= #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(); +#include +#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