angle.h

00001 #ifndef _angle_H
00002 #define _angle_H
00003 #include "mbed.h"
00004 
00005 #define acc_i2c  0x53<<1
00006 #define acc_i2c_write  0x53<<1
00007 #define acc_i2c_read  0x53<<1|1
00008 #define ADXL345_DEVID_REG          0x00
00009 #define ADXL345_THRESH_TAP_REG     0x1D
00010 #define ADXL345_OFSX_REG           0x1E
00011 #define ADXL345_OFSY_REG           0x1F
00012 #define ADXL345_OFSZ_REG           0x20
00013 #define ADXL345_DUR_REG            0x21
00014 #define ADXL345_LATENT_REG         0x22
00015 #define ADXL345_WINDOW_REG         0x23
00016 #define ADXL345_THRESH_ACT_REG     0x24
00017 #define ADXL345_THRESH_INACT_REG   0x25
00018 #define ADXL345_TIME_INACT_REG     0x26
00019 #define ADXL345_ACT_INACT_CTL_REG  0x27
00020 #define ADXL345_THRESH_FF_REG      0x28
00021 #define ADXL345_TIME_FF_REG        0x29
00022 #define ADXL345_TAP_AXES_REG       0x2A
00023 #define ADXL345_ACT_TAP_STATUS_REG 0x2B
00024 #define ADXL345_BW_RATE_REG        0x2C
00025 #define ADXL345_POWER_CTL_REG      0x2D
00026 #define ADXL345_INT_ENABLE_REG     0x2E
00027 #define ADXL345_INT_MAP_REG        0x2F
00028 #define ADXL345_INT_SOURCE_REG     0x30
00029 #define ADXL345_DATA_FORMAT_REG    0x31
00030 #define ADXL345_DATAX0_REG         0x32
00031 #define ADXL345_DATAX1_REG         0x33
00032 #define ADXL345_DATAY0_REG         0x34
00033 #define ADXL345_DATAY1_REG         0x35
00034 #define ADXL345_DATAZ0_REG         0x36
00035 #define ADXL345_DATAZ1_REG         0x37
00036 #define ADXL345_FIFO_CTL           0x38
00037 #define ADXL345_FIFO_STATUS        0x39
00038 
00039 #define ADXL345_3200HZ      0x0F
00040 #define ADXL345_1600HZ      0x0E
00041 #define ADXL345_800HZ       0x0D
00042 #define ADXL345_400HZ       0x0C
00043 #define ADXL345_200HZ       0x0B
00044 #define ADXL345_100HZ       0x0A
00045 #define ADXL345_50HZ        0x09
00046 #define ADXL345_25HZ        0x08
00047 #define ADXL345_12HZ5       0x07
00048 #define ADXL345_6HZ25       0x06
00049 
00050 #define ADXL345_X           0x00
00051 #define ADXL345_Y           0x01
00052 #define ADXL345_Z           0x02
00053 
00054 #define MeasurementMode     0x08
00055 
00056 #define GYR_ADDRESS 0xD4
00057 #define L3GD20_WHO_AM_I      0x0F
00058 
00059 #define L3GD20_CTRL_REG1     0x20
00060 #define L3GD20_CTRL_REG2     0x21
00061 #define L3GD20_CTRL_REG3     0x22
00062 #define L3GD20_CTRL_REG4     0x23
00063 #define L3GD20_CTRL_REG5     0x24
00064 #define L3GD20_REFERENCE     0x25
00065 #define L3GD20_OUT_TEMP      0x26
00066 #define L3GD20_STATUS_REG    0x27
00067 
00068 #define L3GD20_OUT_X_L       0x28
00069 #define L3GD20_OUT_X_H       0x29
00070 #define L3GD20_OUT_Y_L       0x2A
00071 #define L3GD20_OUT_Y_H       0x2B
00072 #define L3GD20_OUT_Z_L       0x2C
00073 #define L3GD20_OUT_Z_H       0x2D
00074 
00075 #define L3GD20_FIFO_CTRL_REG 0x2E
00076 #define L3GD20_FIFO_SRC_REG  0x2F
00077 
00078 #define L3GD20_INT1_CFG      0x30
00079 #define L3GD20_INT1_SRC      0x31
00080 #define L3GD20_INT1_THS_XH   0x32
00081 #define L3GD20_INT1_THS_XL   0x33
00082 #define L3GD20_INT1_THS_YH   0x34
00083 #define L3GD20_INT1_THS_YL   0x35
00084 #define L3GD20_INT1_THS_ZH   0x36
00085 #define L3GD20_INT1_THS_ZL   0x37
00086 #define L3GD20_INT1_DURATION 0x38
00087 class kalman;
00088 class ANGLE{
00089     private:
00090     class kalman
00091             {
00092                 public:
00093                     kalman(void);
00094                     double getAngle(double newAngle, double newRate, double dt);
00095                     
00096                     void setAngle(double newAngle);
00097                     void setQangle(double newQ_angle);
00098                     void setQgyroBias(double newQ_gyroBias);
00099                     void setRangle(double newR_angle);
00100                     
00101                     double getRate(void);
00102                     double getQangle(void);
00103                     double getQbias(void);
00104                     double getRangle(void);
00105                     
00106                 
00107                 private:
00108                     double P[2][2];         //error covariance matrix
00109                     double K[2];            //kalman gain
00110                     double y;               //angle difference
00111                     double S;               //estimation error
00112                 
00113                     double rate;            //rate in deg/s
00114                     double angle;           //kalman angle
00115                     double bias;            //kalman gyro bias
00116                 
00117                     double Q_angle;         //process noise variance for the angle of the accelerometer
00118                     double Q_gyroBias;      //process noise variance for the gyroscope bias
00119                     double R_angle;         //measurement noise variance 
00120             };
00121         I2C i2c_;
00122         //ACC ADXL345
00123         int x_acc,y_acc,z_acc,sampleNum;
00124         double x_offset,y_offset,z_offset;
00125         float gx,gy,gz,xnoise,ynoise,znoise;
00126         //GYALO L3GD20
00127         double Rate;
00128         double  sampleTime;       
00129         float noise[3];
00130         short offset[3];short rate[3],prev_rate[3];
00131         double angle[3],offset_angle[3],Synthesis_angle[3],kalman_angle[3],comp_angle[3];
00132         double t[3];
00133         short tempDATA_ACC[3],tempDATA_ANGLE[3];
00134         //KALMAN
00135             kalman kalma[3];
00136         
00137         char data_single_get(char reg);
00138         int data_single_put(char reg,char data);
00139         void data_multi_get(char reg,char* data, int size);
00140         int data_multi_put(char reg, char* data, int size);
00141         bool write_reg(int addr_i2c,int addr_reg, char v);
00142         bool read_reg(int addr_i2c,int addr_reg, char *v);
00143         bool recv(char sad, char sub, char *buf, int length);
00144     public:
00145         ANGLE(PinName sda, PinName scl);
00146         void ADXL_setup();
00147         void ADXL_setnum(int Num=500,float time=0.001,double rate=0.00390635);//set data member
00148         void Synthesis(double* X,double* Y);
00149         void getaxis_acc(short* DATA_ACC);
00150         char getdeviceID(){return data_single_get(ADXL345_DEVID_REG);}
00151         void getangle_acc(double* DATA_ANGLE);
00152         void get_angle_rate(double *x,double *y,double *z);
00153         void get_rate(short* RATE);
00154         void set_angle();//set always [time]
00155         void set_angle(double ANG_x,double ANG_y,double ANG_z);
00156         void get_angle(double *x,double *y,double *z);
00157         void get_Synthesis_angle(double* X,double* Y);
00158         void get_Kalman_angle(double* X,double* Y);
00159         void get_Comp_angle(double* X,double* Y);
00160         void set_noise();
00161         void set_offset();
00162         void set_angleoffset();
00163 
00164         
00165         double getRate(void);
00166         double getQangle(void);
00167         double getQbias(void);
00168         double getRangle(void);
00169 
00170 
00171         int setPowerMode(char mode);
00172         
00173         char getPowerControl(){return data_single_get(ADXL345_POWER_CTL_REG);}
00174         int setPowerControl(char control){return data_single_put(ADXL345_POWER_CTL_REG, control);}
00175         
00176         char getDataFormatControl(void){return data_single_get(ADXL345_DATA_FORMAT_REG);}
00177         int setDataFormatControl(char control){return data_single_put(ADXL345_DATA_FORMAT_REG, control);}
00178         
00179         int setDataRate(char rate);
00180         
00181         char getOffset(char axis);
00182         int setOffset(char axis, char offset);
00183         
00184         char getFifoControl(void){return data_single_get(ADXL345_FIFO_CTL);}
00185         int setFifoControl(char settings){return data_single_put(ADXL345_FIFO_STATUS, settings);}
00186         char getFifoStatus(void){return data_single_get(ADXL345_FIFO_STATUS);}
00187         
00188         char getTapThreshold(void){return data_single_get(ADXL345_THRESH_TAP_REG);}
00189         int setTapThreshold(char threshold){return data_single_put(ADXL345_THRESH_TAP_REG, threshold);}
00190         float getTapDuration(void){return (float)data_single_get(ADXL345_DUR_REG)*625;}
00191         int setTapDuration(short int duration_us);
00192         float getTapLatency(void){return (float)data_single_get(ADXL345_LATENT_REG)*1.25;}
00193         int setTapLatency(short int latency_ms);
00194         
00195         float getWindowTime(void){return (float)data_single_get(ADXL345_WINDOW_REG)*1.25;}
00196         int setWindowTime(short int window_ms);
00197         
00198         char getActivityThreshold(void){return data_single_get(ADXL345_THRESH_ACT_REG);}
00199         int setActivityThreshold(char threshold){return data_single_put(ADXL345_THRESH_ACT_REG, threshold);}
00200         
00201         char getInactivityThreshold(void){return data_single_get(ADXL345_THRESH_INACT_REG);}
00202         int setInactivityThreshold(char threshold){return data_single_put(ADXL345_THRESH_INACT_REG, threshold);}
00203         char getTimeInactivity(void){return data_single_get(ADXL345_TIME_INACT_REG);}
00204         int setTimeInactivity(char timeInactivity){return data_single_put(ADXL345_TIME_INACT_REG, timeInactivity);}
00205         
00206         char getActivityInactivityControl(void){return data_single_get(ADXL345_ACT_INACT_CTL_REG);}
00207         int setActivityInactivityControl(char settings){return data_single_put(ADXL345_ACT_INACT_CTL_REG, settings);}
00208         
00209         char getFreefallThreshold(void){return data_single_get(ADXL345_THRESH_FF_REG);}
00210         int setFreefallThreshold(char threshold){return data_single_put(ADXL345_THRESH_FF_REG, threshold);}
00211         char getFreefallTime(void){return data_single_get(ADXL345_TIME_FF_REG)*5;}
00212         int setFreefallTime(short int freefallTime_ms);
00213         
00214         char getTapAxisControl(void){return data_single_get(ADXL345_TAP_AXES_REG);}
00215         int setTapAxisControl(char settings){return data_single_put(ADXL345_TAP_AXES_REG, settings);}
00216         char getTapSource(void){return data_single_get(ADXL345_ACT_TAP_STATUS_REG);}
00217         
00218         char getInterruptEnableControl(void){return data_single_get(ADXL345_INT_ENABLE_REG);}
00219         int setInterruptEnableControl(char settings){return data_single_put(ADXL345_INT_ENABLE_REG, settings);};
00220         char getInterruptMappingControl(void){return data_single_get(ADXL345_INT_MAP_REG);};
00221         int setInterruptMappingControl(char settings){return data_single_put(ADXL345_INT_MAP_REG, settings);};
00222         char getInterruptSource(void){return data_single_get(ADXL345_INT_SOURCE_REG);}
00223         bool read(float *gx, float *gy, float *gz);
00224         bool read(short *axis);
00225 
00226 };
00227 
00228 #endif