File content as of revision 1:6ec19e5615d9:

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

Serial rn42(p9, p10); 
//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()
{

    rn42.baud(115200);
    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,A", (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());
            
            rn42.printf(str);
            
            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();
                    autosleep_counter=30;
                }
            }

            /*
            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();

}