BE@R lab / AHRS_fillter

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MPU9250_Interface

Dependents:   MPU9250AHRS_OOP

AHRS.h@1:054bfc57f0f9, 2016-01-20 (annotated)

Committer:
soulx
Date:
Wed Jan 20 15:13:41 2016 +0000
Revision:
1:054bfc57f0f9
Parent:
0:bf9febe45e1d 
ahrs filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soulx 0:bf9febe45e1d 1 #ifndef _AHRS_
soulx 0:bf9febe45e1d 2 #define _AHRS_
soulx 0:bf9febe45e1d 3
soulx 0:bf9febe45e1d 4 #include "MPU9250.h"
soulx 0:bf9febe45e1d 5
soulx 0:bf9febe45e1d 6 // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
soulx 0:bf9febe45e1d 7 // (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
soulx 0:bf9febe45e1d 8 // which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
soulx 0:bf9febe45e1d 9 // device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
soulx 0:bf9febe45e1d 10 // The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
soulx 0:bf9febe45e1d 11 // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
soulx 0:bf9febe45e1d 12
soulx 0:bf9febe45e1d 13 class AHRS:public MPU9250
soulx 0:bf9febe45e1d 14 {
soulx 0:bf9febe45e1d 15 private:
soulx 0:bf9febe45e1d 16 float roll, pitch, yaw;
soulx 0:bf9febe45e1d 17 float q[4]; // vector to hold quaternion
soulx 0:bf9febe45e1d 18 float eInt[3]; // vector to hold integral error for Mahony method
soulx 0:bf9febe45e1d 19
soulx 0:bf9febe45e1d 20 // parameters for 6 DoF sensor fusion calculations
soulx 0:bf9febe45e1d 21 float PI;
soulx 0:bf9febe45e1d 22 float GyroMeasError; // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
soulx 0:bf9febe45e1d 23 float GyroMeasDrift; // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
soulx 0:bf9febe45e1d 24 float beta; // compute beta
soulx 0:bf9febe45e1d 25 float zeta; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
soulx 0:bf9febe45e1d 26
soulx 0:bf9febe45e1d 27 float norm;
soulx 0:bf9febe45e1d 28 float hx, hy, bx, bz;
soulx 0:bf9febe45e1d 29 float vx, vy, vz, wx, wy, wz;
soulx 0:bf9febe45e1d 30 float ex, ey, ez;
soulx 0:bf9febe45e1d 31 float pa, pb, pc;
soulx 0:bf9febe45e1d 32
soulx 0:bf9febe45e1d 33 float q1, q2, q3, q4;
soulx 0:bf9febe45e1d 34
soulx 0:bf9febe45e1d 35 // Auxiliary variables to avoid repeated arithmetic
soulx 0:bf9febe45e1d 36 float q1q1;
soulx 0:bf9febe45e1d 37 float q1q2;
soulx 0:bf9febe45e1d 38 float q1q3;
soulx 0:bf9febe45e1d 39 float q1q4;
soulx 0:bf9febe45e1d 40 float q2q2;
soulx 0:bf9febe45e1d 41 float q2q3;
soulx 0:bf9febe45e1d 42 float q2q4;
soulx 0:bf9febe45e1d 43 float q3q3;
soulx 0:bf9febe45e1d 44 float q3q4;
soulx 0:bf9febe45e1d 45 float q4q4;
soulx 0:bf9febe45e1d 46
soulx 0:bf9febe45e1d 47 float Xh;
soulx 0:bf9febe45e1d 48 float Yh;
soulx 0:bf9febe45e1d 49 float yawmag;
soulx 0:bf9febe45e1d 50
soulx 0:bf9febe45e1d 51 float deltat;
soulx 0:bf9febe45e1d 52 float delt_t;
soulx 0:bf9febe45e1d 53 float Now;
soulx 0:bf9febe45e1d 54 float sum;
soulx 0:bf9febe45e1d 55 float firstUpdate;
soulx 0:bf9febe45e1d 56 float lastUpdate;
soulx 0:bf9febe45e1d 57 uint32_t sumCount;
soulx 0:bf9febe45e1d 58 int count;
soulx 0:bf9febe45e1d 59 //int16_t tempCount;
soulx 0:bf9febe45e1d 60
soulx 0:bf9febe45e1d 61
soulx 0:bf9febe45e1d 62 public:
soulx 0:bf9febe45e1d 63 //MPU9250 imu;
soulx 0:bf9febe45e1d 64 //Serial test;
soulx 0:bf9febe45e1d 65 Timer t;
soulx 0:bf9febe45e1d 66 Serial pc2;
soulx 0:bf9febe45e1d 67 AHRS(PinName sda, PinName scl, PinName tx, PinName rx, int address) : MPU9250(sda,scl,tx,rx,address),pc2(tx,rx) {
soulx 0:bf9febe45e1d 68 for(int i=0; i<=3; i++) {
soulx 0:bf9febe45e1d 69 eInt[i] = 0;
soulx 0:bf9febe45e1d 70 q[i] = 0; // vector to hold quaternion
soulx 0:bf9febe45e1d 71 }
soulx 0:bf9febe45e1d 72
soulx 0:bf9febe45e1d 73 q[0] = 1.0f;
soulx 0:bf9febe45e1d 74
soulx 0:bf9febe45e1d 75 PI = 3.14159265358979323846f;
soulx 0:bf9febe45e1d 76 GyroMeasError = PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
soulx 0:bf9febe45e1d 77 beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
soulx 0:bf9febe45e1d 78 GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
soulx 0:bf9febe45e1d 79 zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
soulx 0:bf9febe45e1d 80
soulx 0:bf9febe45e1d 81 //Time
soulx 0:bf9febe45e1d 82 sum = 0;
soulx 0:bf9febe45e1d 83 sumCount = 0;
soulx 0:bf9febe45e1d 84 count = 0;
soulx 0:bf9febe45e1d 85
soulx 0:bf9febe45e1d 86 delt_t = 0;
soulx 0:bf9febe45e1d 87 lastUpdate = 0;
soulx 0:bf9febe45e1d 88 firstUpdate = 0;
soulx 0:bf9febe45e1d 89 Now = 0;
soulx 0:bf9febe45e1d 90 };
soulx 0:bf9febe45e1d 91 //void MadgwickQuaternionUpdate(float , float , float , float , float , float , float , float , float , float);
soulx 0:bf9febe45e1d 92 //void MahonyQuaternionUpdate(float , float , float , float , float , float , float , float , float , float);
soulx 0:bf9febe45e1d 93
soulx 0:bf9febe45e1d 94 void MadgwickQuaternionUpdate();
soulx 0:bf9febe45e1d 95 void MahonyQuaternionUpdate();
soulx 0:bf9febe45e1d 96
soulx 0:bf9febe45e1d 97 float getRoll() {
soulx 0:bf9febe45e1d 98 return roll;
soulx 0:bf9febe45e1d 99 }
soulx 0:bf9febe45e1d 100 float getPitch() {
soulx 0:bf9febe45e1d 101 return pitch;
soulx 0:bf9febe45e1d 102 }
soulx 0:bf9febe45e1d 103 float getYaw() {
soulx 0:bf9febe45e1d 104 return yaw;
soulx 0:bf9febe45e1d 105 }
soulx 0:bf9febe45e1d 106 void PrintRollPitchYaw();
soulx 0:bf9febe45e1d 107
soulx 0:bf9febe45e1d 108 void TimeStart();
soulx 0:bf9febe45e1d 109
soulx 0:bf9febe45e1d 110 void TimeRead() {
soulx 0:bf9febe45e1d 111 t.read_us();
soulx 0:bf9febe45e1d 112 };
soulx 0:bf9febe45e1d 113
soulx 0:bf9febe45e1d 114 void TimeCal();
soulx 0:bf9febe45e1d 115
soulx 0:bf9febe45e1d 116 void Run();
soulx 0:bf9febe45e1d 117
soulx 0:bf9febe45e1d 118 };
soulx 0:bf9febe45e1d 119 #endif