File content as of revision 0:ca455457108f:

#include "mbed.h"
#include "math.h"

#include "ES920LR.hpp"
#include "Nichrome_lib.h"
#include "SDFileSystem.h"

#include "ADXL375_i2c.h"
#include "INA226.h"
#include "MadgwickFilter.hpp"
#include "mpu9250_i2c.h"
#include "pqLPS22HB_lib.h"
#include "SHT35.h"
#include "TSL2561.h"
#include "GPS_interrupt.h"


/*******************************************************************************
設定値
打ち上げ前に必ず確認！！
*******************************************************************************/
const float TIME_SEP1 = 16.0f;       //1段目分離までの時間
const float TIME_SEP2 = 75.0f;      //2段目分離までの時間

const float TIME_RECOVERY = 120.0f; //回収モード移行までの時間

const float SEP1_ALT_UNDER = 500.0f;//1段目分離条件（高度）
const float SEP1_SPEED = 5.0f;      //1段目分離条件（速度）

const float SEP2_ALT_UP = 300.0f;   //2段目分離条件（高度）

const float OK_UP_ALT = 500.0f;
const float OK_DOWN_ALT = 30.0f;
const float STOP_SPEED_UP = 2.0f;
const float STOP_SPEED_UNDER = -2.0f;

bool wait_GPS = true;   //GPS受信待ちするか
bool ok_PC = false;      //PCを使用するか


/*******************************************************************************
コンストラクタ
*******************************************************************************/
RawSerial pc(USBTX, USBRX, 115200);

RawSerial serial_es920(p9, p10);
ES920LR es920(serial_es920, pc, 115200);

Serial GPS_serial(p13, p14, 38400);
GPS_interrupt GPS(&GPS_serial);
float GPS_freq = 4;

SDFileSystem sd(p5, p6, p7, p8, "sd");
const char file_name[64] = "/sd/Felix_Luminous_LOG.csv";

I2C i2c_bus(p28, p27);
ADXL375_i2c ADXL375(i2c_bus, ADXL375_i2c::ALT_ADDRESS_HIGH);
myINA226 INA226_in(i2c_bus, myINA226::A1_GND, myINA226::A0_GND);
myINA226 INA226_ex(i2c_bus, myINA226::A1_VDD, myINA226::A0_GND);
mpu9250 nine(i2c_bus, AD0_HIGH);
pqLPS22HB_lib LPS22HB(pqLPS22HB_lib::AD0_HIGH, i2c_bus);
pqLPS22HB_lib LPS33HW(pqLPS22HB_lib::AD0_LOW, i2c_bus);
mySHT35 SHT35(i2c_bus, mySHT35::AD0_LOW);
myTSL2561 TSL2561(i2c_bus, myTSL2561::AD0_OPEN);

//InterruptIn FlightPin(p26);
DigitalIn FlightPin(p26);

Nichrome_lib Separate1(p24);
Nichrome_lib Separate2(p25);

DigitalOut Buzzer(p22);
//PwmOut Buzzer_music(p21);

//AnalogIn Pitot(p20);

Timeout timeout_stop;
Timeout timeout_sep;
Timeout timeout_recovery;

Timer time_main;
Timer time_flight;

Ticker tick_gps;
Ticker tick_print;
Ticker tick_header_data;


/*******************************************************************************
関数のプロトタイプ宣言
*******************************************************************************/
void setup();       //無線あり

void readSensor();
void readGPS();

void printData();
void readPC();
void printHelp();

void sendDownLink();//無線あり
void readUpLink();  //無線あり

void startRec();
void stopRec();
void recData();

void modeChange();  //無線あり
void changePhase_SEP1();
void changePhase_SEP2();
void changePhase_RECOVERY();
void buzzerChange();

void separate1();
void separate2();


/*******************************************************************************
変数の宣言
*******************************************************************************/
char rocket_phase;
bool do_first = false;

bool es920_using = false;
char es920_uplink_command = '-';

float adxl_acc[3];

float ina_in_v, ina_in_i, ina_ex_v, ina_ex_i;
bool ex_power;

MadgwickFilter attitude(1.0);
float euler[3];
Quaternion quart(1.0, 0.0, 0.0, 0.0);

float gyro[3], gyro_rad[3], acc[3], mag[3];

float pres22, temp22, alt22, pres22_0, temp22_0;

float pres33, temp33, alt33, pres33_0, temp33_0;
float alt33_buf[10], alt33_ave, alt33_ave_old, speed33, speed33_buf[10], speed33_ave;
int alt33_count = 0, speed33_count = 0;

bool ok_up = false;
bool ok_down = false;
bool ok_top = false;

float temp35, hum35;

int lux;

float pitot_raw, pitot_err, pitot;
float pitot_buf[100];
int pitot_count = 0;
float pitot_speed;

float gps_lat, gps_lon, gps_alt, gps_utc[6], gps_knot, gps_deg;
int gps_sat;
bool gps_yn;

bool flight_pin;

FILE *fp;
bool save = false;
int save_c = 0;

