File content as of revision 0:15e41c824e3b:

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

#define acc_i2c  0x53<<1
#define acc_i2c_write  0x53<<1
#define acc_i2c_read  0x53<<1|1
#define ADXL345_DEVID_REG          0x00
#define ADXL345_THRESH_TAP_REG     0x1D
#define ADXL345_OFSX_REG           0x1E
#define ADXL345_OFSY_REG           0x1F
#define ADXL345_OFSZ_REG           0x20
#define ADXL345_DUR_REG            0x21
#define ADXL345_LATENT_REG         0x22
#define ADXL345_WINDOW_REG         0x23
#define ADXL345_THRESH_ACT_REG     0x24
#define ADXL345_THRESH_INACT_REG   0x25
#define ADXL345_TIME_INACT_REG     0x26
#define ADXL345_ACT_INACT_CTL_REG  0x27
#define ADXL345_THRESH_FF_REG      0x28
#define ADXL345_TIME_FF_REG        0x29
#define ADXL345_TAP_AXES_REG       0x2A
#define ADXL345_ACT_TAP_STATUS_REG 0x2B
#define ADXL345_BW_RATE_REG        0x2C
#define ADXL345_POWER_CTL_REG      0x2D
#define ADXL345_INT_ENABLE_REG     0x2E
#define ADXL345_INT_MAP_REG        0x2F
#define ADXL345_INT_SOURCE_REG     0x30
#define ADXL345_DATA_FORMAT_REG    0x31
#define ADXL345_DATAX0_REG         0x32
#define ADXL345_DATAX1_REG         0x33
#define ADXL345_DATAY0_REG         0x34
#define ADXL345_DATAY1_REG         0x35
#define ADXL345_DATAZ0_REG         0x36
#define ADXL345_DATAZ1_REG         0x37
#define ADXL345_FIFO_CTL           0x38
#define ADXL345_FIFO_STATUS        0x39

#define ADXL345_3200HZ      0x0F
#define ADXL345_1600HZ      0x0E
#define ADXL345_800HZ       0x0D
#define ADXL345_400HZ       0x0C
#define ADXL345_200HZ       0x0B
#define ADXL345_100HZ       0x0A
#define ADXL345_50HZ        0x09
#define ADXL345_25HZ        0x08
#define ADXL345_12HZ5       0x07
#define ADXL345_6HZ25       0x06

#define ADXL345_X           0x00
#define ADXL345_Y           0x01
#define ADXL345_Z           0x02

#define MeasurementMode     0x08

#define GYR_ADDRESS 0xD4
#define L3GD20_WHO_AM_I      0x0F

#define L3GD20_CTRL_REG1     0x20
#define L3GD20_CTRL_REG2     0x21
#define L3GD20_CTRL_REG3     0x22
#define L3GD20_CTRL_REG4     0x23
#define L3GD20_CTRL_REG5     0x24
#define L3GD20_REFERENCE     0x25
#define L3GD20_OUT_TEMP      0x26
#define L3GD20_STATUS_REG    0x27

#define L3GD20_OUT_X_L       0x28
#define L3GD20_OUT_X_H       0x29
#define L3GD20_OUT_Y_L       0x2A
#define L3GD20_OUT_Y_H       0x2B
#define L3GD20_OUT_Z_L       0x2C
#define L3GD20_OUT_Z_H       0x2D

#define L3GD20_FIFO_CTRL_REG 0x2E
#define L3GD20_FIFO_SRC_REG  0x2F

#define L3GD20_INT1_CFG      0x30
#define L3GD20_INT1_SRC      0x31
#define L3GD20_INT1_THS_XH   0x32
#define L3GD20_INT1_THS_XL   0x33
#define L3GD20_INT1_THS_YH   0x34
#define L3GD20_INT1_THS_YL   0x35
#define L3GD20_INT1_THS_ZH   0x36
#define L3GD20_INT1_THS_ZL   0x37
#define L3GD20_INT1_DURATION 0x38
class kalman;
class ANGLE{
    private:
    class kalman
            {
                public:
                    kalman(void);
                    double getAngle(double newAngle, double newRate, double dt);
                    
                    void setAngle(double newAngle);
                    void setQangle(double newQ_angle);
                    void setQgyroBias(double newQ_gyroBias);
                    void setRangle(double newR_angle);
                    
                    double getRate(void);
                    double getQangle(void);
                    double getQbias(void);
                    double getRangle(void);
                    
                
                private:
                    double P[2][2];         //error covariance matrix
                    double K[2];            //kalman gain
                    double y;               //angle difference
                    double S;               //estimation error
                
                    double rate;            //rate in deg/s
                    double angle;           //kalman angle
                    double bias;            //kalman gyro bias
                
                    double Q_angle;         //process noise variance for the angle of the accelerometer
                    double Q_gyroBias;      //process noise variance for the gyroscope bias
                    double R_angle;         //measurement noise variance 
            };
        I2C i2c_;
        //ACC ADXL345
        int x_acc,y_acc,z_acc,sampleNum;
        double x_offset,y_offset,z_offset;
        float gx,gy,gz,xnoise,ynoise,znoise;
        //GYALO L3GD20
        double Rate;
        double  sampleTime;       
        float noise[3];
        short offset[3];short rate[3],prev_rate[3];
        double angle[3],offset_angle[3],Synthesis_angle[3],kalman_angle[3],comp_angle[3];
        double t[3];
        short tempDATA_ACC[3],tempDATA_ANGLE[3];
        //KALMAN
            kalman kalma[3];
        
