DCM.h

00001 #ifndef DCM_H
00002 #define DCM_H
00003 
00004 #include "mbed.h"
00005 
00006 #define GRAVITY 4096
00007 #define Kp_ROLLPITCH 1.515/GRAVITY //1.515
00008 #define Ki_ROLLPITCH 0.00101/GRAVITY //0.00101
00009 
00010 
00011 class DCM{
00012 
00013 public:
00014    
00015  DCM(void);
00016    
00017 float G_Dt;
00018 
00019 float roll;
00020 
00021 float pitch;
00022 
00023 float yaw;
00024 
00025 void Update_DCM(float dt,float a, float b, float c, float d, float e, float f);
00026 
00027 float DCM_Matrix[3][3]; 
00028 
00029 float Update_Matrix[3][3];
00030 
00031 float Temporary_Matrix[3][3];
00032 
00033 private:
00034 
00035 float Accel_Vector[3]; //Store the acceleration in a vector
00036 float Gyro_Vector[3];//Store the gyros rutn rate in a vector
00037 float Omega_Vector[3]; //Corrected Gyro_Vector data
00038 float Omega_P[3];//Omega Proportional correction
00039 float Omega_I[3];//Omega Integrator
00040 float Omega[3];
00041 float errorRollPitch[3]; 
00042 
00043 
00044 float constrain(float x, float a, float b);
00045 
00046 void Normalize(void);
00047 
00048 void Drift_correction(void);
00049 
00050 void Matrix_update(float GyroX,float GyroY,float GyroZ, float AccX, float AccY, float AccZ);
00051 
00052 void Euler_angles(void);
00053 
00054 };
00055 
00056 #endif