File content as of revision 0:384bc42ad0cf:

#include "mbed.h"
#include "kalman.c"
#include "BMA180.h"
#include "ITG3200.h"

#define PI             3.1415926535897932384626433832795
#define Rad2Dree       57.295779513082320876798154814105

Serial pc(USBTX, USBRX);

I2C I2CBus(p28, p27);

Timer GlobalTime;
Timer ProgramTimer;

float R;
double angle[3];
unsigned long timer;
long loopStartTime;

BMA180 Acc(I2CBus, GlobalTime);
ITG3200 Gyro(I2CBus, GlobalTime);
kalman filter_pitch; 
kalman filter_roll;
    
int main() {
    int count = 0;
    pc.baud(115200);
  
    I2CBus.frequency(400000);
    GlobalTime.start();

    Acc.Init();
    wait_ms(500);
    
    //User Calibration
    short Raw1g[3]= {0, 0, 0};
    Acc.userCalibration(Raw1g);
    
    //0.5s Calibration
    Acc.Calibrate(500, Raw1g);
  
    Gyro.Init();
    wait_ms(500);
    
    //0.5s Calibration
    Gyro.Calibrate(500);

    // Parameters ( R_angle, Q_gyro, Q_angle ) 
    kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
    kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
    
    
    ProgramTimer.start();
    loopStartTime = ProgramTimer.read_us();
    timer = loopStartTime;
    
    while(1) {
    
        //Aquire new values for the Gyro and Acc
        Acc.Update();
        Gyro.Update();
        
        //Calcuate the resulting vector R from the 3 acc axes
        R = sqrt(std::pow(Acc.Acc[0] , 2) + std::pow(Acc.Acc[1] , 2) + std::pow(Acc.Acc[2] , 2));
   
        kalman_predict(&filter_pitch, Gyro.Rate[0],  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_pitch, acos(Acc.Acc[0]/R));
        kalman_predict(&filter_roll, Gyro.Rate[1],  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_roll, acos(Acc.Acc[1]/R));
        
        angle[0] = kalman_get_angle(&filter_pitch);
        angle[1] = kalman_get_angle(&filter_roll);
        
        timer = ProgramTimer.read_us();
        
        //count++;
        //if(count == 10000){pc.printf("Program time: %i\n\r", ProgramTimer.read_us() - loopStartTime);loopStartTime = ProgramTimer.read_us(); count = 0;}
        pc.printf("Pitch Angle X: %.6f   Pitch Angle Y: %.6f \n\r", Rad2Dree * angle[1], Rad2Dree * angle[0]);
       
    }
}