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@0:15e41c824e3b, 2014-11-30 (annotated)
- Committer:
- kikoaac
- Date:
- Sun Nov 30 11:06:13 2014 +0000
- Revision:
- 0:15e41c824e3b
We can get angle by ADXL345 and L3GD20.
; I pasted example program.
; ADXL345?L2GD20???????????????
; ???????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kikoaac | 0:15e41c824e3b | 1 | #ifndef _angle_H |
kikoaac | 0:15e41c824e3b | 2 | #define _angle_H |
kikoaac | 0:15e41c824e3b | 3 | #include "mbed.h" |
kikoaac | 0:15e41c824e3b | 4 | |
kikoaac | 0:15e41c824e3b | 5 | #define acc_i2c 0x53<<1 |
kikoaac | 0:15e41c824e3b | 6 | #define acc_i2c_write 0x53<<1 |
kikoaac | 0:15e41c824e3b | 7 | #define acc_i2c_read 0x53<<1|1 |
kikoaac | 0:15e41c824e3b | 8 | #define ADXL345_DEVID_REG 0x00 |
kikoaac | 0:15e41c824e3b | 9 | #define ADXL345_THRESH_TAP_REG 0x1D |
kikoaac | 0:15e41c824e3b | 10 | #define ADXL345_OFSX_REG 0x1E |
kikoaac | 0:15e41c824e3b | 11 | #define ADXL345_OFSY_REG 0x1F |
kikoaac | 0:15e41c824e3b | 12 | #define ADXL345_OFSZ_REG 0x20 |
kikoaac | 0:15e41c824e3b | 13 | #define ADXL345_DUR_REG 0x21 |
kikoaac | 0:15e41c824e3b | 14 | #define ADXL345_LATENT_REG 0x22 |
kikoaac | 0:15e41c824e3b | 15 | #define ADXL345_WINDOW_REG 0x23 |
kikoaac | 0:15e41c824e3b | 16 | #define ADXL345_THRESH_ACT_REG 0x24 |
kikoaac | 0:15e41c824e3b | 17 | #define ADXL345_THRESH_INACT_REG 0x25 |
kikoaac | 0:15e41c824e3b | 18 | #define ADXL345_TIME_INACT_REG 0x26 |
kikoaac | 0:15e41c824e3b | 19 | #define ADXL345_ACT_INACT_CTL_REG 0x27 |
kikoaac | 0:15e41c824e3b | 20 | #define ADXL345_THRESH_FF_REG 0x28 |
kikoaac | 0:15e41c824e3b | 21 | #define ADXL345_TIME_FF_REG 0x29 |
kikoaac | 0:15e41c824e3b | 22 | #define ADXL345_TAP_AXES_REG 0x2A |
kikoaac | 0:15e41c824e3b | 23 | #define ADXL345_ACT_TAP_STATUS_REG 0x2B |
kikoaac | 0:15e41c824e3b | 24 | #define ADXL345_BW_RATE_REG 0x2C |
kikoaac | 0:15e41c824e3b | 25 | #define ADXL345_POWER_CTL_REG 0x2D |
kikoaac | 0:15e41c824e3b | 26 | #define ADXL345_INT_ENABLE_REG 0x2E |
kikoaac | 0:15e41c824e3b | 27 | #define ADXL345_INT_MAP_REG 0x2F |
kikoaac | 0:15e41c824e3b | 28 | #define ADXL345_INT_SOURCE_REG 0x30 |
kikoaac | 0:15e41c824e3b | 29 | #define ADXL345_DATA_FORMAT_REG 0x31 |
kikoaac | 0:15e41c824e3b | 30 | #define ADXL345_DATAX0_REG 0x32 |
kikoaac | 0:15e41c824e3b | 31 | #define ADXL345_DATAX1_REG 0x33 |
kikoaac | 0:15e41c824e3b | 32 | #define ADXL345_DATAY0_REG 0x34 |
kikoaac | 0:15e41c824e3b | 33 | #define ADXL345_DATAY1_REG 0x35 |
kikoaac | 0:15e41c824e3b | 34 | #define ADXL345_DATAZ0_REG 0x36 |
kikoaac | 0:15e41c824e3b | 35 | #define ADXL345_DATAZ1_REG 0x37 |
kikoaac | 0:15e41c824e3b | 36 | #define ADXL345_FIFO_CTL 0x38 |
kikoaac | 0:15e41c824e3b | 37 | #define ADXL345_FIFO_STATUS 0x39 |
kikoaac | 0:15e41c824e3b | 38 | |
kikoaac | 0:15e41c824e3b | 39 | #define ADXL345_3200HZ 0x0F |
kikoaac | 0:15e41c824e3b | 40 | #define ADXL345_1600HZ 0x0E |
kikoaac | 0:15e41c824e3b | 41 | #define ADXL345_800HZ 0x0D |
kikoaac | 0:15e41c824e3b | 42 | #define ADXL345_400HZ 0x0C |
kikoaac | 0:15e41c824e3b | 43 | #define ADXL345_200HZ 0x0B |
kikoaac | 0:15e41c824e3b | 44 | #define ADXL345_100HZ 0x0A |
kikoaac | 0:15e41c824e3b | 45 | #define ADXL345_50HZ 0x09 |
kikoaac | 0:15e41c824e3b | 46 | #define ADXL345_25HZ 0x08 |
kikoaac | 0:15e41c824e3b | 47 | #define ADXL345_12HZ5 0x07 |
kikoaac | 0:15e41c824e3b | 48 | #define ADXL345_6HZ25 0x06 |
kikoaac | 0:15e41c824e3b | 49 | |
kikoaac | 0:15e41c824e3b | 50 | #define ADXL345_X 0x00 |
kikoaac | 0:15e41c824e3b | 51 | #define ADXL345_Y 0x01 |
kikoaac | 0:15e41c824e3b | 52 | #define ADXL345_Z 0x02 |
kikoaac | 0:15e41c824e3b | 53 | |
kikoaac | 0:15e41c824e3b | 54 | #define MeasurementMode 0x08 |
kikoaac | 0:15e41c824e3b | 55 | |
kikoaac | 0:15e41c824e3b | 56 | #define GYR_ADDRESS 0xD4 |
kikoaac | 0:15e41c824e3b | 57 | #define L3GD20_WHO_AM_I 0x0F |
kikoaac | 0:15e41c824e3b | 58 | |
kikoaac | 0:15e41c824e3b | 59 | #define L3GD20_CTRL_REG1 0x20 |
kikoaac | 0:15e41c824e3b | 60 | #define L3GD20_CTRL_REG2 0x21 |
kikoaac | 0:15e41c824e3b | 61 | #define L3GD20_CTRL_REG3 0x22 |
kikoaac | 0:15e41c824e3b | 62 | #define L3GD20_CTRL_REG4 0x23 |
kikoaac | 0:15e41c824e3b | 63 | #define L3GD20_CTRL_REG5 0x24 |
kikoaac | 0:15e41c824e3b | 64 | #define L3GD20_REFERENCE 0x25 |
kikoaac | 0:15e41c824e3b | 65 | #define L3GD20_OUT_TEMP 0x26 |
kikoaac | 0:15e41c824e3b | 66 | #define L3GD20_STATUS_REG 0x27 |
kikoaac | 0:15e41c824e3b | 67 | |
kikoaac | 0:15e41c824e3b | 68 | #define L3GD20_OUT_X_L 0x28 |
kikoaac | 0:15e41c824e3b | 69 | #define L3GD20_OUT_X_H 0x29 |
kikoaac | 0:15e41c824e3b | 70 | #define L3GD20_OUT_Y_L 0x2A |
kikoaac | 0:15e41c824e3b | 71 | #define L3GD20_OUT_Y_H 0x2B |
kikoaac | 0:15e41c824e3b | 72 | #define L3GD20_OUT_Z_L 0x2C |
kikoaac | 0:15e41c824e3b | 73 | #define L3GD20_OUT_Z_H 0x2D |
kikoaac | 0:15e41c824e3b | 74 | |
kikoaac | 0:15e41c824e3b | 75 | #define L3GD20_FIFO_CTRL_REG 0x2E |
kikoaac | 0:15e41c824e3b | 76 | #define L3GD20_FIFO_SRC_REG 0x2F |
kikoaac | 0:15e41c824e3b | 77 | |
kikoaac | 0:15e41c824e3b | 78 | #define L3GD20_INT1_CFG 0x30 |
kikoaac | 0:15e41c824e3b | 79 | #define L3GD20_INT1_SRC 0x31 |
kikoaac | 0:15e41c824e3b | 80 | #define L3GD20_INT1_THS_XH 0x32 |
kikoaac | 0:15e41c824e3b | 81 | #define L3GD20_INT1_THS_XL 0x33 |
kikoaac | 0:15e41c824e3b | 82 | #define L3GD20_INT1_THS_YH 0x34 |
kikoaac | 0:15e41c824e3b | 83 | #define L3GD20_INT1_THS_YL 0x35 |
kikoaac | 0:15e41c824e3b | 84 | #define L3GD20_INT1_THS_ZH 0x36 |
kikoaac | 0:15e41c824e3b | 85 | #define L3GD20_INT1_THS_ZL 0x37 |
kikoaac | 0:15e41c824e3b | 86 | #define L3GD20_INT1_DURATION 0x38 |
kikoaac | 0:15e41c824e3b | 87 | class kalman; |
kikoaac | 0:15e41c824e3b | 88 | class ANGLE{ |
kikoaac | 0:15e41c824e3b | 89 | private: |
kikoaac | 0:15e41c824e3b | 90 | class kalman |
kikoaac | 0:15e41c824e3b | 91 | { |
kikoaac | 0:15e41c824e3b | 92 | public: |
kikoaac | 0:15e41c824e3b | 93 | kalman(void); |
kikoaac | 0:15e41c824e3b | 94 | double getAngle(double newAngle, double newRate, double dt); |
kikoaac | 0:15e41c824e3b | 95 | |
kikoaac | 0:15e41c824e3b | 96 | void setAngle(double newAngle); |
kikoaac | 0:15e41c824e3b | 97 | void setQangle(double newQ_angle); |
kikoaac | 0:15e41c824e3b | 98 | void setQgyroBias(double newQ_gyroBias); |
kikoaac | 0:15e41c824e3b | 99 | void setRangle(double newR_angle); |
kikoaac | 0:15e41c824e3b | 100 | |
kikoaac | 0:15e41c824e3b | 101 | double getRate(void); |
kikoaac | 0:15e41c824e3b | 102 | double getQangle(void); |
kikoaac | 0:15e41c824e3b | 103 | double getQbias(void); |
kikoaac | 0:15e41c824e3b | 104 | double getRangle(void); |
kikoaac | 0:15e41c824e3b | 105 | |
kikoaac | 0:15e41c824e3b | 106 | |
kikoaac | 0:15e41c824e3b | 107 | private: |
kikoaac | 0:15e41c824e3b | 108 | double P[2][2]; //error covariance matrix |
kikoaac | 0:15e41c824e3b | 109 | double K[2]; //kalman gain |
kikoaac | 0:15e41c824e3b | 110 | double y; //angle difference |
kikoaac | 0:15e41c824e3b | 111 | double S; //estimation error |
kikoaac | 0:15e41c824e3b | 112 | |
kikoaac | 0:15e41c824e3b | 113 | double rate; //rate in deg/s |
kikoaac | 0:15e41c824e3b | 114 | double angle; //kalman angle |
kikoaac | 0:15e41c824e3b | 115 | double bias; //kalman gyro bias |
kikoaac | 0:15e41c824e3b | 116 | |
kikoaac | 0:15e41c824e3b | 117 | double Q_angle; //process noise variance for the angle of the accelerometer |
kikoaac | 0:15e41c824e3b | 118 | double Q_gyroBias; //process noise variance for the gyroscope bias |
kikoaac | 0:15e41c824e3b | 119 | double R_angle; //measurement noise variance |
kikoaac | 0:15e41c824e3b | 120 | }; |
kikoaac | 0:15e41c824e3b | 121 | I2C i2c_; |
kikoaac | 0:15e41c824e3b | 122 | //ACC ADXL345 |
kikoaac | 0:15e41c824e3b | 123 | int x_acc,y_acc,z_acc,sampleNum; |
kikoaac | 0:15e41c824e3b | 124 | double x_offset,y_offset,z_offset; |
kikoaac | 0:15e41c824e3b | 125 | float gx,gy,gz,xnoise,ynoise,znoise; |
kikoaac | 0:15e41c824e3b | 126 | //GYALO L3GD20 |
kikoaac | 0:15e41c824e3b | 127 | double Rate; |
kikoaac | 0:15e41c824e3b | 128 | double sampleTime; |
kikoaac | 0:15e41c824e3b | 129 | float noise[3]; |
kikoaac | 0:15e41c824e3b | 130 | short offset[3];short rate[3],prev_rate[3]; |
kikoaac | 0:15e41c824e3b | 131 | double angle[3],offset_angle[3],Synthesis_angle[3],kalman_angle[3],comp_angle[3]; |
kikoaac | 0:15e41c824e3b | 132 | double t[3]; |
kikoaac | 0:15e41c824e3b | 133 | short tempDATA_ACC[3],tempDATA_ANGLE[3]; |
kikoaac | 0:15e41c824e3b | 134 | //KALMAN |
kikoaac | 0:15e41c824e3b | 135 | kalman kalma[3]; |
kikoaac | 0:15e41c824e3b | 136 | |
kikoaac | 0:15e41c824e3b | 137 | char data_single_get(char reg); |
kikoaac | 0:15e41c824e3b | 138 | int data_single_put(char reg,char data); |
kikoaac | 0:15e41c824e3b | 139 | void data_multi_get(char reg,char* data, int size); |
kikoaac | 0:15e41c824e3b | 140 | int data_multi_put(char reg, char* data, int size); |
kikoaac | 0:15e41c824e3b | 141 | bool write_reg(int addr_i2c,int addr_reg, char v); |
kikoaac | 0:15e41c824e3b | 142 | bool read_reg(int addr_i2c,int addr_reg, char *v); |
kikoaac | 0:15e41c824e3b | 143 | bool recv(char sad, char sub, char *buf, int length); |
kikoaac | 0:15e41c824e3b | 144 | public: |
kikoaac | 0:15e41c824e3b | 145 | ANGLE(PinName sda, PinName scl); |
kikoaac | 0:15e41c824e3b | 146 | void ADXL_setup(); |
kikoaac | 0:15e41c824e3b | 147 | void ADXL_setnum(int Num=500,float time=0.001,double rate=0.00390635);//set data member |
kikoaac | 0:15e41c824e3b | 148 | void Synthesis(double* X,double* Y); |
kikoaac | 0:15e41c824e3b | 149 | void getaxis_acc(short* DATA_ACC); |
kikoaac | 0:15e41c824e3b | 150 | char getdeviceID(){return data_single_get(ADXL345_DEVID_REG);} |
kikoaac | 0:15e41c824e3b | 151 | void getangle_acc(double* DATA_ANGLE); |
kikoaac | 0:15e41c824e3b | 152 | void get_angle_rate(double *x,double *y,double *z); |
kikoaac | 0:15e41c824e3b | 153 | void get_rate(short* RATE); |
kikoaac | 0:15e41c824e3b | 154 | void set_angle();//set always [time] |
kikoaac | 0:15e41c824e3b | 155 | void set_angle(double ANG_x,double ANG_y,double ANG_z); |
kikoaac | 0:15e41c824e3b | 156 | void get_angle(double *x,double *y,double *z); |
kikoaac | 0:15e41c824e3b | 157 | void get_Synthesis_angle(double* X,double* Y); |
kikoaac | 0:15e41c824e3b | 158 | void get_Kalman_angle(double* X,double* Y); |
kikoaac | 0:15e41c824e3b | 159 | void get_Comp_angle(double* X,double* Y); |
kikoaac | 0:15e41c824e3b | 160 | void set_noise(); |
kikoaac | 0:15e41c824e3b | 161 | void set_offset(); |
kikoaac | 0:15e41c824e3b | 162 | void set_angleoffset(); |
kikoaac | 0:15e41c824e3b | 163 | |
kikoaac | 0:15e41c824e3b | 164 | |
kikoaac | 0:15e41c824e3b | 165 | double getRate(void); |
kikoaac | 0:15e41c824e3b | 166 | double getQangle(void); |
kikoaac | 0:15e41c824e3b | 167 | double getQbias(void); |
kikoaac | 0:15e41c824e3b | 168 | double getRangle(void); |
kikoaac | 0:15e41c824e3b | 169 | |
kikoaac | 0:15e41c824e3b | 170 | |
kikoaac | 0:15e41c824e3b | 171 | int setPowerMode(char mode); |
kikoaac | 0:15e41c824e3b | 172 | |
kikoaac | 0:15e41c824e3b | 173 | char getPowerControl(){return data_single_get(ADXL345_POWER_CTL_REG);} |
kikoaac | 0:15e41c824e3b | 174 | int setPowerControl(char control){return data_single_put(ADXL345_POWER_CTL_REG, control);} |
kikoaac | 0:15e41c824e3b | 175 | |
kikoaac | 0:15e41c824e3b | 176 | char getDataFormatControl(void){return data_single_get(ADXL345_DATA_FORMAT_REG);} |
kikoaac | 0:15e41c824e3b | 177 | int setDataFormatControl(char control){return data_single_put(ADXL345_DATA_FORMAT_REG, control);} |
kikoaac | 0:15e41c824e3b | 178 | |
kikoaac | 0:15e41c824e3b | 179 | int setDataRate(char rate); |
kikoaac | 0:15e41c824e3b | 180 | |
kikoaac | 0:15e41c824e3b | 181 | char getOffset(char axis); |
kikoaac | 0:15e41c824e3b | 182 | int setOffset(char axis, char offset); |
kikoaac | 0:15e41c824e3b | 183 | |
kikoaac | 0:15e41c824e3b | 184 | char getFifoControl(void){return data_single_get(ADXL345_FIFO_CTL);} |
kikoaac | 0:15e41c824e3b | 185 | int setFifoControl(char settings){return data_single_put(ADXL345_FIFO_STATUS, settings);} |
kikoaac | 0:15e41c824e3b | 186 | char getFifoStatus(void){return data_single_get(ADXL345_FIFO_STATUS);} |
kikoaac | 0:15e41c824e3b | 187 | |
kikoaac | 0:15e41c824e3b | 188 | char getTapThreshold(void){return data_single_get(ADXL345_THRESH_TAP_REG);} |
kikoaac | 0:15e41c824e3b | 189 | int setTapThreshold(char threshold){return data_single_put(ADXL345_THRESH_TAP_REG, threshold);} |
kikoaac | 0:15e41c824e3b | 190 | float getTapDuration(void){return (float)data_single_get(ADXL345_DUR_REG)*625;} |
kikoaac | 0:15e41c824e3b | 191 | int setTapDuration(short int duration_us); |
kikoaac | 0:15e41c824e3b | 192 | float getTapLatency(void){return (float)data_single_get(ADXL345_LATENT_REG)*1.25;} |
kikoaac | 0:15e41c824e3b | 193 | int setTapLatency(short int latency_ms); |
kikoaac | 0:15e41c824e3b | 194 | |
kikoaac | 0:15e41c824e3b | 195 | float getWindowTime(void){return (float)data_single_get(ADXL345_WINDOW_REG)*1.25;} |
kikoaac | 0:15e41c824e3b | 196 | int setWindowTime(short int window_ms); |
kikoaac | 0:15e41c824e3b | 197 | |
kikoaac | 0:15e41c824e3b | 198 | char getActivityThreshold(void){return data_single_get(ADXL345_THRESH_ACT_REG);} |
kikoaac | 0:15e41c824e3b | 199 | int setActivityThreshold(char threshold){return data_single_put(ADXL345_THRESH_ACT_REG, threshold);} |
kikoaac | 0:15e41c824e3b | 200 | |
kikoaac | 0:15e41c824e3b | 201 | char getInactivityThreshold(void){return data_single_get(ADXL345_THRESH_INACT_REG);} |
kikoaac | 0:15e41c824e3b | 202 | int setInactivityThreshold(char threshold){return data_single_put(ADXL345_THRESH_INACT_REG, threshold);} |
kikoaac | 0:15e41c824e3b | 203 | char getTimeInactivity(void){return data_single_get(ADXL345_TIME_INACT_REG);} |
kikoaac | 0:15e41c824e3b | 204 | int setTimeInactivity(char timeInactivity){return data_single_put(ADXL345_TIME_INACT_REG, timeInactivity);} |
kikoaac | 0:15e41c824e3b | 205 | |
kikoaac | 0:15e41c824e3b | 206 | char getActivityInactivityControl(void){return data_single_get(ADXL345_ACT_INACT_CTL_REG);} |
kikoaac | 0:15e41c824e3b | 207 | int setActivityInactivityControl(char settings){return data_single_put(ADXL345_ACT_INACT_CTL_REG, settings);} |
kikoaac | 0:15e41c824e3b | 208 | |
kikoaac | 0:15e41c824e3b | 209 | char getFreefallThreshold(void){return data_single_get(ADXL345_THRESH_FF_REG);} |
kikoaac | 0:15e41c824e3b | 210 | int setFreefallThreshold(char threshold){return data_single_put(ADXL345_THRESH_FF_REG, threshold);} |
kikoaac | 0:15e41c824e3b | 211 | char getFreefallTime(void){return data_single_get(ADXL345_TIME_FF_REG)*5;} |
kikoaac | 0:15e41c824e3b | 212 | int setFreefallTime(short int freefallTime_ms); |
kikoaac | 0:15e41c824e3b | 213 | |
kikoaac | 0:15e41c824e3b | 214 | char getTapAxisControl(void){return data_single_get(ADXL345_TAP_AXES_REG);} |
kikoaac | 0:15e41c824e3b | 215 | int setTapAxisControl(char settings){return data_single_put(ADXL345_TAP_AXES_REG, settings);} |
kikoaac | 0:15e41c824e3b | 216 | char getTapSource(void){return data_single_get(ADXL345_ACT_TAP_STATUS_REG);} |
kikoaac | 0:15e41c824e3b | 217 | |
kikoaac | 0:15e41c824e3b | 218 | char getInterruptEnableControl(void){return data_single_get(ADXL345_INT_ENABLE_REG);} |
kikoaac | 0:15e41c824e3b | 219 | int setInterruptEnableControl(char settings){return data_single_put(ADXL345_INT_ENABLE_REG, settings);}; |
kikoaac | 0:15e41c824e3b | 220 | char getInterruptMappingControl(void){return data_single_get(ADXL345_INT_MAP_REG);}; |
kikoaac | 0:15e41c824e3b | 221 | int setInterruptMappingControl(char settings){return data_single_put(ADXL345_INT_MAP_REG, settings);}; |
kikoaac | 0:15e41c824e3b | 222 | char getInterruptSource(void){return data_single_get(ADXL345_INT_SOURCE_REG);} |
kikoaac | 0:15e41c824e3b | 223 | bool read(float *gx, float *gy, float *gz); |
kikoaac | 0:15e41c824e3b | 224 | bool read(short *axis); |
kikoaac | 0:15e41c824e3b | 225 | |
kikoaac | 0:15e41c824e3b | 226 | }; |
kikoaac | 0:15e41c824e3b | 227 | |
kikoaac | 0:15e41c824e3b | 228 | #endif |