AHRS Library
Embed:
(wiki syntax)
Show/hide line numbers
MadgwickAHRS.h
00001 //============================================================================================= 00002 // MadgwickAHRS.h 00003 //============================================================================================= 00004 // 00005 // Implementation of Madgwick's IMU and AHRS algorithms. 00006 // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 00007 // 00008 // From the x-io website "Open-source resources available on this website are 00009 // provided under the GNU General Public Licence unless an alternative licence 00010 // is provided in source." 00011 // 00012 // Date Author Notes 00013 // 29/09/2011 SOH Madgwick Initial release 00014 // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 00015 // 00016 //============================================================================================= 00017 #ifndef MadgwickAHRS_h 00018 #define MadgwickAHRS_h 00019 #include <math.h> 00020 00021 //-------------------------------------------------------------------------------------------- 00022 // Variable declaration 00023 class Madgwick{ 00024 private: 00025 static float invSqrt(float x); 00026 float beta; // algorithm gain 00027 float q0; 00028 float q1; 00029 float q2; 00030 float q3; // quaternion of sensor frame relative to auxiliary frame 00031 //float invSampleFreq; 00032 float roll; 00033 float pitch; 00034 float yaw; 00035 char anglesComputed; 00036 void computeAngles(); 00037 00038 //------------------------------------------------------------------------------------------- 00039 // Function declarations 00040 public: 00041 float invSampleFreq; 00042 Madgwick(float); 00043 void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; } 00044 void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); 00045 void updateIMU(float gx, float gy, float gz, float ax, float ay, float az); 00046 //float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);}; 00047 //float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);}; 00048 //float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);}; 00049 float getRoll() { 00050 if (!anglesComputed) computeAngles(); 00051 return roll * 57.29578f; 00052 } 00053 float getPitch() { 00054 if (!anglesComputed) computeAngles(); 00055 return pitch * 57.29578f; 00056 } 00057 float getYaw() { 00058 if (!anglesComputed) computeAngles(); 00059 return yaw * 57.29578f + 180.0f; 00060 } 00061 float getRollRadians() { 00062 if (!anglesComputed) computeAngles(); 00063 return roll; 00064 } 00065 float getPitchRadians() { 00066 if (!anglesComputed) computeAngles(); 00067 return pitch; 00068 } 00069 float getYawRadians() { 00070 if (!anglesComputed) computeAngles(); 00071 return yaw; 00072 } 00073 }; 00074 #endif
Generated on Sun Jul 17 2022 16:11:38 by 1.7.2