File content as of revision 9:42b4d337d4cc:

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

/*しきい値など*/
#define ACC_JUDGE_LAUNCH    3.0 //発射判定のしきい値
#define TIME_BURNING        6   //開放判定しない時間（燃焼時間）
#define ALT_JUDGE_FIRE      0
#define ALT_JUDGE_OPEN      1   //落下判定のカウントを１増やす高度差
#define TIME_OPEN           25  //強制的に開放させる時間
#define TIME_SEND           1.0
#define ANGLE_JUDGE_FIRE_MIN    15
#define ANGLE_JUDGE_FIRE_MAX    70
#define CNT_JUDGE           10 
#define TIME_JUDGE_CNT      1.5
#define NUM_CNT_MEDIAN      10
#define RATE_GPS            1.0
#define RATE_DATA           10
#define TIMER_NOTFIRE       15.0
#define p0                  1013.25f
#define RadToDeg            57.295779513082320876798154814105
        
MPU6050 mpu(PB_7,PB_6);
BMP180  bmp(PB_7,PB_6);
KalmanFilter gKfx, gKfy; 
Madgwick MadgwickFilter;
Serial pc(PA_2, PA_3);
Serial gps(PA_9, PA_10);
DigitalOut myled(PA_15);
Timer timer_open;
Timer timer_data;
Ticker tic_data;
Ticker tic_gps;

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

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

/*グローバル変数*/
float t = 0;
//地上高度
float alt_gnd;
float alt_max;
//GPS
int cnt_gps=0;

int p = 1;
//サーボ
int i = 0;
char c[516];
char d [8];
int len;
char f[]="Receive";

void main(){
    /*ローカル変数*/
    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;
    char gps_data[256];
    /*センサの初期化等*/
    pc.baud(38400);
    gps.baud(115200);
    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("Hello World!\r\n");
    wait(1.0);
    pc.printf("f:Flight_mode_on\r\n");
    gps.printf("f:Flight_mode_on\r\n");
    wait(1.0);
    timer_data.start();
    while(1){
        switch(Phase){
            case STANDBY:
                //gps.printf("Phase_STANDBY\r\n");
                /*入力待ち*/
                //tic_data.attach(&_SendData, 1.0/RATE_DATA);
                servo();
                //c[i] = pc.getc();
                //gps.printf("%c",c[i]);
                //i++;
                //if(c[i] == '\n') i=0;
                //wait(1.0);
                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);
                    t = timer_data.read();
                }
                /*加速度判定*/
                if(acc_abs>ACC_JUDGE_LAUNCH){
                    cnt_judge++;
                }
                if(cnt_judge==CNT_JUDGE){
                    cnt_judge=0;
                    timer_open.start();
                    Phase = RISE;
                }
                break;
                
            case RISE:
                while(timer_open.read() < TIME_BURNING){
                    pc.printf("RISE,time from launch:%f\r\n",timer_open.read());
                    wait(1.0);
                    i=0;
                    timer_data.reset();
                }
                Phase = OPEN;
                t = 0.0;
                break;
            case FIRE:

                //カルマン
                /*
                gPrevMicros = timer_open.read();
                mpu.getAccelero(acc);
                mpu.getGyro(gyro);
                degRoll  = atan2(acc[1], acc[2]) * RadToDeg;
                degPitch = atan(-acc[0] / sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * RadToDeg;
                
                float dpsX = gyro[0] * RadToDeg;
                float dpsY = gyro[1] * RadToDeg;
                float dpsZ = gyro[2] * RadToDeg;
                
                float curMicros = timer_open.read();
                float dt = curMicros - gPrevMicros;
                gPrevMicros = curMicros;

                float degX = gKfx.calcAngle(degRoll, dpsX, dt);
                float degY = gKfy.calcAngle(degPitch, dpsY, dt);
                degY -= gCalibrateY;
                degX -= gCalibrateX;
                if(degY>ANGLE_JUDGE_FIRE){
                    Phase = OPEN;
                }
                */

                //madgwick
                float Roll,Pitch,Yaw;
                MadgwickFilter.begin(2);
                mpu.getAccelero(acc);
                mpu.getGyro(gyro);
                gyro[0] *= RadToDeg;
                gyro[1] *= RadToDeg;
                gyro[2] *= RadToDeg;

                MadgwickFilter.updateIMU(gyro[0],gyro[1],gyro[2],acc[0],acc[1],acc[2]);
                Roll  = MadgwickFilter.getRoll();
                Pitch = MadgwickFilter.getPitch();
                Yaw   = MadgwickFilter.getYaw();
                i++;
                if(i==400){
                    //pc.printf("TIME:%f,Pitch:%f\r\n",timer_data.read(),Pitch);
                    pc.printf("FIRE:Pitch:%f,Time%f\r\n",Pitch,timer_open.read());
                    i=0;
                }
                /*if(Pitch>ANGLE_JUDGE_FIRE_MIN && Pitch < ANGLE_JUDGE_FIRE_MAX){
                    Phase = OPEN;
                    i=0;
                }
                */
                
                //if(timer_open.read()>TIMER_NOTFIRE){
                if(timer_data.read()>60.0){
                    Phase = OPEN;
                    pc.printf("NOT_FIRE!!\r\n");
                }
                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);
                    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++;
                    //time_judge = timer_open.read();
                }

                //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;
                    tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); 
                }
                break;
            case RECOVERY:
                /*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,time:%d,max_alt:%f\r\n",lat_north,lon_east,world_time,alt_max);
                                    break;
                                }else{
                                    //pc.printf("%s\r\n",gps_data);
                                    pc.printf("NoGPSSignal,max_alt:%f\r\n",alt_max);
                                    break;
                                }
                            }else{
                                //pc.printf("No_Satellite_signal\r\n");
                            }
                        }else{
                            cnt_gps++;
                        }
                }
    }
 
                //Phase = SEA;*/
                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;
}

