AHRS
Dependencies: Eigen
Mahony.h
- Committer:
- altb
- Date:
- 2018-12-04
- Revision:
- 3:6811c0ce95f6
- Parent:
- 1:36bbe04e1f6f
File content as of revision 3:6811c0ce95f6:
//============================================================================================= // 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; static float invSqrt(float x); //------------------------------------------------------------------------------------------- // Function declarations public: Mahony(float); 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); char anglesComputed; void computeAngles(); 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