int time_read, time_reset = 0;
int time_min, time_sec, time_ms;

int flight_time_read, flight_time_read_old, flight_time_reset = 0;
int flight_time_min, flight_time_sec, flight_time_ms;


/*******************************************************************************
定数
*******************************************************************************/
const float ES920_MAX_20 = 0.000305176;
const float ES920_MAX_100 = 0.001525879;
const float ES920_MAX_200 = 0.003051758;
const float ES920_MAX_500 = 0.007629395;
const float ES920_MAX_1500 = 0.022888184;
const float ES920_MAX_3000 = 0.045776367;


/*******************************************************************************
無線のヘッダまとめ(ROCKET -> GND)
*******************************************************************************/
const char HEADER_SENSOR_SETUP = 0x01;
// 0x01, ADXL, INA_in, INA_ex, MPU, MPU_m, 22HB, 33HW, SHT, TSL, SD
//   0     1      1       1     1     1      1     1    1    1    1
//         0      1       2     3     4      5     6    7    8    9

const char HEADER_GPS_SETUP = 0x02;
// 0x02, readable, wait_count
//   0      1          4
//          0          1
//        0x00 : NG
//        0x01 : OK
//        0xAA : Finish
//        0xFF : Ignore

const char HEADER_DATA = 0x10;
//起動時間, フライト時間, フェーズ, ex_pow&Sep1&Sep2&ok_up&ok_down&ok_top&gps_yn, lat, lon, alt, knot, deg, sat, in_v, in_i, ex_v, ex_i, pres33, alt33, speed33, temp35, hum35, pitot_speed 
//4(2,2)   4(2,2)      2         1                                   2     2    2    2    2    2    2     2     2     2      2       2      2        2       2      2 
//0 2      4 6         8         10                                  11    13   15   17   19   21   23    25    27    29     31      33     35       37      39     41 
//                                  0x01 : ex_pow
//                                  0x02 : Sep1
//                                  0x04 : Sep2
//                                  0x08 : ok_up
//                                  0x10 : ok_down
//                                  0x20 : ok_top
//                                  0x40 : save
//                                  0x80 : flight_pin

const char HEADER_START = 0x11;
// 0x11, What
//   0     1
//         0
//      'W' : 安全モード->離床検知モード
//      'S' : 離床検知モード->安全モード
//      'F' : 離床検知モード->フライトモード
//      '1' : 1段目分離
//      '2' : 2段目分離


/*******************************************************************************
無線のヘッダまとめ(GND -> ROCKET)
*******************************************************************************/
const char HEADER_COMMAND = 0xcd;
// 0xcd,コマンド
//   0     1
//   0     1


/*******************************************************************************
ロケットのフェーズ
*******************************************************************************/
const char ROCKET_SETUP = 0x00;     //セットアップ中
const char ROCKET_SAFETY = 0x01;    //安全モード
const char ROCKET_WAIT = 0x02;      //待機モード
const char ROCKET_FLIGHT = 0x03;    //フライトモード
const char ROCKET_SEP1 = 0x04;      //分離1モード
const char ROCKET_SEP2 = 0x05;      //分離2モード
const char ROCKET_RECOVERY = 0x06;  //回収モード


/*******************************************************************************
メイン関数
*******************************************************************************/
int main() {
    rocket_phase = ROCKET_SETUP;
    setup();
    
    pc.attach(&readPC, Serial::RxIrq);
    
    tick_gps.attach(&readGPS, 1/GPS_freq);
    tick_print.attach(&printData, 1.0f);
    tick_header_data.attach(&sendDownLink, 1.0f);
    
    es920.attach(&readUpLink, HEADER_COMMAND);
    
    FlightPin.mode(PullUp);
    
    time_main.reset();
    time_main.start();
    startRec();
    
    rocket_phase = ROCKET_SAFETY;
    while(1) {
        readSensor();
        recData();
        modeChange();   //状態遷移の判断
    }
}


