ACS.cpp

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