File content as of revision 0:7fd305c81a8e:

#include "mbed.h"
#include "rtos.h"
#include "arm_math.h" 
#include "kalman.c"
#include "LSM303DLHC.h"
#include "L3GD20H.h"

#define Rad2Dree       57.295779513082320876798154814105f

DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);
DigitalIn sw2(SW2);
DigitalIn sw3(SW3);
Serial pc(USBTX, USBRX);

LSM303DLHC imu(D14,D15);
L3GD20H gyro(D14,D15);

LSM303DLHC imu2(PTC11,PTC10);
L3GD20H gyro2(PTC11,PTC10);

Timer GlobalTime;
Timer ProgramTimer;

kalman filter_pitch; 
kalman filter_roll;

kalman filter_pitch2; 
kalman filter_roll2;

int acc[3];
int mag[3];
short gyr[3];

int acc2[3];
int mag2[3];
short gyr2[3];

struct vector {
  float x;
  float y;
  float z;
} Acc, Gyr, Acc2, Gyr2;

float R;
double angle[3];

float R2;
double angle2[3];

unsigned long timer;
long loopStartTime;

void AccRaw2G(int acc[3]);
void GyrRaw2D(short gyr[3]);
void AccRaw2G2(int acc[3]);
void GyrRaw2D2(short gyr[3]);

int main() {
    GlobalTime.start();
    imu.init();
    imu2.init();
    led_green = 1;
    led_red = 1;
    pc.baud(115200);
    pc.printf("Hello World from FRDM-K64F board.\r\n");
    kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
    kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
    kalman_init(&filter_pitch2, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
    kalman_init(&filter_roll2, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
    
    ProgramTimer.start();
    loopStartTime = ProgramTimer.read_us();
    timer = loopStartTime;

    while (true) {
        imu.readAcc(acc);
        AccRaw2G(acc);
        gyro.read(gyr);
        GyrRaw2D(gyr);
        
        imu2.readAcc(acc2);
        AccRaw2G2(acc2);
        gyro2.read(gyr2);
        GyrRaw2D2(gyr2);
        
        R = sqrt(std::pow(Acc.x, 2) + std::pow(Acc.y, 2) + std::pow(Acc.z, 2));
        R2 = sqrt(std::pow(Acc2.x, 2) + std::pow(Acc2.y, 2) + std::pow(Acc2.z, 2));
        
        kalman_predict(&filter_pitch, Gyr.x,  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_pitch, acos(Acc.x/R));
        kalman_predict(&filter_roll, Gyr.y,  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_roll, acos(Acc.y/R));
        
        kalman_predict(&filter_pitch2, Gyr2.x,  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_pitch2, acos(Acc2.x/R2));
        kalman_predict(&filter_roll2, Gyr.y,  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_roll2, acos(Acc2.y/R2));
        
        angle[0] = kalman_get_angle(&filter_pitch);
        angle[1] = kalman_get_angle(&filter_roll);
        
        angle2[0] = kalman_get_angle(&filter_pitch2);
        angle2[1] = kalman_get_angle(&filter_roll2);
        
        timer = ProgramTimer.read_us();
         
        printf("IMU1\tangle0=%6.2f\tangle1=%6.2f\r\n",Rad2Dree*angle[0],Rad2Dree*angle[1]);
        printf("IMI2\tangle0=%6.2f\tangle1=%6.2f\r\n",Rad2Dree*angle2[0],Rad2Dree*angle2[1]);
        wait(0.2);
    }
}

void AccRaw2G(int acc[3]){
    Acc.x = acc[0]/1024.0f; 
    Acc.y = acc[1]/1024.0f; 
    Acc.z = acc[2]/1024.0f;
}

void GyrRaw2D(short gyr[3]){
    Gyr.x = gyr[0]/225.0f; 
    Gyr.y = gyr[1]/225.0f; 
    Gyr.z = gyr[2]/225.0f;
}

void AccRaw2G2(int acc[3]){
    Acc2.x = acc[0]/1024.0f; 
    Acc2.y = acc[1]/1024.0f; 
    Acc2.z = acc[2]/1024.0f;
}

void GyrRaw2D2(short gyr[3]){
    Gyr2.x = gyr[0]/225.0f; 
    Gyr2.y = gyr[1]/225.0f; 
    Gyr2.z = gyr[2]/225.0f;
}