AHRS
Dependencies: Eigen
Mahony.h@1:36bbe04e1f6f, 2018-10-26 (annotated)
- Committer:
- altb
- Date:
- Fri Oct 26 05:56:25 2018 +0000
- Revision:
- 1:36bbe04e1f6f
- Child:
- 3:6811c0ce95f6
Added Mahony Class
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb | 1:36bbe04e1f6f | 1 | |
altb | 1:36bbe04e1f6f | 2 | //============================================================================================= |
altb | 1:36bbe04e1f6f | 3 | // Mahony.h |
altb | 1:36bbe04e1f6f | 4 | //============================================================================================= |
altb | 1:36bbe04e1f6f | 5 | // |
altb | 1:36bbe04e1f6f | 6 | // Madgwick's implementation of Mayhony's AHRS algorithm. |
altb | 1:36bbe04e1f6f | 7 | // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ |
altb | 1:36bbe04e1f6f | 8 | // |
altb | 1:36bbe04e1f6f | 9 | // Date Author Notes |
altb | 1:36bbe04e1f6f | 10 | // 29/09/2011 SOH Madgwick Initial release |
altb | 1:36bbe04e1f6f | 11 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load |
altb | 1:36bbe04e1f6f | 12 | // |
altb | 1:36bbe04e1f6f | 13 | //============================================================================================= |
altb | 1:36bbe04e1f6f | 14 | #ifndef __Mahony_h__ |
altb | 1:36bbe04e1f6f | 15 | #define __Mahony_h__ |
altb | 1:36bbe04e1f6f | 16 | #include <math.h> |
altb | 1:36bbe04e1f6f | 17 | |
altb | 1:36bbe04e1f6f | 18 | //-------------------------------------------------------------------------------------------- |
altb | 1:36bbe04e1f6f | 19 | // Variable declaration |
altb | 1:36bbe04e1f6f | 20 | |
altb | 1:36bbe04e1f6f | 21 | class Mahony { |
altb | 1:36bbe04e1f6f | 22 | private: |
altb | 1:36bbe04e1f6f | 23 | float twoKp; // 2 * proportional gain (Kp) |
altb | 1:36bbe04e1f6f | 24 | float twoKi; // 2 * integral gain (Ki) |
altb | 1:36bbe04e1f6f | 25 | float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame |
altb | 1:36bbe04e1f6f | 26 | float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki |
altb | 1:36bbe04e1f6f | 27 | float invSampleFreq; |
altb | 1:36bbe04e1f6f | 28 | float roll, pitch, yaw; |
altb | 1:36bbe04e1f6f | 29 | char anglesComputed; |
altb | 1:36bbe04e1f6f | 30 | static float invSqrt(float x); |
altb | 1:36bbe04e1f6f | 31 | void computeAngles(); |
altb | 1:36bbe04e1f6f | 32 | |
altb | 1:36bbe04e1f6f | 33 | //------------------------------------------------------------------------------------------- |
altb | 1:36bbe04e1f6f | 34 | // Function declarations |
altb | 1:36bbe04e1f6f | 35 | |
altb | 1:36bbe04e1f6f | 36 | public: |
altb | 1:36bbe04e1f6f | 37 | Mahony(); |
altb | 1:36bbe04e1f6f | 38 | void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; } |
altb | 1:36bbe04e1f6f | 39 | void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); |
altb | 1:36bbe04e1f6f | 40 | void updateIMU(float gx, float gy, float gz, float ax, float ay, float az); |
altb | 1:36bbe04e1f6f | 41 | float getRoll() { |
altb | 1:36bbe04e1f6f | 42 | if (!anglesComputed) computeAngles(); |
altb | 1:36bbe04e1f6f | 43 | return roll * 57.29578f; |
altb | 1:36bbe04e1f6f | 44 | } |
altb | 1:36bbe04e1f6f | 45 | float getPitch() { |
altb | 1:36bbe04e1f6f | 46 | if (!anglesComputed) computeAngles(); |
altb | 1:36bbe04e1f6f | 47 | return pitch * 57.29578f; |
altb | 1:36bbe04e1f6f | 48 | } |
altb | 1:36bbe04e1f6f | 49 | float getYaw() { |
altb | 1:36bbe04e1f6f | 50 | if (!anglesComputed) computeAngles(); |
altb | 1:36bbe04e1f6f | 51 | return yaw * 57.29578f + 180.0f; |
altb | 1:36bbe04e1f6f | 52 | } |
altb | 1:36bbe04e1f6f | 53 | float getRollRadians() { |
altb | 1:36bbe04e1f6f | 54 | if (!anglesComputed) computeAngles(); |
altb | 1:36bbe04e1f6f | 55 | return roll; |
altb | 1:36bbe04e1f6f | 56 | } |
altb | 1:36bbe04e1f6f | 57 | float getPitchRadians() { |
altb | 1:36bbe04e1f6f | 58 | if (!anglesComputed) computeAngles(); |
altb | 1:36bbe04e1f6f | 59 | return pitch; |
altb | 1:36bbe04e1f6f | 60 | } |
altb | 1:36bbe04e1f6f | 61 | float getYawRadians() { |
altb | 1:36bbe04e1f6f | 62 | if (!anglesComputed) computeAngles(); |
altb | 1:36bbe04e1f6f | 63 | return yaw; |
altb | 1:36bbe04e1f6f | 64 | } |
altb | 1:36bbe04e1f6f | 65 | void getQuaternion(float *w, float *x, float *y, float *z) { |
altb | 1:36bbe04e1f6f | 66 | *w = q0; |
altb | 1:36bbe04e1f6f | 67 | *x = q1; |
altb | 1:36bbe04e1f6f | 68 | *y = q2; |
altb | 1:36bbe04e1f6f | 69 | *z = q3; |
altb | 1:36bbe04e1f6f | 70 | } |
altb | 1:36bbe04e1f6f | 71 | }; |
altb | 1:36bbe04e1f6f | 72 | |
altb | 1:36bbe04e1f6f | 73 | #endif |