File content as of revision 8:2ce2c7eebb92:

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

#include "Adafruit_ADS1015.h"
#include "ADXL375_i2c.h"
#include "BME280_lib.h"
#include "BNO055_lib.h"
#include "CCS811_lib.h"
#include "EEPROM_lib.h"
#include "ES920LR.hpp"
#include "GPS_interrupt.h"
#include "INA226.h"
#include "Nichrome_lib.h"
#include "SDFileSystem.h"


/*******************************************************************************
設定値
投下前に必ず確認！！
*******************************************************************************/
const float START_CONTROLL_TIME = 1.0f;
const float START_CONTROLL_ALT = -5.0f;

const float RECOVERY_TIME = 120.0f;
const float RECOVERY_SPEED = -2.0f;

bool wait_GPS = true;
bool ok_PC = false;

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

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

Serial GPS_serial(p13, p14, 9600);
GPS_interrupt GPS(&GPS_serial);
float GPS_freq = 1;

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

I2C i2c_bus(p28, p27);
ADXL375_i2c ADXL375(i2c_bus, ADXL375_i2c::ALT_ADDRESS_HIGH);
myINA226 INA226(i2c_bus, myINA226::A1_VDD, myINA226::A0_VDD);
BME280_lib BME280(i2c_bus, BME280_lib::AD0_LOW);
BNO055_lib BNO055(i2c_bus, BNO055_lib::AD0_HIGH);
CCS811_lib CCS811(i2c_bus, CCS811_lib::AD0_LOW);
EEPROM_lib EEPROM(i2c_bus, 4);

Adafruit_ADS1115 ADS1115(&i2c_bus, ADS1015_ADDRESS);

Nichrome_lib Valve1(p23);
Nichrome_lib Valve2(p24);
Nichrome_lib Valve3(p25);
Nichrome_lib Valve4(p26);
Nichrome_lib Buzzer(p22);

DigitalIn FlightPin(p15);

Timeout timeout_start_controll;
Timeout timeout_recovery;

Timer time_main;
Timer time_flight;

Ticker tick_gps;
Ticker tick_print;
Ticker tick_save;
Ticker tick_header_data;


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

void modeChange();  //無線あり
void changePhase_RECOVERY();
void startControllAttitude();

void controllAttitude();

void readSensor();
void readGPS();

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

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

void startRecSlow();
void startRecFast();
void stopRec();
void recData();

void buzzerChange();


/*******************************************************************************
変数の宣言
*******************************************************************************/
char CanSat_phase;
bool do_first = false;
bool controll_yn = false;
bool start_controll_once = false;

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

float adxl_acc[3];

float ina_v, ina_i;

double amg[9], quart[4], euler[3];

float pres, temp, hum, alt, pres_0, temp_0;
float pres_now;
int speed_time_old;
float alt_buf[10], alt_ave, alt_ave_old, speed, speed_buf[10], speed_ave;
int alt_count = 0, speed_count = 0;
float alt_flight_start;

int CO2, TVOCs;

int16_t bottle_pres_bit;
float bottle_pres;

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_slow = false;
bool save_fast = false;
int save_c = 0;

int eeprom_ptr;

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_2 = 0.0000610370;
const float ES920_MAX_20 = 0.0006103702;
const float ES920_MAX_50 = 0.0015259255;
const float ES920_MAX_100 = 0.0030518509;
const float ES920_MAX_200 = 0.0061037019;
const float ES920_MAX_360 = 0.0109866634;
const float ES920_MAX_500 = 0.0152592547;
const float ES920_MAX_1500 = 0.0457777642;


/*******************************************************************************
無線のヘッダまとめ(CANSAT -> GND)
*******************************************************************************/
const char HEADER_SENSOR_SETUP = 0x01;
// 0x01, ADXL, INA, BME, BNO, CCS, PRES, SD
// 0     1     1    1    1    1    1     1
//       0     1    2    3    4    5     6

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;
// 0x10, 起動時間, フライト時間, フェーズ, status, valve, lat, lon, alt, knot, deg, sat, 
// 0     4(2,2)   4(2,2)      1        1      1       4    4    2    2     2    2
//         0 2      4 6       8        9      10      11   15   19   21    23   25
//                                     0x01 : save_slow
//                                     0x02 : save_fast
//                                     0x04 : controll_yn
//                                     0x08 : flight_pin
//                                     0x10 : gps_yn
//                                     0x20 : Buzzer

