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.cpp@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 #include "angle.h"
kikoaac 0:15e41c824e3b 2 #include "mbed.h"
kikoaac 0:15e41c824e3b 3 ANGLE::ANGLE(PinName sda, PinName scl) : i2c_(sda, scl) {
kikoaac 0:15e41c824e3b 4 Rate=0.00390635;sampleTime=0.001;sampleNum=500;
kikoaac 0:15e41c824e3b 5
kikoaac 0:15e41c824e3b 6 kalma[0].setAngle(0);
kikoaac 0:15e41c824e3b 7 kalma[1].setAngle(0);
kikoaac 0:15e41c824e3b 8 kalma[2].setAngle(0);
kikoaac 0:15e41c824e3b 9 //400kHz, allowing us to use the fastest data rates.
kikoaac 0:15e41c824e3b 10 i2c_.frequency(400000);
kikoaac 0:15e41c824e3b 11 // initialize the BW data rate
kikoaac 0:15e41c824e3b 12 char tx[2];
kikoaac 0:15e41c824e3b 13 tx[0] = ADXL345_BW_RATE_REG;
kikoaac 0:15e41c824e3b 14 tx[1] = ADXL345_200HZ; //value greater than or equal to 0x0A is written into the rate bits (Bit D3 through Bit D0) in the BW_RATE register
kikoaac 0:15e41c824e3b 15 i2c_.write( acc_i2c_write , tx, 2);
kikoaac 0:15e41c824e3b 16
kikoaac 0:15e41c824e3b 17 //Data format (for +-16g) - This is done by setting Bit D3 of the DATA_FORMAT register (Address 0x31) and writing a value of 0x03 to the range bits (Bit D1 and Bit D0) of the DATA_FORMAT register (Address 0x31).
kikoaac 0:15e41c824e3b 18
kikoaac 0:15e41c824e3b 19 char rx[2];
kikoaac 0:15e41c824e3b 20 rx[0] = ADXL345_DATA_FORMAT_REG;
kikoaac 0:15e41c824e3b 21 rx[1] = 0x0B;
kikoaac 0:15e41c824e3b 22 // full res and +_16g
kikoaac 0:15e41c824e3b 23 i2c_.write( acc_i2c_write , rx, 2);
kikoaac 0:15e41c824e3b 24
kikoaac 0:15e41c824e3b 25 // Set Offset - programmed into the OFSX, OFSY, and OFXZ registers, respectively, as 0xFD, 0x03 and 0xFE.
kikoaac 0:15e41c824e3b 26 char x[2];
kikoaac 0:15e41c824e3b 27 x[0] = ADXL345_OFSX_REG ;
kikoaac 0:15e41c824e3b 28 x[1] = 253;
kikoaac 0:15e41c824e3b 29 i2c_.write( acc_i2c_write , x, 2);
kikoaac 0:15e41c824e3b 30 char y[2];
kikoaac 0:15e41c824e3b 31 y[0] = ADXL345_OFSY_REG ;
kikoaac 0:15e41c824e3b 32 y[1] = 5;
kikoaac 0:15e41c824e3b 33 i2c_.write( acc_i2c_write, y, 2);
kikoaac 0:15e41c824e3b 34 char z[2];
kikoaac 0:15e41c824e3b 35 z[0] = ADXL345_OFSZ_REG ;
kikoaac 0:15e41c824e3b 36 z[1] = 0xFE;
kikoaac 0:15e41c824e3b 37 i2c_.write( acc_i2c_write , z, 2);
kikoaac 0:15e41c824e3b 38 char reg_v;
kikoaac 0:15e41c824e3b 39 sampleTime=0.001;
kikoaac 0:15e41c824e3b 40 sampleNum=500;
kikoaac 0:15e41c824e3b 41 angle[0]=angle[1]=angle[2]=0;
kikoaac 0:15e41c824e3b 42 prev_rate[0]=prev_rate[1]=prev_rate[2]=0;
kikoaac 0:15e41c824e3b 43
kikoaac 0:15e41c824e3b 44 // 0x0F = 0b00001111
kikoaac 0:15e41c824e3b 45 // Normal power mode, all axes enabled
kikoaac 0:15e41c824e3b 46 reg_v = 0;
kikoaac 0:15e41c824e3b 47 reg_v |= 0x0F;
kikoaac 0:15e41c824e3b 48 write_reg(GYR_ADDRESS,L3GD20_CTRL_REG1,reg_v);
kikoaac 0:15e41c824e3b 49 set_offset();
kikoaac 0:15e41c824e3b 50 set_noise();
kikoaac 0:15e41c824e3b 51
kikoaac 0:15e41c824e3b 52 offset_angle[0]=0;
kikoaac 0:15e41c824e3b 53 offset_angle[1]=0;
kikoaac 0:15e41c824e3b 54 offset_angle[2]=0;
kikoaac 0:15e41c824e3b 55 ADXL_setup();
kikoaac 0:15e41c824e3b 56 set_angleoffset();
kikoaac 0:15e41c824e3b 57 }
kikoaac 0:15e41c824e3b 58 void ANGLE::ADXL_setup(){
kikoaac 0:15e41c824e3b 59 z_offset=2;x_offset=0;y_offset=0;
kikoaac 0:15e41c824e3b 60 char buffer[6];
kikoaac 0:15e41c824e3b 61
kikoaac 0:15e41c824e3b 62 for(int i=0;i<sampleNum;i++)
kikoaac 0:15e41c824e3b 63 {
kikoaac 0:15e41c824e3b 64 data_multi_get(ADXL345_DATAX0_REG, buffer, 6);
kikoaac 0:15e41c824e3b 65 int Xacc = (int)buffer[1] << 8 | (int)buffer[0];
kikoaac 0:15e41c824e3b 66 int Yacc = (int)buffer[3] << 8 | (int)buffer[2];
kikoaac 0:15e41c824e3b 67 int Zacc = (int)buffer[5] << 8 | (int)buffer[4]-255;
kikoaac 0:15e41c824e3b 68 if((int)Xacc-x_offset>xnoise)
kikoaac 0:15e41c824e3b 69 xnoise=(int)Xacc-x_offset;
kikoaac 0:15e41c824e3b 70 else if((int)Xacc-x_offset<-xnoise)
kikoaac 0:15e41c824e3b 71 xnoise=-(int)Xacc-x_offset;
kikoaac 0:15e41c824e3b 72
kikoaac 0:15e41c824e3b 73 if((int)Yacc-y_offset>ynoise)
kikoaac 0:15e41c824e3b 74 ynoise=(int)Yacc-y_offset;
kikoaac 0:15e41c824e3b 75 else if((int)Yacc-y_offset<-ynoise)
kikoaac 0:15e41c824e3b 76 ynoise=-(int)Yacc-y_offset;
kikoaac 0:15e41c824e3b 77
kikoaac 0:15e41c824e3b 78 if((int)Zacc-z_offset>znoise)
kikoaac 0:15e41c824e3b 79 znoise=(int)Zacc-z_offset;
kikoaac 0:15e41c824e3b 80 else if((int)Zacc-z_offset<-znoise)
kikoaac 0:15e41c824e3b 81 znoise=-(int)Zacc-z_offset;
kikoaac 0:15e41c824e3b 82 }
kikoaac 0:15e41c824e3b 83
kikoaac 0:15e41c824e3b 84
kikoaac 0:15e41c824e3b 85 }
kikoaac 0:15e41c824e3b 86 void ANGLE::ADXL_setnum(int Num,float time,double rate){
kikoaac 0:15e41c824e3b 87 sampleNum=Num;sampleTime=time;Rate=rate;
kikoaac 0:15e41c824e3b 88 }
kikoaac 0:15e41c824e3b 89 char ANGLE::data_single_get(char reg){
kikoaac 0:15e41c824e3b 90 char tx = reg;
kikoaac 0:15e41c824e3b 91 char output;
kikoaac 0:15e41c824e3b 92 i2c_.write( acc_i2c_write , &tx, 1); //tell it what you want to read
kikoaac 0:15e41c824e3b 93 i2c_.read( acc_i2c_read , &output, 1); //tell it where to store the data
kikoaac 0:15e41c824e3b 94 return output;
kikoaac 0:15e41c824e3b 95 }
kikoaac 0:15e41c824e3b 96 int ANGLE::data_single_put(char reg, char data){
kikoaac 0:15e41c824e3b 97 int ack = 0;
kikoaac 0:15e41c824e3b 98 char tx[2];
kikoaac 0:15e41c824e3b 99 tx[0] = reg;
kikoaac 0:15e41c824e3b 100 tx[1] = data;
kikoaac 0:15e41c824e3b 101 return ack | i2c_.write( acc_i2c_write , tx, 2);
kikoaac 0:15e41c824e3b 102 }
kikoaac 0:15e41c824e3b 103
kikoaac 0:15e41c824e3b 104
kikoaac 0:15e41c824e3b 105
kikoaac 0:15e41c824e3b 106 void ANGLE::data_multi_get(char reg, char* data, int size) {
kikoaac 0:15e41c824e3b 107 i2c_.write( acc_i2c_write, &reg, 1); //tell it where to read from
kikoaac 0:15e41c824e3b 108 i2c_.read( acc_i2c_read , data, size); //tell it where to store the data read
kikoaac 0:15e41c824e3b 109 }
kikoaac 0:15e41c824e3b 110
kikoaac 0:15e41c824e3b 111
kikoaac 0:15e41c824e3b 112 int ANGLE::data_multi_put(char reg, char* data, int size) {
kikoaac 0:15e41c824e3b 113 int ack;
kikoaac 0:15e41c824e3b 114
kikoaac 0:15e41c824e3b 115 ack = i2c_.write( acc_i2c_write, &reg, 1); //tell it where to write to
kikoaac 0:15e41c824e3b 116 return ack | i2c_.write( acc_i2c_read, data, size); //tell it what data to write
kikoaac 0:15e41c824e3b 117 }
kikoaac 0:15e41c824e3b 118 void ANGLE::getangle_acc(double* DATA_ANGLE){
kikoaac 0:15e41c824e3b 119 char buffer[6];
kikoaac 0:15e41c824e3b 120 short data[3];
kikoaac 0:15e41c824e3b 121 //data_multi_get(ADXL345_DATAX0_REG, buffer, 6);
kikoaac 0:15e41c824e3b 122 getaxis_acc(data);
kikoaac 0:15e41c824e3b 123 // calculate the absolute of the magnetic field vector // 8-Bit pieces of axis data
kikoaac 0:15e41c824e3b 124 // read axis registers using I2C
kikoaac 0:15e41c824e3b 125
kikoaac 0:15e41c824e3b 126 /*data[0] = (short) (buffer[1] << 8 | buffer[0]);//-x_offset; // join 8-Bit pieces to 16-bit short integers
kikoaac 0:15e41c824e3b 127 data[1] = (short) (buffer[3] << 8 | buffer[2]);//-y_offset;
kikoaac 0:15e41c824e3b 128 data[2] = (short) (buffer[5] << 8 | buffer[4]);//-z_offset;*/
kikoaac 0:15e41c824e3b 129 float R = sqrt(pow((float)data[0],2) + pow((float)data[1],2) + pow((float)data[2],2));
kikoaac 0:15e41c824e3b 130 DATA_ANGLE[1] = -((180 / 3.1415) * acos((float)data[1] / R)-90); // roll - angle of magnetic field vector in x direction
kikoaac 0:15e41c824e3b 131 DATA_ANGLE[0] = (180 / 3.1415) * acos((float)data[0] / R)-90; // pitch - angle of magnetic field vector in y direction
kikoaac 0:15e41c824e3b 132 DATA_ANGLE[2] = (180 / 3.1415) * acos((float)data[2] / R); //*/
kikoaac 0:15e41c824e3b 133 DATA_ANGLE[0] = atan2(data[0],sqrt(pow((float)data[1],2)+pow((float)data[2],2)))*180/3.1415;
kikoaac 0:15e41c824e3b 134 DATA_ANGLE[1] = atan2(data[1],sqrt(pow((float)data[0],2)+pow((float)data[2],2)))*180/3.1415;
kikoaac 0:15e41c824e3b 135 DATA_ANGLE[2] = atan2(sqrt(pow((float)data[1],2)+pow((float)data[2],2)),data[2])*180/3.1415;
kikoaac 0:15e41c824e3b 136 /*DATA_ANGLE[0]=atan2((double)data[0],(double)data[2])* (180 / 3.1415);
kikoaac 0:15e41c824e3b 137 DATA_ANGLE[1]=atan2((double)data[1],(double)data[2])* (180 / 3.1415);
kikoaac 0:15e41c824e3b 138 /* if(data[0]>255)
kikoaac 0:15e41c824e3b 139 DATA_ANGLE[0]=-90;
kikoaac 0:15e41c824e3b 140 else if(data[0]<-263)
kikoaac 0:15e41c824e3b 141 DATA_ANGLE[0]=90;
kikoaac 0:15e41c824e3b 142 if(data[1]>260)
kikoaac 0:15e41c824e3b 143 DATA_ANGLE[1]=90;
kikoaac 0:15e41c824e3b 144 else if(data[1]<-246)
kikoaac 0:15e41c824e3b 145 DATA_ANGLE[1]=-90;
kikoaac 0:15e41c824e3b 146 /*if(DATA[1]>250)
kikoaac 0:15e41c824e3b 147 DATA_ANGLE[1]=90;
kikoaac 0:15e41c824e3b 148 else if(DATA[1]<-260)
kikoaac 0:15e41c824e3b 149 DATA_ANGLE[1]=-90;
kikoaac 0:15e41c824e3b 150 if(DATA[0]>250)
kikoaac 0:15e41c824e3b 151 DATA_ANGLE[0]=90;
kikoaac 0:15e41c824e3b 152 else if(DATA[0]<-260)
kikoaac 0:15e41c824e3b 153 DATA_ANGLE[0]=-90;*/
kikoaac 0:15e41c824e3b 154 }
kikoaac 0:15e41c824e3b 155 void ANGLE::getaxis_acc(short* DATA_ACC){
kikoaac 0:15e41c824e3b 156 char buffer[6];
kikoaac 0:15e41c824e3b 157 data_multi_get(ADXL345_DATAX0_REG, buffer, 6);
kikoaac 0:15e41c824e3b 158
kikoaac 0:15e41c824e3b 159 DATA_ACC[0] = ((short)buffer[1] << 8 | (short)buffer[0]);//+0.1*tempDATA_ACC[0];//-x_offset;
kikoaac 0:15e41c824e3b 160 DATA_ACC[1] = ((short)buffer[3] << 8 | (short)buffer[2]);//+0.1*tempDATA_ACC[1];//-y_offset;
kikoaac 0:15e41c824e3b 161 DATA_ACC[2] = ((short)buffer[5] << 8 | (short)buffer[4]);//+0.1*tempDATA_ACC[2];//-z_offset;
kikoaac 0:15e41c824e3b 162 DATA_ACC[0] = (short)(DATA_ACC[0]*0.9+tempDATA_ACC[0]*0.1);
kikoaac 0:15e41c824e3b 163 DATA_ACC[1] = (short)(DATA_ACC[1]*0.9+tempDATA_ACC[0]*0.1);
kikoaac 0:15e41c824e3b 164 DATA_ACC[2] = (short)(DATA_ACC[2]*0.9+tempDATA_ACC[0]*0.1);
kikoaac 0:15e41c824e3b 165 tempDATA_ACC[0]=DATA_ACC[0];
kikoaac 0:15e41c824e3b 166 tempDATA_ACC[1]=DATA_ACC[1];
kikoaac 0:15e41c824e3b 167 tempDATA_ACC[2]=DATA_ACC[2];//*/
kikoaac 0:15e41c824e3b 168 }
kikoaac 0:15e41c824e3b 169 void ANGLE::get_rate(short* RATE)
kikoaac 0:15e41c824e3b 170 {
kikoaac 0:15e41c824e3b 171 short axis[3];
kikoaac 0:15e41c824e3b 172 short offset_t[3]={-1,+1,0};
kikoaac 0:15e41c824e3b 173 read(axis);
kikoaac 0:15e41c824e3b 174 for(int i=0; i<3; i++){
kikoaac 0:15e41c824e3b 175 RATE[i]=(short)(axis[i])*0.01-offset_t[i];
kikoaac 0:15e41c824e3b 176 //RATE[i]=(floor(RATE[i]));
kikoaac 0:15e41c824e3b 177 }
kikoaac 0:15e41c824e3b 178 }
kikoaac 0:15e41c824e3b 179 void ANGLE::get_angle(double *x,double *y,double *z)
kikoaac 0:15e41c824e3b 180 {
kikoaac 0:15e41c824e3b 181 *x=(floor(angle[0]*100.0)/100.0);
kikoaac 0:15e41c824e3b 182 *y=(floor(angle[1]*100.0)/100.0);
kikoaac 0:15e41c824e3b 183 *z=(floor(angle[2]*100.0)/100.0);
kikoaac 0:15e41c824e3b 184 }
kikoaac 0:15e41c824e3b 185 void ANGLE::get_angle_rate(double *x,double *y,double *z)
kikoaac 0:15e41c824e3b 186 {
kikoaac 0:15e41c824e3b 187
kikoaac 0:15e41c824e3b 188 *x=t[0];
kikoaac 0:15e41c824e3b 189 *y=t[1];
kikoaac 0:15e41c824e3b 190 *z=t[2];
kikoaac 0:15e41c824e3b 191 }
kikoaac 0:15e41c824e3b 192
kikoaac 0:15e41c824e3b 193 void ANGLE::get_Synthesis_angle(double* X,double* Y)
kikoaac 0:15e41c824e3b 194 {
kikoaac 0:15e41c824e3b 195 *X=Synthesis_angle[0];
kikoaac 0:15e41c824e3b 196 *Y=Synthesis_angle[1];
kikoaac 0:15e41c824e3b 197 }
kikoaac 0:15e41c824e3b 198 void ANGLE::get_Comp_angle(double* X,double* Y)
kikoaac 0:15e41c824e3b 199 {
kikoaac 0:15e41c824e3b 200 *X=comp_angle[0];
kikoaac 0:15e41c824e3b 201 *Y=comp_angle[1];
kikoaac 0:15e41c824e3b 202 }
kikoaac 0:15e41c824e3b 203 void ANGLE::get_Kalman_angle(double* X,double* Y)
kikoaac 0:15e41c824e3b 204 {
kikoaac 0:15e41c824e3b 205 *X=kalman_angle[0];
kikoaac 0:15e41c824e3b 206 *Y=kalman_angle[1];
kikoaac 0:15e41c824e3b 207 }
kikoaac 0:15e41c824e3b 208 void ANGLE::set_angle(double ANG_x,double ANG_y,double ANG_z)
kikoaac 0:15e41c824e3b 209 {
kikoaac 0:15e41c824e3b 210 Synthesis_angle[0]=angle[0]=ANG_x;
kikoaac 0:15e41c824e3b 211 Synthesis_angle[1]=angle[1]=ANG_y;
kikoaac 0:15e41c824e3b 212 angle[2]=ANG_z;
kikoaac 0:15e41c824e3b 213 }
kikoaac 0:15e41c824e3b 214 void ANGLE::set_angle()
kikoaac 0:15e41c824e3b 215 {
kikoaac 0:15e41c824e3b 216 get_rate(rate);
kikoaac 0:15e41c824e3b 217 double g[3], d[3];
kikoaac 0:15e41c824e3b 218 get_angle(g,g+1,g+2);
kikoaac 0:15e41c824e3b 219 getangle_acc(d);
kikoaac 0:15e41c824e3b 220 double S[3];
kikoaac 0:15e41c824e3b 221 for(int i=0; i<3; i++) {
kikoaac 0:15e41c824e3b 222 //rate[i]=rate[i]*0.00875;
kikoaac 0:15e41c824e3b 223
kikoaac 0:15e41c824e3b 224 S[i] =((double)(rate[i]+prev_rate[i])*sampleTime/2);
kikoaac 0:15e41c824e3b 225 // S[i]=(floor(S[i]*100.0)/100.0);//-offset_angle[i];
kikoaac 0:15e41c824e3b 226 //angle[i]+=(floor(t[i]*100.0)/100.0);//-offset_angle[i];
kikoaac 0:15e41c824e3b 227 angle[i]+=(double)S[i];
kikoaac 0:15e41c824e3b 228 Synthesis_angle[i]+=(double)S[i];
kikoaac 0:15e41c824e3b 229 Synthesis_angle[i]=0.99*(double)(Synthesis_angle[i]+(double)rate[i]/1020.5)+0.01*d[i];
kikoaac 0:15e41c824e3b 230 kalman_angle[i]=kalma[i].getAngle((double)d[i], (double)rate[i], (double)sampleTime*1000);
kikoaac 0:15e41c824e3b 231 comp_angle[i]=kalman_angle[i]*0.01+Synthesis_angle[i]*0.01+comp_angle[i]*0.98;
kikoaac 0:15e41c824e3b 232 prev_rate[i]=rate[i];
kikoaac 0:15e41c824e3b 233 }
kikoaac 0:15e41c824e3b 234 }
kikoaac 0:15e41c824e3b 235 void ANGLE::set_angleoffset()
kikoaac 0:15e41c824e3b 236 {
kikoaac 0:15e41c824e3b 237 double axis[3],offseta[3];
kikoaac 0:15e41c824e3b 238 offseta[0]=offseta[1]=offseta[2]=0;
kikoaac 0:15e41c824e3b 239 offset_angle[0]=0;
kikoaac 0:15e41c824e3b 240 offset_angle[1]=0;
kikoaac 0:15e41c824e3b 241 offset_angle[2]=0;
kikoaac 0:15e41c824e3b 242 for(int i=0; i<sampleNum; i++) {
kikoaac 0:15e41c824e3b 243 set_angle();
kikoaac 0:15e41c824e3b 244 get_angle_rate(axis,axis+1,axis+2);
kikoaac 0:15e41c824e3b 245 offseta[0]+=axis[0];
kikoaac 0:15e41c824e3b 246 offseta[1]+=axis[1];
kikoaac 0:15e41c824e3b 247 offseta[2]+=axis[2];
kikoaac 0:15e41c824e3b 248 }
kikoaac 0:15e41c824e3b 249 offset_angle[0]=offseta[0]/sampleNum;
kikoaac 0:15e41c824e3b 250 offset_angle[1]=offseta[1]/sampleNum;
kikoaac 0:15e41c824e3b 251 offset_angle[2]=offseta[2]/sampleNum;
kikoaac 0:15e41c824e3b 252 offset_angle[0]=(floor(offset_angle[0]*100.0)/100.0);
kikoaac 0:15e41c824e3b 253 offset_angle[1]=(floor(offset_angle[1]*100.0)/100.0);
kikoaac 0:15e41c824e3b 254 offset_angle[2]=(floor(offset_angle[2]*100.0)/100.0);
kikoaac 0:15e41c824e3b 255 angle[0]=0;
kikoaac 0:15e41c824e3b 256 angle[1]=0;
kikoaac 0:15e41c824e3b 257 angle[2]=0;
kikoaac 0:15e41c824e3b 258 }
kikoaac 0:15e41c824e3b 259 void ANGLE::set_noise()
kikoaac 0:15e41c824e3b 260 {
kikoaac 0:15e41c824e3b 261 short gyal[3];
kikoaac 0:15e41c824e3b 262 noise[0]=noise[1]=noise[2]=0;
kikoaac 0:15e41c824e3b 263
kikoaac 0:15e41c824e3b 264 for(int i=0; i<sampleNum; i++) {
kikoaac 0:15e41c824e3b 265
kikoaac 0:15e41c824e3b 266 for(int t=0; t<3; t++) {
kikoaac 0:15e41c824e3b 267 read(gyal);
kikoaac 0:15e41c824e3b 268 if((int)gyal[t]>noise[t])
kikoaac 0:15e41c824e3b 269 noise[t]=(int)gyal[t];
kikoaac 0:15e41c824e3b 270 else if((int)gyal[t]<-noise[t])
kikoaac 0:15e41c824e3b 271 noise[t]=-(int)gyal[t];
kikoaac 0:15e41c824e3b 272 }
kikoaac 0:15e41c824e3b 273 }
kikoaac 0:15e41c824e3b 274 noise[0]*=sampleTime;
kikoaac 0:15e41c824e3b 275 noise[1]*=sampleTime;
kikoaac 0:15e41c824e3b 276 noise[2]*=sampleTime;
kikoaac 0:15e41c824e3b 277 }
kikoaac 0:15e41c824e3b 278 void ANGLE::set_offset()
kikoaac 0:15e41c824e3b 279 {
kikoaac 0:15e41c824e3b 280 short axis[3],offseta[3];
kikoaac 0:15e41c824e3b 281 offseta[0]=0;
kikoaac 0:15e41c824e3b 282 offseta[1]=0;
kikoaac 0:15e41c824e3b 283 offseta[2]=0;
kikoaac 0:15e41c824e3b 284 for(int i=0; i<sampleNum; i++) {
kikoaac 0:15e41c824e3b 285 read(axis);
kikoaac 0:15e41c824e3b 286 offseta[0]+=axis[0];
kikoaac 0:15e41c824e3b 287 offseta[1]+=axis[1];
kikoaac 0:15e41c824e3b 288 offseta[2]+=axis[2];
kikoaac 0:15e41c824e3b 289 }
kikoaac 0:15e41c824e3b 290 offset[0]=offseta[0]/sampleNum;
kikoaac 0:15e41c824e3b 291 offset[1]=offseta[1]/sampleNum;
kikoaac 0:15e41c824e3b 292 offset[2]=offseta[2]/sampleNum;
kikoaac 0:15e41c824e3b 293 }
kikoaac 0:15e41c824e3b 294 bool ANGLE::read(short *axis)
kikoaac 0:15e41c824e3b 295 {
kikoaac 0:15e41c824e3b 296 char gyr[6];
kikoaac 0:15e41c824e3b 297
kikoaac 0:15e41c824e3b 298 if (recv(GYR_ADDRESS, L3GD20_OUT_X_L, gyr, 6)) {
kikoaac 0:15e41c824e3b 299 //scale is 8.75 mdps/digit
kikoaac 0:15e41c824e3b 300 axis[0] = (short(gyr[1] << 8 | gyr[0]))-offset[0];
kikoaac 0:15e41c824e3b 301 axis[1] = (short(gyr[3] << 8 | gyr[2]))-offset[1];
kikoaac 0:15e41c824e3b 302 axis[2] = (short(gyr[5] << 8 | gyr[4]))-offset[2];
kikoaac 0:15e41c824e3b 303
kikoaac 0:15e41c824e3b 304
kikoaac 0:15e41c824e3b 305 return true;
kikoaac 0:15e41c824e3b 306 }
kikoaac 0:15e41c824e3b 307
kikoaac 0:15e41c824e3b 308 return false;
kikoaac 0:15e41c824e3b 309 }
kikoaac 0:15e41c824e3b 310 bool ANGLE::read(float *gx, float *gy, float *gz)
kikoaac 0:15e41c824e3b 311 {
kikoaac 0:15e41c824e3b 312 char gyr[6];
kikoaac 0:15e41c824e3b 313
kikoaac 0:15e41c824e3b 314 if (recv(GYR_ADDRESS, L3GD20_OUT_X_L, gyr, 6)) {
kikoaac 0:15e41c824e3b 315 //scale is 8.75 mdps/digit
kikoaac 0:15e41c824e3b 316 *gx = float(short(gyr[1] << 8 | gyr[0])-offset[0])*0.00875*sampleTime;
kikoaac 0:15e41c824e3b 317 *gy = float(short(gyr[3] << 8 | gyr[2])-offset[1])*0.00875*sampleTime;
kikoaac 0:15e41c824e3b 318 *gz = float(short(gyr[5] << 8 | gyr[4])-offset[2])*0.00875*sampleTime;
kikoaac 0:15e41c824e3b 319
kikoaac 0:15e41c824e3b 320
kikoaac 0:15e41c824e3b 321 return true;
kikoaac 0:15e41c824e3b 322 }
kikoaac 0:15e41c824e3b 323
kikoaac 0:15e41c824e3b 324 return false;
kikoaac 0:15e41c824e3b 325 }
kikoaac 0:15e41c824e3b 326
kikoaac 0:15e41c824e3b 327
kikoaac 0:15e41c824e3b 328
kikoaac 0:15e41c824e3b 329
kikoaac 0:15e41c824e3b 330 bool ANGLE::write_reg(int addr_i2c,int addr_reg, char v)
kikoaac 0:15e41c824e3b 331 {
kikoaac 0:15e41c824e3b 332 char data[2] = {addr_reg, v};
kikoaac 0:15e41c824e3b 333 return ANGLE::i2c_.write(addr_i2c, data, 2) == 0;
kikoaac 0:15e41c824e3b 334 }
kikoaac 0:15e41c824e3b 335
kikoaac 0:15e41c824e3b 336 bool ANGLE::read_reg(int addr_i2c,int addr_reg, char *v)
kikoaac 0:15e41c824e3b 337 {
kikoaac 0:15e41c824e3b 338 char data = addr_reg;
kikoaac 0:15e41c824e3b 339 bool result = false;
kikoaac 0:15e41c824e3b 340
kikoaac 0:15e41c824e3b 341 __disable_irq();
kikoaac 0:15e41c824e3b 342 if ((i2c_.write(addr_i2c, &data, 1) == 0) && (i2c_.read(addr_i2c, &data, 1) == 0)) {
kikoaac 0:15e41c824e3b 343 *v = data;
kikoaac 0:15e41c824e3b 344 result = true;
kikoaac 0:15e41c824e3b 345 }
kikoaac 0:15e41c824e3b 346 __enable_irq();
kikoaac 0:15e41c824e3b 347 return result;
kikoaac 0:15e41c824e3b 348 }
kikoaac 0:15e41c824e3b 349
kikoaac 0:15e41c824e3b 350
kikoaac 0:15e41c824e3b 351 bool ANGLE::recv(char sad, char sub, char *buf, int length)
kikoaac 0:15e41c824e3b 352 {
kikoaac 0:15e41c824e3b 353 if (length > 1) sub |= 0x80;
kikoaac 0:15e41c824e3b 354
kikoaac 0:15e41c824e3b 355 return i2c_.write(sad, &sub, 1, true) == 0 && i2c_.read(sad, buf, length) == 0;
kikoaac 0:15e41c824e3b 356 }
kikoaac 0:15e41c824e3b 357 int ANGLE::setPowerMode(char mode) {
kikoaac 0:15e41c824e3b 358
kikoaac 0:15e41c824e3b 359 //Get the current register contents, so we don't clobber the rate value.
kikoaac 0:15e41c824e3b 360 char registerContents = (mode << 4) | data_single_get(ADXL345_BW_RATE_REG);
kikoaac 0:15e41c824e3b 361
kikoaac 0:15e41c824e3b 362 return data_single_put(ADXL345_BW_RATE_REG, registerContents);
kikoaac 0:15e41c824e3b 363
kikoaac 0:15e41c824e3b 364 }
kikoaac 0:15e41c824e3b 365 int ANGLE::setDataRate(char rate) {
kikoaac 0:15e41c824e3b 366
kikoaac 0:15e41c824e3b 367 //Get the current register contents, so we don't clobber the power bit.
kikoaac 0:15e41c824e3b 368 char registerContents = data_single_get(ADXL345_BW_RATE_REG);
kikoaac 0:15e41c824e3b 369
kikoaac 0:15e41c824e3b 370 registerContents &= 0x10;
kikoaac 0:15e41c824e3b 371 registerContents |= rate;
kikoaac 0:15e41c824e3b 372
kikoaac 0:15e41c824e3b 373 return data_single_put(ADXL345_BW_RATE_REG, registerContents);
kikoaac 0:15e41c824e3b 374
kikoaac 0:15e41c824e3b 375 }
kikoaac 0:15e41c824e3b 376 char ANGLE::getOffset(char axis) {
kikoaac 0:15e41c824e3b 377
kikoaac 0:15e41c824e3b 378 char address = 0;
kikoaac 0:15e41c824e3b 379
kikoaac 0:15e41c824e3b 380 if (axis == ADXL345_X) {
kikoaac 0:15e41c824e3b 381 address = ADXL345_OFSX_REG;
kikoaac 0:15e41c824e3b 382 } else if (axis == ADXL345_Y) {
kikoaac 0:15e41c824e3b 383 address = ADXL345_OFSY_REG;
kikoaac 0:15e41c824e3b 384 } else if (axis == ADXL345_Z) {
kikoaac 0:15e41c824e3b 385 address = ADXL345_OFSZ_REG;
kikoaac 0:15e41c824e3b 386 }
kikoaac 0:15e41c824e3b 387
kikoaac 0:15e41c824e3b 388 return data_single_get(address);
kikoaac 0:15e41c824e3b 389 }
kikoaac 0:15e41c824e3b 390
kikoaac 0:15e41c824e3b 391 int ANGLE::setOffset(char axis, char offset) {
kikoaac 0:15e41c824e3b 392
kikoaac 0:15e41c824e3b 393 char address = 0;
kikoaac 0:15e41c824e3b 394
kikoaac 0:15e41c824e3b 395 if (axis == ADXL345_X) {
kikoaac 0:15e41c824e3b 396 address = ADXL345_OFSX_REG;
kikoaac 0:15e41c824e3b 397 } else if (axis == ADXL345_Y) {
kikoaac 0:15e41c824e3b 398 address = ADXL345_OFSY_REG;
kikoaac 0:15e41c824e3b 399 } else if (axis == ADXL345_Z) {
kikoaac 0:15e41c824e3b 400 address = ADXL345_OFSZ_REG;
kikoaac 0:15e41c824e3b 401 }
kikoaac 0:15e41c824e3b 402
kikoaac 0:15e41c824e3b 403 return data_single_put(address, offset);
kikoaac 0:15e41c824e3b 404
kikoaac 0:15e41c824e3b 405 }
kikoaac 0:15e41c824e3b 406 int ANGLE::setTapDuration(short int duration_us) {
kikoaac 0:15e41c824e3b 407
kikoaac 0:15e41c824e3b 408 short int tapDuration = duration_us / 625;
kikoaac 0:15e41c824e3b 409 char tapChar[2];
kikoaac 0:15e41c824e3b 410 tapChar[0] = (tapDuration & 0x00FF);
kikoaac 0:15e41c824e3b 411 tapChar[1] = (tapDuration >> 8) & 0x00FF;
kikoaac 0:15e41c824e3b 412 return data_multi_put(ADXL345_DUR_REG, tapChar, 2);
kikoaac 0:15e41c824e3b 413
kikoaac 0:15e41c824e3b 414 }
kikoaac 0:15e41c824e3b 415 int ANGLE::setTapLatency(short int latency_ms) {
kikoaac 0:15e41c824e3b 416
kikoaac 0:15e41c824e3b 417 latency_ms = latency_ms / 1.25;
kikoaac 0:15e41c824e3b 418 char latChar[2];
kikoaac 0:15e41c824e3b 419 latChar[0] = (latency_ms & 0x00FF);
kikoaac 0:15e41c824e3b 420 latChar[1] = (latency_ms << 8) & 0xFF00;
kikoaac 0:15e41c824e3b 421 return data_multi_put(ADXL345_LATENT_REG, latChar, 2);
kikoaac 0:15e41c824e3b 422
kikoaac 0:15e41c824e3b 423 }
kikoaac 0:15e41c824e3b 424 int ANGLE::setWindowTime(short int window_ms) {
kikoaac 0:15e41c824e3b 425
kikoaac 0:15e41c824e3b 426 window_ms = window_ms / 1.25;
kikoaac 0:15e41c824e3b 427 char windowChar[2];
kikoaac 0:15e41c824e3b 428 windowChar[0] = (window_ms & 0x00FF);
kikoaac 0:15e41c824e3b 429 windowChar[1] = ((window_ms << 8) & 0xFF00);
kikoaac 0:15e41c824e3b 430 return data_multi_put(ADXL345_WINDOW_REG, windowChar, 2);
kikoaac 0:15e41c824e3b 431
kikoaac 0:15e41c824e3b 432 }
kikoaac 0:15e41c824e3b 433 int ANGLE::setFreefallTime(short int freefallTime_ms) {
kikoaac 0:15e41c824e3b 434 freefallTime_ms = freefallTime_ms / 5;
kikoaac 0:15e41c824e3b 435 char fallChar[2];
kikoaac 0:15e41c824e3b 436 fallChar[0] = (freefallTime_ms & 0x00FF);
kikoaac 0:15e41c824e3b 437 fallChar[1] = (freefallTime_ms << 8) & 0xFF00;
kikoaac 0:15e41c824e3b 438
kikoaac 0:15e41c824e3b 439 return data_multi_put(ADXL345_TIME_FF_REG, fallChar, 2);
kikoaac 0:15e41c824e3b 440
kikoaac 0:15e41c824e3b 441 }
kikoaac 0:15e41c824e3b 442 ANGLE::kalman::kalman(void){
kikoaac 0:15e41c824e3b 443 P[0][0] = 0;
kikoaac 0:15e41c824e3b 444 P[0][1] = 0;
kikoaac 0:15e41c824e3b 445 P[1][0] = 0;
kikoaac 0:15e41c824e3b 446 P[1][1] = 0;
kikoaac 0:15e41c824e3b 447
kikoaac 0:15e41c824e3b 448 angle = 0;
kikoaac 0:15e41c824e3b 449 bias = 0;
kikoaac 0:15e41c824e3b 450
kikoaac 0:15e41c824e3b 451 Q_angle = 0.001;
kikoaac 0:15e41c824e3b 452 Q_gyroBias = 0.003;
kikoaac 0:15e41c824e3b 453 R_angle = 0.03;
kikoaac 0:15e41c824e3b 454 }
kikoaac 0:15e41c824e3b 455
kikoaac 0:15e41c824e3b 456 double ANGLE::kalman::getAngle(double newAngle, double newRate, double dt){
kikoaac 0:15e41c824e3b 457 //predict the angle according to the gyroscope
kikoaac 0:15e41c824e3b 458 rate = newRate - bias;
kikoaac 0:15e41c824e3b 459 angle = dt * rate;
kikoaac 0:15e41c824e3b 460 //update the error covariance
kikoaac 0:15e41c824e3b 461 P[0][0] += dt * (dt * P[1][1] * P[0][1] - P[1][0] + Q_angle);
kikoaac 0:15e41c824e3b 462 P[0][1] -= dt * P[1][1];
kikoaac 0:15e41c824e3b 463 P[1][0] -= dt * P[1][1];
kikoaac 0:15e41c824e3b 464 P[1][1] += dt * Q_gyroBias;
kikoaac 0:15e41c824e3b 465 //difference between the predicted angle and the accelerometer angle
kikoaac 0:15e41c824e3b 466 y = newAngle - angle;
kikoaac 0:15e41c824e3b 467 //estimation error
kikoaac 0:15e41c824e3b 468 S = P[0][0] + R_angle;
kikoaac 0:15e41c824e3b 469 //determine the kalman gain according to the error covariance and the distrust
kikoaac 0:15e41c824e3b 470 K[0] = P[0][0]/S;
kikoaac 0:15e41c824e3b 471 K[1] = P[1][0]/S;
kikoaac 0:15e41c824e3b 472 //adjust the angle according to the kalman gain and the difference with the measurement
kikoaac 0:15e41c824e3b 473 angle += K[0] * y;
kikoaac 0:15e41c824e3b 474 bias += K[1] * y;
kikoaac 0:15e41c824e3b 475 //update the error covariance
kikoaac 0:15e41c824e3b 476 P[0][0] -= K[0] * P[0][0];
kikoaac 0:15e41c824e3b 477 P[0][1] -= K[0] * P[0][1];
kikoaac 0:15e41c824e3b 478 P[1][0] -= K[1] * P[0][0];
kikoaac 0:15e41c824e3b 479 P[1][1] -= K[1] * P[0][1];
kikoaac 0:15e41c824e3b 480
kikoaac 0:15e41c824e3b 481 return angle;
kikoaac 0:15e41c824e3b 482 }
kikoaac 0:15e41c824e3b 483 void ANGLE::kalman::setAngle(double newAngle) { angle = newAngle; };
kikoaac 0:15e41c824e3b 484 void ANGLE::kalman::setQangle(double newQ_angle) { Q_angle = newQ_angle; };
kikoaac 0:15e41c824e3b 485 void ANGLE::kalman::setQgyroBias(double newQ_gyroBias) { Q_gyroBias = newQ_gyroBias; };
kikoaac 0:15e41c824e3b 486 void ANGLE::kalman::setRangle(double newR_angle) { R_angle = newR_angle; };
kikoaac 0:15e41c824e3b 487
kikoaac 0:15e41c824e3b 488 double ANGLE::kalman::getRate(void) { return rate; };
kikoaac 0:15e41c824e3b 489 double ANGLE::kalman::getQangle(void) { return Q_angle; };
kikoaac 0:15e41c824e3b 490 double ANGLE::kalman::getQbias(void) { return Q_gyroBias; };
kikoaac 0:15e41c824e3b 491 double ANGLE::kalman::getRangle(void) { return R_angle; };
kikoaac 0:15e41c824e3b 492