File content as of revision 5:f6e956e8a060:

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

/*しきい値など*/
#define ACC_JUDGE_LAUNCH    1.5
#define TIME_BURNING        6
#define ALT_JUDGE_FIRE      1
#define ALT_JUDGE_OPEN      2
#define ANGLE_JUDGE_FIRE    90
#define CNT_JUDGE           3
#define TIME_JUDGE_CNT      1.5
#define NUM_CNT_MEDIAN      10
#define RATE_GPS            1
#define RATE_DATA           2
#define p0                  1013.25f
#define RadToDeg            57.295779513082320876798154814105
        
MPU6050 mpu(dp5,dp27);
BMP180  bmp(dp5,dp27);
KalmanFilter gKfx, gKfy; 
Madgwick MadgwickFilter;
Serial pc(dp16,dp15);
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);

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

/*グローバル変数*/
//カルマン用
float gCalibrateX;
float gCalibrateY;
float gPrevMicros; 
float degRoll,degPitch;

int main(){
    /*ローカル変数*/
    float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs;
    float alt_buff[10],alt_md,alt_max;
    float time_judge;
    int cnt_data=0,cnt_judge=0;

    /*センサの初期化等*/
    pc.baud(9600);
    mpu.setAcceleroRange(3);
    bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);

    /*初期位置の設定*/
    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;
    gKfx.setAngle(degRoll);
    gKfy.setAngle(degPitch);
    gCalibrateY = degPitch;
    gCalibrateX = degRoll;

    Phase = STANDBY;

    while(1){
        switch(Phase){
            case STANDBY:
                /*入力待ち*/
                timer_data.start();
                tic_data.attach(&_SendData, 1.0/RATE_DATA); 
                Phase = LAUNCH;
                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(acc_abs>ACC_JUDGE_LAUNCH){
                    cnt_judge++;
                }
                if(cnt_judge==CNT_JUDGE){
                    cnt_judge=0;
                    timer_open.start();
                    Phase = FIRE;
                }
                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);
                if(alt_md > ALT_JUDGE_FIRE){
                    Phase = FIRE;
                }
                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;
                }
                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);
                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) cnt_judge=0;
                if(cnt_judge == CNT_JUDGE){
                    cnt_judge = 0;
                    Phase = RECOVERY;
                }
                break;
            case RECOVERY:
                tic_data.detach();
                //tic_gps.attach(&_SendGPS, 1.0/RATE_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();

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

/*
void _SendGPS(){
    int cnt_gps;
    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);
                    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++;
        }
    }
}

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