GurvIMU.h

00001 #ifndef _GURVIMU_H_
00002 #define _GURVIMU_H_
00003 
00004 #include "mbed.h"
00005 #include "MPU6050.h"
00006 
00007 class GurvIMU {
00008     private:
00009         //Variables
00010         MPU6050 mpu;
00011         float twoKp;            // 2 * proportional gain (Kp)
00012         float twoKi;            // 2 * integral gain (Ki)
00013         float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki
00014         float q0, q1, q2, q3;   // quaternion of sensor frame relative to auxiliary frame
00015         float ax, ay, az, gx, gy, gz;
00016         float sample_freq;
00017         float offset[6];
00018         Timer timer_us;
00019         float dt_us;
00020         float cycle_nb;
00021         
00022         
00023         //Functions
00024         void getOffset();
00025         void getValues(float * values);
00026         void AHRS_update(float gx, float gy, float gz, float ax, float ay, float az);
00027         void getQ(float * q);
00028 
00029     public:
00030         GurvIMU();
00031         void init();
00032         void getYawPitchRollRad(float * ypr);
00033         void getVerticalAcceleration(float * av);
00034         
00035 };
00036 
00037 //Fast Inverse Square Root
00038 float invSqrt(float number);
00039 
00040 #endif /* _GURVIMU_H_ */