File content as of revision 2:4cdbd4b3722d:

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

#define p0 1013.25f
#define JUDGE_ACC   1.0
#define JUDGE_ALT   1.0
#define RATE_GET    20
#define RATE_OPEN   20
#define NUM_DATA    20
#define NUMBER      5
#define TIME_OPEN   14
#define TIME_DROP   104

enum PHASE{SETUP,LAUNCH,RISE,DROP,LAND} Phase;

Serial twe(p13,p14);
Serial pc(USBTX,USBRX);
MPU6050 mpu(p9,p10);
BMP180 bmp(p9, p10);
SDFileSystem sd(p5, p6, p7, p8, "sd");  // the pinout on the mbed Cool Components workshop board
Serial           gps(p28,p27);
PwmOut servo1(p21);
PwmOut servo2(p25);
DigitalOut       fte(p5);
DigitalOut       buzz(p6);
DigitalOut       led1(LED1);
DigitalOut       led2(LED2);
DigitalOut       led3(LED3);
DigitalOut       led4(LED4);



Timer timer_open;
Timer timer_log;
Ticker tic_open;
Ticker tic_log;
Ticker tic_heat;
Ticker tic_buzz;


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

void  _open();
void  _getData();
int   _input(char cha);
void  _heater();
void  _buzzer();
char  comh;
char  comb;
float _median(float data[], int num);
float getAlt(float press, float temp);
float _DMS2DEG(float raw_data);

float Alt_gnd;
float med[NUMBER];
float alt[NUMBER];
float med_alt;
float max_alt=-1000;
float mae_alt=5000;
float med_g;
float pressure,temperature,altitude;
float Time;
float Data[2][NUM_DATA][7]={0};
char  gps_data[256];
int   cnt_gps;
int   Cnt;  
int   cnt_alt=0;
int   cnt_d=0;
int   Cnt_Data;
int   Cnt_GPS=0;
bool  row;