/*******************************************************************************
センサ読み取り
*******************************************************************************/
void readSensor(){
    time_read = time_main.read_ms();
    if(time_read >= 30*60*1000){
        time_main.reset();
        time_reset ++;
    }
    time_min = time_reset * 30 + floor((double)time_read / (60 * 1000));
    time_sec = time_read % (60 * 1000);
    time_sec = floor((double)time_sec / 1000);
    time_ms = time_read % 1000;
    
    if(rocket_phase >= ROCKET_FLIGHT){
        flight_time_read_old = flight_time_read;
        flight_time_read = time_flight.read_ms();
    }
    else{
        flight_time_read = 0;
    }
    if(flight_time_read >= 30*60*1000){
        time_flight.reset();
        flight_time_reset ++;
    }
    flight_time_min = flight_time_reset * 30 + floor((double)flight_time_read / (60 * 1000));
    flight_time_sec = flight_time_read % (60 * 1000);
    flight_time_sec = floor((double)flight_time_sec / 1000);
    flight_time_ms = flight_time_read % 1000;
    
    ADXL375.getOutput(adxl_acc);
    
    INA226_in.get_Voltage_current(&ina_in_v, &ina_in_i);
    ina_in_v = ina_in_v / 1000;
    ina_in_i = ina_in_i / 1000;
    if(ina_in_i > 64){
        ina_in_i = 0.0f;
    }
    INA226_ex.get_Voltage_current(&ina_ex_v, &ina_ex_i);
    ina_ex_v = ina_ex_v / 1000;
    ina_ex_i = ina_ex_i / 1000;
    if(ina_ex_i > 64){
        ina_ex_i = 0.0f;
    }
    if(ina_ex_i > 0.1f){
        ex_power = true;
    }
    else{
        ex_power = false;
    }
    
    nine.getGyro(gyro);
    nine.getAcc(acc);
    nine.getMag(mag);
    for(int i = 0; i < 3; i ++){
        gyro_rad[i] = gyro[i] * 3.1415926535 / 180;
    }
    attitude.MadgwickAHRSupdate(gyro_rad, acc, mag);
    attitude.getAttitude(&quart);
    attitude.getEulerAngle(euler);
    for(int i = 0; i < 3; i ++){
        euler[i] = euler[i] * 180 / 3.145926535;
    }
    
    pres22 = LPS22HB.getPres();
    temp22 = LPS22HB.getTemp();
    alt22 = LPS22HB.getAlt(pres22_0, temp22_0);
    
    pres33 = LPS33HW.getPres();
    temp33 = LPS33HW.getTemp();
    alt33 = LPS33HW.getAlt(pres33_0, temp33_0);
    
    alt33_buf[alt33_count] = alt33;
    alt33_count ++;
    if(alt33_count > 10){
        alt33_count = 0;
    }
    float alt33_sum = 0;
    for(int i = 0; i < 10; i ++){
        alt33_sum += alt33_buf[i];
    }
    alt33_ave_old = alt33_ave;
    alt33_ave = alt33_sum / 10;
    
    if(alt33_ave - alt33_ave_old > 0.01 || alt33_ave - alt33_ave_old < -0.01){
        speed33 = (alt33_ave - alt33_ave_old) / (((float)flight_time_read - (float)flight_time_read_old) / 1000);
    }
    if(rocket_phase <= ROCKET_WAIT){
        speed33 = 0.0f;
    }
    
    speed33_buf[speed33_count] = speed33;
    speed33_count ++;
    if(speed33_count > 10){
        speed33_count = 0;
    }
    float speed33_sum = 0;
    for(int i = 0; i < 10; i ++){
        speed33_sum += speed33_buf[i];
    }
    speed33_ave = speed33_sum / 10;
    
    
    if(rocket_phase <= ROCKET_WAIT){
        pres22_0 = pres22;
        temp22_0 = temp22;
        pres33_0 = pres33;
        temp33_0 = temp33;
    }
    
    SHT35.getTempHum(&temp35, &hum35);
    
    lux = TSL2561.getLuminous();
    
    /*pitot_raw = Pitot.read() * 3.3;
    pitot = (pitot_raw - pitot_err - 0.2) / (5 * 0.0012858);
    if(pitot < 0.0f){
        pitot = 0.0f;
    }
    pitot_speed = sqrt(2 * pitot * 1000 / 1.3);
    if(rocket_phase <= ROCKET_WAIT){
        pitot_buf[pitot_count] = pitot;
        pitot_count ++;
        if(pitot_count > 100){
            pitot_err = 0;
            for(int i = 0; i < 100; i ++){
                pitot_err += pitot_buf[pitot_count];
            }
            pitot_err = pitot_err / 100;
            pitot_count = 0;
        }
    }*/
    
    flight_pin = FlightPin;
}


/*******************************************************************************
GPS読み取り
*******************************************************************************/
void readGPS(){
    gps_yn = GPS.gps_readable;
    if(gps_yn){
        gps_lat = GPS.Latitude();
        gps_lon = GPS.Longitude();
        gps_alt = GPS.Height();
        GPS.getUTC(gps_utc);
        gps_utc[3] += 9;
        if(gps_utc[3] >= 24){
            gps_utc[3] -= 24;
            gps_utc[2] += 1;
        }
        gps_knot = GPS.Knot();
        gps_deg = GPS.Degree();
        gps_sat = GPS.Number();
    }
}


