Eric Leal / Madgwick

Madgwick AHRS library for mbed os6 modified to take inconsistent sample frequencies.

Madgwick.h@0:69340ac25ae9, 2021-12-02 (annotated)

Committer:
ericleal
Date:
Thu Dec 02 15:25:28 2021 +0000
Revision:
0:69340ac25ae9
Madgwick AHRS library for Mbed OS6 modified to take inconsistent sample frequencies.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ericleal 0:69340ac25ae9 1 #ifndef Madgwick_H
ericleal 0:69340ac25ae9 2 #define Madgwick_H
ericleal 0:69340ac25ae9 3
ericleal 0:69340ac25ae9 4 #include "mbed.h"
ericleal 0:69340ac25ae9 5
ericleal 0:69340ac25ae9 6 class Madgwick {
ericleal 0:69340ac25ae9 7 protected:
ericleal 0:69340ac25ae9 8 public:
ericleal 0:69340ac25ae9 9
ericleal 0:69340ac25ae9 10 Madgwick(float gain);
ericleal 0:69340ac25ae9 11 void update_attitude(float gx, float gy, float gz, float ax, float ay, float az, float sampleFreq);
ericleal 0:69340ac25ae9 12 float getRoll();
ericleal 0:69340ac25ae9 13 float getPitch();
ericleal 0:69340ac25ae9 14 float getYaw();
ericleal 0:69340ac25ae9 15
ericleal 0:69340ac25ae9 16
ericleal 0:69340ac25ae9 17 protected:
ericleal 0:69340ac25ae9 18 private:
ericleal 0:69340ac25ae9 19 float invSqrt(float x);
ericleal 0:69340ac25ae9 20 float roll, pitch, yaw;
ericleal 0:69340ac25ae9 21 float recipNorm;
ericleal 0:69340ac25ae9 22 float s0, s1, s2, s3;
ericleal 0:69340ac25ae9 23 float qDot1, qDot2, qDot3, qDot4;
ericleal 0:69340ac25ae9 24 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
ericleal 0:69340ac25ae9 25 volatile float beta; // 2 * proportional gain (Kp)
ericleal 0:69340ac25ae9 26 volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
ericleal 0:69340ac25ae9 27
ericleal 0:69340ac25ae9 28
ericleal 0:69340ac25ae9 29 };
ericleal 0:69340ac25ae9 30
ericleal 0:69340ac25ae9 31 #endif
ericleal 0:69340ac25ae9 32