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