IMU_Filter.h

00001 // by MaEtUgR
00002 
00003 #ifndef IMU_FILTER_H
00004 #define IMU_FILTER_H
00005 
00006 #include "mbed.h"
00007 
00008 #define Rad2Deg         57.295779513082320876798154814105 // factor between radians and degrees of angle (180/Pi)
00009 
00010 class IMU_Filter
00011 {
00012     public:
00013         IMU_Filter();
00014         void compute(float dt, const float * gyro_data, const float * acc_data);
00015         float angle[3];                                 // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw]
00016         
00017         // MARG
00018         float q0, q1, q2, q3;   // quaternion elements representing the estimated orientation
00019         float exInt , eyInt , ezInt;  // scaled integral error
00020         void IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az);
00021     private:
00022         float d_Gyro_angle[3];
00023         void get_Acc_angle(const float * Acc_data);
00024         float Acc_angle[3];
00025 };
00026 
00027 #endif