// V, I, pres, temp, alt, speed, eulerX3, CO2, TVOCs, bottle
// 2  2  2     2     2    2      6(2,2,2) 2    2      2
// 27 29 31    33    35   37       394143 45   47     49

const char HEADER_START = 0x11;
// 0x11, What
// 0     1
//       'W' : 投下検知モード
//       'S' : 安全モード
//       'F' : フライトモード
//       'R' : 回収モード


/*******************************************************************************
無線のヘッダまとめ(GND -> CANSAT)
*******************************************************************************/
const char HEADER_COMMAND = 0xcd;
// 0xcd, command
// 0     1
//       0


/*******************************************************************************
CanSatのフェーズ
*******************************************************************************/
const char CANSAT_SETUP = 0x00;     //セットアップ中
const char CANSAT_SAFETY = 0x01;    //安全モード
const char CANSAT_WAIT = 0x02;      //待機モード
const char CANSAT_FLIGHT = 0x03;    //フライトモード
const char CANSAT_RECOVERY = 0x04;  //回収モード


/*******************************************************************************
メイン関数
*******************************************************************************/
int main() {
    CanSat_phase = CANSAT_SETUP;
    setup();
    
    pc.attach(&readPC, Serial::RxIrq);
    
    tick_gps.attach(&readGPS, 1.0/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();
    startRecSlow();
    
    CanSat_phase = CANSAT_SAFETY;
    while(1) {
        readSensor();
        modeChange();   //状態遷移の判断
        controllAttitude();
        if(save_fast){
            recData();
        }
    }
}


/*******************************************************************************
状態遷移の判断
無線あり：HEADER_START
*******************************************************************************/
void modeChange(){
    switch(CanSat_phase){
        case CANSAT_SAFETY:////////////////////安全モード
        if(!do_first){
            es920_using = true;
            es920 << HEADER_START;
            es920 << 'S';
            es920.send();
            es920_using = false;
            
            do_first = true;
        }
        break;
        
        case CANSAT_WAIT:////////////////////待機モード
        if(!do_first){
            es920_using = true;
            es920 << HEADER_START;
            es920 << 'W';
            es920.send();
            es920_using = false;
            
            do_first = true;
        }
        if(flight_pin){
            CanSat_phase = CANSAT_FLIGHT;
            do_first = false;
        }
        break;
        
        case CANSAT_FLIGHT:////////////////////フライトモード
        if(!do_first){
            es920_using = true;
            es920 << HEADER_START;
            es920 << 'F';
            es920.send();
            es920_using = false;
            
            if(save_slow && !save_fast){
                stopRec();
                startRecFast();
            }
            else if(!save_slow && !save_fast){
                startRecFast();
            }
            
            time_flight.reset();
            time_flight.start();
            
            alt_flight_start = alt;
            timeout_start_controll.attach(&startControllAttitude, START_CONTROLL_TIME);
            
            timeout_recovery.attach(&changePhase_RECOVERY, RECOVERY_TIME);
            
            do_first = true;
        }
        if(alt - alt_flight_start < START_CONTROLL_ALT && !start_controll_once){
            timeout_start_controll.detach();
            startControllAttitude();
            start_controll_once = true;
        }
        /*
        if(speed_ave > RECOVERY_SPEED && alt < 5.0f){
            timeout_recovery.detach();
            changePhase_RECOVERY();
        }
        */
        break;
        
        case CANSAT_RECOVERY:
        if(!do_first){
            es920_using = true;
            es920 << HEADER_START;
            es920 << 'R';
            es920.send();
            es920_using = false;
            
            stopRec();
            startRecSlow();
            
            controll_yn = false;
            
            Buzzer.fire_on();
            
            do_first = true;
        }
        break;
    }
}

void changePhase_RECOVERY(){
    CanSat_phase = CANSAT_RECOVERY;
    do_first = false;
}

void startControllAttitude(){
    controll_yn = true;
}


/*******************************************************************************
姿勢制御
*******************************************************************************/
void controllAttitude(){
    if(controll_yn){
        if(euler[1] > 10){
            Valve1.fire_on();
            Valve3.fire_off();
        }
        else if(euler[1] < -10){
            Valve1.fire_off();
            Valve3.fire_on();
        }
        else{
            Valve1.fire_off();
            Valve3.fire_off();
        }
    }
    
    else{
        Valve1.fire_off();
        Valve2.fire_off();
        Valve3.fire_off();
        Valve4.fire_off();
    }
}


/*******************************************************************************
センサー読み取り
*******************************************************************************/
void readSensor(){
    time_read = time_main.read_ms();
    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(time_read >= 30*60*1000){
        time_main.reset();
        time_reset ++;
    }
    
    if(CanSat_phase >= CANSAT_FLIGHT){
        flight_time_read_old = flight_time_read;
        flight_time_read = time_flight.read_ms();
    }
    else{
        flight_time_read = 0;
    }
    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;
    if(time_read >= 30*60*1000){
        time_flight.reset();
        flight_time_reset ++;
    }
    
    ADXL375.getOutput(adxl_acc);
    
    INA226.get_Voltage_current(&ina_v, &ina_i);
    ina_v = ina_v / 1000;
    ina_i = ina_i / 1000;
    if(ina_i > 64){
        ina_i = 0.0f;
    }
    
    BME280.getData(&temp, &pres_now, &hum);
    if(pres_now != pres){
        pres = pres_now;
        alt = BME280.getAlt2(pres_0, temp_0);
        
        alt_buf[alt_count] = alt;
        alt_count ++;
        if(alt_count > 10){
            alt_count = 0;
        }
        float alt_sum = 0;
        for(int i = 0; i < 10; i ++){
            alt_sum += alt_buf[i];
        }
        alt_ave_old = alt_ave;
        alt_ave = alt_sum / 10;
        
        if(fabs(alt_ave - alt_ave_old) > 0.01){
            speed = (alt_ave - alt_ave_old) / (((float)flight_time_read - (float)speed_time_old) / 1000);
        }
        if(CanSat_phase <= CANSAT_WAIT){
            speed = 0.0f;
        }
        
        speed_buf[speed_count] = speed;
        speed_count ++;
        if(speed_count > 10){
            speed_count = 0;
        }
        float speed_sum = 0;
        for(int i = 0; i < 10; i ++){
            speed_sum = speed_buf[i];
        }
        speed_ave = speed_sum / 10;
        
        speed_time_old = flight_time_read;
    }
    
    if(CanSat_phase < CANSAT_WAIT){
        pres_0 = pres;
        temp_0 = temp;
    }
    
    BNO055.getAMG(amg);
    BNO055.getQuart(quart);
    BNO055.getEulerFromQ(euler);
    
    CCS811.getData(&CO2, &TVOCs);
    
    bottle_pres_bit = ADS1115.readADC_SingleEnded(0);
    bottle_pres_bit = (float)bottle_pres_bit * 0.125f * 3.0f / 2.0f;
    bottle_pres = (float)bottle_pres_bit/1000.0f / 4.0f - 0.25f;
    
    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();
    }
}


