..

## MahonyAHRS.cpp

Committer:
b50559
Date:
2015-08-13
Revision:
0:da9dac34fd93
Child:
1:0d456c561eab

### File content as of revision 0:da9dac34fd93:

```// Header files

#include "mbed.h"
#include "MahonyAHRS.h"
#include <math.h>

//---------------------------------------------------------------------------------------------------
// Definitions

//#define sampleFreq  512.0f      // sample frequency in Hz
#define twoKpDef    (2.0f * 0.5f)   // 2 * proportional gain
#define twoKiDef    (2.0f * 0.0f)   // 2 * integral gain
#define PI     3.14159265359f

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

MahonyAHRS::MahonyAHRS(float Freq){

sampleFreq = Freq;

}

float twoKp = twoKpDef;                                            // 2 * proportional gain (Kp)
float twoKi = twoKiDef;                                            // 2 * integral gain (Ki)
float q4 = 1.0f, q5 = 0.0f, q6 = 0.0f, q7 = 0.0f;                  // quaternion of sensor frame relative to auxiliary frame
float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki

float inv_Sqrt(float x);

//====================================================================================================
// Functions

//---------------------------------------------------------------------------------------------------
// AHRS algorithm update

void MahonyAHRS::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float q4q4, q4q5, q4q6, q4q7, q5q5, q5q6, q5q7, q6q6, q6q7, q7q7;
float hx, hy, bx, bz;
float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
float halfex, halfey, halfez;
float qa, qb, qc;

// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MahonyAHRS::updateIMU(gx, gy, gz, ax, ay, az);
return;
}

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

// Normalise accelerometer measurement
recipNorm = inv_Sqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

// Normalise magnetometer measurement
recipNorm = inv_Sqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;

// Auxiliary variables to avoid repeated arithmetic
q4q4 = q4 * q4;
q4q5 = q4 * q5;
q4q6 = q4 * q6;
q4q7 = q4 * q7;
q5q5 = q5 * q5;
q5q6 = q5 * q6;
q5q7 = q5 * q7;
q6q6 = q6 * q6;
q6q7 = q6 * q7;
q7q7 = q7 * q7;

// Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q6q6 - q7q7) + my * (q5q6 - q4q7) + mz * (q5q7 + q4q6));
hy = 2.0f * (mx * (q5q6 + q4q7) + my * (0.5f - q5q5 - q7q7) + mz * (q6q7 - q4q5));
bx = sqrt(hx * hx + hy * hy);
bz = 2.0f * (mx * (q5q7 - q4q6) + my * (q6q7 + q4q5) + mz * (0.5f - q5q5 - q6q6));

// Estimated direction of gravity and magnetic field
halfvx = q5q7 - q4q6;
halfvy = q4q5 + q6q7;
halfvz = q4q4 - 0.5f + q7q7;
halfwx = bx * (0.5f - q6q6 - q7q7) + bz * (q5q7 - q4q6);
halfwy = bx * (q5q6 - q4q7) + bz * (q4q5 + q6q7);
halfwz = bx * (q4q6 + q5q7) + bz * (0.5f - q5q5 - q6q6);

// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);

// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq);    // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx;  // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}

// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}

// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq));     // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q4;
qb = q5;
qc = q6;
q4 += (-qb * gx - qc * gy - q7 * gz);
q5 += (qa * gx + qc * gz - q7 * gy);
q6 += (qa * gy - qb * gz + q7 * gx);
q7 += (qa * gz + qb * gy - qc * gx);

// Normalise quaternion
recipNorm = inv_Sqrt(q4 * q4 + q5 * q5 + q6 * q6 + q7 * q7);
q4 *= recipNorm;
q5 *= recipNorm;
q6 *= recipNorm;
q7 *= recipNorm;
}

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

void MahonyAHRS::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

// Normalise accelerometer measurement
recipNorm = inv_Sqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

// Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = q5 * q7 - q4 * q6;
halfvy = q4 * q5 + q6 * q7;
halfvz = q4 * q4 - 0.5f + q7 * q7;

// Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);

// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq);    // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx;  // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}

// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}

// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq));     // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q4;
qb = q5;
qc = q6;
q4 += (-qb * gx - qc * gy - q7 * gz);
q5 += (qa * gx + qc * gz - q7 * gy);
q6 += (qa * gy - qb * gz + q7 * gx);
q7 += (qa * gz + qb * gy - qc * gx);

// Normalise quaternion
recipNorm = inv_Sqrt(q4 * q4 + q5 * q5 + q6 * q6 + q7 * q7);
q4 *= recipNorm;
q5 *= recipNorm;
q6 *= recipNorm;
q7 *= recipNorm;
}

//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

float inv_Sqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}

void MahonyAHRS::getEuler(){

float gx = 2*(q5*q7 - q4*q6);
float gy = 2 * (q4*q5 + q6*q7);
float gz = q4*q4 - q5*q5 - q6*q6 + q7*q7;

roll = atan(gy / sqrt(gx*gx + gz*gz));
pitch = atan(gx / sqrt(gy*gy + gz*gz));
yaw = atan2(2 * q5 * q6 - 2 * q4 * q7, 2 * q4*q4 + 2 * q5 * q5 - 1);

roll = roll*180/PI;
pitch = pitch*180/PI;
yaw = yaw*180/PI;

if (ceil(roll) - roll <= .5){
roll = ceil(roll);
}
else{
roll = floor(roll);
}

if (ceil(pitch) - pitch <= .5){
pitch = ceil(pitch);
}
else{
pitch = floor(pitch);
}

if (ceil(yaw) - yaw <= .5){
yaw = ceil(yaw);
}
else{
yaw = floor(yaw);
}
}

int16_t MahonyAHRS::getRoll(){
return (int16_t)roll;
}

int16_t MahonyAHRS::getPitch(){
return (int16_t)pitch;
}

int16_t MahonyAHRS::getYaw(){
return (int16_t)yaw;
}

//====================================================================================================
// END OF CODE
//====================================================================================================

```