angle.cpp

00001 #include "angle.h"
00002 #include "mbed.h"
00003 ANGLE::ANGLE(PinName sda, PinName scl) : i2c_(sda, scl) {
00004     Rate=0.00390635;sampleTime=0.001;sampleNum=500;
00005 
00006     kalma[0].setAngle(0);
00007     kalma[1].setAngle(0);
00008     kalma[2].setAngle(0);
00009     //400kHz, allowing us to use the fastest data rates.
00010     i2c_.frequency(400000);
00011     // initialize the BW data rate
00012     char tx[2];
00013     tx[0] = ADXL345_BW_RATE_REG;
00014     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 
00015     i2c_.write( acc_i2c_write , tx, 2);  
00016     
00017     //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).
00018     
00019     char rx[2];
00020     rx[0] = ADXL345_DATA_FORMAT_REG;
00021     rx[1] = 0x0B; 
00022      // full res and +_16g
00023     i2c_.write( acc_i2c_write , rx, 2); 
00024     
00025     // Set Offset  - programmed into the OFSX, OFSY, and OFXZ registers, respectively, as 0xFD, 0x03 and 0xFE.
00026     char x[2];
00027     x[0] = ADXL345_OFSX_REG ;
00028     x[1] = 253; 
00029     i2c_.write( acc_i2c_write , x, 2);
00030     char y[2];
00031     y[0] = ADXL345_OFSY_REG ;
00032     y[1] = 5; 
00033     i2c_.write( acc_i2c_write, y, 2);
00034     char z[2];
00035     z[0] = ADXL345_OFSZ_REG ;
00036     z[1] = 0xFE; 
00037     i2c_.write( acc_i2c_write , z, 2);
00038      char reg_v;
00039     sampleTime=0.001;
00040     sampleNum=500;
00041     angle[0]=angle[1]=angle[2]=0;
00042     prev_rate[0]=prev_rate[1]=prev_rate[2]=0;
00043     
00044     // 0x0F = 0b00001111
00045     // Normal power mode, all axes enabled
00046     reg_v = 0;
00047     reg_v |= 0x0F;
00048     write_reg(GYR_ADDRESS,L3GD20_CTRL_REG1,reg_v);
00049     set_offset();
00050     set_noise();
00051     
00052     offset_angle[0]=0;
00053     offset_angle[1]=0;
00054     offset_angle[2]=0;
00055     ADXL_setup();
00056     set_angleoffset();
00057     }
00058 void ANGLE::ADXL_setup(){
00059       z_offset=2;x_offset=0;y_offset=0;
00060       char buffer[6];    
00061 
00062           for(int i=0;i<sampleNum;i++)
00063   {
00064         data_multi_get(ADXL345_DATAX0_REG, buffer, 6);
00065         int Xacc = (int)buffer[1] << 8 | (int)buffer[0];
00066         int Yacc = (int)buffer[3] << 8 | (int)buffer[2];
00067         int Zacc = (int)buffer[5] << 8 | (int)buffer[4]-255;
00068     if((int)Xacc-x_offset>xnoise)
00069       xnoise=(int)Xacc-x_offset;
00070     else if((int)Xacc-x_offset<-xnoise)
00071       xnoise=-(int)Xacc-x_offset;
00072 
00073     if((int)Yacc-y_offset>ynoise)
00074       ynoise=(int)Yacc-y_offset;
00075     else if((int)Yacc-y_offset<-ynoise)
00076       ynoise=-(int)Yacc-y_offset; 
00077 
00078     if((int)Zacc-z_offset>znoise)
00079       znoise=(int)Zacc-z_offset;
00080     else if((int)Zacc-z_offset<-znoise)
00081       znoise=-(int)Zacc-z_offset; 
00082   }
00083         
00084       
00085 }
00086 void ANGLE::ADXL_setnum(int Num,float time,double rate){
00087         sampleNum=Num;sampleTime=time;Rate=rate;
00088 }
00089 char ANGLE::data_single_get(char reg){   
00090    char tx = reg;
00091    char output; 
00092     i2c_.write( acc_i2c_write , &tx, 1);  //tell it what you want to read
00093     i2c_.read( acc_i2c_read , &output, 1);    //tell it where to store the data
00094     return output;
00095 }
00096 int ANGLE::data_single_put(char reg, char data){ 
00097    int ack = 0;
00098    char tx[2];
00099    tx[0] = reg;
00100    tx[1] = data;
00101    return   ack | i2c_.write( acc_i2c_write , tx, 2);   
00102 }
00103 
00104 
00105 
00106 void ANGLE::data_multi_get(char reg, char* data, int size) {
00107     i2c_.write( acc_i2c_write, &reg, 1);  //tell it where to read from
00108     i2c_.read( acc_i2c_read , data, size);      //tell it where to store the data read
00109 }
00110 
00111 
00112 int ANGLE::data_multi_put(char reg, char* data, int size) {
00113         int ack;
00114    
00115                ack = i2c_.write( acc_i2c_write, &reg, 1);  //tell it where to write to
00116         return ack | i2c_.write( acc_i2c_read, data, size);  //tell it what data to write                                   
00117 }
00118 void ANGLE::getangle_acc(double* DATA_ANGLE){
00119     char buffer[6];
00120     short data[3];    
00121     //data_multi_get(ADXL345_DATAX0_REG, buffer, 6);
00122     getaxis_acc(data);
00123       // calculate the absolute of the magnetic field vector                                    // 8-Bit pieces of axis data
00124       // read axis registers using I2C
00125     
00126     /*data[0] = (short) (buffer[1] << 8 | buffer[0]);//-x_offset;     // join 8-Bit pieces to 16-bit short integers
00127     data[1] = (short) (buffer[3] << 8 | buffer[2]);//-y_offset;
00128     data[2] = (short) (buffer[5] << 8 | buffer[4]);//-z_offset;*/
00129     float R = sqrt(pow((float)data[0],2) + pow((float)data[1],2) + pow((float)data[2],2));
00130     DATA_ANGLE[1] =   -((180 / 3.1415) * acos((float)data[1] / R)-90);                                    // roll - angle of magnetic field vector in x direction
00131     DATA_ANGLE[0] =   (180 / 3.1415) * acos((float)data[0] / R)-90;                                    // pitch - angle of magnetic field vector in y direction
00132     DATA_ANGLE[2] =   (180 / 3.1415) * acos((float)data[2] / R); //*/
00133     DATA_ANGLE[0] = atan2(data[0],sqrt(pow((float)data[1],2)+pow((float)data[2],2)))*180/3.1415;
00134     DATA_ANGLE[1] = atan2(data[1],sqrt(pow((float)data[0],2)+pow((float)data[2],2)))*180/3.1415;
00135     DATA_ANGLE[2] = atan2(sqrt(pow((float)data[1],2)+pow((float)data[2],2)),data[2])*180/3.1415;
00136    /*DATA_ANGLE[0]=atan2((double)data[0],(double)data[2])* (180 / 3.1415);
00137     DATA_ANGLE[1]=atan2((double)data[1],(double)data[2])* (180 / 3.1415);
00138    /* if(data[0]>255)
00139       DATA_ANGLE[0]=-90;
00140     else if(data[0]<-263)
00141       DATA_ANGLE[0]=90;
00142     if(data[1]>260)
00143       DATA_ANGLE[1]=90;
00144     else if(data[1]<-246)
00145       DATA_ANGLE[1]=-90;
00146     /*if(DATA[1]>250)
00147       DATA_ANGLE[1]=90;
00148     else if(DATA[1]<-260)
00149       DATA_ANGLE[1]=-90;
00150     if(DATA[0]>250)
00151       DATA_ANGLE[0]=90;
00152     else if(DATA[0]<-260)
00153       DATA_ANGLE[0]=-90;*/
00154       }
00155 void ANGLE::getaxis_acc(short* DATA_ACC){
00156     char buffer[6];
00157     data_multi_get(ADXL345_DATAX0_REG, buffer, 6);
00158     
00159     DATA_ACC[0] = ((short)buffer[1] << 8 | (short)buffer[0]);//+0.1*tempDATA_ACC[0];//-x_offset;
00160     DATA_ACC[1] = ((short)buffer[3] << 8 | (short)buffer[2]);//+0.1*tempDATA_ACC[1];//-y_offset;
00161     DATA_ACC[2] = ((short)buffer[5] << 8 | (short)buffer[4]);//+0.1*tempDATA_ACC[2];//-z_offset;
00162     DATA_ACC[0] = (short)(DATA_ACC[0]*0.9+tempDATA_ACC[0]*0.1);
00163     DATA_ACC[1] = (short)(DATA_ACC[1]*0.9+tempDATA_ACC[0]*0.1);
00164     DATA_ACC[2] = (short)(DATA_ACC[2]*0.9+tempDATA_ACC[0]*0.1);
00165     tempDATA_ACC[0]=DATA_ACC[0];
00166     tempDATA_ACC[1]=DATA_ACC[1];
00167     tempDATA_ACC[2]=DATA_ACC[2];//*/
00168 }
00169 void ANGLE::get_rate(short* RATE)
00170 {
00171     short axis[3];
00172     short offset_t[3]={-1,+1,0};
00173     read(axis);
00174     for(int i=0; i<3; i++){
00175         RATE[i]=(short)(axis[i])*0.01-offset_t[i];
00176         //RATE[i]=(floor(RATE[i]));
00177         }
00178 }
00179 void ANGLE::get_angle(double *x,double *y,double *z)
00180 {
00181     *x=(floor(angle[0]*100.0)/100.0);
00182     *y=(floor(angle[1]*100.0)/100.0);
00183     *z=(floor(angle[2]*100.0)/100.0);
00184 }
00185 void ANGLE::get_angle_rate(double *x,double *y,double *z)
00186 {
00187     
00188     *x=t[0];
00189     *y=t[1];
00190     *z=t[2];
00191 }
00192 
00193 void ANGLE::get_Synthesis_angle(double* X,double* Y)
00194 {
00195         *X=Synthesis_angle[0];
00196         *Y=Synthesis_angle[1];
00197 }
00198 void ANGLE::get_Comp_angle(double* X,double* Y)
00199 {
00200         *X=comp_angle[0];
00201         *Y=comp_angle[1];
00202 }
00203 void ANGLE::get_Kalman_angle(double* X,double* Y)
00204 {
00205         *X=kalman_angle[0];
00206         *Y=kalman_angle[1];
00207 }
00208 void ANGLE::set_angle(double ANG_x,double ANG_y,double ANG_z)
00209 {
00210     Synthesis_angle[0]=angle[0]=ANG_x;
00211     Synthesis_angle[1]=angle[1]=ANG_y;
00212     angle[2]=ANG_z;
00213 }
00214 void ANGLE::set_angle()
00215 {
00216     get_rate(rate);
00217     double g[3], d[3];
00218     get_angle(g,g+1,g+2);
00219     getangle_acc(d);
00220     double S[3];
00221     for(int i=0; i<3; i++) {
00222         //rate[i]=rate[i]*0.00875;
00223         
00224         S[i]  =((double)(rate[i]+prev_rate[i])*sampleTime/2);
00225         // S[i]=(floor(S[i]*100.0)/100.0);//-offset_angle[i];
00226         //angle[i]+=(floor(t[i]*100.0)/100.0);//-offset_angle[i];
00227         angle[i]+=(double)S[i];
00228         Synthesis_angle[i]+=(double)S[i];
00229         Synthesis_angle[i]=0.99*(double)(Synthesis_angle[i]+(double)rate[i]/1020.5)+0.01*d[i];
00230         kalman_angle[i]=kalma[i].getAngle((double)d[i], (double)rate[i], (double)sampleTime*1000);
00231         comp_angle[i]=kalman_angle[i]*0.01+Synthesis_angle[i]*0.01+comp_angle[i]*0.98;
00232         prev_rate[i]=rate[i];
00233     }
00234 }
00235 void ANGLE::set_angleoffset()
00236 {
00237      double axis[3],offseta[3];
00238      offseta[0]=offseta[1]=offseta[2]=0;
00239     offset_angle[0]=0;
00240     offset_angle[1]=0;
00241     offset_angle[2]=0;
00242     for(int i=0; i<sampleNum; i++) {
00243         set_angle();
00244         get_angle_rate(axis,axis+1,axis+2);
00245         offseta[0]+=axis[0];
00246         offseta[1]+=axis[1];
00247         offseta[2]+=axis[2];
00248     }
00249     offset_angle[0]=offseta[0]/sampleNum;
00250     offset_angle[1]=offseta[1]/sampleNum;
00251     offset_angle[2]=offseta[2]/sampleNum;
00252     offset_angle[0]=(floor(offset_angle[0]*100.0)/100.0);
00253     offset_angle[1]=(floor(offset_angle[1]*100.0)/100.0);
00254     offset_angle[2]=(floor(offset_angle[2]*100.0)/100.0);
00255     angle[0]=0;
00256     angle[1]=0;
00257     angle[2]=0;
00258 }
00259 void ANGLE::set_noise()
00260 {
00261     short gyal[3];
00262     noise[0]=noise[1]=noise[2]=0;
00263 
00264     for(int i=0; i<sampleNum; i++) {
00265 
00266         for(int t=0; t<3; t++) {
00267             read(gyal);
00268             if((int)gyal[t]>noise[t])
00269                 noise[t]=(int)gyal[t];
00270             else if((int)gyal[t]<-noise[t])
00271                 noise[t]=-(int)gyal[t];
00272         }
00273     }
00274     noise[0]*=sampleTime;
00275     noise[1]*=sampleTime;
00276     noise[2]*=sampleTime;
00277 }
00278 void ANGLE::set_offset()
00279 {
00280     short axis[3],offseta[3];
00281     offseta[0]=0;
00282     offseta[1]=0;
00283     offseta[2]=0;
00284     for(int i=0; i<sampleNum; i++) {
00285         read(axis);
00286         offseta[0]+=axis[0];
00287         offseta[1]+=axis[1];
00288         offseta[2]+=axis[2];
00289     }
00290     offset[0]=offseta[0]/sampleNum;
00291     offset[1]=offseta[1]/sampleNum;
00292     offset[2]=offseta[2]/sampleNum;
00293 }
00294 bool ANGLE::read(short *axis)
00295 {
00296     char gyr[6];
00297 
00298     if (recv(GYR_ADDRESS, L3GD20_OUT_X_L, gyr, 6)) {
00299         //scale is 8.75 mdps/digit
00300         axis[0] =  (short(gyr[1] << 8 | gyr[0]))-offset[0];
00301         axis[1] =  (short(gyr[3] << 8 | gyr[2]))-offset[1];
00302         axis[2] =  (short(gyr[5] << 8 | gyr[4]))-offset[2];
00303 
00304 
00305         return true;
00306     }
00307 
00308     return false;
00309 }
00310 bool ANGLE::read(float *gx, float *gy, float *gz)
00311 {
00312     char gyr[6];
00313 
00314     if (recv(GYR_ADDRESS, L3GD20_OUT_X_L, gyr, 6)) {
00315         //scale is 8.75 mdps/digit
00316         *gx =  float(short(gyr[1] << 8 | gyr[0])-offset[0])*0.00875*sampleTime;
00317         *gy =  float(short(gyr[3] << 8 | gyr[2])-offset[1])*0.00875*sampleTime;
00318         *gz =  float(short(gyr[5] << 8 | gyr[4])-offset[2])*0.00875*sampleTime;
00319 
00320 
00321         return true;
00322     }
00323 
00324     return false;
00325 }
00326 
00327 
00328 
00329 
00330 bool ANGLE::write_reg(int addr_i2c,int addr_reg, char v)
00331 {
00332     char data[2] = {addr_reg, v};
00333     return ANGLE::i2c_.write(addr_i2c, data, 2) == 0;
00334 }
00335 
00336 bool ANGLE::read_reg(int addr_i2c,int addr_reg, char *v)
00337 {
00338     char data = addr_reg;
00339     bool result = false;
00340 
00341     __disable_irq();
00342     if ((i2c_.write(addr_i2c, &data, 1) == 0) && (i2c_.read(addr_i2c, &data, 1) == 0)) {
00343         *v = data;
00344         result = true;
00345     }
00346     __enable_irq();
00347     return result;
00348 }
00349 
00350 
00351 bool ANGLE::recv(char sad, char sub, char *buf, int length)
00352 {
00353     if (length > 1) sub |= 0x80;
00354 
00355     return i2c_.write(sad, &sub, 1, true) == 0 && i2c_.read(sad, buf, length) == 0;
00356 }
00357 int ANGLE::setPowerMode(char mode) { 
00358 
00359     //Get the current register contents, so we don't clobber the rate value.
00360     char registerContents = (mode << 4) | data_single_get(ADXL345_BW_RATE_REG);
00361 
00362    return data_single_put(ADXL345_BW_RATE_REG, registerContents);
00363 
00364 }
00365 int ANGLE::setDataRate(char rate) {
00366 
00367     //Get the current register contents, so we don't clobber the power bit.
00368     char registerContents = data_single_get(ADXL345_BW_RATE_REG);
00369 
00370     registerContents &= 0x10;
00371     registerContents |= rate;
00372 
00373     return data_single_put(ADXL345_BW_RATE_REG, registerContents);
00374 
00375 }
00376 char ANGLE::getOffset(char axis) {     
00377 
00378     char address = 0;
00379 
00380     if (axis == ADXL345_X) {
00381         address = ADXL345_OFSX_REG;
00382     } else if (axis == ADXL345_Y) {
00383         address = ADXL345_OFSY_REG;
00384     } else if (axis == ADXL345_Z) {
00385         address = ADXL345_OFSZ_REG;
00386     }
00387 
00388    return data_single_get(address);
00389 }
00390 
00391 int ANGLE::setOffset(char axis, char offset) {        
00392 
00393     char address = 0;
00394 
00395     if (axis == ADXL345_X) {
00396         address = ADXL345_OFSX_REG;
00397     } else if (axis == ADXL345_Y) {
00398         address = ADXL345_OFSY_REG;
00399     } else if (axis == ADXL345_Z) {
00400         address = ADXL345_OFSZ_REG;
00401     }
00402 
00403    return data_single_put(address, offset);
00404 
00405 }
00406 int ANGLE::setTapDuration(short int duration_us) {
00407 
00408     short int tapDuration = duration_us / 625;
00409     char tapChar[2];
00410      tapChar[0] = (tapDuration & 0x00FF);
00411      tapChar[1] = (tapDuration >> 8) & 0x00FF;
00412     return data_multi_put(ADXL345_DUR_REG, tapChar, 2);
00413 
00414 }
00415 int ANGLE::setTapLatency(short int latency_ms) {
00416 
00417     latency_ms = latency_ms / 1.25;
00418     char latChar[2];
00419      latChar[0] = (latency_ms & 0x00FF);
00420      latChar[1] = (latency_ms << 8) & 0xFF00;
00421     return data_multi_put(ADXL345_LATENT_REG, latChar, 2);
00422 
00423 }
00424 int ANGLE::setWindowTime(short int window_ms) {
00425 
00426     window_ms = window_ms / 1.25;
00427     char windowChar[2];
00428     windowChar[0] = (window_ms & 0x00FF);
00429     windowChar[1] = ((window_ms << 8) & 0xFF00);
00430    return data_multi_put(ADXL345_WINDOW_REG, windowChar, 2);
00431 
00432 }
00433 int ANGLE::setFreefallTime(short int freefallTime_ms) {
00434      freefallTime_ms = freefallTime_ms / 5;
00435      char fallChar[2];
00436      fallChar[0] = (freefallTime_ms & 0x00FF);
00437      fallChar[1] = (freefallTime_ms << 8) & 0xFF00;
00438     
00439     return data_multi_put(ADXL345_TIME_FF_REG, fallChar, 2);
00440 
00441 }
00442 ANGLE::kalman::kalman(void){
00443     P[0][0]     = 0;
00444     P[0][1]     = 0;
00445     P[1][0]     = 0;
00446     P[1][1]     = 0;
00447     
00448     angle       = 0;
00449     bias        = 0;
00450     
00451     Q_angle     = 0.001;
00452     Q_gyroBias  = 0.003;
00453     R_angle     = 0.03;     
00454 }
00455 
00456 double ANGLE::kalman::getAngle(double newAngle, double newRate, double dt){
00457     //predict the angle according to the gyroscope
00458     rate         = newRate - bias;
00459     angle        = dt * rate;
00460     //update the error covariance
00461     P[0][0]     += dt * (dt * P[1][1] * P[0][1] - P[1][0] + Q_angle);
00462     P[0][1]     -= dt * P[1][1];
00463     P[1][0]     -= dt * P[1][1];
00464     P[1][1]     += dt * Q_gyroBias;
00465     //difference between the predicted angle and the accelerometer angle
00466     y            = newAngle - angle;
00467     //estimation error
00468     S            = P[0][0] + R_angle;
00469     //determine the kalman gain according to the error covariance and the distrust
00470     K[0]         = P[0][0]/S;
00471     K[1]         = P[1][0]/S;
00472     //adjust the angle according to the kalman gain and the difference with the measurement
00473     angle       += K[0] * y;
00474     bias        += K[1] * y;
00475     //update the error covariance
00476     P[0][0]     -= K[0] * P[0][0];
00477     P[0][1]     -= K[0] * P[0][1];
00478     P[1][0]     -= K[1] * P[0][0];
00479     P[1][1]     -= K[1] * P[0][1];
00480 
00481     return angle;
00482 }
00483 void ANGLE::kalman::setAngle(double newAngle) { angle = newAngle; };
00484 void ANGLE::kalman::setQangle(double newQ_angle) { Q_angle = newQ_angle; };
00485 void ANGLE::kalman::setQgyroBias(double newQ_gyroBias) { Q_gyroBias = newQ_gyroBias; };
00486 void ANGLE::kalman::setRangle(double newR_angle) { R_angle = newR_angle; };
00487 
00488 double ANGLE::kalman::getRate(void) { return rate; };
00489 double ANGLE::kalman::getQangle(void) { return Q_angle; };
00490 double ANGLE::kalman::getQbias(void) { return Q_gyroBias; };
00491 double ANGLE::kalman::getRangle(void) { return R_angle; };
00492