File content as of revision 17:6538f06e2f47:

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

#define UNLOCK 1
#define LOCK 0
#define HANTEI 12 //判定開始タイマー
#define TIMER 30 //開放タイマー
#define RATE 50.0//判定周期 浮動小数点型で記述してください
#define ALT 1 //落下判断高度
 
SDFileSystem    sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
BMP180          bmp(p28,p27);
MPU6050         mpu(p28,p27);
DigitalIn       fly(p12);
DigitalOut      myled(p20);
DigitalOut      myled2(p19);
DigitalOut      myled3(p18);
Serial          twe(p9,p10);
Serial          gps(p13,p14);
Serial          pc(USBTX,USBRX);
PwmOut          servo_para(p22);
PwmOut          servo_para2(p23);

LocalFileSystem local("local");
FILE *fp;
FILE *lfp;

Ticker kaihou;
Ticker ochiru;
Timer timer1;

/*とりあえず全てグローバル変数化してます。*/
//カウント変数
int Cnt_buff = 0;
int Cnt_para = 0;
int Cnt_open = 0;
int i, j, t;
//高度取得
float Alt_buff[256],Alt_buff2[10],Alt_gnd,Alt_now, Max_Alt;
float p0 = 1013.25;             //海面気圧
float pressure,temperature, Alt;
//位置情報取得
char gps_data[256];
int cnt_gps =0;
//加速度,ジャイロ取得
float a_abs;
float a[3];
float d[3];

bool tf_para = true;

float median(float data[], int num);    //中央値求める
float get_Alt(float press, float temp);
void _open(void);                        //kaihou
float _DMS2DEG(float raw_data);         //GPSデータ60進数→10進数
void _para(int motion);
void _recovery();
int _input(char c);
 
int main() {
    twe.baud(115200);
    twe.printf("Hello World!\r\n");
    
    int ret;
    lfp = fopen("/local/Alt.txt","r");
    if(lfp == NULL){
        goto normal;
        }    
    ret = fscanf(lfp,"GND:%f",&Alt_gnd);
    fclose(lfp);
    if(ret == -1){
        goto normal;
    }else{
        twe.printf("reboot now\r\n");
        goto yabai;
    }
    
normal:
    
    while(1){
        char c = twe.getc();
        if(_input(c) == 1){
            _para(UNLOCK);
            twe.printf("servo_open\r\n");
        }
        else if(_input(c) == 2){
            _para(LOCK);
            twe.printf("servo_lock\r\n");
        }
        else if(_input(c) == 3){
            break;
        }

    }
yabai:

    mpu.setAcceleroRange(3);
    mpu.setGyroRange(3);
    bmp.Initialize(60,BMP180_OSS_ULTRA_LOW_POWER);
    twe.printf("I2C_initialize_ok\r\n");
    

//地上高度取得
    Cnt_buff = 0;
    for(i=0; i<10; i++){
        bmp.ReadData(&temperature,&pressure);
        Alt_buff[i]=get_Alt(pressure, temperature);
    }
    Alt_gnd = median(Alt_buff, 10);
    
    twe.printf("Alt_gnd:%f\r\n", Alt_gnd);
    
    lfp = fopen("/local/Alt.txt","a");
    fprintf(lfp, "GND:%f\r\n",Alt_gnd);
    fclose(lfp);
    twe.printf("lacal_ok\r\n");
    
    mkdir("/sd/mydir", 0777);
    fp = fopen("/sd/mydir/sdtest.txt", "a");
    if(fp == NULL) {
        error("Could not open file for write\n");
    }
    for(i=0; i<10; i++){
        fprintf(fp, "%d:%f\r\n",i+1,Alt_buff[i]);
    }
    
    fprintf(fp, "Alt_gnd:%f\r\n",Alt_gnd); 
    fclose(fp); 
    
    twe.printf("SD_write_OK!!\r\n");
    
    twe.printf("ALL_READY\r\n");
    
    while(fly == 1);        //フライトピン抜けるまで待機
    

    twe.printf("launch!!!!!!!\r\n");
    i = 0;
    Alt_buff[0] = 0;
    fp = fopen("/sd/mydir/sdtest.txt", "a");
    if(fp == NULL)twe.printf("ERROR\r\n");
    timer1.start();
    kaihou.attach(_open,1.0/RATE);

    
    while(1){

/*        twe.printf("MAX:%f,Cnt:%d\r\n",Max_Alt, Cnt_para);
        wait(1.0); 
*/        
        /*  GPS取得&送信    */
       gps_data[cnt_gps] = gps.getc();
        if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
            cnt_gps = 0;
            memset(gps_data,'\0',256);
        }else if(gps_data[cnt_gps] == '\r'){
            float world_time, lon_east, lat_north;
            int rlock, sat_num;
            char lat,lon;
            if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
                if(rlock==1){
                    lat_north = _DMS2DEG(lat_north);
                    lon_east = _DMS2DEG(lon_east);
//                    pc.printf("phase:%d,max altitude:%f\r\n",Phase,Max_Alt);
//                    twe.printf("%s\r\n",gps_data);
                    twe.printf("Lat,Lon:%f,%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
                }else{
//                    pc.printf("max altitude:%f\r\n",Max_Alt);
                    twe.printf("%s\r\n",gps_data);
                }
                twe.printf("MAX:%f,Cnt:%d\r\n",Max_Alt,Cnt_para);
            }
        }else{
            cnt_gps++;
        }
        
    }//while_gps    
}//main

