BE@R lab / Mbed 2 deprecated MPU9250AHRS_OOP

Class of MPU9250

Dependencies:   AHRS_fillter mbed

Fork of MPU9250AHRS by BE@R lab

AHRS.h@6:5665d427bceb, 2016-01-20 (annotated)

Committer:
icyzkungz
Date:
Wed Jan 20 02:34:07 2016 +0000
Revision:
6:5665d427bceb
Child:
8:928673148b55
MPU9250-OOP

Who changed what in which revision?

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