ACS.cpp

00001 #include "ACS.h"
00002 #include "MPU3300.h"
00003 #include "pin_config.h"
00004 
00005 //PwmOut PWM1(PTD4);        //Functions used to generate PWM signal 
00006                         //PWM output comes from pins p6
00007 Serial pc1(USBTX, USBRX);
00008 SPI spi_acs (PIN16, PIN17, PIN15); // mosi, miso, sclk  PTE18,19,17
00009 DigitalOut SSN_MAG (PTC16); // ssn for magnetometer PTB11
00010 DigitalInOut DRDY (PTE3); // drdy for magnetometer PTA17
00011 DigitalOut ssn_gyr (PTE2);         //Slave Select pin of gyroscope  PTB16
00012 InterruptIn dr(PTC6);       //Interrupt pin for gyro PTC5
00013 PwmOut PWM1(D2);        //Functions used to generate PWM signal 
00014 PwmOut PWM2(D3); 
00015 PwmOut PWM3(D4);                   //PWM output comes from pins p6
00016 Ticker tr;              //Ticker function to give values for limited amount of time for gyro
00017 Timeout tr_mag;
00018 uint8_t trflag_mag;
00019 uint8_t trFlag;         //ticker Flag for gyro
00020 uint8_t drFlag;         //data-ready interrupt flag for gyro
00021 float pwm;
00022 //--------------------------------TORQUE ROD--------------------------------------------------------------------------------------------------------------//
00023 
00024 void FUNC_ACS_GENPWM(float M[3])
00025  {
00026      
00027 
00028      printf("\n\rEnterd PWMGEN function\n\r\r");
00029      for(int i = 0 ; i<3;i++)
00030         {
00031             printf(" %f \t ",M[i]);
00032         }
00033      float DCx = 0;         //Duty cycle of Moment in x, y, z directions
00034      float ix = 0;          //Current sent in x, y, z TR's
00035      float timep = 0.02 ;  
00036      float Mx=M[0];            //Time period is set to 0.02s  
00037                              //Moment in x, y, z directions
00038      
00039       
00040      
00041         ix = Mx * 0.3 ;      //Moment and Current always have the linear relationship
00042         ix = 0.554999;
00043         if( ix>0&& ix < 0.006 )                     //Current and Duty cycle have the linear relationship between 1% and 100%
00044          {
00045              DCx =  6*1000000*pow(ix,4) - 377291*pow(ix,3) + 4689.6*pow(ix,2) + 149.19*ix - 0.0008;
00046              PWM1.period(timep);
00047              PWM1 = DCx/100 ;
00048          }
00049         else if( ix >= 0.006&& ix < 0.0116)
00050          { 
00051             DCx = 1*100000000*pow(ix,4) - 5*1000000*pow(ix,3) + 62603*pow(ix,2) - 199.29*ix + 0.7648;
00052             PWM1.period(timep);
00053             PWM1 = DCx/100 ;             
00054          }
00055         else if (ix >= 0.0116&& ix < 0.0624)
00056          {
00057               
00058             DCx = 212444*pow(ix,4) - 33244*pow(ix,3) + 1778.4*pow(ix,2) + 120.91*ix + 0.3878;
00059             PWM1.period(timep);
00060             PWM1 = DCx/100 ;            
00061          }
00062         else if(ix >= 0.0624&& ix < 0.555)
00063          {
00064             printf("\n\rACS entered if\n\r");
00065             DCx =  331.15*pow(ix,4) - 368.09*pow(ix,3) + 140.43*pow(ix,2) + 158.59*ix + 0.0338;
00066             PWM1.period(timep);
00067             PWM1 = DCx/100 ;            
00068          }
00069          //printf("\n \r b4 %f ",ix);
00070           else if(ix==0)
00071          {
00072              printf("\n \r ix====0");
00073              DCx = 75;
00074             PWM1.period(timep);
00075             PWM1 = DCx/100 ;            
00076          }
00077          else
00078          {
00079             // printf("!!!!!!!!!!Error!!!!!!!!!");
00080          } 
00081     pwm = PWM1;
00082     //DCx = 25; 
00083     //PWM1 = 0.50;    
00084     printf("\n\r icx :%f pwm : %f \n\r",ix,pwm);
00085     float DCy = 0;         //Duty cycle of Moment in x, y, z directions
00086      float iy = 0;          //Current sent in x, y, z TR's
00087        
00088     float My=M[1];            //Time period is set to 0.2s  
00089                              //Moment in x, y, z directions
00090       
00091      
00092         iy = My * 0.3 ;      //Moment and Current always have the linear relationship
00093      
00094         if( iy>0&& iy < 0.006 )                     //Current and Duty cycle have the linear relationship between 1% and 100%
00095          {
00096              DCy =  6*1000000*pow(iy,4) - 377291*pow(iy,3) + 4689.6*pow(iy,2) + 149.19*iy - 0.0008;
00097              PWM2.period(timep);
00098              PWM2 = DCy/100 ;
00099          }
00100         else if( iy >= 0.006&& iy < 0.0116)
00101          { 
00102             DCy = 1*100000000*pow(iy,4) - 5*1000000*pow(iy,3) + 62603*pow(iy,2) - 199.29*iy + 0.7648;
00103             PWM2.period(timep);
00104             PWM2 = DCy/100 ;             
00105          }
00106         else if (iy >= 0.0116&& iy < 0.0624)
00107          {
00108               
00109             DCy = 212444*pow(iy,4) - 33244*pow(iy,3) + 1778.4*pow(iy,2) + 120.91*iy + 0.3878;
00110             PWM2.period(timep);
00111             PWM2 = DCy/100 ;            
00112          }
00113         else if(iy >= 0.0624&& iy < 0.555)
00114          {
00115             printf("\n\rACS entered if\n\r");
00116             DCy =  331.15*pow(iy,4) - 368.09*pow(iy,3) + 140.43*pow(iy,2) + 158.59*iy + 0.0338;
00117             PWM2.period(timep);
00118             PWM2 = DCy/100 ;            
00119          }
00120          else if(iy==0)
00121          {
00122              DCy = 0;
00123             PWM2.period(timep);
00124             PWM2 = DCy/100 ;            
00125          }
00126          else
00127          {
00128             // printf("!!!!!!!!!!Error!!!!!!!!!");
00129          } 
00130     
00131     pwm = PWM2; 
00132     printf("\n\r icy :%f pwm : %f \n\r",iy,pwm);    
00133          
00134          
00135     float DCz = 0;         //Duty cycle of Moment in x, y, z directions
00136     float iz = 0;          //Current sent in x, y, z TR's
00137        
00138      float Mz=M[2];            //Time period is set to 0.2s  
00139                              //Moment in x, y, z directions
00140       
00141      
00142         iz = Mz * 0.3 ;      //Moment and Current always have the linear relationship
00143      
00144         if( iz>0&& iz < 0.006 )                     //Current and Duty cycle have the linear relationship between 1% and 100%
00145          {
00146              DCz =  6*1000000*pow(iz,4) - 377291*pow(iz,3) + 4689.6*pow(iz,2) + 149.19*iz - 0.0008;
00147              PWM3.period(timep);
00148              PWM3 = DCz/100 ;
00149          }
00150         else if( iz >= 0.006&& iz < 0.0116)
00151          { 
00152             DCz = 1*100000000*pow(iz,4) - 5*1000000*pow(iz,3) + 62603*pow(iz,2) - 199.29*iz + 0.7648;
00153             PWM3.period(timep);
00154             PWM3 = DCz/100 ;             
00155          }
00156         else if (iz >= 0.0116&& iz < 0.0624)
00157          {
00158               
00159             DCz = 212444*pow(iz,4) - 33244*pow(iz,3) + 1778.4*pow(iz,2) + 120.91*iz + 0.3878;
00160             PWM3.period(timep);
00161             PWM3 = DCz/100 ;            
00162          }
00163         else if(iz >= 0.0624&& iz < 0.555)
00164          {
00165             printf("\n\rACS entered if\n\r");
00166             DCz =  331.15*pow(iz,4) - 368.09*pow(iz,3) + 140.43*pow(iz,2) + 158.59*iz + 0.0338;
00167             PWM3.period(timep);
00168             PWM3 = DCz/100 ;            
00169          }
00170          else if(iz==0)
00171          {
00172              DCz = 0;
00173             PWM3.period(timep);
00174             PWM3 = DCz/100 ;            
00175          }
00176          else
00177          {
00178             // printf("!!!!!!!!!!Error!!!!!!!!!");
00179          }   
00180     
00181 printf("\n\rExited PWMGEN function\n\r");
00182 }
00183 /*-------------------------------------------------------------------------------------------------------------------------------------------------------
00184 -------------------------------------------MAGNETOMETER-------------------------------------------------------------------------------------------------*/
00185 
00186 void trsub_mag()
00187 {
00188   trflag_mag=0;
00189 } 
00190 
00191 void FUNC_ACS_MAG_INIT()
00192  {
00193   //DRDY.output();
00194   DRDY = 0;
00195   int a ;
00196   a=DRDY;
00197   printf("\n\r DRDY is %d\n\r",a);
00198   SSN_MAG=1;                                    //pin is disabled
00199   spi_acs.format(8,0);                         //   8bits,Mode 0
00200   spi_acs.frequency(100000);                   //clock frequency
00201   
00202   SSN_MAG=0;                                   // Selecting pin
00203   wait_ms(10);                            //accounts for delay.can be minimised.
00204   
00205   spi_acs.write(0x83);                      //
00206   
00207   wait_ms(10);              
00208   
00209   unsigned char i;
00210   for(i=0;i<3;i++)//initialising values.
00211       {
00212          spi_acs.write(0x00);              //MSB of X,y,Z
00213          spi_acs.write(0xc8);             //LSB of X,Y,z;pointer increases automatically.
00214         }
00215    SSN_MAG=1;
00216  
00217 }
00218 
00219 float* FUNC_ACS_MAG_EXEC()
00220 {
00221    printf("\n\rEntered magnetometer function\n\r");
00222    //DRDY.output();
00223    DRDY.write(0);
00224    int a;
00225    a = DRDY;
00226    printf("\n\r DRDY is %d\n\r",a);
00227    SSN_MAG=0;                                //enabling slave to measure the values
00228    wait_ms(10);
00229    spi_acs.write(0x82);                     //initiates measurement
00230    wait_ms(10);
00231    spi_acs.write(0x01);                   //selecting x,y and z axes, measurement starts now
00232    SSN_MAG=1;
00233    wait_ms(10);
00234 
00235    trflag_mag=1;        
00236    tr_mag.attach(&trsub_mag,1);   //runs in background,makes trflag_mag=0 after 1s
00237    DRDY.input();
00238    while(trflag_mag)              /*initially flag is 1,so loop is executed,if DRDY is high,then data is retrieved and programme ends,else 
00239                                  loop runs for at the max 1s and if still DRDY is zero,the flag becomes 0 and loop is not executed and 
00240                                programme is terminated*/
00241   {
00242     wait_ms(5);
00243     if(DRDY==1)
00244     {
00245         printf("\n\rwth\n");
00246         SSN_MAG=0;
00247         spi_acs.write(0xc9);                  //command  byte for retrieving data
00248  
00249         unsigned char axis;
00250         float Bnewvalue[3]={0.0,0.0,0.0};
00251         int32_t Bvalue[3]={0,0,0}; 
00252         int32_t a= pow(2.0,24.0);
00253         int32_t b= pow(2.0,23.0);
00254  
00255         for(axis=0;axis<3;axis++)
00256         {
00257             Bvalue[axis]=spi_acs.write(0x00)<<16;    //MSB 1 is send first 
00258             wait_ms(10);
00259             Bvalue[axis]|=spi_acs.write(0x00)<<8;    //MSB 2 is send next
00260             wait_ms(10);
00261             Bvalue[axis]|=spi_acs.write(0x00);       //LSB is send.....total length is 24 bits(3*8bits)...which are appended to get actual bit configuration
00262   
00263    
00264             if((Bvalue[axis]&b)==b)              
00265             {
00266                 Bvalue[axis]=Bvalue[axis]-a;   //converting 2s complement to  signed decimal
00267 
00268             }
00269             Bnewvalue[axis]=(float)Bvalue[axis]*22.0*pow(10.0,-3.0);  //1 LSB=(22nT)...final value of field obtained in micro tesla
00270   
00271             wait_ms(10);
00272             printf("\t%lf\n\r",Bnewvalue[axis]);
00273 
00274         }
00275         SSN_MAG=1;
00276         /* for test only to removed */
00277         Bnewvalue[0]=Bnewvalue[1]=Bnewvalue[2]=100;
00278         return Bnewvalue;          //return here? doubt..
00279         break;
00280     }
00281     
00282  }
00283  
00284 } 
00285 
00286 
00287 /*------------------------------------------------------------------------------------------------------------------------------------------------------
00288 -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
00289 
00290 void FUNC_ACS_CNTRLALGO(float b[3],float omega[3],float tauc[3])
00291 {
00292     float db[3]; /// inputs
00293 //initialization
00294     float bb[3] = {0, 0, 0};
00295     float d[3] = {0, 0, 0};
00296     float Jm[3][3] = {{0.2730, 0, 0}, {0, 0.3018, 0}, {0, 0, 0.3031}};
00297     float den = 0; 
00298     float den2;
00299     int i, j; //temporary variables
00300     float Mu[2], z[2], dv[2], v[2], u[2]; //outputs
00301     //float tauc[3];
00302     float *tauc1;
00303     float invJm[3][3];
00304     float kmu2 = 0.07, gamma2 = 1.9e4, kz2 = 0.4e-2, kmu = 0.003, gamma = 5.6e4, kz = 0.1e-4;
00305     printf("\n\r Entered cntrl algo\n\r");
00306     //structure parameters
00307 
00308     void inverse (float mat[3][3], float inv[3][3]); 
00309     void getInput (float x[9]);
00310     //functions
00311  
00312 ////////// Input from Matlab //////////////
00313   //  while(1)                          //removed assumin while is used coz of matlab
00314     //{
00315        
00316  /*getInput(inputs);
00317 //while(1)
00318  b[0] = inputs[0];
00319  b[1] = inputs[1];
00320  b[2] = inputs[2];
00321  db[0] = inputs[3];
00322  db[1] = inputs[4];
00323  db[2] = inputs[5]; 
00324  omega[0] = inputs[6];
00325  omega[1] = inputs[7];
00326  omega[2] = inputs[8];*/
00327 /////////// Control Algorithm //////////////////////
00328 // calculate norm b, norm db
00329         tauc[0]=0;
00330         tauc[1]=0;
00331         tauc[2]=0;
00332         
00333         den = sqrt((b[0]*b[0]) + (b[1]*b[1]) + (b[2]*b[2]));
00334         den2 = (b[0]*db[0]) + (b[1]*db[1]) + (b[2]*db[2]);
00335        
00336         for(i=0;i<3;i++)
00337         {
00338             db[i] = (db[i]*den*den-b[i]*den2) / (pow(den,3));
00339 //db[i]/=den*den*den;
00340         }
00341         
00342         for(i=0;i<3;i++)
00343         {
00344             printf("\n\rreached here\n\r");
00345             if(den!=0)
00346                 b[i]=b[i]/den;                                      //there is a problem here. The code gets stuck here.  Maf value is required 
00347                 
00348         }
00349         
00350 // select kz, kmu, gamma
00351         if(b[0]>0.9 || b[0]<-0.9)
00352         {
00353             kz = kz2;
00354             kmu = kmu2;
00355             gamma = gamma2;
00356         }
00357 // calculate Mu, v, dv, z, u
00358         for(i=0;i<2;i++)
00359         {
00360             Mu[i] = b[i+1];
00361             v[i] = -kmu*Mu[i];
00362             dv[i] = -kmu*db[i+1];
00363             z[i] = db[i+1] - v[i];
00364             u[i] = -kz*z[i] + dv[i]-(Mu[i] / gamma);
00365         }
00366         inverse(Jm, invJm);
00367 // calculate cross(omega,J*omega)for(i=0;i<3;i++)
00368          
00369         for(j=0;j<3;j++)
00370             bb[i] += omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j] - omega[(i+2)%3]*Jm[(i+1)%3][j]);
00371 
00372 // calculate invJm*cross(omega,J*omega) store in d
00373         for(i=0;i<3;i++)
00374         {
00375             for(j=0;j<3;j++)
00376                 d[i] += bb[j]*invJm[i][j];
00377         }
00378 // calculate d = cross(invJm*cross(omega,J*omega),b) -cross(omega,db) 
00379 // bb =[0;u-d(2:3)] 
00380 // store in bb
00381         bb[1] = u[0] + (d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2]) + (omega[2]*db[0]);
00382         bb[2] = u[1]-(d[0]*b[1]) + (d[1]*b[0]) + (omega[0]*db[1])-(omega[1]*db[0]);
00383         bb[0] = 0;
00384 // calculate N 
00385 // reusing invJm as N
00386        
00387         for(i=0;i<3;i++)
00388         {
00389             d[i] = invJm[1][i];
00390             invJm[ 1][i] = b[2]*invJm[0][i] - b[0]*invJm[2][i];
00391             invJm[2][i] = -b[1]*invJm[0][i] + b[0]*d[i];
00392             invJm[0][i] = b[i];
00393         }
00394 // calculate inv(N) store in Jm
00395         inverse(invJm, Jm);
00396 // calculate tauc
00397          printf("\n \r calculatin tauc");
00398         for(i=0;i<3;i++)
00399         {
00400            
00401             for(j=0;j<3;j++)
00402                 tauc[i] += Jm[i][j]*bb[j];
00403             //printf(" %d \t",i);
00404             //tauc1[i] = tauc[i];
00405             printf(" %f \t",tauc[i]);    
00406         }
00407         
00408         //printf("    %f \n ", tauc[2]);
00409         //return tauc;
00410     
00411 }
00412 /////////// Output to Matlab //////////////////
00413 /* for(i=0;i<3;i++) {
00414  printf("%f\n\r",tauc[i]*10000000);
00415  wait_ms(10);
00416  }
00417  }
00418  
00419 }*/
00420  void inverse(float mat[3][3], float inv[3][3])
00421 {
00422 int i, j;
00423 float det = 0;
00424 for(i=0;i<3;i++)
00425 { for(j=0;j<3;j++)
00426 inv[j][i] = (mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3]) - (mat[(i+2)%3]
00427 [(j+1)%3]*mat[(i+1)%3][(j+2)%3]);
00428 }
00429 det += (mat[0][0]*inv[0][0]) + (mat[0][1]*inv[1][0]) + (mat[0][2]*inv[2][0]);
00430 for(i=0;i<3;i++)
00431 { for(j=0;j<3;j++)
00432 inv[i][j] /= det;
00433 }
00434 }/*
00435 void getInput (float x[9]) {
00436          //Functions used to generate PWM signal 
00437                         //PWM output comes from pins p6
00438 Serial pc1(USBTX, USBRX);
00439  char c[10];
00440  char tempchar[8];
00441  int i, j;
00442  //float f[9];
00443  long n = 0;
00444  float flval = 0;
00445  for(j=0;j<9;j++) {
00446  for(i=0;i<9;i++) {
00447  c[i] = pc1.getc(); if(i<8) {
00448  tempchar[i] = c[i];
00449  }
00450  }
00451  sscanf (tempchar, "%8x", &n);
00452  memcpy(&flval, &n, sizeof(long));
00453  printf("%f\n\r", flval);
00454  x[j] = flval;
00455  }
00456 }*/
00457 
00458 void trSub();               
00459 void drSub(); 
00460 void init_gyro();       
00461 float * FUNC_ACS_EXEC_GYR();
00462 
00463 void drSub()            //In this function we setting data-ready flag to 1              
00464 {
00465     drFlag=1;
00466 }
00467 void trSub()                    //In this function we are setting ticker flag to 0
00468 {
00469     trFlag=0;
00470 }
00471 void FUNC_ACS_INIT_GYR()
00472 {
00473     uint8_t response;               
00474     ssn_gyr=1;                  //Deselecting the chip 
00475     spi_acs.format(8,0);                // Spi format is 8 bits, and clock mode 3 
00476     spi_acs.frequency(1000000);     //frequency to be set as 1MHz
00477     drFlag=0;                   //Intially defining data-ready flag to be 0 
00478     dr.mode(PullDown);          
00479     dr.rise(&drSub);
00480     __disable_irq();
00481     
00482 /*Following the above mentioned algorithm for initializing the register and changing its configuration*/
00483     ssn_gyr=0;                      //Selecting chip(Mpu-3300)
00484     spi_acs.write(USER_CTRL|READFLAG);   //sending USER_CTRL address with read bit
00485     response=spi_acs.write(DUMMYBIT);   //sending dummy bit to get default values of the register
00486                         
00487     ssn_gyr=1;                  //Deselecting the chip  
00488     wait(0.1);                  //waiting according the product specifications 
00489     
00490     ssn_gyr=0;                  //again selecting the chip  
00491     spi_acs.write(USER_CTRL);           //sending USER_CTRL address without read bit 
00492     spi_acs.write(response|BIT_I2C_IF_DIS);  //disabling the I2C mode in the register
00493     ssn_gyr=1;                  //deselecting the chip 
00494     wait(0.1);                  // waiting for 100ms before going for another register 
00495     
00496     ssn_gyr=0;
00497     spi_acs.write(PWR_MGMT_1|READFLAG); //Power Management register-1 
00498     response=spi_acs.write(DUMMYBIT);
00499     ssn_gyr=1;
00500     wait(0.1);
00501         
00502     ssn_gyr=0;
00503     spi_acs.write(PWR_MGMT_1);
00504     response=spi_acs.write(response|BIT_CLKSEL_X);  //Selecting the X axis gyroscope as clock as mentioned above 
00505     ssn_gyr=1;                          
00506     wait(0.1);
00507     
00508     ssn_gyr=0;
00509     spi_acs.write(GYRO_CONFIG|READFLAG); //sending GYRO_CONFIG address with read bit
00510     response=spi_acs.write(DUMMYBIT);
00511     ssn_gyr=1;
00512     wait(0.1);
00513     
00514     ssn_gyr=0;
00515     spi_acs.write(GYRO_CONFIG); //sending GYRO_CONFIG address to write to register
00516     spi_acs.write(response&(~(BITS_FS_SEL_3|BITS_FS_SEL_4))); //selecting a full scale mode of +/=225 deg/sec
00517     ssn_gyr=1;
00518     wait(0.1);
00519     
00520     ssn_gyr=0;
00521     spi_acs.write(CONFIG|READFLAG); //sending CONFIG address with read bit
00522     response=spi_acs.write(DUMMYBIT);
00523     ssn_gyr=1;
00524     wait(0.1);
00525     
00526     ssn_gyr=0;
00527     spi_acs.write(CONFIG); //sending CONFIG address to write to register
00528     spi_acs.write(response|BITS_DLPF_CFG); //selecting a bandwidth of 42 hz and delay of 4.8ms
00529     ssn_gyr=1;
00530     wait(0.1);
00531     
00532     ssn_gyr=0;
00533     spi_acs.write(SMPLRT_DIV|READFLAG); //sending SMPLRT_DIV address with read bit
00534     response=spi_acs.write(DUMMYBIT);
00535     ssn_gyr=1;
00536     wait(0.1);
00537         
00538     ssn_gyr=0;
00539     spi_acs.write(SMPLRT_DIV); //sending SMPLRT_DIV address to write to register
00540     spi_acs.write(response&BITS_SMPLRT_DIV); //setting the sampling rate division to be 0 to make sample rate = gyroscopic output rate
00541     ssn_gyr=1;
00542     wait(0.1);
00543     
00544     ssn_gyr=0;
00545     spi_acs.write(INT_ENABLE|READFLAG);       //sending address of INT_ENABLE with readflag
00546     response=spi_acs.write(DUMMYBIT);              //sending dummy byte to get the default values of the
00547                                                                           // regiser
00548     ssn_gyr=1;   
00549     wait(0.1);
00550     
00551     ssn_gyr=0;
00552     spi_acs.write(INT_ENABLE);                           //sending INT_ENABLE address to write to register
00553     spi_acs.write(response|BIT_DATA_RDY_ENABLE);  //Enbling data ready interrupt
00554     ssn_gyr=1;
00555     wait(0.1);
00556     
00557     __enable_irq();
00558 }
00559 
00560 float * FUNC_ACS_EXEC_GYR()
00561 {
00562     printf("\n\rEntered gyro\n\r");
00563     uint8_t response;
00564     uint8_t MSB,LSB;
00565     int16_t bit_data;
00566     float data[3],error[3]={0,0,0}; //declaring error array to add to the values when required
00567     float senstivity = 145.6;     //senstivity is 145.6 for full scale mode of +/-225 deg/sec
00568     ssn_gyr=0;
00569     spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of INT_ENABLE with readflag
00570     response=spi_acs.write(DUMMYBIT); //
00571     ssn_gyr=1;
00572     wait(0.1);
00573         
00574     ssn_gyr=0;
00575     spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register
00576     response=spi_acs.write(response&(~(BIT_SLEEP))); //waking up the gyroscope from sleep
00577     ssn_gyr=1;
00578     wait(0.1);
00579     
00580     trFlag=1;
00581     tr.attach(&trSub,1); //executes the function trSub afer 1sec
00582     
00583     while(trFlag)
00584     {
00585         wait_ms(5);   //This is required for this while loop to exit. I don't know why.
00586         if(drFlag==1)
00587         {
00588             ssn_gyr=0;
00589             spi_acs.write(GYRO_XOUT_H|READFLAG); //sending address of PWR_MGMT_1 with readflag
00590             for(int i=0;i<3;i++)
00591             {
00592                 MSB = spi_acs.write(DUMMYBIT); //reading the MSB values of x,y and z respectively
00593                 LSB = spi_acs.write(DUMMYBIT); //reading the LSB values of x,y and z respectively
00594                 bit_data= ((int16_t)MSB<<8)|LSB; //concatenating to get 16 bit 2's complement of the required gyroscope values
00595                 data[i]=(float)bit_data;
00596                 data[i]=data[i]/senstivity; //dividing with senstivity to get the readings in deg/sec
00597                 data[i]+=error[i]; //adding with error to remove offset errors
00598             }
00599             ssn_gyr=1;      
00600             for (int i=0;i<3;i++)
00601             {
00602                 printf("%f\t",data[i]); //printing the angular velocity values
00603             }
00604             printf("\n\r");
00605             break;
00606         }
00607             drFlag=0;
00608     }
00609     ssn_gyr=0;
00610     spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of PWR_MGMT_1 with readflag
00611     response=spi_acs.write(DUMMYBIT);
00612     ssn_gyr=1;
00613     wait(0.1);
00614         
00615     ssn_gyr=0;
00616     spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register
00617     response=spi_acs.write(response|BIT_SLEEP); //setting the gyroscope in sleep mode
00618     ssn_gyr=1;
00619     wait(0.1);
00620     printf("\n\rExited gyro\n\r");
00621     return data;
00622 }
00623 
00624  
00625  
00626 
00627 
00628