File content as of revision 4:4b3ae90ec778:

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

/*マクロ処理*/
//動作レート
#define RATE_LOG            20
#define RATE_OPEN           50
//発射判定
#define ACC_JUDGE_LAUNCH    1.5
#define CNT_JUDGE_LAUNCH    5
//待機時間
#define TIME_GAP            3.0
//頂点判定
#define ALT_JUDGE_OPEN      1.5
#define CNT_JUDGE_OPEN      5
#define TIME_JUDGE_OPEN     30
//放出判定
#define TIME_JUDGE_OUT      60
#define ALT_JUDGE_OUT       -1.5
#define CNT_JUDGE_OUT       5  
//共通    
#define TIME_JUDGE_CNT      1.5
#define p0                  1013.25f
//ログ    //2018.06.28 edit
#define NUM_DATA            20

MPU6050         mpu(p9,p10);
BMP180          bmp(p9,p10);
BusOut          led(LED1,LED2,LED3,LED4);
Serial          pc(USBTX,USBRX);
Serial          twe(p28,p27);
Serial          gps(p13,p14);
PwmOut          servo1(p21);
PwmOut          servo2(p22);
PwmOut          servo3(p23);
PwmOut          servo4(p24);
DigitalOut      sw(p25);
SDFileSystem    sd(p5,p6,p7,p8,"sd"); 
LocalFileSystem local("local");

Timer   timer_open;
Timer   timer_log;    
Ticker  tic_open;
Ticker  tic_log;     

void    _open();
void    _log();
float   _getAlt();
float   _DMS2DEG(float raw_data);
float   _median(float data[],int num);
int     _input(char cha);

float   Alt_buf[10], Alt_gnd, Alt_md, Alt_max=-300; //TODO:Alt_maxの値を調整すること
float   Time_alt;
float   Data[2][NUM_DATA][8];
int     Cnt=0, Cnt_acc=0, Cnt_alt=0;
int     Cnt_data=0;
int     cnt_gps;
char    gps_data[256];
bool    Row;
FILE    *lfp;
FILE    *fp;

enum PHASE{SETUP=0,LAUNCH=1,WAIT=3,RISE=7,DROP1=15,DROP2=9,SEA=10} Phase;
enum SERVO{LOCK=0,UNLOCK=1} Servo;

int main(){
    /*センサ類初期化*/
    bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
    twe.baud(115200);
    sw=1;
    /*ファイル作成*/
    mkdir("/sd/mydir",0777);
    fp=fopen("/sd/mydir/data.txt","a");
    fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n");
    fclose(fp);
    lfp=fopen("/local/data.txt","a");
    fclose(lfp);
    /*サーボ調整*/
    servo1.period_ms(20);
    servo2.period_ms(20);
    servo3.period_ms(20);
    servo4.period_ms(20);
    /*初めの挨拶*/
    //pc.printf("POWER ON\r\n");
    twe.printf("POWER ON\r\n");
    /*入力待ち*/
    while(1){
        char cha = twe.getc();
        if(_input(cha)==-1){
            timer_log.start();  
            tic_log.attach(&_log, 1.0/RATE_LOG); 
            break;
        }
    }
    /*フライト準備*/
    Phase = SETUP;
    tic_open.attach(&_open, 1.0/RATE_OPEN);

    /*GPS*/
    while(1){     
        /*GPS取得*/
        if(Phase != RISE){
            if(gps.readable()){
                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);
                            twe.printf("%s\r\n",gps_data);
                            twe.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
                        }else{
                            twe.printf("%s\r\n",gps_data);
                        }
                    }
                }else{
                    cnt_gps++;
                }
            }
        }
       
        if(timer_log.read()>=30.0*60.0){
            timer_log.reset();
            /*なんかずっとこればっか書き込まれた 2018/7/1
            fp=fopen("/sd/mydir/data.txt","w");
            fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n");
            fclose(fp);
            */
        }
    }
}

