File content as of revision 0:72e0bf0dbe7c:

#include "mbed.h"

#define N_CHAN      3   // the numbers of channels
#define SIZE_DATA   16  // size of vz data

Serial pc(USBTX, USBRX);

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

//InterruptIn ch1_in(p5);     // roll
InterruptIn ch2_in(p5);     // pitch
InterruptIn ch3_in(p6);     // throttle
//InterruptIn ch4_in(p7);     // taw
InterruptIn ch5_in(p7);     // aux1
//InterruptIn ch6_in(p18);    // aux2

DigitalIn button(p12);

Serial xbee(p13, p14);   // tx, rx

//PwmOut ch1_out(p21);        // rall
PwmOut ch2_out(p21);        // pitch
PwmOut ch3_out(p22);        // throttle
//PwmOut ch4_out(p24);        // yaw
//PwmOut ch5_out(p25);        // aux1
//PwmOut ch6_out(p26);        // aux2

///////////////////// Function prototype ////////////////////
//void roll_up(void);
void pitch_up(void);
void thr_up(void);
//void yaw_up(void);
void aux1_up(void);
//void aux2_up(void);

//void rall_down(void);
void pitch_down(void);
void thr_down(void);
//void yaw_down(void);
void aux1_down(void);
//void aux2_down(void);

///////////////////// Timers ////////////////////
//Timer t_roll;
Timer t_pitch;
Timer t_thr;
//Timer t_yaw;
Timer t_aux1;
//Timer t_aux2;

///////////////////// Global variable ////////////////////
int g_pwm_pulse[N_CHAN]      =   {0};    // pitch thr aux1

///////////////////// Local file system ////////////////////
LocalFileSystem local("local"); 

