File content as of revision 0:80d32420bc63:

#include "ADXL345_I2C.h"
#include "ITG3200.h"

ADXL345_I2C accelerometer(p9, p10);
ITG3200 gyro(p9, p10);
Serial pc(USBTX, USBRX);

int main(){ 
    float dt = 0.1;
    float t  = 0.0;    
    pc.baud(115200);
    int    bitAcc[3] = {0, 0, 0};
    double Acc   [3] = {0, 0, 0};
    double Gyro0 [3] = {0, 0, 0};
    double Gyro1 [3] = {0, 0, 0};
    double Gyro2 [3] = {0, 0, 0};
    double Ang0  [3] = {0, 0, 0};
    double Ang1  [3] = {0, 0, 0};
    double Ang2  [3] = {0, 0, 0};
    
    // *** Part of the accelerometer ***
    // These are here to test whether any of the initialization fails. It will print the failure
    if (accelerometer.setPowerControl(0x00)){
        pc.printf("didn't intitialize power control\n"); 
        return 0;  }
    // Full resolution, +/-16g, 4mg/LSB.
    wait(.001);
     
    if(accelerometer.setDataFormatControl(0x0B)){
        pc.printf("didn't set data format\n");
        return 0;  }
    wait(.001);
     
    // 3.2kHz data rate.
    if(accelerometer.setDataRate(ADXL345_3200HZ)){
        pc.printf("didn't set data rate\n");
        return 0;    }
    wait(.001);
     
    // Measurement mode.
     
    if(accelerometer.setPowerControl(MeasurementMode)) {
        pc.printf("didn't set the power control to measurement\n"); 
        return 0;   } 
    // *** Part of the accelerometer ***
    
    gyro.setLpBandwidth(LPFBW_42HZ);
    
    pc.printf("Starting ADXL345 and ITG3200 test...\n");
    //pc.printf("  Time   AccX   AccY   AccZ   GyroX   GyroY   GyroZ\n");
    pc.printf("  Time    GyroX    GyroY    GyroZ   AngleX   AngleY   AngleZ\n");
                
    while(1){
        accelerometer.getOutput(bitAcc);
        // Transfering units (Acc[g], Gyro[deg/s])
        Acc[0] = ((int16_t)bitAcc[0])*0.004;         Acc[1] = ((int16_t)bitAcc[1])*0.004;         Acc[2] = ((int16_t)bitAcc[2]+50)*0.004;
        Gyro2[0] = (gyro.getGyroX()+30)/14.375;      Gyro2[1] = (gyro.getGyroY()-28)/14.375;      Gyro2[2] = (gyro.getGyroZ()-17)/14.375;
        
        // Low pass filter for gyro
        if( -1.0<Gyro2[0] && Gyro2[0]<1.0 ){    Gyro2[0]=0;  }
        if( -1.0<Gyro2[1] && Gyro2[1]<1.0 ){    Gyro2[1]=0;  }
        if( -1.0<Gyro2[2] && Gyro2[2]<1.0 ){    Gyro2[2]=0;  }
        
        // Trapezoidal integration
        Ang1[0] = Ang0[0]+(Gyro0[0]+Gyro1[0])*dt/2;
        Ang1[1] = Ang0[1]+(Gyro0[1]+Gyro1[1])*dt/2;
        Ang1[2] = Ang0[2]+(Gyro0[2]+Gyro1[2])*dt/2;
        
        // Simpson integration
        // Ang1[0] = Ang0[0]+(Gyro0[0]+4*Gyro1[0]+Gyro2[0])*dt/6;
        // Ang1[1] = Ang0[1]+(Gyro0[1]+4*Gyro1[1]+Gyro2[1])*dt/6;
        // Ang1[2] = Ang0[2]+(Gyro0[2]+4*Gyro1[2]+Gyro2[2])*dt/6;
        
        //pc.printf("%6.1f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, Acc[0], Acc[1], Acc[2], Gyro1[0], Gyro1[1], Gyro1[2]);
        pc.printf("%6.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, Gyro1[0], Gyro1[1], Gyro1[2], Ang1[0], Ang1[1], Ang1[2]);
        //pc.printf("%6.1f, %6i, %6i, %6i, %7.2f, %7.2f, %7.2f\n\r", t, gyro.getGyroX()+32, gyro.getGyroY()-27, gyro.getGyroZ()-17, Ang1[0], Ang1[1], Ang1[2]);
        //pc.printf("%6.1f, %6i, %6i, %6i, %7.3f, %7.3f, %7.3f\n\r", t, (int16_t)bitAcc[0], (int16_t)bitAcc[1], (int16_t)bitAcc[2], Acc[0], Acc[1], Acc[2]);
        Gyro1[0] = Gyro2[0];  Gyro1[1] = Gyro2[1];  Gyro1[2] = Gyro2[2];
        Gyro0[0] = Gyro1[0];  Gyro0[1] = Gyro1[1];  Gyro0[2] = Gyro1[2];
        Ang0[0]  = Ang1[0];   Ang0[1]  = Ang1[1];   Ang0[2]  = Ang1[2];
        
        t += dt;
        wait(dt);
    }
 
}