File content as of revision 0:aa715ea672cb:

#include "mbed.h"
#include "math.h"
#include "MPU9250.h"
#include "BME280.h"
#include "SDFileSystem.h"
#define NUM_CNT_MEDIAN      10  //中央値をとる個数
#define CNT_LAUNCH_TIMES 5//発射判定に必要な連続数
#define TIME_SEND           1.0 //無線送信する間隔
#define CNT_LAUNCH          5  //発射判定するときのしきい値(TBD)
#define ACC_JUDGE_LAUNCH    3.0 //発射判定の合成加速度のしきい値
#define TIME_BURNING        6   //燃焼時間(TBD)
#define ALT_JUDGE_OPEN      5   //落下判定のカウントを１増やす高度差
#define CNT_JUDGE           10  //頂点判定する時に落下のカウント数
#define p0 1013.25f //海面気圧
#define N 5
#define sampleFreq 100.0f
#define beta 0.33f // gain（大きいと加速度による補正が早い）
#define PI 3.14159265358979323846f

MPU9250 mpu = MPU9250(PB_7,PB_6);// pin30,29 SDA,SCL
Serial gps(PB_4,PB_3, 9600);//pin19,20 tx,rx
DigitalOut key(PA_8);//pin22
Serial pc(PA_10,PA_9, 115200);//pin8,9 TX,RX
BME280 bme = BME280(PB_7, PB_6);//pin30,29 SDA,SCL
SDFileSystem sd(PA_7, PA_6, PA_5, PA_4, "sd");
Timer timer_standby;
Timer timer_flight;
Timer timer_burning;
FILE *fp;
FILE *al;
FILE *ac;
//関数
float _getAlt();
float _median(float data[],int num);
void  _SendGPS();
float _DMS2DEG(float raw_data);
void _calcurateAcc();


void mpu_init(){
    uint8_t whoami_MPU9250, whoami_AK8963;
    float mag_init[3];
    
    whoami_MPU9250 = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
    whoami_AK8963 = mpu.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);
    pc.printf("MPU9250 IS 0x%x\n\r", whoami_MPU9250); // 0x71で正常
    pc.printf("AK8963 IS 0x%x\n\r", whoami_AK8963); // 0x48で正常
    
    if (whoami_MPU9250 == 0x71 || whoami_AK8963 == 0x48){  
        pc.printf("MPU9250 is detected.\n\r");
        wait(0.1);
        mpu.resetMPU9250();
        mpu.initMPU9250();
        wait(0.1);
        mpu.initAK8963(mag_init);
        mpu.getGres();
        mpu.getAres();
        mpu.getMres();
        wait(0.1);
    }
   else{
        pc.printf("Could not detect MPU9250.\n\r");
        pc.printf("whoami_MPU9250 = 0x%x\n\rwhoami_AK8963 = 0x%x\r\n",
                  whoami_MPU9250, whoami_AK8963);
        while(1);
    }
}

enum PHASE{Standby=0,Flight=1,Burning=3,Parachute=7} Phase;
float alt_gnd;
float alt_max;
char c[516];
int i = 0;
int cnt_gps=0;
int timeout;
int cnt_data;
int cnt_judge=0;
int16_t acc[3];
    
float buff_ax[N], buff_ay[N], buff_az[N];
float sum_ax = 0.0f, sum_ay = 0.0f, sum_az = 0.0f;
float ax_new = 0.0f, ay_new = 0.0f, az_new = 0.0f;
float ax = 0.0f, ay = 0.0f, az = 0.0f;
int cnt = 0;
float acc_abs;
float alt_buff[10],alt_md;

