altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

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?

UserRevisionLine numberNew 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