library using mahony's algorithm for 9dof
Dependents: NerfGun_nRF24L01P_TX_9d0f
Diff: MahonyAHRS.cpp
- Revision:
- 0:da9dac34fd93
--- /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 +//====================================================================================================