/*******************************************************************************
データ表示
*******************************************************************************/
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(CanSat_phase){
            case CANSAT_SETUP:
            pc.printf("SETUP\r\n");
            break;
            case CANSAT_SAFETY:
            pc.printf("SAFETY\r\n");
            break;
            case CANSAT_WAIT:
            pc.printf("WAIT\r\n");
            break;
            case CANSAT_FLIGHT:
            pc.printf("FLIGHT\r\n");
            break;
            case CANSAT_RECOVERY:
            pc.printf("RECOVERY\r\n");
            break;
        }
        
        pc.printf("SAVE : ");
        if(save_slow){
            pc.printf("SLOW\r\n");
        }
        else if(save_fast){
            pc.printf("FAST\r\n");
        }
        else{
            pc.printf("STOP\r\n");
        }
        
        pc.printf("Controll : %d\r\n", controll_yn);
        
        pc.printf("Flight Pin : %d\r\n", flight_pin);
        pc.printf("Valve: %d, %d, %d, %d\r\n", Valve1.status, Valve2.status, Valve3.status, Valve4.status);
        pc.printf("Buzzer : %d\r\n", Buzzer.status); 
        
        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("INA226 : %.2f[V], %.2f[A]\r\n", ina_v, ina_i);
        pc.printf("BME280 : %.2f[hPa], %.2f[degC], %.2f[%%], %.2f[m], %.2f[m/s]\r\n", pres, temp, hum, alt_ave, speed_ave);
        pc.printf("BNO055 : %f, %f, %f\r\n", euler[0], euler[1], euler[2]);
        pc.printf("CCS811 : %d[ppm], %d[ppm]\r\n", CO2, TVOCs);
        pc.printf("Bottle : %f[MPa]\r\n", bottle_pres);
        
        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(CanSat_phase == CANSAT_SAFETY){
                CanSat_phase = CANSAT_WAIT;
                do_first = false;
            }
            break;
            
            case 'S':   //安全モードへ移行
            if(CanSat_phase == CANSAT_WAIT || CanSat_phase == CANSAT_FLIGHT){
                CanSat_phase = CANSAT_SAFETY;
                do_first = false;
            }
            break;
            
            case 'F':   //フライトモードへ移行
            if(CanSat_phase == CANSAT_WAIT){
                CanSat_phase = CANSAT_FLIGHT;
                do_first = false;
            }
            break;
            
            case 'C':   //記録停止
            if((CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY) && (save_slow || save_fast)){
                stopRec();
            }
            break;
            
            case 'o':   //低速記録開始
            if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){
                startRecSlow();
            }
            break;
            
            case 'O':   //高速記録開始
            if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){
                startRecFast();
            }
            break;
            
            case 'B':   //ブザー切り替え
            buzzerChange();
            break;
            
            case '1':   //姿勢制御再開
            if(CanSat_phase == CANSAT_FLIGHT && !controll_yn){
                controll_yn = true;
            }
            break;
            
            case '0':   //姿勢制御停止
            if(CanSat_phase == CANSAT_FLIGHT && controll_yn){
                controll_yn = false;
            }
            break;
        }
    }
}


