File content as of revision 1:939b3a2453a6:

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

#define PI              3.14159265358979f
#define FILTER_FREQ     800
#define FILTER_SPLIT    0.95
#define ANGLE_SAMPLES   8

//Pin Defines
DigitalOut  led(LED1);
Serial      pc(USBTX,USBRX);
PwmOut      m0(p21);
PwmOut      m1(p22);
PwmOut      m2(p23);
PwmOut      m3(p24);
ADXL345_I2C accelerometer(p9, p10);
ITG3200     gyroscope(p9, p10);

Ticker filter_tick;
Ticker debug_tick;

//Prototypes
void accelSetup(void);
void accelSample(void);
void accelCal(int samples);
void accelAngle(void);
void gyroSetup(void);
void gyroSample(void);
void gyroCal(int samples);
void angleIni(void);

//Vars
float accel[3];
float accel_offset[3];
float gyro[3];
float gyro_offset[3];
float angle[2];
float angle_buf[2];
float angle_sam[2][ANGLE_SAMPLES];
float last_sample[2];

int buf_cnt = 0;

void filter(void){
    gyroSample();
    accelSample();
    accelAngle();
    for(int h=0;h<2;h++){
        float tmpf = (float)FILTER_SPLIT*(angle[h]+(gyro[h]/(float)FILTER_FREQ))+(1-(float)FILTER_SPLIT)*accel[h];
        angle_buf[h] += tmpf - angle_sam[h][buf_cnt];
        angle_sam[h][buf_cnt] = tmpf;
        angle[h] = angle_buf[h]/ANGLE_SAMPLES;
    }
    if(buf_cnt++ >= ANGLE_SAMPLES-1) buf_cnt = 0;
    led = !led;
}

void debug(void){
    pc.printf("x:%6.3f y:%6.3f\n\r",angle[0],angle[1]);
}

int main() {
    accelSetup();
    gyroSetup();
    
    gyroCal(100);
    
    pc.printf("Calibrated\n\r");
    while(1) if(pc.readable()) break;
    
    angleIni();
    
    pc.printf("x:%5.3f y:%5.3f\n\r",angle[0],angle[1]);
    wait(0.5);
    pc.printf("%d - %d\n\r",(int)(1000000/FILTER_FREQ),ANGLE_SAMPLES);
    wait(0.5);
    
    debug_tick.attach(&debug, 0.1);
    filter_tick.attach_us(&filter, (int)(1000000/FILTER_FREQ));
    
    while(1) {
 //       pc.printf("x:%5.3f y:%5.3f \n\r",angle[0],angle[1]);
 //       wait_us(1000);
    }
}

void accelSetup(void) {
    accelerometer.setPowerControl(0x00);        //Go into standby mode to configure the device.
    accelerometer.setDataFormatControl(0x0B);   //Full resolution, +/-16g, 4mg/LSB.
    accelerometer.setDataRate(ADXL345_200HZ);   //200Hz data rate.
    accelerometer.setPowerControl(0x08);        //Measurement mode.
    wait_ms(22);                                //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
}

void accelSample(void){
    int readings[3];
    accelerometer.getOutput(readings);
    for(int h=0;h<3;h++) accel[h] = ((float)(int16_t)readings[h]/256)-accel_offset[h]; 
}

void accelCal(int samples){
    float  temp[3]  = {0,0,0};
    for(int h=0;h<=samples;h++){
        accelSample();
        for(int j=0;j<=2;j++) temp[j] += accel[j];
    }
    for(int h=0;h<=2;h++) accel_offset[h] = temp[h]/samples;
    accel_offset[2] -= 1;
}

void accelAngle(void){
    accel[0] = atan2(accel[2],accel[0])*(180/(float)PI)-90;
    accel[1] = atan2(accel[2],accel[1])*(180/(float)PI)-90;
}

void gyroSetup(void){
    gyroscope.setLpBandwidth(LPFBW_42HZ);       //Set highest bandwidth.
    wait_ms(100);
}

void gyroSample(void){
    gyro[0] = (gyroscope.getGyroX()-gyro_offset[0])/14.375;
    gyro[1] = (gyroscope.getGyroY()-gyro_offset[1])/14.375;
    gyro[2] = (gyroscope.getGyroZ()-gyro_offset[2])/14.375;
}

void gyroCal(int samples){
    int temp[3] = {0,0,0};
    for(int h=0;h<=samples;h++){
        temp[0] += gyroscope.getGyroX();
        temp[1] += gyroscope.getGyroY();
        temp[2] += gyroscope.getGyroZ();
    } 
    for(int h=0;h<=2;h++) gyro_offset[h] = temp[h]/samples;
}

void angleIni(void){
    for(int h=0;h<2;h++) angle_buf[h] = 0;
    for(int h=0;h<ANGLE_SAMPLES;h++){
        accelSample();
        accelAngle();
        for(int j=0;j<2;j++){
            angle_buf[j] += accel[j];
            angle_sam[j][h] = accel[j];
        }
    }
    for(int h=0;h<2;h++) angle[h] = angle_buf[h]/ANGLE_SAMPLES;
}