Kiko Ishimoto / ANGLE

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 Kiko Ishimoto

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?

UserRevisionLine numberNew 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