/*******************************************************************************
ヘルプ表示
*******************************************************************************/
void printHelp(){
    pc.printf("\r\n********************\r\n");
    pc.printf("Commands\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("C          : Stop Recording\r\n");
    pc.printf("o(small o) : Start Recording (Slow)\r\n");
    pc.printf("O(large o) : Start Recording (Fast)\r\n");
    pc.printf("B          : Buzzer Change\r\n");
    pc.printf("1(one)     : Restart Controll Attitude\r\n");
    pc.printf("0(zero)    : Stop Controll Attitude\r\n");
    
    pc.printf("********************\r\n\n");
    wait(1.0f);
}


/*******************************************************************************
ダウンリンク送信
無線あり：HEADER_DATA
*******************************************************************************/
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 << (char)CanSat_phase;
        
        char status = 0x00;
        if(save_slow)     status |= (char)0x01;
        if(save_fast)     status |= (char)0x02;
        if(controll_yn)   status |= (char)0x04;
        if(flight_pin)    status |= (char)0x08;
        if(gps_yn)        status |= (char)0x10;
        if(Buzzer.status) status |= (char)0x20;
        es920 << status;
        
        status = 0x00;
        if(Valve1.status) status |= (char)0x01;
        if(Valve2.status) status |= (char)0x02;
        if(Valve3.status) status |= (char)0x04;
        if(Valve4.status) status |= (char)0x08;
        es920 << status;
        
        es920 << (float)gps_lat;
        es920 << (float)gps_lon;
        es920 << (short)(gps_alt / ES920_MAX_1500);
        es920 << (short)(gps_knot / ES920_MAX_200);
        es920 << (short)(gps_deg / ES920_MAX_360);
        es920 << (short)gps_sat;
        
        es920 << (short)(ina_v / ES920_MAX_20);
        es920 << (short)(ina_i / ES920_MAX_20);
        
        es920 << (short)(pres / ES920_MAX_1500);
        es920 << (short)(temp / ES920_MAX_100);
        es920 << (short)(alt / ES920_MAX_500);
        es920 << (short)(speed / ES920_MAX_50);
        
        es920 << (short)(euler[0] / ES920_MAX_360);
        es920 << (short)(euler[1] / ES920_MAX_360);
        es920 << (short)(euler[2] / ES920_MAX_360);
        
        es920 << (short)CO2;
        
        es920 << (short)(bottle_pres / ES920_MAX_2);
        
        es920.send();
    }
}


