From 9613a9bbef5f15dba0faf7530822c5ccfe08062a Mon Sep 17 00:00:00 2001 From: Jaehoon Jeong Date: Fri, 25 Aug 2023 16:03:44 +0900 Subject: [PATCH 1/3] Apply Dr.Seo's idea --- keywords.txt | 2 ++ src/MadgwickAHRS.cpp | 28 ++++++++++++++++++---------- src/MadgwickAHRS.h | 2 ++ 3 files changed, 22 insertions(+), 10 deletions(-) diff --git a/keywords.txt b/keywords.txt index 9ad144c..494935d 100644 --- a/keywords.txt +++ b/keywords.txt @@ -13,7 +13,9 @@ Madgwick KEYWORD1 ####################################### update KEYWORD2 +updateDt KEYWORD2 updateIMU KEYWORD2 +updateIMUDt KEYWORD2 getPitch KEYWORD2 getYaw KEYWORD2 getRoll KEYWORD2 diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp index ef2bbce..cc02d15 100644 --- a/src/MadgwickAHRS.cpp +++ b/src/MadgwickAHRS.cpp @@ -45,7 +45,7 @@ Madgwick::Madgwick() { anglesComputed = 0; } -void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { +void Madgwick::updateDt(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt) { float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; @@ -133,10 +133,10 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az } // Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * invSampleFreq; - q1 += qDot2 * invSampleFreq; - q2 += qDot3 * invSampleFreq; - q3 += qDot4 * invSampleFreq; + q0 += qDot1 * dt; + q1 += qDot2 * dt; + q2 += qDot3 * dt; + q3 += qDot4 * dt; // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); @@ -147,10 +147,14 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az anglesComputed = 0; } +void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { + updateDt(gx, gy, gz, ax, ay, az, mx, my, mz, invSampleFreq); +} + //------------------------------------------------------------------------------------------- // IMU algorithm update -void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) { +void Madgwick::updateIMUDt(float gx, float gy, float gz, float ax, float ay, float az, float dt) { float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; @@ -210,10 +214,10 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float } // Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * invSampleFreq; - q1 += qDot2 * invSampleFreq; - q2 += qDot3 * invSampleFreq; - q3 += qDot4 * invSampleFreq; + q0 += qDot1 * dt; + q1 += qDot2 * dt; + q2 += qDot3 * dt; + q3 += qDot4 * dt; // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); @@ -224,6 +228,10 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float anglesComputed = 0; } +void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) { + updateIMUDt(gx, gy, gz, ax, ay, az, invSampleFreq); +} + //------------------------------------------------------------------------------------------- // Fast inverse square-root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root diff --git a/src/MadgwickAHRS.h b/src/MadgwickAHRS.h index 7689523..50976bf 100644 --- a/src/MadgwickAHRS.h +++ b/src/MadgwickAHRS.h @@ -40,7 +40,9 @@ class Madgwick{ public: Madgwick(void); void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; } + void updateDt(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt); void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); + void updateIMUDt(float gx, float gy, float gz, float ax, float ay, float az, float dt); 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);}; From 4e57e20c15719d5c409cf509d6bfc919bd48ae8c Mon Sep 17 00:00:00 2001 From: Jaehoon Jeong Date: Fri, 25 Aug 2023 18:24:57 +0900 Subject: [PATCH 2/3] Merge *Dt methods to original function --- keywords.txt | 2 -- src/MadgwickAHRS.cpp | 8 ++++---- src/MadgwickAHRS.h | 4 ++-- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/keywords.txt b/keywords.txt index 494935d..9ad144c 100644 --- a/keywords.txt +++ b/keywords.txt @@ -13,9 +13,7 @@ Madgwick KEYWORD1 ####################################### update KEYWORD2 -updateDt KEYWORD2 updateIMU KEYWORD2 -updateIMUDt KEYWORD2 getPitch KEYWORD2 getYaw KEYWORD2 getRoll KEYWORD2 diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp index cc02d15..5ac72fe 100644 --- a/src/MadgwickAHRS.cpp +++ b/src/MadgwickAHRS.cpp @@ -45,7 +45,7 @@ Madgwick::Madgwick() { anglesComputed = 0; } -void Madgwick::updateDt(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt) { +void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt) { float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; @@ -148,13 +148,13 @@ void Madgwick::updateDt(float gx, float gy, float gz, float ax, float ay, float } void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { - updateDt(gx, gy, gz, ax, ay, az, mx, my, mz, invSampleFreq); + update(gx, gy, gz, ax, ay, az, mx, my, mz, invSampleFreq); } //------------------------------------------------------------------------------------------- // IMU algorithm update -void Madgwick::updateIMUDt(float gx, float gy, float gz, float ax, float ay, float az, float dt) { +void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt) { float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; @@ -229,7 +229,7 @@ void Madgwick::updateIMUDt(float gx, float gy, float gz, float ax, float ay, flo } void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) { - updateIMUDt(gx, gy, gz, ax, ay, az, invSampleFreq); + updateIMU(gx, gy, gz, ax, ay, az, invSampleFreq); } //------------------------------------------------------------------------------------------- diff --git a/src/MadgwickAHRS.h b/src/MadgwickAHRS.h index 50976bf..ba58142 100644 --- a/src/MadgwickAHRS.h +++ b/src/MadgwickAHRS.h @@ -40,9 +40,9 @@ class Madgwick{ public: Madgwick(void); void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; } - void updateDt(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt); + void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt); void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); - void updateIMUDt(float gx, float gy, float gz, float ax, float ay, float az, float dt); + void updateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt); 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);}; From a64eacfe444d8d45b562fab8d53ded8565318c9f Mon Sep 17 00:00:00 2001 From: Jaehoon Jeong Date: Sat, 26 Aug 2023 12:32:48 +0900 Subject: [PATCH 3/3] Modify default logic for using deltatime --- src/MadgwickAHRS.cpp | 21 ++++++++++++++++++--- src/MadgwickAHRS.h | 10 +++++++++- 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp index 5ac72fe..c95426d 100644 --- a/src/MadgwickAHRS.cpp +++ b/src/MadgwickAHRS.cpp @@ -21,6 +21,7 @@ #include "MadgwickAHRS.h" #include +#include //------------------------------------------------------------------------------------------- // Definitions @@ -41,8 +42,10 @@ Madgwick::Madgwick() { q1 = 0.0f; q2 = 0.0f; q3 = 0.0f; - invSampleFreq = 1.0f / sampleFreqDef; + invSampleFreq = 0; anglesComputed = 0; + last_clock = clock(); + getDt = &Madgwick::calculateDt; } void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt) { @@ -148,7 +151,7 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az } void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { - update(gx, gy, gz, ax, ay, az, mx, my, mz, invSampleFreq); + update(gx, gy, gz, ax, ay, az, mx, my, mz, (*this.*getDt)()); } //------------------------------------------------------------------------------------------- @@ -229,7 +232,7 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float } void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) { - updateIMU(gx, gy, gz, ax, ay, az, invSampleFreq); + updateIMU(gx, gy, gz, ax, ay, az, (*this.*getDt)()); } //------------------------------------------------------------------------------------------- @@ -257,3 +260,15 @@ void Madgwick::computeAngles() anglesComputed = 1; } +float Madgwick::calculateDt() +{ + clock_t now = clock(); + float result = double(now - last_clock) / CLOCKS_PER_SEC; + last_clock = now; + return result; +} + +float Madgwick::getInvSampleFreq() +{ + return invSampleFreq; +} diff --git a/src/MadgwickAHRS.h b/src/MadgwickAHRS.h index ba58142..e422bb9 100644 --- a/src/MadgwickAHRS.h +++ b/src/MadgwickAHRS.h @@ -17,6 +17,7 @@ #ifndef MadgwickAHRS_h #define MadgwickAHRS_h #include +#include //-------------------------------------------------------------------------------------------- // Variable declaration @@ -34,12 +35,19 @@ class Madgwick{ float yaw; char anglesComputed; void computeAngles(); + clock_t last_clock; + float (Madgwick::*getDt)(); + float calculateDt(); + float getInvSampleFreq(); //------------------------------------------------------------------------------------------- // Function declarations public: Madgwick(void); - void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; } + void begin(float sampleFrequency) { + invSampleFreq = 1.0f / sampleFrequency; + getDt = &Madgwick::getInvSampleFreq; + } void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt); void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); void updateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt);