AHRS
Dependencies: Eigen
Diff: Mahony.h
- Revision:
- 1:36bbe04e1f6f
- Child:
- 3:6811c0ce95f6
diff -r 6661e1395e30 -r 36bbe04e1f6f Mahony.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Mahony.h Fri Oct 26 05:56:25 2018 +0000 @@ -0,0 +1,73 @@ + +//============================================================================================= +// Mahony.h +//============================================================================================= +// +// Madgwick's implementation of Mayhony's AHRS algorithm. +// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ +// +// Date Author Notes +// 29/09/2011 SOH Madgwick Initial release +// 02/10/2011 SOH Madgwick Optimised for reduced CPU load +// +//============================================================================================= +#ifndef __Mahony_h__ +#define __Mahony_h__ +#include <math.h> + +//-------------------------------------------------------------------------------------------- +// Variable declaration + +class Mahony { +private: + float twoKp; // 2 * proportional gain (Kp) + float twoKi; // 2 * integral gain (Ki) + float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame + float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki + float invSampleFreq; + float roll, pitch, yaw; + char anglesComputed; + static float invSqrt(float x); + void computeAngles(); + +//------------------------------------------------------------------------------------------- +// Function declarations + +public: + Mahony(); + 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 updateIMU(float gx, float gy, float gz, float ax, float ay, float az); + float getRoll() { + if (!anglesComputed) computeAngles(); + return roll * 57.29578f; + } + float getPitch() { + if (!anglesComputed) computeAngles(); + return pitch * 57.29578f; + } + float getYaw() { + if (!anglesComputed) computeAngles(); + return yaw * 57.29578f + 180.0f; + } + float getRollRadians() { + if (!anglesComputed) computeAngles(); + return roll; + } + float getPitchRadians() { + if (!anglesComputed) computeAngles(); + return pitch; + } + float getYawRadians() { + if (!anglesComputed) computeAngles(); + return yaw; + } + void getQuaternion(float *w, float *x, float *y, float *z) { + *w = q0; + *x = q1; + *y = q2; + *z = q3; + } +}; + +#endif \ No newline at end of file