/*******************************************************************************
アップリンク解析
無線あり：HEADER_COMMAND（受信）
*******************************************************************************/
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(CanSat_phase == CANSAT_SAFETY){
            CanSat_phase = CANSAT_WAIT;
            do_first = false;
        }
        break;
        
        case 'S':   //安全モードへ移行
        if(CanSat_phase == CANSAT_WAIT || CanSat_phase == CANSAT_FLIGHT){
            CanSat_phase = CANSAT_SAFETY;
            do_first = false;
        }
        break;
        
        case 'F':   //フライトモードへ移行
        if(CanSat_phase == CANSAT_WAIT){
            CanSat_phase = CANSAT_FLIGHT;
            do_first = false;
        }
        break;
        
        case 'C':   //記録停止
        if((CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY) && (save_slow || save_fast)){
            stopRec();
        }
        break;
        
        case 'o':   //低速記録開始
        if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){
            startRecSlow();
        }
        break;
        
        case 'O':   //高速記録開始
        if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){
            startRecFast();
        }
        break;
        
        case 'B':   //ブザー切り替え
        buzzerChange();
        break;
        
        case '1':   //姿勢制御再開
        if(CanSat_phase == CANSAT_FLIGHT && !controll_yn){
            controll_yn = true;
        }
        break;
        
        case '0':   //姿勢制御停止
        if(CanSat_phase == CANSAT_FLIGHT && controll_yn){
            controll_yn = false;
        }
        break;
    }
}


/*******************************************************************************
データを1秒ごとに書き込み開始
*******************************************************************************/
void startRecSlow(){
    if(!save_slow && !save_fast){
        fp = fopen(file_name, "a");
        fprintf(fp, "Time,Flight Time,Phase,Controll,Flight Pin,");
        fprintf(fp, "Valve1,Valve2,Valve3,Valve4,Buzzer,");
        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],Battery_V[V],Battery_I[A],");
        fprintf(fp, "Pres[hPa],Temp[degC],Hum[%%],Alt[m],Alt_ave[m],Speed[m/s],Speed_ave[m/s],");
        fprintf(fp, "Acc_x[m/s^2],Acc_y[m/s^2],Acc_z[m/s^2],Mag_x[uT],Mag_y[uT],Mag_z[uT],Gyro_x[deg/s],Gyro_y[deg/s],Gyro_z[deg/s],");
        fprintf(fp, "Q_w,Q_x,Q_y,Q_z,Yaw[deg],Roll[deg],Pitch[deg],");
        fprintf(fp, "CO2[ppm],TVOCs[ppm],");
        fprintf(fp, "Bottle Pres[MPa]\r\n");
        tick_save.detach();
        tick_save.attach(&recData, 1.0f);
        save_slow = true;
    }
}


/*******************************************************************************
データを最速で書き込み開始
*******************************************************************************/
void startRecFast(){
    if(!save_slow && !save_fast){
        fp = fopen(file_name, "a");
        fprintf(fp, "Time,Flight Time,Phase,Controll,Flight Pin,");
        fprintf(fp, "Valve1,Valve2,Valve3,Valve4,Buzzer,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],Battery_V[V],Battery_I[A],");
        fprintf(fp, "Pres[hPa],Temp[degC],Hum[%%],Alt[m],Alt_ave[m],Speed[m/s],Speed_ave[m/s],");
        fprintf(fp, "Acc_x[m/s^2],Acc_y[m/s^2],Acc_z[m/s^2],Mag_x[uT],Mag_y[uT],Mag_z[uT],Gyro_x[deg/s],Gyro_y[deg/s],Gyro_z[deg/s],");
        fprintf(fp, "Q_w,Q_x,Q_y,Q_z,Yaw[deg],Roll[deg],Pitch[deg],");
        fprintf(fp, "CO2[ppm],TVOCs[ppm],");
        fprintf(fp, "Bottle Pres[MPa]\r\n");
        tick_save.detach();
        save_fast = true;
    }
}


