File content as of revision 0:ebbc3cd3a61e:

#ifndef Quaternion_h
#define Quaternion_h
#include "mbed.h"


#define twoKpDef    (2.0f * 0.5f)   // 2 * proportional gain
#define twoKiDef    (2.0f * 0.0f)   // 2 * integral gain

class Quaternion {

    public:
        Quaternion();
        void update6DOF(float gx, float gy, float gz, float ax, float ay, float az);
        void update9DOF(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
        void getQ(float * q);
        void getEulerAngles(float * angles);
        void getYawPitchRoll(double * ypr);
        void getGravity(float * gravity);
        void getLinearAcceleration(float * linearAccel, float ax, float ay, float az);
    
    private:
        float invSqrt(float x);
        volatile float q0, q1, q2, q3;
        volatile float twoKp;   // 2 * proportional gain (Kp)
        volatile float twoKi;   // 2 * integral gain (Ki)
        volatile float integralFBx,  integralFBy, integralFBz;
        unsigned long lastUpdate, now;
        float sampleFreq;
        void updateSampleFrequency();
};

#endif