File content as of revision 3:71a45bae8a37:

#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 c);

/*  グローバル変数 */
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;
int     cnt_gps;
int     Log_cnt=0;


int main() {
    printf("Hello World!\n");
    twe.baud(115200); 
    mpu.setAcceleroRange(0);
    bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);  
    oshirase2.output();
    mkdir("/sd/mydir", 0777);
    oshirase1 = 0;
    oshirase2 = 0;
/*    while(1){
        if(_input(c)==-1){
            oshirase1 = 0;
            oshirase2 = 1;
*/            _flight();
/*            break;
        }else if(_input(c)==1){
            oshirase1 = 1;
            oshirase2 = 0;
            while(1){
                if(_input(c)==3){
                    oshirase1 = 1;
                    oshirase2 = 1;
                    break;
                }else if(_input(c)==7){
                    oshirase1 = 0;
                    oshirase2 = 0;
                    break;
                }
            }
        }
    }
*/    while(1);
}


/*  フライトモード用関数  */
void _flight(){

    /*  地上高度取得  */
    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);
    for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
    
    /*  データ取得開始 */
    timer.start();
    logtimer.attach(_log,1.0/RATE);
    alt_timer.start();  //発射判定後にスタートするようにする！！

    /*  GPS取得&送信    */
    while(1){
        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("max altitude:%f\r\n",Max_Alt);
                    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("max altitude:%f\r\n",Max_Alt);
                    twe.printf("%s\r\n",gps_data);
                }
            }
        }else{
            cnt_gps++;
        }
    }
}


/*  データ取得&保存関数  */
void _log(){
    Time = timer.read();
    mpu.getAccelero(a);
    bmp.ReadData(&Temperature,&Pressure);
    Altitude = _getAlt(Pressure,Temperature);
    //TODO:発射判定後に行うようにする！！(ここから)
    if(Altitude > Max_Alt){
        Max_Alt = Altitude;
        alt_timer.reset();
    }
    if(alt_timer.read() > MAX_JUDGE_TIME_s){
        alt_timer.reset();
        alt_timer.stop();
        FILE *lfp = fopen("/local/alt.txt","a");
        fprintf(lfp,"最高高度：%f\r\n",Max_Alt);
        fclose(lfp);
    }
    //TODO:発射判定後に行うようにする！！(ここまで)
    if(Log_cnt==0) {
        fp = fopen("/sd/mydir/sdtest.txt", "a");
    }
    fprintf(fp, "%f, %f, %f, %f, %f, %f, %f \r\n",Time, Temperature, Pressure, Altitude, a[0],a[1],a[2]);
    Log_cnt++;
    if(Log_cnt==RATE){
        fclose(fp);
        Log_cnt = 0;
    }
    pc.printf("%f, %f, %f, %f, %f, %f, %f \r\n",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 c){
    if(c!='\0'){
      // pc.printf("%c\r\n",c);  
    } 
    if(c=='f'){
        // pc.printf("flight mode on\r\n");
        return -1;
    }
    return 0;
}