altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

Mahony.h@30:9b0cd3caf0ec, 2020-01-27 (annotated)

Committer:
pmic
Date:
Mon Jan 27 10:54:13 2020 +0000
Revision:
30:9b0cd3caf0ec
Parent:
3:6811c0ce95f6 
Correct magnetometer.

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 static float invSqrt(float x);
altb 3:6811c0ce95f6 30
altb 1:36bbe04e1f6f 31
altb 1:36bbe04e1f6f 32 //-------------------------------------------------------------------------------------------
altb 1:36bbe04e1f6f 33 // Function declarations
altb 1:36bbe04e1f6f 34
altb 1:36bbe04e1f6f 35 public:
altb 3:6811c0ce95f6 36 Mahony(float);
altb 1:36bbe04e1f6f 37 void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
altb 1:36bbe04e1f6f 38 void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
altb 1:36bbe04e1f6f 39 void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
altb 3:6811c0ce95f6 40 char anglesComputed;
altb 3:6811c0ce95f6 41 void computeAngles();
altb 1:36bbe04e1f6f 42 float getRoll() {
altb 1:36bbe04e1f6f 43 if (!anglesComputed) computeAngles();
altb 1:36bbe04e1f6f 44 return roll * 57.29578f;
altb 1:36bbe04e1f6f 45 }
altb 1:36bbe04e1f6f 46 float getPitch() {
altb 1:36bbe04e1f6f 47 if (!anglesComputed) computeAngles();
altb 1:36bbe04e1f6f 48 return pitch * 57.29578f;
altb 1:36bbe04e1f6f 49 }
altb 1:36bbe04e1f6f 50 float getYaw() {
altb 1:36bbe04e1f6f 51 if (!anglesComputed) computeAngles();
altb 1:36bbe04e1f6f 52 return yaw * 57.29578f + 180.0f;
altb 1:36bbe04e1f6f 53 }
altb 1:36bbe04e1f6f 54 float getRollRadians() {
altb 1:36bbe04e1f6f 55 if (!anglesComputed) computeAngles();
altb 1:36bbe04e1f6f 56 return roll;
altb 1:36bbe04e1f6f 57 }
altb 1:36bbe04e1f6f 58 float getPitchRadians() {
altb 1:36bbe04e1f6f 59 if (!anglesComputed) computeAngles();
altb 1:36bbe04e1f6f 60 return pitch;
altb 1:36bbe04e1f6f 61 }
altb 1:36bbe04e1f6f 62 float getYawRadians() {
altb 1:36bbe04e1f6f 63 if (!anglesComputed) computeAngles();
altb 1:36bbe04e1f6f 64 return yaw;
altb 1:36bbe04e1f6f 65 }
altb 1:36bbe04e1f6f 66 void getQuaternion(float *w, float *x, float *y, float *z) {
altb 1:36bbe04e1f6f 67 *w = q0;
altb 1:36bbe04e1f6f 68 *x = q1;
altb 1:36bbe04e1f6f 69 *y = q2;
altb 1:36bbe04e1f6f 70 *z = q3;
altb 1:36bbe04e1f6f 71 }
altb 1:36bbe04e1f6f 72 };
altb 1:36bbe04e1f6f 73
altb 1:36bbe04e1f6f 74 #endif