int main(){
    twe.baud(115200);
    mpu.setAcceleroRange(3);
    bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
    mkdir("/sd/mydir",0777);
    bp=fopen("/sd/mydir/data.bin","ab");
    fclose(bp);
    lfp=fopen("/local/data.txt","a");
    fclose(lfp);
    servo1.period_ms(20);
    servo2.period_ms(20);
    led1=0;
    led2=0;
    led3=0;
twe.printf("HELLO!\r\n");
    led1=1;
    led2=1;
    led3=1;
    led4=1;
    fte=0;
    buzz=0;
    pc.printf("hello\r\n");
    
    while(1){
        char cha = twe.getc();
        if(_input(cha)==-1){
            timer_log.start();
            tic_log.attach(&_getData, 1.0/RATE_GET);
            break;
        }
    }   
    tic_heat.attach(&_heater,0.1);
    led1=0;
    led2=0;
    led3=0;
    led4=0;
    Phase = SETUP;
    tic_open.attach(&_open, 1.0/RATE_OPEN);
 
    while(1){     
        
                
            if(gps.readable()){
                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();
    }
}



void _open(){
    switch(Phase){
        case SETUP: 
                    for(Cnt=0;Cnt<10;Cnt++){
                        bmp.ReadData(&temperature,&pressure);
                        altitude = getAlt(pressure,temperature);
                        alt[Cnt]=altitude;
                    }
                    Alt_gnd = _median(alt,10);
                    lfp=fopen("/local/data.txt", "a");
                    fprintf(lfp,"地上高度：%f\r\n",Alt_gnd);
                    fclose(lfp);
                    Phase = LAUNCH;
                    twe.printf("Phase=LAUNCH\r\n");
                    pc.printf("Phase=LAUNCH\r\n");
                    break;
                    
        case LAUNCH:
                    
                    float a[3],g;
                    int i=0;
                    for(i=0;i<NUMBER;i++){
                        mpu.getAccelero(a);
                        g=sqrt(pow(a[0],2)+(a[1],2)+(a[2],2))/9.81;
                        med[i]=g;
                        }
                    med_g=_median(med,NUMBER);
                    twe.printf("%f\n\r",med_g);
                    if(med_g>JUDGE_ACC){
                        tic_heat.detach();
                        timer_open.start();
                        pc.printf("発射\r\n");
                        twe.printf("発射\r\n");
                        led1=0;
                        led4=0;
                        Phase=RISE;
                        twe.printf("PHASE=RISE\r\n");
                    }
                        led1=!led1;
                        break;
        
        case RISE:
                int j=0;
                for(j=0;j<NUMBER;j++){
                    bmp.ReadData(&temperature,&pressure);
                    altitude = getAlt(pressure,temperature);
                    alt[j]=altitude;
                }       
        med_alt=_median(alt,NUMBER);
            twe.printf("%f, %f\r\n",max_alt,med_alt);
        if(med_alt>max_alt){
           max_alt=med_alt;
            cnt_alt=0;
            }else if(max_alt-med_alt>JUDGE_ALT){
                cnt_alt++;
            }
            twe.printf("%d\n\r",cnt_alt);
            if(cnt_alt>=3 || timer_open.read()>=TIME_OPEN){
                pc.printf("頂点\r\n");
                twe.printf("頂点\r\n");
                 lfp = fopen("/local/data.txt", "a");
                 fprintf(lfp,"最高高度：%f\r\n",max_alt);
                 fclose(lfp);
                 servo1.pulsewidth(0.0015);
                 servo2.pulsewidth(0.0006);
                 led2=0;
                Phase=DROP;
                twe.printf("PHASE=DROP");
            }
                led2=!led2;
                break;
            
    case DROP:
            int k=0;
            for(k=0;k<NUMBER;k++){
            bmp.ReadData(&temperature,&pressure);
            altitude = getAlt(pressure,temperature);
            alt[k]=altitude;
        }       
        med_alt=_median(alt,NUMBER);
        mae_alt=med_alt;
        if(mae_alt-med_alt<=1 && mae_alt-med_alt>=-1){
            cnt_d++;
        }else{
            cnt_d=0;
        }
        twe.printf("%d\n\r",cnt_d);
        if(cnt_d==3 || timer_open.read()>TIME_DROP){
            pc.printf("着水\r\n");
            twe.printf("着水\r\n");
            timer_open.stop();
            tic_buzz.attach(&_buzzer,0.1);
            Phase=LAND;
            pc.printf("Judge DROP\r\n");
            twe.printf("FINISH\r\n");
            led3=0;
        }
            led3=!led3;
            break;
    }    
}       

void _getData(){
    Data[row][Cnt_Data][0] = timer_log.read();
    mpu.getAccelero(&Data[row][Cnt_Data][1]);
    bmp.ReadData(&Data[row][Cnt_Data][4],&Data[row][Cnt_Data][5]);
    Data[row][Cnt_Data][6] = (pow((p0/Data[row][Cnt_Data][5]), (1.0f/5.257f))-1.0f)*(Data[row][Cnt_Data][4]+273.15f)/0.0065f;
    Cnt_Data++;
    /*Log*/
    if(Cnt_Data==NUM_DATA){
        Cnt_Data = 0;
        row =! row;
        bp = fopen("/sd/mydir/data.bin","ab");
        fwrite(&Data[!row][0][0],sizeof(float),7*NUM_DATA,bp);
        fclose(bp);
    }
}

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

float getAlt(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){
    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 cha){
    if(cha=='F'){
        twe.printf("flight mode on\r\n");
        return -1;
    }else if(cha=='U'){
        servo1.pulsewidth(0.0015);
        servo2.pulsewidth(0.0006);
    }else if(cha=='L'){
        servo1.pulsewidth(0.0008);
        servo2.pulsewidth(0.0014);
    }
    return 0;
}

void _heater(){
    if(twe.readable()){
    char comh=twe.getc();
    if(comh=='H'){
        fte=1;
        led4=1;
    }else if(comh=='C'){
        fte=0;
        led4=0;
    }
 }
}

void _buzzer(){
    if(twe.readable()){
    char comb=twe.getc();
    if(comb=='N'){
        buzz=1;
        led4=1;
    }else if(comb=='S'){
        buzz=0;
        led4=0;
    }
 }
    
}