File content as of revision 8:8b7e9aa3a2fc:

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

#define RATE                20
#define MAX_JUDGE_TIME_s    3.0
#define p0                  1013.25f

BMP180          bmp(p28,p27);
MPU6050         mpu(p28,p27);
Timer           timer;
Timer           alt_timer;
Ticker          logtimer;
DigitalOut      oshirase1(p19);
DigitalInOut    oshirase2(p20);
//Serial          pc(USBTX,USBRX);
Serial          gps(p13,p14);
Serial          twe(p9,p10);
SDFileSystem    sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
LocalFileSystem local("local");
FILE            *fp;

/*  自作関数    */
void    _flight();
void    _log();
float   _getAlt(float press, float temp);
float   _DMS2DEG(float raw_data);
float   _median(float data[], int num);
int     _input(char moji);

/*  グローバル変数 */
float   a[3];
float   Land_Alt;
float   Max_Alt = 0;
float   Alt_buff[10];
float   Pressure,Temperature,Altitude;
float   Time;
char    gps_data[256];
char    c;
char    command;
int     cnt_gps  = 0;
int     Log_cnt  = 0;
int     Phase    = 0;


int main() {
//    pc.printf("Hello, Happy World!\r\n");
    oshirase2.output();
    oshirase1 = 0;
    oshirase2 = 0;
    twe.baud(115200); 
    twe.printf("Hello World!\r\n");
    mpu.setAcceleroRange(0);
    bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);  
    mkdir("/sd/mydir", 0777);
    while(1){
        command = twe.getc();
        if(_input(command)==-1){        //フライトモードへ移行
            oshirase1 = 0;
            oshirase2 = 1;
            _flight();
            break;
        }else if(_input(command)==1){   //サーボ動かすだけモードへ
            oshirase1 = 1;
            oshirase2 = 0;
            while(1){
                command = twe.getc();
                if(_input(command)==3){         //開放用サーボ(MG996)を動かす
                    oshirase1 = 1;
                    oshirase2 = 1;
                    break;
                }else if(_input(command)==7){   //リーフィングのサーボ(SG92)を動かす
                    oshirase1 = 0;
                    oshirase2 = 0;
                    break;
                }
            }
        }
    }
    while(1);
}


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

    /*  地上高度取得  */
    for(int i=0;i<10;i++){
        bmp.ReadData(&Temperature,&Pressure);
        Alt_buff[i] = _getAlt(Pressure,Temperature);
    }
    Land_Alt = _median(Alt_buff,10);
    Max_Alt  = Land_Alt;
    FILE *lfp = fopen("/local/alt.txt","a");
    fprintf(lfp,"地上高度：%f\r\n",Land_Alt);
    fclose(lfp);
    twe.printf("地上高度：%f\r\n",Land_Alt);
//    for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
    
    /*  データ取得開始 */
    timer.start();
    logtimer.attach(_log,1.0/RATE);

    while(1){

        /*  フェーズ管理  */
        if(Phase==0){
            if(oshirase2==1){
                Phase = 1;  //上昇フェーズへ移行
                alt_timer.start();
            }
        }else if(Phase==1){
            if(oshirase2==0){
                Phase = 2;  //降下1フェーズへ移行
            }
        }else if(Phase==2){
            if(oshirase2==1){
                Phase = 3;  //降下2フェーズへ移行
            }
        }
        
        /*  30分ごとにタイマーをリセット(オーバーフロー対策)  */
        if(Time > 60*30){
            timer.reset();
        }
        
//        twe.printf("phase:%d,max altitude:%f\r\n",Phase,Max_Alt);

        /*  GPS取得&送信    */
//        twe.printf("0");
        gps_data[cnt_gps] = gps.getc();
//        twe.printf("1");
        if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
//           twe.printf("2");
            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;
//            twe.printf("3");
            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){
//                    twe.printf("4");
                    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);
                    twe.printf("phase:%d,max altitude:%f\r\n",Phase,Max_Alt);
                }else{
//                    twe.printf("5");
                    twe.printf("%s\r\n",gps_data);
                    twe.printf("phase:%d,max altitude:%f\r\n",Phase,Max_Alt);
                }
            }
        }else{
            cnt_gps++;
//            twe.printf("6");
        }
//        twe.printf("Hi!");
    }
}


/*  データ取得&保存関数  */
void _log(){
    Time = timer.read();
    mpu.getAccelero(a);
    bmp.ReadData(&Temperature,&Pressure);
    Altitude = _getAlt(Pressure,Temperature);

    /*  最高高度取得＆保存   */
    if(Altitude > Max_Alt){
            Max_Alt = Altitude;
            alt_timer.reset();
    }else if(alt_timer.read() > MAX_JUDGE_TIME_s){
        alt_timer.stop();
        alt_timer.reset();
        FILE *lfp = fopen("/local/alt.txt","a");
        fprintf(lfp,"最高高度：%f\r\n",Max_Alt);
        fclose(lfp);
//        pc.printf("最高高度：%f\r\n",Max_Alt);
    }

    /*  データ保存   */
    if(Log_cnt==0) {
        fp = fopen("/sd/mydir/log.txt", "a");
    }
    fprintf(fp, "%d,%f, %f, %f, %f, %f, %f, %f \r\n",Phase, Time, Temperature, Pressure, Altitude, a[0],a[1],a[2]);
    Log_cnt++;
    if(Log_cnt==RATE){
        fclose(fp);
        Log_cnt = 0;
    }
//    pc.printf("%d,%f, %f, %f, %f, %f, %f, %f \r\n",Phase, Time, Temperature, Pressure, Altitude, a[0],a[1],a[2]);
}


/*  高度計算関数  */
float _getAlt(float press, float temp){
    return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
}


/*  緯度経度計算関数    */
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){//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;
}

/*  コマンド判別用関数   */
int _input(char moji){
    if(moji!='\0'){
       twe.printf("%c\r\n",c);  
//       pc.printf("%c\r\n");
    } 
    if(moji=='f'){
        return -1;
    }else if(moji=='s'){
        return 1;
    }else if(moji=='p'){
        return 3;
    }else if(moji=='l'){
        return 7;
    }
    return 0;
}