I am able to get angle from ADXL345 and L3GD20. Please use this program. Angle is made by deg/sec and acceramater. I used Kalmanfilter.
Fork of ANGLE by
angle.h
- Committer:
- kikoaac
- Date:
- 2014-11-30
- Revision:
- 0:15e41c824e3b
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