Quaternion.h

00001 #ifndef Quaternion_h
00002 #define Quaternion_h
00003 #include "mbed.h"
00004 
00005 
00006 #define twoKpDef    (2.0f * 0.5f)   // 2 * proportional gain
00007 #define twoKiDef    (2.0f * 0.0f)   // 2 * integral gain
00008 
00009 class Quaternion {
00010 
00011     public:
00012         Quaternion();
00013         void update6DOF(float gx, float gy, float gz, float ax, float ay, float az);
00014         void update9DOF(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
00015         void getQ(float * q);
00016         void getEulerAngles(float * angles);
00017         void getYawPitchRoll(double * ypr);
00018         void getGravity(float * gravity);
00019         void getLinearAcceleration(float * linearAccel, float ax, float ay, float az);
00020     
00021     private:
00022         float invSqrt(float x);
00023         volatile float q0, q1, q2, q3;
00024         volatile float twoKp;   // 2 * proportional gain (Kp)
00025         volatile float twoKi;   // 2 * integral gain (Ki)
00026         volatile float integralFBx,  integralFBy, integralFBz;
00027         unsigned long lastUpdate, now;
00028         float sampleFreq;
00029         void updateSampleFrequency();
00030 };
00031 
00032 #endif