Revision 0:da9dac34fd93, committed 2015-08-13

Comitter:

b50559

Date:

Thu Aug 13 22:12:39 2015 +0000

Commit message:

created library for mahony's ahrs algorithm

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MahonyAHRS.cpp	Thu Aug 13 22:12:39 2015 +0000
@@ -0,0 +1,272 @@
+// 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
+//====================================================================================================

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MahonyAHRS.h	Thu Aug 13 22:12:39 2015 +0000
@@ -0,0 +1,45 @@
+//=====================================================================================================
+// MahonyAHRS.h
+//=====================================================================================================
+//
+// Madgwick's implementation of Mayhony's AHRS algorithm.
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+//
+// Date         Author          Notes
+// 29/09/2011   SOH Madgwick    Initial release
+// 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
+//
+//=====================================================================================================
+#ifndef MahonyAHRS_h
+#define MahonyAHRS_h
+
+//----------------------------------------------------------------------------------------------------
+
+class MahonyAHRS{
+    
+public:
+
+    MahonyAHRS(float Freq);
+    
+    //Function declarations
+    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);
+    void getEuler();
+    int16_t getRoll();
+    int16_t getPitch();
+    int16_t getYaw();
+    
+    
+private:
+    
+    // Variable declaration
+    float sampleFreq;
+    float roll;
+    float pitch;
+    float yaw;
+};
+
+#endif
+//=====================================================================================================
+// End of file
+//=====================================================================================================