AHRS
Dependencies: Eigen
Diff: Mahony.cpp
- Revision:
- 1:36bbe04e1f6f
- Child:
- 3:6811c0ce95f6
diff -r 6661e1395e30 -r 36bbe04e1f6f Mahony.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Mahony.cpp Fri Oct 26 05:56:25 2018 +0000 @@ -0,0 +1,275 @@ +//============================================================================================= +// Mahony.c +//============================================================================================= +// +// Madgwick's implementation of Mayhony's AHRS algorithm. +// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ +// +// From the x-io website "Open-source resources available on this website are +// provided under the GNU General Public Licence unless an alternative licence +// is provided in source." +// +// Date Author Notes +// 29/09/2011 SOH Madgwick Initial release +// 02/10/2011 SOH Madgwick Optimised for reduced CPU load +// +// Algorithm paper: +// http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934 +// +//============================================================================================= + +//------------------------------------------------------------------------------------------- +// Header files + +#include "Mahony.h" +#include <math.h> + +//------------------------------------------------------------------------------------------- +// Definitions + +#define DEFAULT_SAMPLE_FREQ 100.0f // sample frequency in Hz +#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain +#define twoKiDef (2.0f * 0.0f) // 2 * integral gain + + +//============================================================================================ +// Functions + +//------------------------------------------------------------------------------------------- +// AHRS algorithm update + +Mahony::Mahony() +{ + twoKp = twoKpDef; // 2 * proportional gain (Kp) + twoKi = twoKiDef; // 2 * integral gain (Ki) + q0 = 1.0f; + q1 = 0.0f; + q2 = 0.0f; + q3 = 0.0f; + integralFBx = 0.0f; + integralFBy = 0.0f; + integralFBz = 0.0f; + anglesComputed = 0; + invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ; +} + +void Mahony::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) +{ + float recipNorm; + float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + 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)) { + updateIMU(gx, gy, gz, ax, ay, az); + return; + } + + // Convert gyroscope degrees/sec to radians/sec +// gx *= 0.0174533f; +// gy *= 0.0174533f; +// gz *= 0.0174533f; + + // 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 = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Normalise magnetometer measurement + recipNorm = invSqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; + + // Reference direction of Earth's magnetic field + hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); + hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); + bx = sqrtf(hx * hx + hy * hy); + bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); + + // Estimated direction of gravity and magnetic field + halfvx = q1q3 - q0q2; + halfvy = q0q1 + q2q3; + halfvz = q0q0 - 0.5f + q3q3; + halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); + halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); + halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); + + // 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) { + // integral error scaled by Ki + integralFBx += twoKi * halfex * invSampleFreq; + integralFBy += twoKi * halfey * invSampleFreq; + integralFBz += twoKi * halfez * invSampleFreq; + 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 * invSampleFreq); // pre-multiply common factors + gy *= (0.5f * invSampleFreq); + gz *= (0.5f * invSampleFreq); + qa = q0; + qb = q1; + qc = q2; + q0 += (-qb * gx - qc * gy - q3 * gz); + q1 += (qa * gx + qc * gz - q3 * gy); + q2 += (qa * gy - qb * gz + q3 * gx); + q3 += (qa * gz + qb * gy - qc * gx); + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + anglesComputed = 0; +} + +//------------------------------------------------------------------------------------------- +// IMU algorithm update + +void Mahony::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; + + // Convert gyroscope degrees/sec to radians/sec + // gx *= 0.0174533f; + // gy *= 0.0174533f; + // gz *= 0.0174533f; + + // 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 = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Estimated direction of gravity + halfvx = q1 * q3 - q0 * q2; + halfvy = q0 * q1 + q2 * q3; + halfvz = q0 * q0 - 0.5f + q3 * q3; + + // 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) { + // integral error scaled by Ki + integralFBx += twoKi * halfex * invSampleFreq; + integralFBy += twoKi * halfey * invSampleFreq; + integralFBz += twoKi * halfez * invSampleFreq; + 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 * invSampleFreq); // pre-multiply common factors + gy *= (0.5f * invSampleFreq); + gz *= (0.5f * invSampleFreq); + qa = q0; + qb = q1; + qc = q2; + q0 += (-qb * gx - qc * gy - q3 * gz); + q1 += (qa * gx + qc * gz - q3 * gy); + q2 += (qa * gy - qb * gz + q3 * gx); + q3 += (qa * gz + qb * gy - qc * gx); + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + anglesComputed = 0; +} + +//------------------------------------------------------------------------------------------- +// Fast inverse square-root +// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root + +float Mahony::invSqrt(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)); + y = y * (1.5f - (halfx * y * y)); + return y; +} + +//------------------------------------------------------------------------------------------- + +void Mahony::computeAngles() +{ + roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2); + pitch = asinf(-2.0f * (q1*q3 - q0*q2)); + yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); + anglesComputed = 1; +} + + +//============================================================================================ +// END OF CODE +//============================================================================================ +