File content as of revision 2:6238109d564d:

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

#define LAUNCH_JUDGE_ACC_g  3.0
#define PARA_OPEN_TIME_s    5.0
#define LEAF_UNLOCK_TIME_s  5.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      mg995(dp1);
PwmOut      sg92(dp18);
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],Acc_buff[10];
float land_alt;
float t;


int main() {
    pc.printf("Hello!");
    mg995.period_ms(20);
    sg92.period_ms(20);
    _close();
    _leaf_lock();
    mpu.setAcceleroRange(0);
    bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
    _flight();
    while(1);
}


/*フライトモード用関数    */
void _flight(){
    
    /*  地上高度取得  */
    for(int i=0;i<10;i++){
        Alt_buff[i] = _getAlt();
    }
    land_alt = _median(Alt_buff,10);
    for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);

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

    /*  開放判定    */
    while(1){
        Alt_cnt = 0;
        Alt_buff[0] = _getAlt();
        for(int i=1;i<10;i++){
            Alt_buff[i] = _getAlt();
            if(Alt_buff[i] < Alt_buff[i-1]) Alt_cnt++;
        }
        if(Alt_cnt > 6 || para_timer.read() >= PARA_OPEN_TIME_s){
            _open();
            leaf_timer.start();
            para_timer.stop();
            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 <= LEAF_UNLOCK_ALT_m || leaf_timer.read() >= LEAF_UNLOCK_TIME_s){
            _leaf_unlock();
            leaf_timer.stop();
            for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
            break;
        }
    }
}


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


/*  高度取得関数    */
float _getAlt(){
    bmp.ReadData(&temperature,&pressure);
    return (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f;
}


/*  加速度取得関数 */
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;
}