Skip to content

Added getYPR, getYPRRadians and getQuaternion. Possible usage for variable integration steps. #18

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 2 commits into
base: master
Choose a base branch
from
Open

Added getYPR, getYPRRadians and getQuaternion. Possible usage for variable integration steps. #18

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f6d703d
Added getYPR, getYPRRadians and getQuaternion methods
afrancescon Nov 25, 2017
e7cbef9
Added methods to be used for variable integration time
afrancescon Nov 26, 2017
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
4 changes: 3 additions & 1 deletion keywords.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,9 @@ updateIMU KEYWORD2
getPitch KEYWORD2
getYaw KEYWORD2
getRoll KEYWORD2

getYPR KEYWORD2
getYPRRadians KEYWORD2
getQuaternion KEYWORD2

#######################################
# Constants (LITERAL1)
Expand Down
52 changes: 43 additions & 9 deletions src/MadgwickAHRS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,10 +42,20 @@ Madgwick::Madgwick() {
q2 = 0.0f;
q3 = 0.0f;
invSampleFreq = 1.0f / sampleFreqDef;
lastUpdateMillis = 0;
anglesComputed = 0;
}

void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
innerUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, invSampleFreq);
}

void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, unsigned long currentMillis) {
float ellapsedTime = computeEllapsedTime(currentMillis);
innerUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, ellapsedTime);
}

void Madgwick::innerUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float ellapsedTime) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
Expand All @@ -54,7 +64,7 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az

// 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);
innerUpdateIMU(gx, gy, gz, ax, ay, az, ellapsedTime);
return;
}

Expand Down Expand Up @@ -133,10 +143,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 * ellapsedTime;
q1 += qDot2 * ellapsedTime;
q2 += qDot3 * ellapsedTime;
q3 += qDot4 * ellapsedTime;

// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
Expand All @@ -151,6 +161,15 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az
// IMU algorithm update

void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
innerUpdateIMU(gx, gy, gz, ax, ay, az, invSampleFreq);
}

void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az, unsigned long currentMillis) {
float ellapsedTime = computeEllapsedTime(currentMillis);
innerUpdateIMU(gx, gy, gz, ax, ay, az, ellapsedTime);
}

void Madgwick::innerUpdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float ellapsedTime) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
Expand Down Expand Up @@ -210,10 +229,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 * ellapsedTime;
q1 += qDot2 * ellapsedTime;
q2 += qDot3 * ellapsedTime;
q3 += qDot4 * ellapsedTime;

// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
Expand Down Expand Up @@ -249,3 +268,18 @@ void Madgwick::computeAngles()
anglesComputed = 1;
}

//-------------------------------------------------------------------------------------------

float Madgwick::computeEllapsedTime(unsigned long currentMillis)
{
float ellapsedTime = 0.0f;

if (lastUpdateMillis != 0) {
ellapsedTime = (currentMillis - lastUpdateMillis) / 1000.0f;
}

lastUpdateMillis = currentMillis;

return ellapsedTime;
}

24 changes: 24 additions & 0 deletions src/MadgwickAHRS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,19 +29,25 @@ class Madgwick{
float q2;
float q3; // quaternion of sensor frame relative to auxiliary frame
float invSampleFreq;
unsigned long lastUpdateMillis;
float roll;
float pitch;
float yaw;
char anglesComputed;
void innerUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float ellapsedTime);
void innerUpdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float ellapsedTime);
void computeAngles();
float computeEllapsedTime(unsigned long currentMillis);

//-------------------------------------------------------------------------------------------
// Function declarations
public:
Madgwick(void);
void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, unsigned long currentMillis);
void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
void updateIMU(float gx, float gy, float gz, float ax, float ay, float az, unsigned long currentMillis);
//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);};
Expand All @@ -57,6 +63,12 @@ class Madgwick{
if (!anglesComputed) computeAngles();
return yaw * 57.29578f + 180.0f;
}
void getRPY(float * _roll, float * _pitch, float * _yaw) {
if (!anglesComputed) computeAngles();
* _roll = roll * 57.29578f;
* _pitch = pitch * 57.29578f;
* _yaw = yaw * 57.29578f + 180.0f;
}
float getRollRadians() {
if (!anglesComputed) computeAngles();
return roll;
Expand All @@ -69,6 +81,18 @@ class Madgwick{
if (!anglesComputed) computeAngles();
return yaw;
}
void getRPYRadians(float * _roll, float * _pitch, float * _yaw) {
if (!anglesComputed) computeAngles();
* _roll = roll;
* _pitch = pitch;
* _yaw = yaw;
}
void getQuaternion(float * _q0, float * _q1, float * _q2, float * _q3) {
* _q0 = q0;
* _q1 = q1;
* _q2 = q2;
* _q3 = q3;
}
};
#endif