File content as of revision 4:44e13dd5250b:

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

#define UNLOCK 1
#define LOCK 0
#define TIMER 30 //開放タイマー
 
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(p14);
Serial          twe(p9,p10);
Serial          gps(p13,p14);
Serial          pc(USBTX,USBRX);
PwmOut          servo_para(p21);

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

Ticker kaihou;
Timer timer1;
/*とりあえず全てグローバル変数化してます。*/
//カウント変数
int Cnt_buff = 0;
int Cnt_para = 0;
int Cnt_open = 0;
int i, j, t;
//高度取得
float Alt_buff[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);

int main() {
    
    twe.printf("Hello World!\r\n");   
/*SDカード動作確認*/ 
    mkdir("/sd/mydir", 0777);
    FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
    if(fp == NULL) {
        error("Could not open file for write\n");
    }
    fprintf(fp, "Hello fun SD Card World!");
    fclose(fp);
    
/*I2C初期化*/    
    bmp.Initialize(60,BMP180_OSS_ULTRA_LOW_POWER); 
    mpu.setAcceleroRange(0);
    mpu.setGyroRange(0);
    twe.printf("I2C_initialize_ok\r\n");

/*地上高度取得*/    
    for(i=0; i<10; i++){
        bmp.ReadData(&temperature,&pressure);
        Alt_buff[i]=get_Alt(pressure, temperature);
    }
    
    Alt_gnd = median(Alt_buff, 10);
    
    fp = fopen("/sd/mydir/sdtest.txt", "w");
    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("Alt_gnd:%f\r\n",Alt_gnd); 
    twe.printf("Standby ok!\r\n");
    
    while(fly == 1);        //フライトピン抜けるまで待機
    
    timer1.start();
    kaihou.attach(_open, 0.05); 
    
    while(1){

 
        /*  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,%d\r\n",Max_Alt);
            }
        }else{
            cnt_gps++;
        }
        
    }
    
    
}

void _open(){
    
    if(Cnt_open == 0){  //20回に一回保存（20回に1回openとclose）
        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;
    
    if(Alt_now > Max_Alt){
        Max_Alt = Alt_now ;
    }
    
    if(tf_para == true){    //パラ開く前は頂点判定する
        Alt_buff[Cnt_buff+1] = Alt_now; 
        if(Alt_buff[Cnt_buff]>Alt_buff[Cnt_buff+1]) Cnt_para++; //直前の値より小さければカウント+1
        twe.printf("Cnt_para:%d\r\n", Cnt_para);
    }
    
    if(Cnt_buff == 10 && tf_para == true){
        t = timer1.read();                       
        if(Cnt_para>=7 || t > TIMER){        //10回中7回小さけれ落下と判断（最初はぜってぇ大きいから実質7/9）
            _para(UNLOCK);
            tf_para = false;
            timer1.stop();
            twe.printf("PARA_OPEN\r\n");
        }
        Cnt_buff = 0;
        Cnt_para = 0;
    }
    

    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]); 
    twe.printf("%f,%f,%f,%f,%f,%f,%f\r\n",Alt_now,a[0],a[1],a[2],d[0],d[1],d[2]);   //todo要らん場合は適宜コメントアウト
    Cnt_buff++;
    Cnt_open++;
      
    if(Cnt_open == 20){
        fclose(fp);
        
        Cnt_open = 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.0005); // pulse servo out sita
    }else if(motion==LOCK){
            servo_para.pulsewidth(0.0025); // pulse servo outu sita     
    }
}