File content as of revision 0:5ca44aa85a30:

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

Serial pc(USBTX, USBRX);
ITG3200 gyro(p28, p27);
ADXL345_I2C accel(p28, p27);
DigitalOut WakeLed(LED1);
DigitalOut SleepLed(LED2);
DigitalOut ReadLed(LED3);
DigitalOut CalLed(LED4);

double w_last_x=0;
double w_last_y=0;
double w_last_z=0;
int autosleep_counter=100;

int accel_read[3] = {0, 0, 0};  
double twocomp16_to_double(int16_t x);
int16_t double_to_twocomp16(double x);
char RegPeekAcc(char address);
void RegPokeAcc(char address, char value);
void AccCalib();
void RegdumpAcc();

void sleep1();

InterruptIn pushbutt(p21);
void bushbutt_handler()
{ WakeLed=1;
  SleepLed=0;
}
InterruptIn accinterupt(p22);
void acc_int_handler()
{ RegPeekAcc(0x30); //clear interrput
  WakeLed=1;
  SleepLed=0;
}


int main() {


    char str[512];
    gyro.setWhoAmI(0x68);
    pc.printf("Now starting 6-degrees-of-freedom (ITG-3200 ADXL345) test...\n");
    pc.printf("Accelerometer Device ID is: 0x%02x\n", accel.getDeviceID());
    pc.printf("Gyro Devide ID is: 0x%02x\n", gyro.getWhoAmI()); 
    
    // Accel setup
    // These are here to test whether any of the initialization fails. It will print the failure
    if (accel.setPowerControl(0x00)){
         pc.printf("didn't intitialize power control\n"); 
         return 0;  }
    //Full resolution, +/-16g, 4mg/LSB.
    wait(.001);    
    if(accel.setDataFormatControl(0x0B)){
        pc.printf("didn't set data format\n");
        return 0;  }
    wait(.001);
    //3.2kHz data rate.
    if(accel.setDataRate(ADXL345_3200HZ)){
       pc.printf("didn't set data rate\n");
       return 0;    }
    wait(.001);   
    //Measurement mode.    
    if(accel.setPowerControl(MeasurementMode)) {
       pc.printf("didn't set the power control to measurement\n"); 
       return 0;   
    }     
    // Gyro setup   
    gyro.setLpBandwidth(LPFBW_42HZ);
       
    //Added for Acc
    WakeLed=1;
    AccCalib();
    RegPokeAcc(0x24,0x9);//set threshold for activity
    RegPokeAcc(0x27,0xF0);//Activity axis control
    RegPokeAcc(0x2E,0x10);//interrupt enable
    RegPeekAcc(0x30); //clear interrput
    wait(0.1);
    pushbutt.rise(&bushbutt_handler); //enable mbed trigger detect
    accinterupt.rise(&acc_int_handler);  
    RegdumpAcc();

    
    if(true)
    for(int i=0;i<=100000;i++)
    {
        wait(0.01);
        
        if(ReadLed==1) ReadLed=0;
        else ReadLed=1;
        
        accel.getOutput(accel_read);
        
        sprintf(str, "%i,%i,%i,%i,%i,%i,%i\n\r", (int16_t)accel_read[0], (int16_t)accel_read[1], (int16_t)accel_read[2],
                                                     gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ(), (int16_t)gyro.getTemperature());  
    
        double d_accelx=(double) (int16_t)accel_read[0];
        double d_accely=(double) (int16_t)accel_read[1];
        double d_accelz=(double) (int16_t)accel_read[2];  
        double wp=6.28e-2; //IIR pole for DC cancellation
        double w_x,w_y,w_z;
        double dc_x,dc_y,dc_z;
            
        //IIR calculate DC term in acceleration
        w_x=d_accelx+ (1-wp)*w_last_x; dc_x=wp*w_x; w_last_x=w_x;     
        w_y=d_accely+ (1-wp)*w_last_y; dc_y=wp*w_y; w_last_y=w_y;  
        w_z=d_accelz+ (1-wp)*w_last_z; dc_z=wp*w_z; w_last_z=w_z;   
      
        d_accelx=d_accelx-dc_x;
        d_accely=d_accely-dc_y;
        d_accelz=d_accelz-dc_z;            
        double mag_accel=d_accelx*d_accelx+d_accely*d_accely+d_accelz*d_accelz;
        mag_accel=sqrt(mag_accel);
        pc.printf("With DC Cal:Double %f %f %f . Offset is %f %f %f. Mag %f\n\r",d_accelx,d_accely,d_accelz,dc_x,dc_y,dc_z,mag_accel);       
        RegPeekAcc(0x30);
        
        if(autosleep_counter>0) autosleep_counter-=1;
        else
        { if(mag_accel<15) 
             {sleep1();
             }   
        }
        
        /*
        pc.printf("Gyro[%5i, %5i, %5i]   Accel[%4i, %4i, %4i]\n\t", 
                            gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ(),
                            (int16_t)accel_read[0], (int16_t)accel_read[1], (int16_t)accel_read[2]);
        */
    }


}