/*******************************************************************************
PCでデータ表示
*******************************************************************************/
void printData(){
    if(ok_PC){
        pc.printf("Time : %d:%02d\r\n", time_min, time_sec);
        pc.printf("FlightTime : %d:%02d\r\n", flight_time_min, flight_time_sec);
        
        pc.printf("Phase : ");
        switch(rocket_phase){
            case ROCKET_SETUP:
            pc.printf("SETUP\r\n");
            break;
            case ROCKET_SAFETY:
            pc.printf("SAFETY\r\n");
            break;
            case ROCKET_WAIT:
            pc.printf("WAIT\r\n");
            break;
            case ROCKET_FLIGHT:
            pc.printf("FLIGHT\r\n");
            break;
            case ROCKET_SEP1:
            pc.printf("SEPARATE 1\r\n");
            break;
            case ROCKET_SEP2:
            pc.printf("SEPARATE 2\r\n");
            break;
            case ROCKET_RECOVERY:
            pc.printf("RECOVERY\r\n");
            break;
        }
        
        pc.printf("Power : ");
        if(ex_power){
            pc.printf("External\r\n");
        }
        else{
            pc.printf("Battery\r\n");
        }
        
        pc.printf("Separate : %d, %d\r\n", Separate1.status, Separate2.status);
        pc.printf("UP DOWN TOP : %d, %d, %d\r\n", ok_up, ok_down, ok_top);
        pc.printf("SAVE : %d\r\n", save);
        pc.printf("Flight Pin : %d\r\n", flight_pin);
        
        pc.printf("GPS : %3.7f, %3.7f, %.2f\r\n", gps_lat, gps_lon, gps_alt);
        pc.printf("Move : %.2f[knot], %.2f[deg]\r\n", gps_knot, gps_deg);
        pc.printf("Sats : %d\r\n", gps_sat);
        
        pc.printf("INA_in : %.2f[V], %.2f[A]\r\n", ina_in_v, ina_in_i);
        pc.printf("INA_ex : %.2f[V], %.2f[A]\r\n", ina_ex_v, ina_ex_i);
        
        pc.printf("LPS33HW : %.4f[hPa], %.2f[m], %.2f[m/s]\r\n", pres33, alt33, speed33);
        pc.printf("SHT35 : %.2f[degC], %.2f[%%]\r\n", temp35, hum35);
       // pc.printf("Pitot : %.2f[m/s]\r\n", pitot_speed);
        
        pc.printf("\n\n\n");
    }
}


/*******************************************************************************
PCからコマンド読み取り
*******************************************************************************/
void readPC(){
    if(ok_PC){
        char command = pc.getc();
        pc.printf("PC Command = %c\r\n", command);
        
        switch(command){
            case '?':
            printHelp();
            break;
            
            case 'W':                           //離床検知モードへ移行
            if(rocket_phase == ROCKET_SAFETY){
                rocket_phase = ROCKET_WAIT;
                do_first = false;
            }
            break;
            
            case 'S':                           //安全モードへ移行
            if(rocket_phase == ROCKET_WAIT){
                rocket_phase = ROCKET_SAFETY;
                do_first = false;    
            }
            if(rocket_phase == ROCKET_FLIGHT){
                rocket_phase = ROCKET_SAFETY;
                do_first = false;
                timeout_sep.detach();
            }
            break;
            
            case 'F':                           //フライトモードへ移行（手動）
            if(rocket_phase == ROCKET_WAIT){
                rocket_phase = ROCKET_FLIGHT;
                do_first = false;
            }
            break;
            
            case '1':                           //1段目強制分離
            if(rocket_phase == ROCKET_FLIGHT){
                rocket_phase = ROCKET_SEP1;
                do_first = false;
            }
            else if(rocket_phase >= ROCKET_SEP1){
                separate1();
            }
            break;
            
            case '2':                           //2段目強制分離
            if(rocket_phase == ROCKET_SEP1){
                rocket_phase = ROCKET_SEP2;
                do_first = false;
            }
            else if(rocket_phase >= ROCKET_SEP2){
                separate2();
            }
            break;
            
            case 'C':
            if(rocket_phase == ROCKET_RECOVERY || rocket_phase == ROCKET_SAFETY && save){
                stopRec();
            }
            break;
            
            case 'O':
            if(rocket_phase == ROCKET_RECOVERY || rocket_phase == ROCKET_SAFETY && !save){
                startRec();
            }
            break;
            
            case 'B':
            buzzerChange();
            break;
        }
    }
}


/*******************************************************************************
コマンド情報を表示
*******************************************************************************/
void printHelp(){
    for(int i = 0; i < 20; i ++){
        pc.printf("*");
    }
    
    pc.printf("\r\nCommands\r\n");
    pc.printf("W      : Safety -> Wait\r\n");
    pc.printf("S      : Wait -> Safety\r\n");
    pc.printf("F      : Wait -> Flight\r\n");
    pc.printf("1(one) : 1st Separate 5[s]\r\n");
    pc.printf("2(two) : 2nd Separate 5[s]\r\n");
    pc.printf("C      : Stop Recording\r\n");
    pc.printf("?      : HELP\r\n");
    
    for(int i = 0; i < 20; i ++){
        pc.printf("*");
    }
    pc.printf("\r\n");
    wait(1.0f);
}


