Class of MPU9250
Dependencies: AHRS_fillter mbed
Fork of MPU9250AHRS by
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?
User | Revision | Line number | New 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 |