main.cpp

00001 // IMU for Sparkfun 9DOF Sensor Stick
00002 
00003 #include "mbed.h"
00004 #include "ADXL345_I2C.h"
00005 #include "ITG3200.h"
00006 #include "HMC5883L.h"
00007 
00008 DigitalOut led1(LED1);
00009 DigitalOut led2(LED2);
00010 DigitalOut led3(LED3);
00011 DigitalOut led4(LED4);
00012 ADXL345_I2C accelerometer(p9, p10);
00013 ITG3200 gyro(p9, p10);
00014 HMC5883L compass(p9, p10);
00015 Serial pc(USBTX, USBRX);
00016 
00017 #define pi       3.14159265
00018 
00019 #define N        3
00020 #define N_LWMA   10
00021 
00022 int preparing_acc();
00023 double* calib();
00024 double* RK4( double, double[N], double[N] );
00025 double* func( double[N], double[N] );
00026 double* LWMA( double[N] );
00027 double* EKF_predict( double[N], double[N] );
00028 double* EKF_correct( double[N], double[N], double[N] );
00029 
00030 //             0       1       2
00031 //        [ accXn-1 accXn-2   ...   ] 0
00032 // zBuf = [ accYn-1 accYn-2   ...   ] 1
00033 //        [ accZn-1 accZn-2   ...   ] 2
00034 double z_buf[N][N_LWMA]    =    {0};                // For LWMA
00035 
00036 double P[N][N]    =    {{1,0,0},{0,1,0},{0,0,1}};    // For EKF
00037 
00038 int main(){ 
00039     
00040     pc.baud(921600);
00041     
00042     int correct_loop    =   0;
00043     
00044     double dt_wait = 0.00514;
00045     double dt =   0.01;
00046     double t  = 0;    
00047 
00048     int bit_acc[N]   =   {0};    // Buffer of the accelerometer
00049     int get_mag[N]   =   {0};    // Buffer of the compass
00050     
00051     // Calibration routine
00052     double calib_acc[N]         =   {0};
00053     double calib_gyro[N]        =   {0};
00054     double compass_basis_rad    =   0;
00055     
00056     // Getting data
00057     double acc[N]       =   {0};
00058     double gyro_deg[N]  =   {0};
00059     double gyro_rad[N]  =   {0};
00060     int    mag[N]       =   {0};
00061     
00062     // Measurement algorithm
00063     double ang_acc[2]   =   {0};
00064     double ang_deg[N]   =   {0};
00065     double ang_rad[N]   =   {0};
00066     double zLWMA[N]     =   {0};
00067     double compass_rad  =   0;
00068     double compass_deg  =   0;
00069     
00070     for( int i=0;i<N_LWMA;i++ ){    z_buf[2][i]    =    1;    }
00071     
00072     double* p_calib;
00073     double* p_RK4;
00074     double* p_EKF;
00075     double* p_zLWMA;
00076     
00077     // ***  Setting up sensors ***    
00078     int preparing_acc();
00079     gyro.setLpBandwidth(LPFBW_42HZ);
00080     compass.setDefault();
00081     wait(0.1);                        // Wait some time for all sensors (Need at least 5ms)
00082     
00083     // *** Calibration routine ***
00084     p_calib = calib();
00085     for( int i=0;i<3;i++ ){     calib_acc[i]    =   *p_calib;   p_calib = p_calib+1;    }
00086     for( int i=3;i<6;i++ ){     calib_gyro[i-3] =   *p_calib;   p_calib = p_calib+1;    }
00087     compass_basis_rad   =   *p_calib;
00088 
00089     pc.printf("Starting IMU unit\n\r");
00090     pc.printf("   Time     AccX     AccY     AccZ    GyroX    GyroY    GyroZ   MagX   MagY   MagZ\n\r");
00091     while(1){
00092         
00093         // Updating accelerometer and compass
00094         accelerometer.getOutput(bit_acc);
00095         compass.readData(get_mag);
00096         
00097         // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
00098         acc[0]      =  ((int16_t)bit_acc[0]-calib_acc[0])*0.004;
00099         acc[1]      =  ((int16_t)bit_acc[1]-calib_acc[1])*0.004;
00100         acc[2]      =  ((int16_t)bit_acc[2]-calib_acc[2])*0.004+1;
00101         
00102         gyro_deg[0] = (gyro.getGyroX()-calib_gyro[0])/14.375;
00103         gyro_deg[1] = -(gyro.getGyroY()-calib_gyro[1])/14.375;          // Modify the differencial of the sensor axis 
00104         gyro_deg[2] = -(gyro.getGyroZ()-calib_gyro[2])/14.375;
00105                 
00106         for( int i=0;i<N;i++ ){     mag[i] = (int16_t)get_mag[i];   }
00107         
00108         // Low pass filter for acc
00109         //for( int i=0;i<N;i++ ){    if( -0.05<acc[i] && acc[i]<0.05 ){    acc[i] = 0;  }  }
00110         
00111         for( int i=0;i<N;i++ ){     if( acc[i]<-2 ){    acc[i] = -2;    }  }
00112         for( int i=0;i<N;i++ ){     if( acc[i]>2 ){    acc[i] = 2;    }  }
00113         
00114         // Low pass filter for gyro
00115         //for( int i=0;i<N;i++ ){    if( -0.5<gyro_deg[i] && gyro_deg[i]<0.5 ){    gyro_deg[i] = 0;  } }
00116         
00117         for( int i=0;i<N;i++ ){    gyro_rad[i] = gyro_deg[i]*pi/180;    }
00118         
00119         // Compass yaw
00120         /*compass_rad    =    (double)mag[1]/mag[0];
00121         compass_rad    =    atan(compass_rad);
00122         //compass_rad    =    compass_rad-compass_basis_rad;
00123         compass_deg    =    compass_rad*180/pi;
00124         */
00125         // LWMA (Observation)
00126         p_zLWMA = LWMA(acc);
00127         for( int i=0;i<N;i++ ){     zLWMA[i] = *p_zLWMA;    p_zLWMA = p_zLWMA+1;    }
00128         // LWMA angle
00129         ang_acc[0] = asin(zLWMA[1])*180/pi;
00130         ang_acc[1] = asin(zLWMA[0])*180/pi;
00131         
00132         // RK4 (Prediction)
00133         p_RK4 = RK4(dt,ang_rad,gyro_rad);
00134         for( int i=0;i<N;i++ ){    ang_rad[i] = *p_RK4;        p_RK4 = p_RK4+1;    }
00135         
00136         // EKF (Correction)
00137         EKF_predict(ang_rad,gyro_rad);
00138         correct_loop++;
00139         if (correct_loop>=10){
00140             p_EKF = EKF_correct(ang_rad,gyro_rad,zLWMA);
00141             for ( int i=0;i<N;i++ ){    ang_rad[i] = *p_EKF;        p_EKF = p_EKF+1;    }
00142             correct_loop    =    0;
00143         }
00144         for( int i=0;i<N;i++ ){    ang_deg[i] = ang_rad[i]*180/pi;    }
00145         
00146         //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], mag[0], mag[1], mag[2]);
00147         //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f\n\r", t, zLWMA[0], zLWMA[1], zLWMA[2], ang_acc[0], ang_acc[1]);
00148         //pc.printf("%7.2f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, ang_acc[0], ang_acc[1], ang_deg[0], ang_deg[1], ang_deg[2]);
00149         pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, gyro_deg[0], gyro_deg[1], gyro_deg[2], ang_deg[0], ang_deg[1], ang_deg[2]);
00150                 
00151         wait(dt_wait);
00152         t += dt;
00153         
00154     }
00155 }
00156 
00157 
00158 double* EKF_predict( double y[N], double x[N] ){ 
00159     // x = F * x;  
00160     // P = F * P * F' + G * Q * G';
00161 
00162     //double Q[N][N]        =    { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} };
00163     double Q[N][N]        =    { {0.00263, 0, 0}, {0, 0.00373, 0}, {0, 0, 0.00509} };
00164 
00165     double Fjac[N][N]    =    {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0}, {-x[1]*sin(y[0])-x[2]*cos(y[0]), 0, 0}, {x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1]), x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1])), 0}};
00166     double Fjac_t[N][N]    =    {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), -x[1]*sin(y[0])-x[2]*cos(y[0]), x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1])}, {x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0, x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))}, {0, 0, 0}};
00167     double Gjac[N][N]    =    {{1, sin(y[0])*tan(y[1]), cos(y[0])*tan(y[1])}, {0, cos(y[0]), -sin(y[0])}, {0, sin(y[0])/cos(y[1]), cos(y[0])/cos(y[1])}};
00168     double Gjac_t[N][N]    =    {{1, 0, 0}, {sin(y[0])*tan(y[1]), cos(y[0]), sin(y[0])/cos(y[1])}, {cos(y[0])*tan(y[1]), -sin(y[0]), cos(y[0])/cos(y[1])}};
00169 
00170     double FPF[N][N]    =    {0};
00171     double GQG[N][N]    =    {0};
00172 
00173     double FP[N][N]        =    {0};
00174     double GQ[N][N]        =    {0};
00175     
00176     for( int i=0;i<N;i++ ){
00177         for( int j=0;j<N;j++ ){
00178             for( int k=0;k<N;k++ ){
00179                 FP[i][j]    +=    Fjac[i][k]*P[k][j];
00180                 GQ[i][j]    +=    Gjac[i][k]*Q[k][j];
00181 
00182             }
00183         }
00184     }
00185 
00186     for( int i=0;i<N;i++ ){
00187         for( int j=0;j<N;j++ ){
00188             for( int k=0;k<N;k++ ){
00189                 FPF[i][j]        +=    FP[i][k]*Fjac_t[k][j];
00190                 GQG[i][j]        +=    GQ[i][k]*Gjac_t[k][j];
00191             }
00192         }
00193     }
00194     for( int i=0;i<N;i++ ){
00195         for( int j=0;j<N;j++ ){
00196             P[i][j]        =    FPF[i][j]+GQG[i][j];
00197         }
00198     }
00199     
00200     return 0;
00201 
00202 }
00203 
00204 double* EKF_correct( double y[N], double x[N], double z[N] ){
00205     // K = P * H' / ( H * P * H' + R )
00206     // x = x + K * ( yobs(t) - H * x )
00207     // P = P - K * H * P
00208 
00209     //double R[N][N]        =    { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} };
00210     double R[N][N]        =    { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} };
00211 
00212     double Hjac[N][N]    =    {{0, cos(y[1]), 0}, {cos(y[0]), 0, 0}, {-sin(y[0])*cos(y[1]), -cos(y[0])*sin(y[1]), 0}};
00213     double Hjac_t[N][N]    =    {{0, cos(y[0]), -sin(y[0])*cos(y[1])}, {cos(y[1]), 0, -cos(y[0])*sin(y[1])}, {0, 0, 0}};
00214     double K[N][N]        =    {0};    // Kalman gain
00215     double K_deno[N][N]    =    {0};    // Denominator of the kalman gain
00216     double det_K_deno_inv    =    0;
00217     double K_deno_inv[N][N]    =    {0};
00218     double HPH[N][N]    =    {0};
00219     double HP[N][N]        =    {0};
00220     double PH[N][N]        =    {0};
00221     double KHP[N][N]    =    {0};
00222 
00223     double Hx[N]        =    {0};
00224     double z_Hx[N]        =    {0};
00225     double Kz_Hx[N]        =    {0};
00226 
00227     double* py    =    y;
00228 
00229     // Kalman gain
00230     for( int i=0;i<N;i++ ){
00231         for( int j=0;j<N;j++ ){
00232             for( int k=0;k<N;k++ ){
00233                 HP[i][j]    +=    Hjac[i][k]*P[k][j];
00234                 PH[i][j]    +=    P[i][k]*Hjac_t[k][j];
00235             }
00236         }
00237     }
00238     for( int i=0;i<N;i++ ){
00239         for( int j=0;j<N;j++ ){
00240             for( int k=0;k<N;k++ ){
00241                 HPH[i][j]        +=    HP[i][k]*Hjac_t[k][j];
00242             }
00243         }
00244     }
00245     for( int i=0;i<N;i++ ){
00246         for( int j=0;j<N;j++ ){
00247             K_deno[i][j]    =    HPH[i][j]+R[i][j];
00248         }
00249     }
00250     // Inverce matrix
00251     det_K_deno_inv      =    K_deno[0][0]*K_deno[1][1]*K_deno[2][2]+K_deno[1][0]*K_deno[2][1]*K_deno[0][2]+K_deno[2][0]*K_deno[0][1]*K_deno[1][2]-K_deno[0][0]*K_deno[2][1]*K_deno[1][2]-K_deno[2][0]*K_deno[1][1]*K_deno[0][2]-K_deno[1][0]*K_deno[0][1]*K_deno[2][2];
00252     K_deno_inv[0][0]    =    (K_deno[1][1]*K_deno[2][2]-K_deno[1][2]*K_deno[2][1])/det_K_deno_inv;
00253     K_deno_inv[0][1]    =    (K_deno[0][2]*K_deno[2][1]-K_deno[0][1]*K_deno[2][2])/det_K_deno_inv;
00254     K_deno_inv[0][2]    =    (K_deno[0][1]*K_deno[1][2]-K_deno[0][2]*K_deno[1][1])/det_K_deno_inv;
00255     K_deno_inv[1][0]    =    (K_deno[1][2]*K_deno[2][0]-K_deno[1][0]*K_deno[2][2])/det_K_deno_inv;
00256     K_deno_inv[1][1]    =    (K_deno[0][0]*K_deno[2][2]-K_deno[0][2]*K_deno[2][0])/det_K_deno_inv;
00257     K_deno_inv[1][2]    =    (K_deno[0][2]*K_deno[1][0]-K_deno[0][0]*K_deno[1][2])/det_K_deno_inv;
00258     K_deno_inv[2][0]    =    (K_deno[1][0]*K_deno[2][1]-K_deno[1][1]*K_deno[2][0])/det_K_deno_inv;
00259     K_deno_inv[2][1]    =    (K_deno[0][1]*K_deno[2][0]-K_deno[0][0]*K_deno[2][1])/det_K_deno_inv;
00260     K_deno_inv[2][2]    =    (K_deno[0][0]*K_deno[1][1]-K_deno[0][1]*K_deno[1][0])/det_K_deno_inv;
00261 
00262     for( int i=0;i<N;i++ ){
00263         for( int j=0;j<N;j++ ){
00264             for( int k=0;k<N;k++ ){
00265                 K[i][j]        +=    PH[i][k]*K_deno_inv[k][j];
00266             }
00267         }
00268     }
00269 
00270     // Filtering
00271     for( int i=0;i<N;i++ ){
00272         for( int j=0;j<N;j++ ){
00273             Hx[i]        +=    Hjac[i][j]*y[j];
00274         }
00275     }
00276     for( int i=0;i<N;i++ ){
00277         z_Hx[i]    =    z[i]-Hx[i];
00278     }
00279     for( int i=0;i<N;i++ ){
00280         for( int j=0;j<N;j++ ){
00281             Kz_Hx[i]    +=    K[i][j]*z_Hx[j];
00282         }
00283     }
00284     for( int i=0;i<N;i++ ){
00285             y[i]    =    y[i]+Kz_Hx[i];
00286     }
00287 
00288     // Covarience
00289     for( int i=0;i<N;i++ ){
00290         for( int j=0;j<N;j++ ){
00291             for( int k=0;k<N;k++ ){
00292                 KHP[i][j]    +=    K[i][k]*HP[k][j];
00293             }
00294         }
00295     }
00296     for( int i=0;i<N;i++ ){
00297         for( int j=0;j<N;j++ ){
00298             P[i][j]    =    P[i][j]-KHP[i][j];
00299         }
00300     }
00301 
00302     return py;
00303 
00304 }
00305 
00306 double* LWMA( double z[N] ){
00307     
00308     double zLWMA[N]        =    {0};
00309     double zLWMA_num[N]    =    {0};
00310     double zLWMA_den    =    0;
00311 
00312     double* p_zLWMA    =    zLWMA;
00313 
00314     for( int i=1;i<N_LWMA;i++ ){
00315         for( int j=0;j<N;j++ ){
00316             z_buf[j][N_LWMA-i]    =    z_buf[j][N_LWMA-i-1];
00317         }
00318     }
00319     for( int i=0;i<N;i++ ){
00320         z_buf[i][0]    =    z[i];
00321     }
00322 
00323     for( int i=0;i<N_LWMA;i++ ){
00324         for( int j=0;j<N;j++ ){
00325             zLWMA_num[j]    +=    (N_LWMA-i)*z_buf[j][i];    
00326         }
00327         zLWMA_den    +=    N_LWMA-i;
00328     }
00329     for( int i=0;i<N;i++ ){
00330         zLWMA[i]    =    zLWMA_num[i]/zLWMA_den;
00331     }
00332     
00333     return p_zLWMA;
00334 
00335 }
00336 
00337 double* RK4( double dt, double y[N], double x[N] ){
00338     
00339     double yBuf[N]    =    {0};
00340     double k[N][4]    =    {0};
00341 
00342     double* p_y    =    y;
00343 
00344     double* pk1;
00345     double* pk2;
00346     double* pk3;
00347     double* pk4;
00348 
00349         for( int i=0;i<N;i++){    yBuf[i]    = y[i];    }
00350         pk1    =    func (yBuf,x);
00351         for( int i=0;i<N;i++ ){    k[i][0] = *pk1;        pk1    = pk1+1;    }
00352 
00353         for( int i=0;i<N;i++){    yBuf[i]    = y[i]+0.5*dt*k[i][1];    }
00354         pk2    =    func (yBuf,x);
00355         for( int i=0;i<N;i++ ){    k[i][1]    = *pk2;        pk2    = pk2+1;    }
00356 
00357         for( int i=0;i<N;i++){    yBuf[i]    = y[i]+0.5*dt*k[i][2];    }
00358         pk3    =    func (yBuf,x);
00359         for( int i=0;i<N;i++ ){    k[i][2]    = *pk3;        pk3    = pk3+1;    }
00360 
00361         for( int i=0;i<N;i++){    yBuf[i]    = y[i]+dt*k[i][3];    }
00362         pk4    =    func (yBuf,x);
00363         for( int i=0;i<N;i++ ){    k[i][3]    = *pk4;        pk4 = pk4+1;    }
00364 
00365         for( int i=0;i<N;i++){    y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0;    }
00366 
00367     return p_y;
00368 
00369 }
00370 
00371 double* func( double y[N], double x[N] ){
00372 
00373     double f[N]    =    {0};
00374     double* p_f    =    f;
00375     
00376     f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]);
00377     f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]);
00378     f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]);
00379 
00380     return p_f;
00381 
00382 }
00383 
00384 double* calib(){
00385 
00386     int calib_loop  =   1000;
00387 
00388     int bit_acc[N]   =   {0};    // Buffer of the accelerometer
00389     int get_mag[N]   =   {0};    // Buffer of the compass
00390 
00391     double calib_acc[N]         =   {0};
00392     double calib_gyro_buf[N]    =   {0};
00393     double calib_gyro[N]        =   {0};
00394     double compass_basis_buf[N] =   {0};
00395     double compass_basis_rad    =   0;
00396     double calib_result[7]      =   {0};
00397 
00398     double* p_calib_result      =   calib_result;
00399 
00400     pc.printf("\n\r\n\rDon't touch... Calibrating now!!\n\r");
00401     led1    =   1;
00402     
00403     for( int i=0;i<calib_loop;i++ ){
00404         
00405         accelerometer.getOutput(bit_acc);
00406         compass.readData(get_mag);
00407         
00408         calib_gyro_buf[0]   =   gyro.getGyroX();
00409         calib_gyro_buf[1]   =   gyro.getGyroY();
00410         calib_gyro_buf[2]   =   gyro.getGyroZ();
00411         
00412         for( int j=0;j<N;j++ ){
00413             calib_acc[j]            +=  (int16_t)bit_acc[j];
00414             calib_gyro[j]           +=  calib_gyro_buf[j];
00415             compass_basis_buf[j]    +=  (int16_t)get_mag[j];
00416         }
00417         
00418         if( i>calib_loop*3/4 ){
00419              led4   =   1;
00420         }else if( i>calib_loop/2 ){
00421             led3    =   1;
00422         }else if( i>calib_loop/4 ){
00423             led2    =   1;
00424         }
00425         
00426     }
00427     
00428     for( int i=0;i<N;i++ ){
00429         calib_acc[i]            =   calib_acc[i]/calib_loop;
00430         calib_gyro[i]           =   calib_gyro[i]/calib_loop;
00431         compass_basis_buf[i]    =   compass_basis_buf[i]/calib_loop;
00432     }
00433     
00434     compass_basis_rad   =   compass_basis_buf[1]/compass_basis_buf[0];
00435     compass_basis_rad   =   atan(compass_basis_rad);
00436     led1 = 0;    led2 = 0;  led3 = 0;   led4    =   0;
00437     
00438     pc.printf("  accX    accY    accZ   gyroX   gyroY   gyroZ    yaw_basis\n\r");
00439     pc.printf("%6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f\n\r",calib_acc[0],calib_acc[1],calib_acc[2],calib_gyro[0],calib_gyro[1],calib_gyro[2],compass_basis_rad*180/pi);
00440     
00441     for( int i=0;i<3;i++ ){     calib_result[i] =   calib_acc[i];    }
00442     for( int i=3;i<6;i++ ){     calib_result[i] =   calib_gyro[i-3];    }    
00443     calib_result[6] =  compass_basis_rad;
00444     
00445     for( int i=0;i<3;i++ ){
00446         wait(0.5);
00447         led1 = 1;    led2 = 1;  led3 = 1;   led4    =   1;
00448         wait(0.5);
00449         led1 = 0;    led2 = 0;  led3 = 0;   led4    =   0;
00450     }
00451 
00452     return p_calib_result;
00453 
00454 }
00455 
00456 int preparing_acc(){
00457  
00458     // These are here to test whether any of the initialization fails. It will print the failure.
00459     if (accelerometer.setPowerControl(0x00)){
00460         pc.printf("didn't intitialize power control\n\r"); 
00461         return 0;
00462     }
00463     // Full resolution, +/-16g, 4mg/LSB.
00464     wait(.001);
00465      
00466     if(accelerometer.setDataFormatControl(0x0B)){
00467         pc.printf("didn't set data format\n\r");
00468         return 0;
00469     }
00470     wait(.001);
00471      
00472     // 3.2kHz data rate.
00473     if(accelerometer.setDataRate(ADXL345_3200HZ)){
00474         pc.printf("didn't set data rate\n\r");
00475         return 0;
00476     }
00477     wait(.001);
00478       
00479     if(accelerometer.setPowerControl(MeasurementMode)) {
00480         pc.printf("didn't set the power control to measurement\n\r"); 
00481         return 0;
00482     } 
00483     wait(.001);
00484     
00485     return 0;
00486  
00487 }