/*******************************************************************************
データ記録停止
*******************************************************************************/
void stopRec(){
    if(save_slow){
        save_slow = false;
        tick_save.detach();
        fprintf(fp, "\r\n\n");
        fclose(fp);
    }
    else if(save_fast){
        save_fast = false;
        fprintf(fp, "\r\n\n");
        fclose(fp);
    }
}


/*******************************************************************************
データの書き込み
*******************************************************************************/
void recData(){
    if(save_slow || save_fast){
        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(CanSat_phase){
            case CANSAT_SETUP:
            fprintf(fp, "SETUP,");
            break;
            
            case CANSAT_SAFETY:
            fprintf(fp, "SAFETY,");
            break;
            
            case CANSAT_WAIT:
            fprintf(fp, "WAIT,");
            break;
            
            case CANSAT_FLIGHT:
            fprintf(fp, "FLIGHT,");
            break;
            
            case CANSAT_RECOVERY:
            fprintf(fp, "RECOVERY,");
            break;
        }
        
        fprintf(fp, "%d,", controll_yn);
        fprintf(fp, "%d,", flight_pin);
        fprintf(fp, "%d,%d,%d,%d,%d,", Valve1.status, Valve2.status, Valve3.status, Valve4.status, Buzzer.status);
        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,%f,%f,", adxl_acc[0], adxl_acc[1], adxl_acc[2], ina_v, ina_i);
        fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,", pres, temp, hum, alt, alt_ave, speed, speed_ave);
        fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f,", amg[0], amg[1], amg[2], amg[3], amg[4], amg[5], amg[6], amg[7], amg[8]);
        fprintf(fp, "%f,%f,%f,%f,", quart[0], quart[1], quart[2], quart[3]);
        fprintf(fp, "%.2f,%.2f,%.2f,", euler[0], euler[1], euler[2]);
        fprintf(fp, "%d,%d,", CO2, TVOCs);
        fprintf(fp, "%f\r\n", bottle_pres);
        
        /////////////////////////////////////////////////////////////////////EEPROM
        EEPROM.chargeBuff((short)time_reset);
        EEPROM.chargeBuff((int)time_read);
        EEPROM.chargeBuff((short)flight_time_reset);
        EEPROM.chargeBuff((int)flight_time_read);
        EEPROM.chargeBuff((char)CanSat_phase);
        char status = 0x00;
        if(controll_yn)   status |= 0x01;
        if(flight_pin)    status |= 0x02;
        if(Valve1.status) status |= 0x04;
        if(Valve2.status) status |= 0x08;
        if(Valve3.status) status |= 0x10;
        if(Valve4.status) status |= 0x20;
        if(Buzzer.status) status |= 0x40;
        EEPROM.chargeBuff((char)status);
        EEPROM.chargeBuff((char)es920_uplink_command);
        EEPROM.chargeBuff((char)gps_yn);
        EEPROM.chargeBuff((float)gps_lat);
        EEPROM.chargeBuff((float)gps_lon);
        EEPROM.chargeBuff((float)gps_alt);
        EEPROM.chargeBuff((float)gps_knot);
        EEPROM.chargeBuff((float)gps_deg);
        EEPROM.chargeBuff((short)gps_sat);
        EEPROM.chargeBuff((float)adxl_acc[0]);
        EEPROM.chargeBuff((float)adxl_acc[1]);
        EEPROM.chargeBuff((float)adxl_acc[2]);
        EEPROM.chargeBuff((float)ina_v);
        EEPROM.chargeBuff((float)ina_i);
        EEPROM.chargeBuff((float)pres);
        EEPROM.chargeBuff((float)temp);
        EEPROM.chargeBuff((float)hum);
        EEPROM.chargeBuff((float)alt);
        EEPROM.chargeBuff((float)speed);
        EEPROM.chargeBuff((short)(amg[0] / ES920_MAX_20));
        EEPROM.chargeBuff((short)(amg[1] / ES920_MAX_20));
        EEPROM.chargeBuff((short)(amg[2] / ES920_MAX_20));
        EEPROM.chargeBuff((short)(amg[3] / ES920_MAX_20));
        EEPROM.chargeBuff((short)(amg[4] / ES920_MAX_20));
        EEPROM.chargeBuff((short)(amg[5] / ES920_MAX_20));
        EEPROM.chargeBuff((short)(amg[6] / ES920_MAX_1500));
        EEPROM.chargeBuff((short)(amg[7] / ES920_MAX_1500));
        EEPROM.chargeBuff((short)(amg[8] / ES920_MAX_1500));
        EEPROM.chargeBuff((float)quart[0]);
        EEPROM.chargeBuff((float)quart[1]);
        EEPROM.chargeBuff((float)quart[2]);
        EEPROM.chargeBuff((float)quart[3]);
        EEPROM.chargeBuff((short)(euler[0] / ES920_MAX_360));
        EEPROM.chargeBuff((short)(euler[1] / ES920_MAX_360));
        EEPROM.chargeBuff((short)(euler[2] / ES920_MAX_360));
        EEPROM.chargeBuff((short)CO2);
        EEPROM.chargeBuff((short)TVOCs);
        EEPROM.chargeBuff((float)bottle_pres);
        EEPROM.writeBuff();
        eeprom_ptr = EEPROM.setNextPage();
    }
}