void _open(){           
    bmp.ReadData(&temperature,&pressure);
    mpu.getAccelero(a);
    mpu.getGyro(d);
    Alt_buff2[i] = get_Alt(pressure, temperature);
    i++;
    if(i == 10){        //10回ごとに中央値計算
        Alt_now = median(Alt_buff2, 10);
        Alt_now = Alt_now - Alt_gnd;
        t = timer1.read();
        fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%d,%d\r\n",Alt_now,a[0],a[1],a[2],d[0],d[1],d[2],t,Cnt_para); 
        if(Alt_now > Max_Alt) Max_Alt = Alt_now;
        i = 0;
        if(tf_para == true && t > HANTEI ){ 
            Alt_buff[Cnt_buff+1] = Alt_now; 
            if(Alt_buff[Cnt_buff]-Alt_buff[Cnt_buff+1] > ALT) Cnt_para++; //直前の値(0.2秒前より1ALTm降下)より小さければカウント+1
//            twe.printf("Cnt_para:%d\r\n", Cnt_para); 
            Cnt_buff++;                   
            if(Cnt_para == 10 || t > TIMER){    //頂点！！！
                kaihou.detach();                //SD閉じる前にサーボ動かす前にTicker止める
                fprintf(fp,"OPEN!\r\n");        
                fclose(fp);
                lfp = fopen("/local/Alt.txt","a");
                fprintf(lfp, "MAX:%f\r\n",Max_Alt);
                fclose(lfp);        
                _para(UNLOCK);
                tf_para = false;
                timer1.stop();
                twe.printf("PARA_OPEN\r\n");
                twe.printf("GPS_MODE_ON!!!!\r\n");
                    
                ochiru.attach(_recovery,0.2);   //パラメータ保存のためのやつ
                
            }
        }
    }
    
    if(Cnt_buff == 250) Cnt_buff = 0;
                   
}

void _recovery(){
    
    if(i == 0){
        fp = fopen("/sd/mydir/sdtest.txt", "a");
        if(fp == NULL)twe.printf("ERROR\r\n");
    }
    
    bmp.ReadData(&temperature,&pressure);
    mpu.getAccelero(a);
    mpu.getGyro(d);
    Alt_now = get_Alt(pressure, temperature);
    Alt_now = Alt_now - Alt_gnd;
    fprintf(fp, "%f,%f,%f,%f,%f,%f,%f\r\n",Alt_now,a[0],a[1],a[2],d[0],d[1],d[2]);
    i++;
    
    if(i == 50){
        fclose(fp);
        i = 0;
    }
}
float get_Alt(float press, float temp){
    return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
}

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 _DMS2DEG(float raw_data){
    int d = (int)(raw_data/100);
    float m = (raw_data - (float)d*100);
    return (float)d + m/60;
}

void _para(int motion){
    if(motion==UNLOCK){
            servo_para.pulsewidth(0.0008); // pulse servo out sita
            servo_para2.pulsewidth(0.0008);
    }else if(motion==LOCK){
            servo_para.pulsewidth(0.0023); // pulse servo outu sita
            servo_para2.pulsewidth(0.0023);     
    }
}

int _input(char c){

    if(c=='f'){
        return 3;
    }else if(c=='l'){
        return 2;
    }else if(c=='u'){
        return 1;
    }
    return 0;
}