int main(){
    
    wait(0.1);
    mpu_init();
    bme.initialize();//BME初期化
    mkdir("/sd/mydir",0777);//SDファイル作成
    fp = fopen("/sd/mydir/gps.txt", "a");//最初のSDopen時間かかるのでwhile外デ
    al = fopen("/sd/mydir/altitude.txt", "a");
    ac = fopen("/sd/mydir/acc.txt", "a");
    if(fp == NULL) {
       error("Could not open file for write\n");
       }

    wait(0.1);

    
    switch(Phase){
             case Standby: //待機モード
                pc.printf("Standby\n\r");
                key = 0;
                if(pc.readable()){
                    c[0]=pc.getc();
                }

                if(c[0] == 'f'){
                pc.printf("Flight\r\n");
                Phase = Flight;
                timer_standby.stop();
                timer_flight.start();
                }
                
                break;
                
            case Flight:
                     key = 0;
                while(1){
                     mpu.readAccelData(acc);
                     _calcurateAcc();
                     acc_abs = sqrt(pow(ax/9.81f,2)+pow(ay/9.81f,2)+pow(az/9.81f,2));//合成加速度
                     fp = fopen("/sd/mydir/gps.txt", "a");
                     _SendGPS();
                     fclose(fp);
                     timeout = timer_flight.read();
                     ac = fopen("/sd/mydir/acc.txt","a");
                     fprintf(ac,"%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
                     fclose(ac);
                     pc.printf("%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
                    /*加速度判定*/
                    if(acc_abs>ACC_JUDGE_LAUNCH){
                        cnt_judge++;
                     }
                    if(cnt_judge==CNT_LAUNCH){
                        cnt_judge=0;
                        timer_burning.start();
                        Phase = Burning;
                        timer_flight.stop();
                    }
                  }
                  break;
            case Burning:
                key = 0;
                while(timer_flight.read() < TIME_BURNING){
                    mpu.readAccelData(acc);
                     _calcurateAcc();
                    pc.printf("Burning\n\r");
                    fp = fopen("/sd/mydir/gps.txt", "a");
                    _SendGPS();
                     fclose(fp);
                     timeout = timer_flight.read();
                     ac = fopen("/sd/mydir/acc.txt","a");
                     fprintf(ac,"%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
                     fclose(ac);
                     pc.printf("%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
                    
                }
                Phase = Parachute;
                pc.printf("Parachute\n\r");
                break;
           
               
            case Parachute:
                while(1){
                    mpu.readAccelData(acc);
                     _calcurateAcc();
                    fp = fopen("/sd/mydir/gps.txt", "a");
                    _SendGPS();
                     fclose(fp);
                     timeout = timer_flight.read();
                     ac = fopen("/sd/mydir/acc.txt","a");
                     fprintf(ac,"%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
                     fclose(ac);
                     pc.printf("%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
                    
                    for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
                        alt_buff[cnt_data] = _getAlt();
                    }
                    alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
                    alt_md = alt_md - alt_gnd;
                
                    if(alt_md > alt_max){
                    alt_max = alt_md;
                    cnt_judge = 0;
                }
                else if((alt_max-alt_md) > ALT_JUDGE_OPEN){
                    cnt_judge++;
                }
                //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0;
                 if(cnt_judge == CNT_JUDGE){
                    key = 1;
                    pc.printf("Open\n\r");
                }
                break;
                
             }
           }    
    }
         
    



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

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(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;
}


void _SendGPS(){
    char gps_data[256];
    while(1){
        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);
                        pc.printf("%s\r\n",gps_data);
                        pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
                        pc.printf("Lat:%f,Lon:%f,MAX_ALT:%f\r\n",lat_north,lon_east,alt_max);
                        for(i=0;i<2;i++){
                            c[i]=pc.getc();
                        }
                        break;
                    }else{
                        //pc.printf("%s\r\n",gps_data);
                        pc.printf("NoGPSSignal\r\n");
                        break;
                    }
                }else{
                    //pc.printf("No_Satellite_signal\r\n");
                }
            }else{
                cnt_gps++;
            }
        }
    }
}

void _calcurateAcc(){
    /* 5回移動平均 */
        sum_ax = sum_ax - buff_ax[cnt];
        sum_ay = sum_ay - buff_ay[cnt];
        sum_az = sum_az - buff_az[cnt];

        ax_new = acc[0] / 2049.81;
        ay_new = acc[1] / 2049.81;
        az_new = acc[2] / 2049.81;

        buff_ax[cnt] = ax_new;
        buff_ay[cnt] = ay_new;
        buff_az[cnt] = az_new;

        sum_ax = sum_ax + buff_ax[cnt];
        sum_ay = sum_ay + buff_ay[cnt];
        sum_az = sum_az + buff_az[cnt];

        cnt++;
        if(cnt == N) cnt = 0;
        
        ax = sum_ax / N;
        ay = sum_ay / N;
        az = sum_az / N;
    }