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
Diff: angle.h
- Revision:
- 0:15e41c824e3b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/angle.h Sun Nov 30 11:06:13 2014 +0000 @@ -0,0 +1,228 @@ +#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 \ No newline at end of file