AHRS
Dependencies: Eigen
MadgwickAHRS.h@3:6811c0ce95f6, 2018-12-04 (annotated)
- Committer:
- altb
- Date:
- Tue Dec 04 15:49:48 2018 +0000
- Revision:
- 3:6811c0ce95f6
- Parent:
- 0:6661e1395e30
AHRS Klasse mit Mahony filter etc
Who changed what in which revision?
User | Revision | Line number | New 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 | 3:6811c0ce95f6 | 42 | Madgwick(float); |
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 |