/*******************************************************************************
ダウンリンクを表示
*******************************************************************************/
void sendDownLink(){
    //無線あり
    if(!es920_using){
        es920 << HEADER_DATA;
        
        es920 << (short)time_reset;
        es920 << (short)(time_read / 1000);
        
        es920 << (short)flight_time_reset;
        es920 << (short)(flight_time_read / 1000);
        
        es920 << rocket_phase;
        
        char status = 0x00;
        if(ex_power)        status |= (char)0x01;
        if(Separate1.status)status |= (char)0x02;
        if(Separate2.status)status |= (char)0x04;
        if(ok_up)           status |= (char)0x08;
        if(ok_down)         status |= (char)0x10;
        if(ok_top)          status |= (char)0x20;
        if(save)            status |= (char)0x40;
        if(flight_pin)      status |= (char)0x80;
        es920 << status;
        
        es920 << (short)(gps_lat / ES920_MAX_100);
        es920 << (short)(gps_lon / ES920_MAX_500);
        es920 << (short)(gps_alt / ES920_MAX_500);
        es920 << (short)(gps_knot / ES920_MAX_200);
        es920 << (short)(gps_deg / ES920_MAX_1500);
        es920 << (short)gps_sat;
        
        es920 << (short)(ina_in_v / ES920_MAX_20);
        es920 << (short)(ina_in_i / ES920_MAX_20);
        es920 << (short)(ina_ex_v / ES920_MAX_20);
        es920 << (short)(ina_ex_i / ES920_MAX_20);
        
        es920 << (short)(pres33 / ES920_MAX_3000);
        es920 << (short)(alt33_ave / ES920_MAX_500);
        es920 << (short)(speed33_ave / ES920_MAX_200);
        es920 << (short)(temp35 / ES920_MAX_100);
        es920 << (short)(hum35 / ES920_MAX_200);
        
        es920 << (short)(pitot_speed / ES920_MAX_200);
        
        es920.send();
    }
    else{
        es920_using = false;
    }
}


/*******************************************************************************
アップリンクを解析
*******************************************************************************/
void readUpLink(){
    //無線あり
    es920_uplink_command = es920.data[0];
    pc.printf("GND Command = %c\r\n", es920_uplink_command);
    
    switch(es920_uplink_command){
        case 'W':                           //離床検知モードへ移行
        if(rocket_phase == ROCKET_SAFETY){
            rocket_phase = ROCKET_WAIT;
            do_first = false;
        }
        break;
        
        case 'S':                           //安全モードへ移行
        if(rocket_phase == ROCKET_WAIT){
            rocket_phase = ROCKET_SAFETY;
            do_first = false;
        }
        if(rocket_phase == ROCKET_FLIGHT){
            rocket_phase = ROCKET_SAFETY;
            do_first = false;
            timeout_sep.detach();
        }
        
        break;
        
        case 'F':                           //フライトモードへ移行（手動）
        if(rocket_phase == ROCKET_WAIT){
            rocket_phase = ROCKET_FLIGHT;
            do_first = false;
        }
        break;
        
        case '1':                           //1段目強制分離
        if(rocket_phase == ROCKET_FLIGHT){
            rocket_phase = ROCKET_SEP1;
            do_first = false;
        }
        else if(rocket_phase >= ROCKET_SEP1){
            es920_using = true;
            es920 << HEADER_START;
            es920 << '1';
            es920.send();
            
            separate1();
        }
        break;
        
        case '2':                           //2段目強制分離
        if(rocket_phase == ROCKET_SEP1){
            rocket_phase = ROCKET_SEP2;
            do_first = false;
        }
        else if(rocket_phase >= ROCKET_SEP2){
            es920_using = true;
            es920 << HEADER_START;
            es920 << '2';
            es920.send();
            
            separate2();
        }
        break;
        
        case 'C':
        if(rocket_phase == ROCKET_RECOVERY || rocket_phase == ROCKET_SAFETY && save){
            stopRec();
        }
        break;
        
        case 'O':
        if(rocket_phase == ROCKET_RECOVERY || rocket_phase == ROCKET_SAFETY && !save){
            startRec();
        }
        break;
        
        case 'B':
        buzzerChange();
        break;
    }
}


/*******************************************************************************
ログ記録開始
*******************************************************************************/
void startRec(){
    fp = fopen(file_name, "a");
    fprintf(fp, "Time,Flight Time,Phase,Flight Pin,Power,Separate1,Separate2,");
    fprintf(fp, "upper100,under30,top,uplink,");
    fprintf(fp, "GPS Y/N,GPS Time,Latitude,Longitude,GPS Alt,Knot,Deg,Sattelite,");
    fprintf(fp, "ADXL_x[G],ADXL_y[G],ADXL_z[G],In_V[V],In_I[A],Ex_V[V],Ex_I[A],");
    fprintf(fp, "Gyro_x[deg],Gyro_y[deg],Gyro_z[deg],Acc_x[G],Acc_y[G],Acc_z[G],Mag_x[deg],Mag_y[deg],Mag_z[deg],");
    fprintf(fp, "Q_w,Q_x,Q_y,Q_z,Roll[deg],Pitch[deg],Yaw[deg],");
    fprintf(fp, "Pres_22[hPa],Temp_22[degC],Alt_22[m],Pres_33[hPa],Temp_33[degC],Alt_33[m],Alt_33_ave[m],Speed_33[m/s],Speed_33_ave[m/s],");
    fprintf(fp, "Temp_35[degC],Humid_35[%],Lux[lux],Pitot_raw[V]\r\n");
    save = true;
}