double twocomp16_to_double(int16_t x)
{double a;
a= double(x);
return a;
}
int16_t double_to_twocomp16(double x)
{int16_t a;
 a=(signed int16_t) x;
 return a;
}

char RegPeekAcc(char address)
{
    char value=accel.SingleByteRead(address);
    pc.printf("AccReg address: %X value: %X\n\r",address,value);
    return value;
}
void RegPokeAcc(char address, char value)
{
    accel.SingleByteWrite(address,value);
}

void AccCalib()
{   CalLed=1;
    double acc0=0,acc1=0,acc2=0; 
    double sum0=0,sum1=0,sum2=0; 
    wait(0.1) ;
    accel.getOutput(accel_read);
//    RegPokeAcc(0x1e,offsetcal((int16_t)accel_read[0])); 
//    RegPokeAcc(0x1f,offsetcal((int16_t)accel_read[1])); 
//    RegPokeAcc(0x20,offsetcal((int16_t)accel_read[2]));
    for(int n=1;n<=10;n++){
        wait(0.1);
        accel.getOutput(accel_read);
        acc0=(double)(int16_t)accel_read[0];
        acc1=(double)(int16_t)accel_read[1];
        acc2=(double)(int16_t)accel_read[2];
        sum0+=acc0;sum1+=acc1;sum2+=acc2;
    }
    sum0=sum0/10/(-4);
    sum1=sum1/10/(-4);
    sum2=sum2/10/(-4);    
    int16_t acc0_16=(signed int16_t)sum0;
    int16_t acc1_16=(signed int16_t)sum1;
    int16_t acc2_16=(signed int16_t)sum2;    
    RegPokeAcc(0x1e,acc0_16); 
    RegPokeAcc(0x1f,acc1_16); 
    RegPokeAcc(0x20,acc2_16); 
 //   pc.printf("Calibration. Calculated offset %i   %i   %i\n\r",acc0_16,acc1_16,acc2_16);
    int valid=1;
    for(int n=1;n<=10;n++)
    {wait(0.1);
    accel.getOutput(accel_read);
    accel.getOutput(accel_read);
    acc0=(double)(int16_t)accel_read[0]; if(abs(acc0)>50) valid=0;
    acc1=(double)(int16_t)accel_read[1];if(abs(acc1)>50) valid=0;
    acc2=(double)(int16_t)accel_read[2];if(abs(acc2)>50) valid=0;
 //   pc.printf("verifying cal %i,%i,%i \n\r", (int16_t)accel_read[0], (int16_t)accel_read[1], (int16_t)accel_read[2]);
 //   pc.printf("verifying cal (double)%f %f %f\n\r",acc0,acc1,acc2);
 //   pc.printf("valid= %d \n\r",valid);
    }    
//    if(valid==1)
//    CalLed=0;
//    else AccCalib();
}

void RegdumpAcc()
{
pc.printf("\r---------a new regdump start\n\r-------");
pc.printf("Acc DeviceID \t");RegPeekAcc(0x00);
pc.printf("X_asis: \t");RegPeekAcc(0x1e);
pc.printf("Y_asis: \t");RegPeekAcc(0x1f);
pc.printf("Z_asis: \t");RegPeekAcc(0x20);
pc.printf("Act Thresh: \t");RegPeekAcc(0x24);
pc.printf("Axis Enable: \t");RegPeekAcc(0x27);
pc.printf("Interpt Ctrl: \t");RegPeekAcc(0x2e);
pc.printf("Interpt Mapping: \t");RegPeekAcc(0x2f);
//pc.printf("Source of interupt:\t");RegPeekAcc(0x30);
pc.printf("Data format control\t");RegPeekAcc(0x31);
pc.printf("FIFO Ctrl:\t");RegPeekAcc(0x38);
pc.printf("FIFO Status:\t");RegPeekAcc(0x39);
}

void sleep1()
{
WakeLed=0;
SleepLed=1;
sleep();

}