void _open(){
    led = Phase;
    float acc[3],acc_buf[10],acc_abs,time_acc,time_alt;
    switch(Phase){
        case SETUP:     //地上高度取得
            //pc.printf("Hello,World!\r\n");
            twe.printf("Hello,World!\r\n");
            for(Cnt=0;Cnt<10;Cnt++){
                Alt_buf[Cnt] = _getAlt();
            }
            Alt_gnd = _median(Alt_buf,10); 
            /*データをローカルファイルに保存*/ 
            lfp=fopen("/local/data.txt", "a");
            fprintf(lfp,"地上高度：%f\r\n",Alt_gnd);
            fclose(lfp);

            timer_open.start();
            Phase = LAUNCH;
            //pc.printf("Phase = LAUNCH\r\n");
            twe.printf("Phase = LAUNCH\r\n");
            break;

        case LAUNCH:    //発射判定
            for(Cnt=0;Cnt<10;Cnt++){
                mpu.getAccelero(acc);
                acc_buf[Cnt] = sqrt(pow(acc[0]/9.81,2)+pow(acc[1]/9.81,2)+pow(acc[2]/9.81,2)); 
            }
            acc_abs = _median(acc_buf,10);
            //pc.printf("acc:%f\r\n",acc_abs);
            //pc.printf("cnt:%d\r\n",Cnt_acc);
            //加速度判定
            if(acc_abs>ACC_JUDGE_LAUNCH){
                Cnt_acc++;
                time_acc = timer_open.read();
            }
            if(timer_open.read()-time_acc > TIME_JUDGE_CNT) Cnt_acc = 0;
            if(Cnt_acc == CNT_JUDGE_LAUNCH){
                //pc.printf("Phase = WAIT\r\n");
                twe.printf("LAUNCHED\r\n");
                timer_open.reset();
                Phase = WAIT;
            }
            break;

        case WAIT:      //待機時間
            if(timer_open.read()>TIME_GAP) Phase = RISE;
            break;

        case RISE:      //頂点判定
            for(Cnt=0;Cnt<5;Cnt++){
                Alt_buf[Cnt] = _getAlt();
                //pc.printf("alt:%f\r\n",Alt_buf[Cnt]);
            }
            Alt_md = _median(Alt_buf,5);
            twe.printf("alt:%f\r\n",Alt_md);
            //twe.printf("cnt:%d\r\n",Cnt_alt);
            if(Alt_md > Alt_max){
                Alt_max = Alt_md;
                Cnt_alt = 0;
            }
            else if((Alt_max-Alt_md) > ALT_JUDGE_OPEN){
                Cnt_alt++;
                Time_alt = timer_open.read();
            }
            if((timer_open.read()-Time_alt) > TIME_JUDGE_CNT){
                Cnt_alt = 0;
            }
            if(/*(*/Cnt_alt == CNT_JUDGE_OPEN/*)||(timer_open.read() > TIME_JUDGE_OPEN)*/){
                //pc.printf("Phase = DROP1\r\n");
                twe.printf("OPEN PARA\r\n");
                Cnt_alt = 0;
                /*ローカルファイルに最高高度を記録*/    
                lfp = fopen("/local/data.txt", "a");
                fprintf(lfp,"最高高度：%f\r\n",Alt_max);
                fclose(lfp);

                servo1.pulsewidth(0.0006);  //TODO:サーボホーンの角度調整
                servo2.pulsewidth(0.0006);  //TODO:サーボホーンの角度調整
                Phase = DROP1;
            }
            break;

        case DROP1:     //放出機構作動？
            for(Cnt=0;Cnt<5;Cnt++){
                Alt_buf[Cnt] = _getAlt();
            }
            Alt_md = _median(Alt_buf,5);
            twe.printf("Alt:%f\r\n",Alt_md);
            //twe.printf("Cnt:%d\r\n",Cnt_alt);
            if((Alt_md-Alt_gnd) > ALT_JUDGE_OUT){
                Cnt_alt++;
            }
            if((Cnt_alt == CNT_JUDGE_OUT)||(timer_open.read() > TIME_JUDGE_OUT)){ //地上高度からの高度にする？到達高度低いかも？
                //pc.printf("Phase = DROP2\r\n");
                twe.printf("GO\r\n");
                servo3.pulsewidth(0.0006);  //TODO:サーボホーンの角度調整
                servo4.pulsewidth(0.0006);  //TODO:サーボホーンの角度調整
                Phase = DROP2;
            }
            break;
            
        case DROP2:     //着水検知．サーボの電源落とす．
            //twe.printf("cnt:%d\r\n",Cnt_alt);
            wait(3.0);
            sw = 0;
            //pc.printf("Goodbye Servo\r\n");
            twe.printf("FINISH\r\n");
            Phase=SEA;
            break;
    }
}

void _log(){    
    Data[Row][Cnt_data][0]=Phase;    
    Data[Row][Cnt_data][1]=timer_log.read();
    mpu.getAccelero(&Data[Row][Cnt_data][2]);
    bmp.ReadData(&Data[Row][Cnt_data][5],&Data[Row][Cnt_data][6]);
    Data[Row][Cnt_data][7] = (pow((p0/Data[Row][Cnt_data][6]), (1.0f/5.257f))-1.0f)*(Data[Row][Cnt_data][5]+273.15f)/0.0065f;
    Cnt_data++;
    //pc.printf("Cnt_data:%d\r\n",Cnt_data);
    if(Cnt_data==NUM_DATA){
        Cnt_data = 0;
        Row =! Row;
        fp = fopen("/sd/mydir/data.txt","a");
        for(int i=0;i<NUM_DATA;i++){
            fprintf(fp,"%2.0f,%f,%f,%f,%f,%f,%f,%f\r\n",Data[!Row][i][0],Data[!Row][i][1],Data[!Row][i][2],Data[!Row][i][3],Data[!Row][i][4],Data[!Row][i][5],Data[!Row][i][6],Data[!Row][i][7]);
        }
        fclose(fp);
    }
}

float _getAlt(){
    float altitude,pressure,temperature;
    bmp.ReadData(&temperature,&pressure);
    altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f;
    return altitude;
}

int _input(char cha){
    twe.printf("%c\r\n",cha);
    if(cha!='\0'){
//        pc.printf("%c\r\n",c);  
    } 
    if(cha=='F'){
        twe.printf("flight mode on\r\n");
        return -1;
    }else if(cha=='u'){ //開放機構のUNLOCK
        servo1.pulsewidth(0.0006);
        servo2.pulsewidth(0.0006);
    }else if(cha=='l'){ //開放機構のLOCK
        servo1.pulsewidth(0.0024);
        servo2.pulsewidth(0.0024);
    }else if(cha=='a'){ //収穫機構のUNLOCK(AKERU)
        servo3.pulsewidth(0.0010);
        servo4.pulsewidth(0.0010);
    }else if(cha=='s'){ //収穫機構のLOCK(SHIMERU)
        servo3.pulsewidth(0.0024);
        servo4.pulsewidth(0.0024);
    }
    return 0;
}

float _DMS2DEG(float raw_data){
    int d=(int)(raw_data/100);
    float m=(raw_data-(float)d*100);
    return (float)d+m/60;
}

float _median(float data[], int num){
    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;
}