/*******************************************************************************
ログ記録終了
*******************************************************************************/
void stopRec(){
    save = false;
    fprintf(fp, "\r\n\n");
    fclose(fp);
}


/*******************************************************************************
データ書き込み
*******************************************************************************/
void recData(){
    if(save){
        fprintf(fp, "%d:%02d.%03d,", time_min, time_sec, time_ms);
        fprintf(fp, "%d:%02d.%03d,", flight_time_min, flight_time_sec, flight_time_ms);
        
        switch(rocket_phase){
            case ROCKET_SETUP:
            fprintf(fp, "SETUP,");
            break;
            case ROCKET_SAFETY:
            fprintf(fp, "SAFETY,");
            break;
            case ROCKET_WAIT:
            fprintf(fp, "WAIT,");
            break;
            case ROCKET_FLIGHT:
            fprintf(fp, "FLIGHT,");
            break;
            case ROCKET_SEP1:
            fprintf(fp, "SEPARATE1,");
            break;
            case ROCKET_SEP2:
            fprintf(fp, "SEPARATE2,");
            break;
            case ROCKET_RECOVERY:
            fprintf(fp, "RECOVERY,");
            break;
        }
        
        fprintf(fp, "%d,", flight_pin);
        
        if(ex_power){
            fprintf(fp, "External,");
        }
        else{
            fprintf(fp, "Battery,");
        }
        
        fprintf(fp, "%d,%d,", Separate1.status, Separate2.status);
        fprintf(fp, "%d,%d,%d,", ok_up, ok_down, ok_top);
        
        fprintf(fp, "%c,", es920_uplink_command);
        es920_uplink_command = '-';
        
        fprintf(fp, "%d,", gps_yn);
        fprintf(fp, "%d/%d/%d ", (int)gps_utc[0], (int)gps_utc[1], (int)gps_utc[2]);  //日付
        fprintf(fp, "%d:%02d:%02.2f,", (int)gps_utc[3], (int)gps_utc[4], gps_utc[5]); //時間
        fprintf(fp, "%3.7f,%3.7f,%.2f,", gps_lat, gps_lon, gps_alt);      //GPS座標
        fprintf(fp, "%.2f,%.2f,", gps_knot, gps_deg);                     //GPS速度
        fprintf(fp, "%d,", gps_sat);                                      //GPS衛星数
        
        fprintf(fp, "%.2f,%.2f,%.2f,", adxl_acc[0], adxl_acc[1], adxl_acc[2]);
        fprintf(fp, "%.4f,%.4f,%.4f,%.4f,", ina_in_v, ina_in_i, ina_ex_v, ina_ex_i);
        fprintf(fp, "%f,%f,%f,", gyro[0], gyro[1], gyro[2]);
        fprintf(fp, "%f,%f,%f,", acc[0], acc[1], acc[2]);
        fprintf(fp, "%.2f,%.2f,%.2f,", mag[0], mag[1], mag[2]);
        fprintf(fp, "%f,%f,%f,%f,", quart.w, quart.x, quart.y, quart.z);
        fprintf(fp, "%f,%f,%f,", euler[0], euler[1], euler[2]);
        fprintf(fp, "%f,%.2f,%.2f,", pres22, temp22, alt22);
        fprintf(fp, "%f,%.2f,%.2f,%.2f,%.2f,%2f,", pres33, temp33, alt33, alt33_ave, speed33, speed33_ave);
        fprintf(fp, "%.2f,%.2f,", temp35, hum35);
        fprintf(fp, "%d,", lux);
        //fprintf(fp, "%f,%f,%f\r\n", pitot_raw, pitot, pitot_speed);
        
        save_c ++;
        if(save_c > 10000){
            fclose(fp);
            fp = fopen(file_name, "a");
            save_c = 0;
        }
    }
}


