File content as of revision 6:07f4aaae5339:

#include "mbed.h"
#include "ITG3200.h"
#include "HMC5883L.h"

#define N    3
#define chan 6

ITG3200 gyro(p9, p10);
HMC5883L compass(p9, p10);
InterruptIn ch1(p13);       // aileron
InterruptIn ch2(p14);       // elevator
InterruptIn ch3(p15);       // throttle
InterruptIn ch4(p16);       // rudder
InterruptIn ch5(p17);       // switch1
InterruptIn ch6(p18);       // switch2
AnalogIn sonar(p19);        // sonar
PwmOut ch1_out(p21);
PwmOut ch2_out(p22);
PwmOut ch3_out(p23);
PwmOut ch4_out(p24);
PwmOut ch5_out(p25);
PwmOut ch6_out(p26);
Serial pc(USBTX, USBRX);

Timer t_aile;       Timer t_elev;       Timer t_thro;       Timer t_rudd;       Timer t_switch1;        Timer t_switch2;

void aile_up();     void elev_up();     void thro_up();     void rudd_up();     void switch1_up();      void switch2_up();
void aile_down();   void elev_down();   void thro_down();   void rudd_down();   void switch1_down();    void switch2_down();

double pwm_pulse[chan]      =   {0.001,0.001,0.001,0.001,0.001,0.001};        // aile, elev, thro, rudd, switch1, switch2

int main(){ 
    
    float dt = 0.1;
    float t  = 0;    
    pc.baud(921600);
    
    double range        =   0; 
    int    getMag[N]    =   {0};    // Buffer of the compass
    double Gyro [N]    =   {0};
    int    Mag  [N]    =   {0};
    
    double servo[6]    =   {0.001, 0.001, 0.001, 0.001, 0.001, 0.001};
    
    ch1_out.period(0.02);    ch2_out.period(0.02);    ch3_out.period(0.02);    ch4_out.period(0.02);    ch5_out.period(0.02);    ch6_out.period(0.02);
    // pulsewidth 0.001~0.002
    ch1_out.pulsewidth(0.001);   ch2_out.pulsewidth(0.001);   ch3_out.pulsewidth(0.001);   ch4_out.pulsewidth(0.001);   ch5_out.pulsewidth(0.001);   ch6_out.pulsewidth(0.001);
    
    // *** start up ***
    gyro.setLpBandwidth(LPFBW_42HZ);
    compass.setDefault();
    wait(0.1);                        // Wait some time for all sensors (Need at least 5ms)
    // *** start up ***
    
    // Reciever function
    ch1.rise(&aile_up);     ch1.fall(&aile_down);
    ch2.rise(&elev_up);     ch2.fall(&elev_down);
    ch3.rise(&thro_up);     ch3.fall(&thro_down);
    ch4.rise(&rudd_up);     ch4.fall(&rudd_down);
    ch5.rise(&switch1_up);  ch5.fall(&switch1_down);
    ch6.rise(&switch2_up);  ch6.fall(&switch2_down);
    
    pc.printf("\n\rHiko-robo!!\n\r");
    
    while(1){
        
        range   =   sonar;
        
        // Updating accelerometer and compass
        compass.readData(getMag);

        Gyro[0] =  (gyro.getGyroX())/14.375;
        Gyro[1] =  (gyro.getGyroY())/14.375;
        Gyro[2] =  (gyro.getGyroZ())/14.375;
        // Calibration YAL 9DOF Compass:X, Y, Z
        Mag[0]  =  (int16_t)getMag[0];
        Mag[1]  =  (int16_t)getMag[1];
        Mag[2]  =  (int16_t)getMag[2];
        
        for( int i=0;i<6;i++ ){     servo[i] =   pwm_pulse[i];  }
        
        ch1_out.pulsewidth(servo[0]);  ch2_out.pulsewidth(servo[1]);  ch3_out.pulsewidth(servo[2]);  ch4_out.pulsewidth(servo[3]);  ch5_out.pulsewidth(servo[4]);  ch6_out.pulsewidth(servo[5]);
        
        pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, Gyro[0], Gyro[1], Gyro[2], range, pwm_pulse[0]*1000, pwm_pulse[1]*1000, pwm_pulse[2]*1000, pwm_pulse[3]*1000, pwm_pulse[4]*1000, pwm_pulse[5]*1000);
        
        t += dt;
        wait(dt);
        
    }
    
 }

void aile_up(){     t_aile.start();    }
void elev_up(){     t_elev.start();    }
void thro_up(){     t_thro.start();    }
void rudd_up(){     t_rudd.start();    }
void switch1_up(){  t_switch1.start(); }
void switch2_up(){  t_switch2.start(); }

void aile_down(){
    t_aile.stop();
    pwm_pulse[0]        =   t_aile.read();
    t_aile.reset();
}

void elev_down(){
    t_elev.stop();
    pwm_pulse[1]        =   t_elev.read();
    t_elev.reset();
}

void thro_down(){
    t_thro.stop();
    pwm_pulse[2]        =   t_thro.read();
    t_thro.reset();
}

void rudd_down(){
    t_rudd.stop();
    pwm_pulse[3]        =   t_rudd.read();
    t_rudd.reset();
}

void switch1_down(){
    t_switch1.stop();
    pwm_pulse[4]   =   t_switch1.read();
    t_switch1.reset();
}

void switch2_down(){
    t_switch2.stop();
    pwm_pulse[5]   =   t_switch2.read();
    t_switch2.reset();
}