///////////////////// Main loop ////////////////////
int main() {
    
    pc.baud(921600);
    xbee.baud(57600);
    
    double GAIN_VEL_X   =   0.2;
    //double GAIN_VEL_Z   =   0;
    
    double VEL_X_TARGET =   500;
    
    double LIMIT_PULTH_WIDTH_CH2_OUT_LOW    =   0.0013;
    double LIMIT_PULTH_WIDTH_CH2_OUT_UP     =   0.0017;
    
    int pos[2]   =   {0};
    int vel[2]   =   {0};
    
    int PULTH_WIDTH_RC_TRIM_CH2 =   0;
    int PULTH_WIDTH_RC_TRIM_CH3 =   0;
    
    double pulse_width_ch2_out  =   0.0015;
    double pulse_width_ch3_out  =   0.001;
    
    ch2_out.period(0.02);
    ch3_out.period(0.02);

    ch2_out.pulsewidth(0.0015); // pitch
    ch3_out.pulsewidth(0.0011); // thr
    
    /////////////////// Interrupt for xbee ////////////////////
    //xbee.attach(&get_vel);
    
    //////////////////// Interrupt for RC reciever ////////////////////
    //ch1_in.rise(&roll_up);      ch1_in.fall(&roll_down);
    ch2_in.rise(&pitch_up);    ch2_in.fall(&pitch_down);
    ch3_in.rise(&thr_up);      ch3_in.fall(&thr_down);
    //ch4_in.rise(&yaw_up);     ch4_in.fall(&yaw_down);
    ch5_in.rise(&aux1_up);     ch5_in.fall(&aux1_down);
    //ch6_in.rise(&aux2_up);     ch6_in.fall(&aux2_down);
    
    // pos初期値取得
    // if pos 初期値(0,0) -> noVzかも
    
    // Get RC trim
    for(int i=0;i<10;i++){
        wait(.1);
        PULTH_WIDTH_RC_TRIM_CH2 +=   g_pwm_pulse[0];
        PULTH_WIDTH_RC_TRIM_CH3 +=   g_pwm_pulse[1];
    }
    PULTH_WIDTH_RC_TRIM_CH2 =   PULTH_WIDTH_RC_TRIM_CH2 / 10;
    PULTH_WIDTH_RC_TRIM_CH3 =   PULTH_WIDTH_RC_TRIM_CH3 / 10;
    
    // Check RC trim
    if(PULTH_WIDTH_RC_TRIM_CH2 < 450 || 550 < PULTH_WIDTH_RC_TRIM_CH2){
        //pc.printf("%d\r\n", PULTH_WIDTH_RC_TRIM_CH2);
        while(1){
            led2 = 1;
            led3 = 0;
            wait(0.5);
            led2 = 0;
            led3 = 1;
            wait(0.5);
        }
    }
    
    //////////////////// Stand by ////////////////////
    
    ch2_in.disable_irq();
    ch3_in.disable_irq();
    ch5_in.disable_irq();
    
    while(button < 0.5){
        led1 = 1;
        led4 = 1;
        wait(0.5);
        led1 = 0;
        led4 = 0;
        wait(0.5);
    }
    
    wait(3);
    
    FILE *fp = fopen("/local/out.txt", "w");  // Open "out.txt" on the local file system for writing
    
    led1 = 1;
    led4 = 1;
    
    while(button < 0.5){
    //while(1){
        
        for(int i=0;i<5;i++){
            
            //////////////////// Get PWM ////////////////////
            ch2_in.enable_irq();
            ch3_in.enable_irq();
            ch5_in.enable_irq();
            
            wait(.02);   // Interval to get pwm
            
            ch2_in.disable_irq();
            ch3_in.disable_irq();
            ch5_in.disable_irq();
            
            //////////////////// Get pos and vel ////////////////////
            if(xbee.readable()){
                // get vz data
                int buf_char[SIZE_DATA] =   {0};
                
                // decode vz data
                for(int j=0;j<SIZE_DATA;j++){   buf_char[j] =   (int)xbee.getc() - 48;  }
                
                //for(int i=0;i<SIZE_DATA;i++){  pc.printf("%5d ", buf_char[i]);  }
                //pc.printf("\r\n");
                
                pos[1] = 1000*buf_char[0] + 100*buf_char[1] + 10*buf_char[2] + buf_char[3] - 5000;
                pos[0] = 1000*buf_char[4] + 100*buf_char[5] + 10*buf_char[6] + buf_char[7] - 5000;
                
                vel[1] = 1000* buf_char[8] + 100* buf_char[9] + 10*buf_char[10] + buf_char[11] - 5000;
                vel[0] = 1000*buf_char[12] + 100*buf_char[13] + 10*buf_char[14] + buf_char[15] - 5000;
            }else{
                
                // しばらくこなかったらautoにならないフラグとか
                
            }
            
            //////////////////// Control law ////////////////////
            if(500 < g_pwm_pulse[2]){
                
                // Constant velocity F/B
                
                //pulse_width_ch2_out =   0.001 + 0.000001 * g_pwm_pulse[0];
                pulse_width_ch2_out =   0.001 + 0.000001 * PULTH_WIDTH_RC_TRIM_CH2 + (-1) * 0.000001 * GAIN_VEL_X * (VEL_X_TARGET - vel[0]);
                
                pulse_width_ch3_out =   0.001 + 0.000001 * g_pwm_pulse[1];
                //pulse_width_ch3_out =   0.001 + PULTH_WIDTH_RC_TRIM_CH3 + GAIN_VEL_Z *  (-vel[1]);
                
            }else{
                
                // without RC-hack
                pulse_width_ch2_out =   0.001 + 0.000001 * g_pwm_pulse[0];
                pulse_width_ch3_out =   0.001 + 0.000001 * g_pwm_pulse[1];
                
            }
            
            // Limmiter for RC
            if(pulse_width_ch2_out < LIMIT_PULTH_WIDTH_CH2_OUT_LOW){    pulse_width_ch2_out = LIMIT_PULTH_WIDTH_CH2_OUT_LOW;    }
            if(LIMIT_PULTH_WIDTH_CH2_OUT_UP < pulse_width_ch2_out) {    pulse_width_ch2_out = LIMIT_PULTH_WIDTH_CH2_OUT_UP;     }
            
            ch2_out.pulsewidth(pulse_width_ch2_out);
            ch3_out.pulsewidth(pulse_width_ch3_out);
        
        }
        
        
        //for(int i=0;i<N_CHAN;i++){    pc.printf("%5d ", g_pwm_pulse[i]);  }
        /*
        pc.printf(" ");
        for(int i=0;i<2;i++){   pc.printf("%5d ", pos[i]); }
        for(int i=0;i<2;i++){   pc.printf("%5d ", vel[i]); }
        pc.printf("\r\n");
        */
        fprintf(fp, "%f %f %5d  %5d %5d %5d %5d \r\n", pulse_width_ch2_out, pulse_width_ch3_out, g_pwm_pulse[2], pos[0], pos[1], vel[0], vel[1]);
        
    }
    
    ch2_out.pulsewidth(0.0015);
    ch3_out.pulsewidth(0.0011);
    
    ch2_in.disable_irq();
    ch3_in.disable_irq();
    ch5_in.disable_irq();
    
    fclose(fp);
    
    led1 = 0;
    led4 = 0;
    led2 = 1;
    led3 = 1;
}


//////////////////// RC interrupt (up) ////////////////////
//void roll_up(){     t_roll.start();   }
void pitch_up(){    t_pitch.start();  }
void thr_up(){      t_thr.start();   }
//void yaw_up(){      t_yaw.start();    }
void aux1_up(){     t_aux1.start();   }
//void aux2_up(){     t_aux2.start();   }


//////////////////// RC interrupt (down) ////////////////////
/*
void roll_down(){
    t_roll.stop();
    g_pwm_pulse[]    =   (int)(t_roll.read()*1000000-1000);
    t_roll.reset();
}
*/
void pitch_down(){
    t_pitch.stop();
    g_pwm_pulse[0]    =   (int)(t_pitch.read()*1000000-1000);
    t_pitch.reset();
}

void thr_down(){
    t_thr.stop();
    g_pwm_pulse[1]    =   (int)(t_thr.read()*1000000-1000);
    t_thr.reset();
}
/*
void yaw_down(){
    t_yaw.stop();
    g_pwm_pulse[]    =   (int)(t_yaw.read()*1000000-1000);
    t_yaw.reset();
}
*/
void aux1_down(){
    t_aux1.stop();
    g_pwm_pulse[2]    =   (int)(t_aux1.read()*1000000-1000);
    t_aux1.reset();
}
/*
void aux2_down(){
    t_aux2.stop();
    g_pwm_pulse[]    =   (int)(t_aux2.read()*1000000-1000);
    t_aux2.reset();
}
*/