        char data_single_get(char reg);
        int data_single_put(char reg,char data);
        void data_multi_get(char reg,char* data, int size);
        int data_multi_put(char reg, char* data, int size);
        bool write_reg(int addr_i2c,int addr_reg, char v);
        bool read_reg(int addr_i2c,int addr_reg, char *v);
        bool recv(char sad, char sub, char *buf, int length);
    public:
        ANGLE(PinName sda, PinName scl);
        void ADXL_setup();
        void ADXL_setnum(int Num=500,float time=0.001,double rate=0.00390635);//set data member
        void Synthesis(double* X,double* Y);
        void getaxis_acc(short* DATA_ACC);
        char getdeviceID(){return data_single_get(ADXL345_DEVID_REG);}
        void getangle_acc(double* DATA_ANGLE);
        void get_angle_rate(double *x,double *y,double *z);
        void get_rate(short* RATE);
        void set_angle();//set always [time]
        void set_angle(double ANG_x,double ANG_y,double ANG_z);
        void get_angle(double *x,double *y,double *z);
        void get_Synthesis_angle(double* X,double* Y);
        void get_Kalman_angle(double* X,double* Y);
        void get_Comp_angle(double* X,double* Y);
        void set_noise();
        void set_offset();
        void set_angleoffset();

        
        double getRate(void);
        double getQangle(void);
        double getQbias(void);
        double getRangle(void);


        int setPowerMode(char mode);
        
        char getPowerControl(){return data_single_get(ADXL345_POWER_CTL_REG);}
        int setPowerControl(char control){return data_single_put(ADXL345_POWER_CTL_REG, control);}
        
        char getDataFormatControl(void){return data_single_get(ADXL345_DATA_FORMAT_REG);}
        int setDataFormatControl(char control){return data_single_put(ADXL345_DATA_FORMAT_REG, control);}
        
        int setDataRate(char rate);
        
        char getOffset(char axis);
        int setOffset(char axis, char offset);
        
        char getFifoControl(void){return data_single_get(ADXL345_FIFO_CTL);}
        int setFifoControl(char settings){return data_single_put(ADXL345_FIFO_STATUS, settings);}
        char getFifoStatus(void){return data_single_get(ADXL345_FIFO_STATUS);}
        
        char getTapThreshold(void){return data_single_get(ADXL345_THRESH_TAP_REG);}
        int setTapThreshold(char threshold){return data_single_put(ADXL345_THRESH_TAP_REG, threshold);}
        float getTapDuration(void){return (float)data_single_get(ADXL345_DUR_REG)*625;}
        int setTapDuration(short int duration_us);
        float getTapLatency(void){return (float)data_single_get(ADXL345_LATENT_REG)*1.25;}
        int setTapLatency(short int latency_ms);
        
        float getWindowTime(void){return (float)data_single_get(ADXL345_WINDOW_REG)*1.25;}
        int setWindowTime(short int window_ms);
        
        char getActivityThreshold(void){return data_single_get(ADXL345_THRESH_ACT_REG);}
        int setActivityThreshold(char threshold){return data_single_put(ADXL345_THRESH_ACT_REG, threshold);}
        
        char getInactivityThreshold(void){return data_single_get(ADXL345_THRESH_INACT_REG);}
        int setInactivityThreshold(char threshold){return data_single_put(ADXL345_THRESH_INACT_REG, threshold);}
        char getTimeInactivity(void){return data_single_get(ADXL345_TIME_INACT_REG);}
        int setTimeInactivity(char timeInactivity){return data_single_put(ADXL345_TIME_INACT_REG, timeInactivity);}
        
        char getActivityInactivityControl(void){return data_single_get(ADXL345_ACT_INACT_CTL_REG);}
        int setActivityInactivityControl(char settings){return data_single_put(ADXL345_ACT_INACT_CTL_REG, settings);}
        
        char getFreefallThreshold(void){return data_single_get(ADXL345_THRESH_FF_REG);}
        int setFreefallThreshold(char threshold){return data_single_put(ADXL345_THRESH_FF_REG, threshold);}
        char getFreefallTime(void){return data_single_get(ADXL345_TIME_FF_REG)*5;}
        int setFreefallTime(short int freefallTime_ms);
        
        char getTapAxisControl(void){return data_single_get(ADXL345_TAP_AXES_REG);}
        int setTapAxisControl(char settings){return data_single_put(ADXL345_TAP_AXES_REG, settings);}
        char getTapSource(void){return data_single_get(ADXL345_ACT_TAP_STATUS_REG);}
        
        char getInterruptEnableControl(void){return data_single_get(ADXL345_INT_ENABLE_REG);}
        int setInterruptEnableControl(char settings){return data_single_put(ADXL345_INT_ENABLE_REG, settings);};
        char getInterruptMappingControl(void){return data_single_get(ADXL345_INT_MAP_REG);};
        int setInterruptMappingControl(char settings){return data_single_put(ADXL345_INT_MAP_REG, settings);};
        char getInterruptSource(void){return data_single_get(ADXL345_INT_SOURCE_REG);}
        bool read(float *gx, float *gy, float *gz);
        bool read(short *axis);

};

#endif