Yukina Yamakawa / Madgwick

MadgwickFilterコピペ

Dependents:   201903_pf_lower_judgment_sashida 201903_pf_lower_judgment 201903_pf_lower_judgment

MadgwickAHRS.h@0:c32a5fb80d4d, 2018-12-11 (annotated)

Committer:
Yukina
Date:
Tue Dec 11 13:59:10 2018 +0000
Revision:
0:c32a5fb80d4d
Madgwick Filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yukina 0:c32a5fb80d4d 1 //=============================================================================================
Yukina 0:c32a5fb80d4d 2 // MadgwickAHRS.h
Yukina 0:c32a5fb80d4d 3 //=============================================================================================
Yukina 0:c32a5fb80d4d 4 //
Yukina 0:c32a5fb80d4d 5 // Implementation of Madgwick's IMU and AHRS algorithms.
Yukina 0:c32a5fb80d4d 6 // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
Yukina 0:c32a5fb80d4d 7 //
Yukina 0:c32a5fb80d4d 8 // From the x-io website "Open-source resources available on this website are
Yukina 0:c32a5fb80d4d 9 // provided under the GNU General Public Licence unless an alternative licence
Yukina 0:c32a5fb80d4d 10 // is provided in source."
Yukina 0:c32a5fb80d4d 11 //
Yukina 0:c32a5fb80d4d 12 // Date Author Notes
Yukina 0:c32a5fb80d4d 13 // 29/09/2011 SOH Madgwick Initial release
Yukina 0:c32a5fb80d4d 14 // 02/10/2011 SOH Madgwick Optimised for reduced CPU load
Yukina 0:c32a5fb80d4d 15 //
Yukina 0:c32a5fb80d4d 16 //=============================================================================================
Yukina 0:c32a5fb80d4d 17 #ifndef MadgwickAHRS_h
Yukina 0:c32a5fb80d4d 18 #define MadgwickAHRS_h
Yukina 0:c32a5fb80d4d 19 #include <math.h>
Yukina 0:c32a5fb80d4d 20
Yukina 0:c32a5fb80d4d 21 //--------------------------------------------------------------------------------------------
Yukina 0:c32a5fb80d4d 22 // Variable declaration
Yukina 0:c32a5fb80d4d 23 class Madgwick{
Yukina 0:c32a5fb80d4d 24 private:
Yukina 0:c32a5fb80d4d 25 static float invSqrt(float x);
Yukina 0:c32a5fb80d4d 26 float beta; // algorithm gain
Yukina 0:c32a5fb80d4d 27 float q0;
Yukina 0:c32a5fb80d4d 28 float q1;
Yukina 0:c32a5fb80d4d 29 float q2;
Yukina 0:c32a5fb80d4d 30 float q3; // quaternion of sensor frame relative to auxiliary frame
Yukina 0:c32a5fb80d4d 31 float invSampleFreq;
Yukina 0:c32a5fb80d4d 32 float roll;
Yukina 0:c32a5fb80d4d 33 float pitch;
Yukina 0:c32a5fb80d4d 34 float yaw;
Yukina 0:c32a5fb80d4d 35 char anglesComputed;
Yukina 0:c32a5fb80d4d 36 void computeAngles();
Yukina 0:c32a5fb80d4d 37
Yukina 0:c32a5fb80d4d 38 //-------------------------------------------------------------------------------------------
Yukina 0:c32a5fb80d4d 39 // Function declarations
Yukina 0:c32a5fb80d4d 40 public:
Yukina 0:c32a5fb80d4d 41 Madgwick(void);
Yukina 0:c32a5fb80d4d 42 void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
Yukina 0:c32a5fb80d4d 43 void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
Yukina 0:c32a5fb80d4d 44 void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
Yukina 0:c32a5fb80d4d 45 //float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);};
Yukina 0:c32a5fb80d4d 46 //float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);};
Yukina 0:c32a5fb80d4d 47 //float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
Yukina 0:c32a5fb80d4d 48 float getRoll() {
Yukina 0:c32a5fb80d4d 49 if (!anglesComputed) computeAngles();
Yukina 0:c32a5fb80d4d 50 return roll * 57.29578f;
Yukina 0:c32a5fb80d4d 51 }
Yukina 0:c32a5fb80d4d 52 float getPitch() {
Yukina 0:c32a5fb80d4d 53 if (!anglesComputed) computeAngles();
Yukina 0:c32a5fb80d4d 54 return pitch * 57.29578f;
Yukina 0:c32a5fb80d4d 55 }
Yukina 0:c32a5fb80d4d 56 float getYaw() {
Yukina 0:c32a5fb80d4d 57 if (!anglesComputed) computeAngles();
Yukina 0:c32a5fb80d4d 58 return yaw * 57.29578f + 180.0f;
Yukina 0:c32a5fb80d4d 59 }
Yukina 0:c32a5fb80d4d 60 float getRollRadians() {
Yukina 0:c32a5fb80d4d 61 if (!anglesComputed) computeAngles();
Yukina 0:c32a5fb80d4d 62 return roll;
Yukina 0:c32a5fb80d4d 63 }
Yukina 0:c32a5fb80d4d 64 float getPitchRadians() {
Yukina 0:c32a5fb80d4d 65 if (!anglesComputed) computeAngles();
Yukina 0:c32a5fb80d4d 66 return pitch;
Yukina 0:c32a5fb80d4d 67 }
Yukina 0:c32a5fb80d4d 68 float getYawRadians() {
Yukina 0:c32a5fb80d4d 69 if (!anglesComputed) computeAngles();
Yukina 0:c32a5fb80d4d 70 return yaw;
Yukina 0:c32a5fb80d4d 71 }
Yukina 0:c32a5fb80d4d 72 };
Yukina 0:c32a5fb80d4d 73 #endif
Yukina 0:c32a5fb80d4d 74