File content as of revision 20:89fdec3731f8:

/*Designed by CORE in HINO*/

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

/*しきい値など*/
#define CNT_LAUNCH          10  //発射判定するときのしきい値
#define ACC_JUDGE_LAUNCH    3.0 //発射判定の合成加速度のしきい値
#define TIME_BURNING        6   //開放判定しない時間（燃焼時間）
#define ALT_JUDGE_OPEN      5   //落下判定のカウントを１増やす高度差
#define TIME_OPEN           25  //強制的に開放させる時間
#define TIME_SEND           1.0 //無線送信する間隔
#define CNT_JUDGE           10  //頂点判定する時に落下のカウント数
#define NUM_CNT_MEDIAN      10  //中央値をとる個数
#define RATE_GPS            1.0 //GPSのTickerを動作させるタイミング
#define p0                  1013.25f //海面気圧
        
MPU6050 mpu(PB_7,PB_6);
BMP180  bmp(PB_7,PB_6);
Serial pc(PA_2, PA_3);
Serial gps(PA_9, PA_10);
DigitalOut myled(PA_15);
DigitalOut ES920_RST(PA_5);
PwmOut     servo1(PB_4);
PwmOut     servo2(PB_5);
DigitalOut servo1_signal(PA_0);
DigitalOut servo2_signal(PA_1);
Timer timer_open;
Timer timer_data;
Ticker tic_gps;

/*自作関数*/
float _getAlt();
float _median(float data[],int num);
void  _SendGPS();
float _DMS2DEG(float raw_data);

enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase;

/*グローバル変数*/
//地上高度
float alt_gnd;
float alt_max;
char c[516];
int i = 0;
int cnt_gps=0;
void main(){
    ES920_RST = 0;
    myled =0;
    /*ローカル変数*/
    float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs;
    float alt_buff[10],alt_md;
    float time_judge;
    int cnt_data=0,cnt_judge=0;
    float t = 0;
    float t_hour = 0;
    float t_send = 0;
    /*センサの初期化等*/
    pc.baud(38400);
    mpu.setAcceleroRange(3);
    bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);

    /*初期位置の設定*/
    mpu.getAccelero(acc);
    mpu.getGyro(gyro);

    Phase = STANDBY;
    for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
        alt_buff[cnt_data] = _getAlt();
    }
    alt_gnd = _median(alt_buff,NUM_CNT_MEDIAN);
    wait(2.0);
    pc.printf("0,0,0,0,0\r\n");
    wait(1.0);
    timer_data.start();
    servo1_signal = 1;
    servo2_signal = 1;
    servo1.pulsewidth(0.0015);//close
    servo2.pulsewidth(0.0006);//close
    while(1){
        switch(Phase){
            case STANDBY:
                /*サーボ入力待ち*/
                //servo();
                if(pc.readable()){
                    c[0]=pc.getc();
                }
                t = timer_data.read();
                if(t > 1800){
                    t_hour += 1800;
                    timer_data.reset();
                }
                t = t + t_hour;
                t = t/60.0;
                
                if(c[0] == 'o'){
                    myled = 1;
                    servo1.pulsewidth(0.0006);//open
                    servo2.pulsewidth(0.0015);//open
                    mpu.getAccelero(acc);
                    //pc.printf("OPEN:%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)));
                    pc.printf("0,%f,0,%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)),t);
                }
                if(c[0] == 'l'){
                    myled = 0;
                    servo1.pulsewidth(0.0015);
                    servo2.pulsewidth(0.0006);
                    mpu.getAccelero(acc);
                    //pc.printf("LOCK:%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)));
                    pc.printf("0,%f,0,%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)),t);
                }
                if(c[0] == 'f'){
                //pc.printf("flight_mode\r\n");
                Phase = LAUNCH;
                servo1_signal = 0;
                servo2_signal = 0;
                t = 0;
                timer_data.reset();
                }
                if(c[0] == 'g'){
                    pc.printf("9,0,0,0,0\r\n");
                    wait(0.5);
                    tic_gps.attach(&_SendGPS, 1.0/RATE_GPS);
                    Phase = RECOVERY;
                }
                break;
                
            case LAUNCH:
                for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
                    mpu.getAccelero(acc);
                    acc_buff[cnt_data] = sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0));
                }
                acc_abs = _median(acc_buff,NUM_CNT_MEDIAN);
                if(timer_data.read() - t > TIME_SEND){
                    //pc.printf("LAUNCH,acc:%f,time:%3f,cnt:%d\r\n",acc_abs,timer_data.read(),cnt_judge);
                    pc.printf("1,%f,0,%3f\r\n",acc_abs,timer_data.read());
                    cnt_judge = 0;
                    t = timer_data.read();
                }
                /*加速度判定*/
                if(acc_abs>ACC_JUDGE_LAUNCH){
                    cnt_judge++;
                }
                if(cnt_judge==CNT_LAUNCH){
                    servo1_signal = 1;
                    servo1_signal = 1;
                    cnt_judge=0;
                    timer_open.start();
                    Phase = RISE;
                    timer_data.stop();
                }
                break;
                
            case RISE:
                while(timer_open.read() < TIME_BURNING){
                    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;
                    for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
                        mpu.getAccelero(acc);
                        acc_buff[cnt_data] = sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0));
                    }
                    acc_abs = _median(acc_buff,NUM_CNT_MEDIAN);
                    //pc.printf("RISE,time from launch:%f\r\n",timer_open.read());
                    pc.printf("3,%f,%f,%3f\r\n",acc_abs,alt_md,timer_open.read());
                    wait(0.9);
                    i=0;
                    //timer_data.reset();
                }
                Phase = OPEN;
                t = 0.0;
                break;
            case FIRE:
                break;
            case OPEN:
                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(timer_open.read() - t > TIME_SEND){
                    //pc.printf("OPEN,alt:%f,time:%3f,cnt:%d\r\n",alt_md,timer_open.read(),cnt_judge);
                    pc.printf("15,%f,%f,%3f\r\n",acc_abs,alt_md,timer_open.read());
                    t = timer_open.read();
                }
                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 || timer_open.read() > TIME_OPEN){
                    Phase = RECOVERY;
                    servo1_signal = 1;
                    servo2_signal = 1;
                    wait(0.1);
                    servo1.pulsewidth(0.0006);//open
                    servo2.pulsewidth(0.0015);//open
                    pc.printf("9,0,0,0,%d\r\n",cnt_judge);
                    wait(0.5);
                    tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); 
                }
                break;
            case RECOVERY:
                break;
            case SEA:
                break;
        }
    }
}

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

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);
                        int japan_time = int(world_time) - 9;
                        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();
                        }
                        //pc.printf("%c",c[1]);
                        if(c[1]!='O'){
                            ES920_RST = 1;
                            wait(0.1);
                            ES920_RST = 0;
                            wait(1.0);
                            myled = 1;
                        }else{
                            myled = 0;
                        }
                        
                        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++;
            }
        }
    }
 
}