/*******************************************************************************
状態遷移の判断
*******************************************************************************/
void modeChange(){
    //無線あり
    switch(rocket_phase){
        case ROCKET_SAFETY://///////////////安全モード
        if(!do_first){
            es920_using = true;
            es920 << HEADER_START;
            es920 << 'S';
            es920.send();
            
            do_first = true;
        }
        break;
        
        case ROCKET_WAIT://////////////////離床検知モード
        if(!do_first){
            es920_using  = true;
            es920 << HEADER_START;
            es920 << 'W';
            es920.send();
            
            
            if(!save){
                startRec();
            }
            
            do_first = true;
        }
        if(flight_pin){
            rocket_phase = ROCKET_FLIGHT;
            do_first = false;
        }
        break;
        
        case ROCKET_FLIGHT:///////////////フライトモード
        if(!do_first){
            es920_using = true;
            es920 << HEADER_START;
            es920 << 'F';
            es920.send();
            
            if(!save){
                startRec();
            }
            
            time_flight.reset();
            time_flight.start();
            
            timeout_sep.attach(&changePhase_SEP1, TIME_SEP1);
            timeout_recovery.attach(&changePhase_RECOVERY, TIME_RECOVERY);
            
            do_first = true;
        }
        if(alt33_ave > SEP1_ALT_UNDER && speed33_ave < SEP1_SPEED){
            ok_top = true;
            
            rocket_phase = ROCKET_SEP1;
            do_first = false;
        }
        break;
        
        case ROCKET_SEP1:////////////////分離1モード
        if(!do_first){
            es920_using = true;
            es920 << HEADER_START;
            es920 << '1';
            es920.send();
            
            timeout_sep.detach();
            
            stopRec();
            
            startRec();
            
            separate1();
            
            timeout_sep.attach(&changePhase_SEP2, TIME_SEP2 - TIME_SEP1);
            
            do_first = true;
        }
        if(alt33_ave < SEP2_ALT_UP && speed33_ave < 0.0f && ok_up){
            rocket_phase = ROCKET_SEP2;
            do_first = false;
        }
        break;
        
        case ROCKET_SEP2:////////////////分離2モード
        if(!do_first){
            es920_using = true;
            es920 << HEADER_START;
            es920 << '2';
            es920.send();
            
            timeout_sep.detach();
            
            separate2();
            
            do_first = true;
        }
        break;
        
        case ROCKET_RECOVERY:////////////回収モード
        if(!do_first){
            stopRec();
            startRec();
            
            Buzzer = 1;
            
            do_first = true;
        }
        break;
    }
    
    if(rocket_phase >= ROCKET_FLIGHT){//////////////////フライトモード以降共通
        if(alt33_ave > SEP1_ALT_UNDER && speed33_ave < SEP1_SPEED){
            ok_top = true;
        }
        
        if(!ok_up && alt33_ave > OK_UP_ALT){
            ok_up = true;
        }
        if(ok_up && !ok_down && alt33_ave < OK_DOWN_ALT){
            ok_down = true;
        }
        if(ok_up && ok_down && speed33_ave > STOP_SPEED_UNDER && speed33_ave < STOP_SPEED_UP){
            changePhase_RECOVERY();
        }
    }
}

void changePhase_SEP1(){/////////////分離1モードへ
    rocket_phase = ROCKET_SEP1;
    do_first = false;
}

void changePhase_SEP2(){/////////////分離2モードへ
    rocket_phase = ROCKET_SEP2;
    do_first = false;
}

void changePhase_RECOVERY(){/////////回収モードへ
    rocket_phase = ROCKET_RECOVERY;
    do_first = false;
}


/*******************************************************************************
ブザーのON/OFF
*******************************************************************************/
void buzzerChange(){
    if(Buzzer){
        Buzzer = 0;
    }
    else{
        Buzzer = 1;
    }
}


/*******************************************************************************
分離機構1段目作動
*******************************************************************************/
void separate1(){
    if(!Separate1.status && rocket_phase >= ROCKET_FLIGHT){
        Separate1.fire(6.0f);
        pc.printf("1st Separation\r\n");
    }
}


/*******************************************************************************
分離機構2段目作動
*******************************************************************************/
void separate2(){
    if(!Separate2.status && rocket_phase >= ROCKET_FLIGHT){
        Separate2.fire(5.0f);
        pc.printf("2nd Separation\r\n");
    }
}


