File content as of revision 2:a4895f7c3058:

/*
説明
Nucleo-F303K8とBMP180を使った気温・気圧・高度計算のサンプルプログラム

ライブラリ
https://developer.mbed.org/users/spiridion/code/BMP180/

以下ピン配置
Nucleo  BMP180
GND-----GND-------0V
+3V3----VIN
D4------SDA
D5------SCL

*/
#include "mbed.h"
#include "MPU6050.h"
#include "math.h"
#include "BMP180.h"

#define LUNCH_G 1.0
#define TIMER1 500
#define TIMER2 30
#define ALT_LEAF 300
#define UNLOCK 0
#define LOCK 1
#define RATE_OPEN 1

enum PHASE{LUNCH=1,RISE=3,DROP=7} Phase;

DigitalIn pr(dp10);
DigitalInOut le(dp11);
MPU6050     mpu(dp5, dp27);
BMP180      bmp(dp5, dp27);
PwmOut      servo_para(dp1);
PwmOut      servo_leaf(dp18);
DigitalOut myled(dp14);
DigitalOut myled2(dp28);

Timer timer1;
Timer timer2;
Ticker tic_open;

//高度取得
float Alt_buff[10];
float Alt_gnd,Alt_drop;
float pressure,temperature;
float p0 = 1013.25;
//カウント変数
int Cnt_buff;
int i;
int Cnt_para;

//加速度取得
float a_abs;
float a[3];
float Acc_buff[10];
float Acc_lnc;
//その他
int t1, t2;
bool tf_para = true;
bool tf_leaf = true;

void _para(int motion);
void _leaf(int motion);
void kaihou(void);
/*雑関数*/
float median(float data[], int num);
float get_Alt(float press, float temp);
int _input(char c);

int main() {
    
    le.input();
    
    wait(1);        //良い子は待つ
    if(pr == 1){    //途中で電源落ちたら
        Phase = RISE;
        Cnt_buff = 0;
        timer1.start();
        goto yabai;
    }
    
    myled2 = 1;
    
    
    while(1){


        while(pr== 0 && le==0);
        wait(0.2);              //LPC1768の信号待ち
        if(pr==1 && le==0){
            if(tf_para == true){
                _para(UNLOCK);
                myled = 1;
//                myled2 = 0;
                tf_para = false;
            }
            else{
                _para(LOCK);
                myled = 0;
//                myled2 = 0;
                tf_para = true;
            }
            wait(1);
        }
        if(pr==0 && le==1){
            if(tf_leaf == true){
                _leaf(UNLOCK);
                myled2 = 0;
                tf_leaf = false;
            }
            else{
                _leaf(LOCK);
                myled2 = 0;
                tf_leaf = true;
            }
            wait(1);
        }
        
        
        if(pr==1 && le==1) break;

    }
    
    le.output();
    le = 0;
    wait(2.5);      //LPC1768の初期化処理待ち
    
    Phase = LUNCH;
    
yabai:
    
    mpu.setAcceleroRange(0);    
    bmp.Initialize(60,BMP180_OSS_ULTRA_LOW_POWER);
    
    
    Cnt_buff = 0;
    for(i=0; i<10; i++){
        bmp.ReadData(&temperature,&pressure);
        Alt_buff[Cnt_buff]=get_Alt(temperature, pressure);
        Cnt_buff++;
    }
    Alt_gnd = median(Alt_buff, 10);
    
    Cnt_buff = 0;
    
    
    tic_open.attach(&kaihou,1.0/RATE_OPEN);
    
    while(1);
    
        
}

 
void kaihou(void){
    
    switch(Phase){
        
        case LUNCH:
                            
                    mpu.getAccelero(a);       
                    Acc_buff[Cnt_buff] = sqrt(pow(a[0]/9.81,2)+pow(a[1]/9.81,2)+pow(a[2]/9.81,2));
                    Cnt_buff++;
                    
                    //myled2=!myled2;
                     
                    if(Cnt_buff == 10){
                        myled2 = 0;
                        Acc_lnc = median(Acc_buff, 10);
                        Cnt_buff = 0;
                        //myled2 = 0;
                    }
                    if(Acc_lnc>LUNCH_G){
                        Phase = RISE;
                        //myled2 = 0;
                        timer1.start();
                        le = 1;         //LPC1768に発射をお知らせする
                    }
                    
                    break; 

                    
        case RISE:
                    if(Cnt_buff == 0){
                        bmp.ReadData(&temperature,&pressure); 
                        Alt_buff[Cnt_buff]=get_Alt(temperature, pressure);  //まず高度取る    
                        }
                    
                    bmp.ReadData(&temperature,&pressure); 
                    Alt_buff[Cnt_buff+1] = get_Alt(temperature, pressure); 
                    if(Alt_buff[Cnt_buff]>Alt_buff[Cnt_buff+1]) Cnt_para++;
                    Cnt_buff++;
                    
                    if(Cnt_buff == 9){
                        //myled2 = 1;
                        t1 = timer1.read();
                        
                        if(Cnt_para>=5 || t1>TIMER1){
                            _para(UNLOCK);
                            myled2 =0;
                            le = 0;         //LPC1768にパラ放出をお知らせする
                            timer1.stop();
                            timer2.start();
                            Phase = DROP;
                        }
                    Cnt_buff = 0;
                    
                    }
                    break;

        case DROP:
                    
                    bmp.ReadData(&temperature,&pressure);
                    Alt_buff[Cnt_buff]=get_Alt(temperature, pressure);
                    Cnt_buff ++;
                    if(Cnt_buff == 9){
                        Alt_drop = median(Alt_buff, 10)-Alt_gnd;
                        t2 = timer2.read();
                        //twe.printf("%d\r\n",t2);
                        myled2 = !myled2;
        
                        if(Alt_drop<ALT_LEAF && t2>TIMER2){
                            _leaf(UNLOCK);
                            le = 1 ;           //LPC1768にリーフィングをお知らせする
                            myled2 = 1;
        
                        }
                    Cnt_buff = 0;
                    }
                    
                    break ;
    }
}

void _para(int motion){
    if(motion==UNLOCK){
            servo_para.pulsewidth(0.0005); // pulse servo out sita
    }else if(motion==LOCK){
            servo_para.pulsewidth(0.0025); // pulse servo outu sita     
    }
}
void _leaf(int motion){
    if(motion==UNLOCK){
            servo_leaf.pulsewidth(0.0006); // pulse servo out
    }else if(motion==LOCK){
            servo_leaf.pulsewidth(0.0024); // pulse servo out          
    }
}

float median(float data[], int num){//todo:処理時間計測
    float *data_cpy, ans;
    data_cpy = new float[num];
    memcpy(data_cpy,data,sizeof(float)*num);
 
    for(int i=0; i<num; i++){
        for(int j=0; j<num-i-1; j++){
            if(data_cpy[j]>data_cpy[j+1]){
                float buff = data_cpy[j+1];
                data_cpy[j+1] = data_cpy[j];
                data_cpy[j] = buff;
            }
        }
    }
    
    if(num%2!=0) ans = data_cpy[num/2];
    else         ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
    delete[] data_cpy;
    return ans;
}

float get_Alt(float temp, float press){
    return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
}