File content as of revision 8:b10169df8f5f:

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

#define LAUNCH_JUDGE_ACC_g  3.0
#define PARA_OPEN_TIME_s    20.0
#define PARA_OPEN_CNT       7
#define PARA_OPEN_FILTER_s  8.0
#define LEAF_UNLOCK_TIME_s  85.0
#define LEAF_UNLOCK_ALT_m   300.0
#define p0                  1013.25f

BMP180          bmp(dp5,dp27);
MPU6050         mpu(dp5,dp27);
Timer           para_timer;
Timer           leaf_timer;
Ticker          readtimer;
PwmOut          mg996(dp1);
PwmOut          sg92(dp18);
DigitalIn       oshirase1(dp11);
DigitalInOut    oshirase2(dp10);
//DigitalOut      myled(LED1);
//Serial          pc(USBTX,USBRX);

/*  自作関数    */
void    _flight();
void    _open();
void    _close();
void    _leaf_lock();
void    _leaf_unlock();
float   _getAlt();
float   _getAcc();
float   _median(float data[], int num);


/*  グローバル変数 */
int     Alt_cnt;
float   a[3],Acc;
float   Pressure,Temperature,Altitude;
float   Alt_buff[10],Alt_median_buff[10],Acc_buff[10];
float   Land_Alt;
bool    Mg996Open = true;
bool    Sg92Open  = true;


int main() {
//    pc.printf("Hello!\r\n");
    mg996.period_ms(20);
    sg92.period_ms(20);
    mpu.setAcceleroRange(0);
    bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
    oshirase2.input();
    wait(0.5);
    while(1){
        if(oshirase1==0 && oshirase2==1){
            _flight();
            break;
        }else if(oshirase1==1 && oshirase2==0){
            while(1){
                if(oshirase1==1 && oshirase2==1){
                    if(Mg996Open){
                        _close();
                        Mg996Open = false;
                    }else{
                        _open();
                        Mg996Open = true;
                    }
                    break;
                }else if(oshirase1==0 && oshirase2==0){
                    if(Sg92Open){
                        _leaf_lock();
                        Sg92Open = false;
                    }else{
                        _leaf_unlock();
                        Sg92Open = true;
                    }
                    break;
                }
            }
        }
    }
    while(1);
}


/*  フライトモード用関数  */
void _flight(){
//    pc.printf("flight mode on\r\n");
    oshirase2.output();
    oshirase2 = 0;
    _close();
    _leaf_lock();

    /*  地上高度取得  */
    for(int i=0;i<10;i++){
        Alt_buff[i] = _getAlt();
    }
    Land_Alt = _median(Alt_buff,10);
//    pc.printf("Land_Alt = %f\r\n",Land_Alt);
//    for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);

    /*  発射判定    */
    while(1){
//        myled = !myled;
        for(int i=0;i<10;i++){
            Acc_buff[i] = _getAcc();
        }
        Acc = _median(Acc_buff,10);
        if(Acc >= LAUNCH_JUDGE_ACC_g){
            para_timer.start();
            oshirase2 = 1;
//            for(int i=0;i<10;i++)pc.printf("%f\n\r",Acc_buff[i]);
//            pc.printf("Acc = %f\r\n",Acc);
            break;
        }
    }

    /*  開放判定    */
    while(1){
        Alt_cnt = 0;
        for(int i=0;i<10;i++){
            Alt_buff[i] = _getAlt();
        }
        Alt_median_buff[0] = _median(Alt_buff,10);
        for(int i=1;i<10;i++){
            for(int j=0;j<10;j++){
                Alt_buff[j] = _getAlt();
            }
            Alt_median_buff[i] = _median(Alt_buff,10);
//            pc.printf("%f\r\n",Alt_median_buff[i]);
            if(Alt_median_buff[i] < Alt_median_buff[i-1]-0.1) Alt_cnt++;
        }
//        pc.printf("%d\r\n",Alt_cnt);
        if(para_timer.read() >= PARA_OPEN_FILTER_s){
            if(Alt_cnt >= PARA_OPEN_CNT || para_timer.read() >= PARA_OPEN_TIME_s){
                _open();
                leaf_timer.start();
                oshirase2 = 0;
                para_timer.stop();
//                pc.printf("para open\r\n");
//                for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
                break;
            }
        }
    }

    /*  リーフィング解除判定  */
    while(1){
        for(int i=0;i<10;i++){
            Alt_buff[i] = _getAlt();
        }
        Altitude = _median(Alt_buff,10);
        if(Altitude - Land_Alt <= LEAF_UNLOCK_ALT_m || leaf_timer.read() >= LEAF_UNLOCK_TIME_s){
            _leaf_unlock();
            oshirase2 = 1;
            leaf_timer.stop();
//            pc.printf("leafing unlock\r\n");
//            for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
            break;
        }
    }
}


/*  サーボ用関数  */
void _close() {mg996.pulsewidth(0.0005);}
void _open() {mg996.pulsewidth(0.0024);}
void _leaf_lock() {sg92.pulsewidth(0.0024);}
void _leaf_unlock() {sg92.pulsewidth(0.0005);}


/*  高度取得関数    */
float _getAlt(){
    float altitude;
    bmp.ReadData(&Temperature,&Pressure);
    altitude = (pow((p0/Pressure), (1.0f/5.257f))-1.0f)*(Temperature+273.15f)/0.0065f;
//    pc.printf("%f, %f, %f\r\n",Pressure,Temperature,altitude);
    return altitude;
}


/*  合成加速度取得関数 */
float _getAcc(){
    mpu.getAccelero(a);
    return sqrt(pow(a[0]/9.81,2)+pow(a[1]/9.81,2)+pow(a[2]/9.81,2));
}


/*  中央値計算関数 */
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;
}