/*******************************************************************************
ブザーのオンオフ切り替え
*******************************************************************************/
void buzzerChange(){
    if(Buzzer.status){
        Buzzer.fire_off();
    }
    else{
        Buzzer.fire_on();
    }
}


/*******************************************************************************
セットアップ（最初に1回実行）
無線あり：HEADER_SETUP_SENSOR
無線あり：HEADER_GPS_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
    INA226.set_callibretion();
    if(INA226.Connection_check() == 0){
        pc.printf("INA226 : OK\r\n");
        strcat(setup_string, "INA226 : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("INA226 : NG\r\n");
        strcat(setup_string, "INA226 : NG\r\n");
        es920 << (char)0x00;
    }
    
    //////////////////////////////////////////////////BME280
    BME280.configMeasure(BME280_lib::NORMAL, 1, 2, 1);
    BME280.configFilter(4);
    if(BME280.connectCheck() == 1){
        pc.printf("BME280 OK!!\r\n");
        strcat(setup_string, "BME280 : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("BME280 NG\r\n");
        strcat(setup_string, "BME280 : NG\r\n");
        es920 << (char)0x00;
    }
    
    //////////////////////////////////////////////////BNO055
    BNO055.setOperationMode(BNO055_lib::CONFIG);
    BNO055.setAccRange(BNO055_lib::_16G);
    BNO055.setAxis(BNO055_lib::Z, BNO055_lib::X, BNO055_lib::Y);
    BNO055.setAxisPM(1, 1, -1);
    if(BNO055.connectCheck() == 1){
        pc.printf("BNO055 OK!!\r\n");
        strcat(setup_string, "BNO055 : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("BNO055 NG\r\n");
        strcat(setup_string, "BNO055 : NG\r\n");
        es920 << (char)0x00;
    }
    BNO055.setOperationMode(BNO055_lib::NDOF);
    
    //////////////////////////////////////////////////CCS811
    if(CCS811.connectCheck() == 1){
        pc.printf("CCS811 OK!!\r\n");
        strcat(setup_string, "CCS811 : OK!!\r\n");
        es920 << (char)0x01;
    }
    else{
        pc.printf("CCS811 NG\r\n");
        strcat(setup_string, "CCS811 : NG\r\n");
        es920 << (char)0x00;
    }
    
    //////////////////////////////////////////////////圧力センサ
    pc.printf("PRES : ");
    ADS1115.setGain(GAIN_TWOTHIRDS);
    pc.printf("OK!!\r\n");
    strcat(setup_string, "PRES : OK!!\r\n");
    es920 << (char)0x01;
    
    //////////////////////////////////////////////////SD
    fp = fopen("/sd/Space_SWAN_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];
        sprintf(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");
    fp = fopen(file_name, "a");
    fprintf(fp, setup_string);
    fclose(fp);
    setup_string[0] = NULL;
    
    EEPROM.setWriteAddr(1, 0, 0x00, 0x00);
    
    printHelp();
    wait(1.0f);
}