BE@R lab / Mbed 2 deprecated MPU9250AHRS_OOP

Class of MPU9250

Dependencies:   AHRS_fillter mbed

Fork of MPU9250AHRS by BE@R lab

AHRS.h@8:928673148b55, 2016-01-20 (annotated)

Committer:
icyzkungz
Date:
Wed Jan 20 02:42:22 2016 +0000
Revision:
8:928673148b55
Parent:
6:5665d427bceb 
add tx,rx in AHRS class

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 8:928673148b55 66 Serial pc2;
icyzkungz 8:928673148b55 67 AHRS(PinName sda, PinName scl, PinName tx, PinName rx, int address) : MPU9250(sda,scl,tx,rx,address),pc2(tx,rx) {
icyzkungz 6:5665d427bceb 68 for(int i=0; i<=3; i++) {
icyzkungz 6:5665d427bceb 69 eInt[i] = 0;
icyzkungz 6:5665d427bceb 70 q[i] = 0; // vector to hold quaternion
icyzkungz 6:5665d427bceb 71 }
icyzkungz 6:5665d427bceb 72
icyzkungz 6:5665d427bceb 73 q[0] = 1.0f;
icyzkungz 6:5665d427bceb 74
icyzkungz 6:5665d427bceb 75 PI = 3.14159265358979323846f;
icyzkungz 6:5665d427bceb 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
icyzkungz 6:5665d427bceb 77 beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
icyzkungz 6:5665d427bceb 78 GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
icyzkungz 6:5665d427bceb 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
icyzkungz 6:5665d427bceb 80
icyzkungz 6:5665d427bceb 81 //Time
icyzkungz 6:5665d427bceb 82 sum = 0;
icyzkungz 6:5665d427bceb 83 sumCount = 0;
icyzkungz 6:5665d427bceb 84 count = 0;
icyzkungz 6:5665d427bceb 85
icyzkungz 6:5665d427bceb 86 delt_t = 0;
icyzkungz 6:5665d427bceb 87 lastUpdate = 0;
icyzkungz 6:5665d427bceb 88 firstUpdate = 0;
icyzkungz 6:5665d427bceb 89 Now = 0;
icyzkungz 6:5665d427bceb 90 };
icyzkungz 6:5665d427bceb 91 //void MadgwickQuaternionUpdate(float , float , float , float , float , float , float , float , float , float);
icyzkungz 6:5665d427bceb 92 //void MahonyQuaternionUpdate(float , float , float , float , float , float , float , float , float , float);
icyzkungz 6:5665d427bceb 93
icyzkungz 6:5665d427bceb 94 void MadgwickQuaternionUpdate();
icyzkungz 6:5665d427bceb 95 void MahonyQuaternionUpdate();
icyzkungz 6:5665d427bceb 96
icyzkungz 6:5665d427bceb 97 float getRoll() {
icyzkungz 6:5665d427bceb 98 return roll;
icyzkungz 6:5665d427bceb 99 }
icyzkungz 6:5665d427bceb 100 float getPitch() {
icyzkungz 6:5665d427bceb 101 return pitch;
icyzkungz 6:5665d427bceb 102 }
icyzkungz 6:5665d427bceb 103 float getYaw() {
icyzkungz 6:5665d427bceb 104 return yaw;
icyzkungz 6:5665d427bceb 105 }
icyzkungz 8:928673148b55 106 void PrintRollPitchYaw();
icyzkungz 6:5665d427bceb 107
icyzkungz 6:5665d427bceb 108 void TimeStart();
icyzkungz 6:5665d427bceb 109 void TimeRead() {
icyzkungz 6:5665d427bceb 110 t.read_us();
icyzkungz 6:5665d427bceb 111 };
icyzkungz 6:5665d427bceb 112 void TimeCal();
icyzkungz 6:5665d427bceb 113
icyzkungz 6:5665d427bceb 114 void Run();
icyzkungz 6:5665d427bceb 115
icyzkungz 6:5665d427bceb 116 };
icyzkungz 6:5665d427bceb 117 #endif