/*******************************************************************************
セットアップ（最初に1回実行）
*******************************************************************************/
//無線あり(HEADER_SETUP)
void setup(){
    wait(1.0f);
    char setup_string[1024];
    
    pc.printf("\r\n**************************************************\r\n");
    pc.printf("Sensor Setting Start!!\r\n");
    strcat(setup_string, "Sensor Setting Start!!\r\n");
    es920 << HEADER_SENSOR_SETUP;
    
    ///////////////////////////////////////////ADXL375
    ADXL375.setDataRate(ADXL375_100HZ);
    if(ADXL375.whoAmI() == 1){
        pc.printf("ADXL375 : OK\r\n");
        strcat(setup_string, "ADXL375 : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("ADXL375 : NG...\r\n");
        strcat(setup_string, "ADXL375 : NG...\r\n");
        es920 << (char)0x00;
    }
    ADXL375.offset(0.0f, 0.0f, 0.0f);
    
    ///////////////////////////////////////////INA226_in
    INA226_in.set_callibretion();
    if(INA226_in.Connection_check() == 0){
        pc.printf("INA226_in : OK!!\r\n");
        strcat(setup_string, "INA_in : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("INA226_in : NG...\r\n");
        strcat(setup_string, "INA_in : NG...\r\n");
        es920 << (char)0x00;
    }
    
    ///////////////////////////////////////////INA226_ex
    INA226_ex.set_callibretion();
    if(INA226_ex.Connection_check() == 0){
        pc.printf("INA226_ex OK!!\r\n");
        strcat(setup_string, "INA_ex : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("INA226_ex NG...\r\n");
        strcat(setup_string, "INA_ex : NG...\r\n");
        es920 << (char)0x00;
    }

    ///////////////////////////////////////////MPU9250
    nine.setAcc(_16G);
    nine.setGyro(_2000DPS);
    nine.setOffset(0.58424, 0.622428, 0.063746,
                   0.008125, -0.00106, 0.0024145,
                   -2.25, 3.825, -24.75);
    
    if(nine.senserTest()){
        pc.printf("MPU9250 : OK!!\r\n");
        strcat(setup_string, "MPU9250 : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("MPU9250 : NG...\r\n");
        strcat(setup_string, "MPU9250 : NG...\r\n");
        es920 << (char)0x00;
    }
    if(nine.mag_senserTest()){
        pc.printf("MPU9250 MAG : OK!!\r\n");
        strcat(setup_string, "MPU9250 MAG : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("MPU9250 MAG : NG...\r\n");
        strcat(setup_string, "MPU9250 MAG : NG...\r\n");
        es920 << (char)0x00;
    }
    
    ///////////////////////////////////////////LPS22HB
    LPS22HB.begin(50);
    if(LPS22HB.whoAmI() == 0){
        pc.printf("LPS22HB : OK!!\r\n");
        strcat(setup_string, "LPS22HB : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("LPS22HB : NG...\r\n");
        strcat(setup_string, "LPS22HB : NG...\r\n");
        es920 << (char)0x00;
    }
    
    ///////////////////////////////////////////LPS33HW
    LPS33HW.begin(50);
    if(LPS33HW.whoAmI() == 0){
        pc.printf("LPS33HW : OK!!\r\n");
        strcat(setup_string, "LPS33HW : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("LPS33HW : NG...\r\n");
        strcat(setup_string, "LPS33HW : NG...\r\n");
        es920 << (char)0x00;
    }
    
    ///////////////////////////////////////////SHT35
    int sht_state = SHT35.getState();
    if(sht_state == 32784 || sht_state == 32848){
        pc.printf("SHT35 : OK!!\r\n");
        strcat(setup_string, "SHT35 : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("SHT35 : NG...\r\n");
        strcat(setup_string, "SHT35 : NG...\r\n");
        es920 << (char)0x00;
    }
    
    ///////////////////////////////////////////TSL2561
    TSL2561.begin();
    //lux_time = TSL2561.setRate(1);
    if(TSL2561.connectCheck() == 1){
        pc.printf("TSL2561 : OK!!\r\n");
        strcat(setup_string, "TSL2561 : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("SL2561 : NG...\r\n");
        strcat(setup_string, "TSL2561 : NG...\r\n");
        es920 << (char)0x00;
    }
    
    ///////////////////////////////////////////SD
    fp = fopen("/sd/Felix_Luminous_setup.txt", "r");
    if(fp != NULL){
        pc.printf("SD : OK!!\r\n");
        strcat(setup_string, "SD : OK!!\r\n");
        es920 << (char)0x01;
        fclose(fp);
    }
    else{
        pc.printf("SD : NG...\r\n");
        strcat(setup_string, "SD : NG...\r\n");
        es920 << (char)0x00;
    }
    
    pc.printf("Sensor Setting Finished!!\r\n");
    pc.printf("**************************************************\r\n");
    strcat(setup_string, "Sensor Setting Finished!!\r\n");
    es920.send();
    wait(1.0f);
    
    pc.printf("\r\n**************************************************\r\n");
    if(wait_GPS){
        pc.printf("GPS Setting Start!!\r\n");
        strcat(setup_string, "GPS Setting Start!!\r\n");
        
        pc.printf("Waiting : 0");
        strcat(setup_string, "Wait : ");
        int gps_wait_count = 0;
        while(!GPS.gps_readable){
            pc.printf("\rWaiting : %d", gps_wait_count);//
            es920 << HEADER_GPS_SETUP;
            es920 << (char)GPS.gps_readable;
            es920 << (int)gps_wait_count;
            es920.send();
            wait(1.0f);
            gps_wait_count ++;
        }
        char ss[8];
        sscanf(ss, "%d", &gps_wait_count);
        strcat(setup_string, ss);
        pc.printf("  OK!!\r\n");
        strcat(setup_string, " OK!!\r\n");
        pc.printf("GPS Setting Finished!!\r\n");
        strcat(setup_string, "GPS Setting Finished!!\r\n");
        es920 << HEADER_GPS_SETUP;
        es920 << (char)0xAA;
        es920 << (int)0;
        es920.send();
    }
    else{
        pc.printf("GPS Setting Ignore...\r\n");
        strcat(setup_string, "GPS Setting Ignore...\r\n");
        es920 << HEADER_GPS_SETUP;
        es920 << (char)0xFF;
        es920 << (int)0;
        es920.send();
    }
    pc.printf("\r\n**************************************************\r\n");
    fp = fopen(file_name, "a");
    fprintf(fp, setup_string);
    fclose(fp);
    setup_string[0] = NULL;
    
    printHelp();
    wait(1.0f);
}