void _SendData(){
    float pretime,a[3],g[3],alt;
    //カルマン
    /*
    pretime = timer_data.read();
    mpu.getAccelero(a);
    mpu.getGyro(g);
    float degroll  = atan2(a[1], a[2]) * RadToDeg;
    float degpitch = atan(-a[0] / sqrt(a[1] * a[1] + a[2] * a[2])) * RadToDeg;
    
    float dpsx = g[0] * RadToDeg;
    float dpsy = g[1] * RadToDeg;
    float dpsz = g[2] * RadToDeg;
    
    float curtime = timer_data.read();
    float t = curtime - pretime;
    pretime = curtime;

    float degx = gKfx.calcAngle(degroll, dpsx, t);
    float degy = gKfy.calcAngle(degpitch, dpsx, t);
    degy -= gCalibrateY;
    degx -= gCalibrateX;
    */
    /*madgwick*/
    float Roll,Pitch,Yaw;
    MadgwickFilter.begin(2);
    mpu.getAccelero(a);
    mpu.getGyro(g);
    g[0] *= RadToDeg;
    g[1] *= RadToDeg;
    g[2] *= RadToDeg;

    MadgwickFilter.updateIMU(g[0],g[1],g[2],a[0],a[1],a[2]);
    Roll  = MadgwickFilter.getRoll();
    Pitch = MadgwickFilter.getPitch();
    Yaw   = MadgwickFilter.getYaw();

    alt = _getAlt();
    alt = alt - alt_gnd;
    //pc.printf("%d,%f,%f,%f\r\n",Phase,alt,degx,degy);
    pc.printf("%d,%f,%f,%f\r\n",Phase,alt,Roll,Pitch);
}

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

int servo(void){
    do{
        c[i] = pc.getc();
        gps.printf("%c",c[i]);
        i++;
    }while(c[i-1] != '\n');
    gps.printf("path\r\n");
    //gps.printf("%s",c);
    myled = 1;
    //gps.printf("%s",c);
    len = strlen(f);
    i=0;
    while(i != 4){
        if (c[i] != f[i]) break;
        i++;
    }
    if(i > 3){
        i=0;
        do{
            d[i]=c[13+i];
            i++;
        }while(d[i-1] != ')');
        d[i-1] = '\0';
        gps.printf("d:%s",d);
        i = 0;
        if(d[0] == 'o') myled = 1;
        if(d[0] == 'l') myled = 0;
        if(d[0] == 'f'){
        //pc.printf("flight_mode\r\n");
        Phase = LAUNCH;
        }
    }
        
    i=0;
    memset(c,'\0',64);
    memset(d,'\0',8);
    //pc.printf("2\r\n");
    return 0;
}

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);
                        break;
                    }else{
                        //pc.printf("%s\r\n",gps_data);
                        pc.printf("NoGPSSignal\r\n");
                        break;
                    }
                }else{
                    //ffpc.printf("No_Satellite_signal\r\n");
                }
            }else{
                cnt_gps++;
            }
        }
    }
 
}