Skip to content

Allow flexible update intervals #53

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Allow flexible update intervals #53

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9613a9b
Apply Dr.Seo's idea
monolith-jaehoon Aug 25, 2023
4e57e20
Merge *Dt methods to original function
monolith-jaehoon Aug 25, 2023
a64eacf
Modify default logic for using deltatime
monolith-jaehoon Aug 26, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 34 additions & 11 deletions src/MadgwickAHRS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

#include "MadgwickAHRS.h"
#include <math.h>
#include <time.h>

//-------------------------------------------------------------------------------------------
// Definitions
Expand All @@ -41,11 +42,13 @@ 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) {
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;
Expand Down Expand Up @@ -133,10 +136,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);
Expand All @@ -147,10 +150,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) {
update(gx, gy, gz, ax, ay, az, mx, my, mz, (*this.*getDt)());
}

//-------------------------------------------------------------------------------------------
// IMU algorithm update

void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
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;
Expand Down Expand Up @@ -210,10 +217,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);
Expand All @@ -224,6 +231,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) {
updateIMU(gx, gy, gz, ax, ay, az, (*this.*getDt)());
}

//-------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
Expand All @@ -249,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;
}
12 changes: 11 additions & 1 deletion src/MadgwickAHRS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#ifndef MadgwickAHRS_h
#define MadgwickAHRS_h
#include <math.h>
#include <time.h>

//--------------------------------------------------------------------------------------------
// Variable declaration
Expand All @@ -34,13 +35,22 @@ 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);
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);};
Expand Down
Loading