altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

MadgwickAHRS.h@0:6661e1395e30, 2018-10-25 (annotated)

Committer:
altb
Date:
Thu Oct 25 15:04:30 2018 +0000
Revision:
0:6661e1395e30
Child:
3:6811c0ce95f6
AHRS Lib for Madgwick filter

Who changed what in which revision?

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