File content as of revision 0:3e847864d796:

#include "mbed.h"
#include "SDFileSystem.h"
#include "Quaternion.hpp"
#include "MadgwickFilter.hpp"
#include "mpu9250_i2c.h"
#include "GPS_interrupt.h"
#include "ExternalTrigger.hpp"
#include "nicrom.hpp"
#include "battery_monitoring.hpp"
#include "XbeeApiLib.h"     // XbeeApiLib のインクルード
#include "Servo.h"
#include <string>
//------------------------------------------------------------------------------
//   なんかこう計算を速くしたり，よく使うもの
//------------------------------------------------------------------------------
const double PI = 3.1415926535897932384626433832795;//4.0*atan(1.0);
const double DEG_TO_RAD = 0.01745329251994329576923690768489;//PI / 180.0;
const double RAD_TO_DEG = 57.295779513082320876798154814105;//180.0 / PI;
const double ACC_LPF_COEF = 0.95;
const double GYRO_LPF_COEF = 0.5f;
const double MAG_LPF_COEF = 0.95;
#define  EARTH_EQUATOR_RADIUS  6378136.6//地球の赤道半径[m]
#define  EARTH_POLAR_RADIUS  6356751.9//地球の極半径[m]
static int phaseNumber = 0; 
static unsigned long system_count = 0;
FILE *fp;
char datafile[64] = "datafile0.txt";
//#include <new>

DigitalOut sdWriteLed(LED3);
/*
void no_memory () {
    printf("panic: can't allocate to memory!\r\n");
    exit(-1);
}
//
extern "C" void HardFault_Handler() {
    printf("Hard Fault!\n");
    while(1);
}*/
//------------------------------------------------------------------------------
//   すぐに変更できたらいいもの
//------------------------------------------------------------------------------
#define SET_ACC_LPF NO_USE
#define SET_GYRO _1000DPS
#define SET_ACC  _16G
const double nine_offset[9] = {0.32766334f, 3.067240359f, -0.326759467f,
                               0.0052546f, -0.009152758f, 0.142725298f,
                               10.875, -24, 41.55f};
//------------------------------------------------------------------------------
//   諸々のパラメータ
//------------------------------------------------------------------------------
#define SECOND_SEPA_DELAY_TIME 5.0f//[s] 気圧で1段目分離機構を作動させてから2段目を起動させるまでの時間
#define MAIN_PARA_WAIT_TIME 15.0f//フライトピンが抜けてから分離するまでの時間[s]
#define DROGUE_WAIT_TIME 10.0f
#define FALLING_THRESHOLD -0.04//hPa 落下しているとみなせる気圧差
const float SAFETY_TIME = 6.0f;//フライトピンが抜けてから分離しない時間

const int FALLING_COUNT = 100;//この回数，気圧が連続で上昇したら落下とする

//------------------------------------------------------------------------------
//   ダウンリンク系のインスタンス生成 
//------------------------------------------------------------------------------
Serial pc(USBTX,USBRX, 115200);
void downLink();
//------------------------------------------------------------------------------
//   内部時間タイマー
//------------------------------------------------------------------------------
Timer innerTime;

//------------------------------------------------------------------------------
//   9軸センサー
//------------------------------------------------------------------------------
#define NINE_SDA p9
#define NINE_SCL p10

I2C i2cBus(NINE_SDA, NINE_SCL);
mpu9250 nine(i2cBus, AD0_HIGH);

//------------------------------------------------------------------------------
//   サーボモーター ドラッグシュート放出用分離機構
//------------------------------------------------------------------------------
int fastPressureFlag = 0;//気圧による分離命令がタイマーより先だったとき 1 になる．
#define MAX_PULSE 0.00245//[s]
#define MIN_PULSE 0.00065//[s]
#define SERVO_PIN p26
#define SERVO_PIN2 p25
Servo servo(SERVO_PIN, MAX_PULSE, MIN_PULSE);
Servo servo2(SERVO_PIN2,  MAX_PULSE, MIN_PULSE);
volatile int drogueSepaFlag = 0;//ドラッグシュート放出フラグ
Timeout drogueSeparationTimer;//ドラッグシュート放出タイマー
float currentAngle = 0;
float zeroAngle = 0;

void drogueSeparation(){
    if(fastPressureFlag == 0){
        drogueSepaFlag = 1;
    }
    return;   
}
//------------------------------------------------------------------------------
//   ニクロム線 メインパラシュート放出分離機構
//------------------------------------------------------------------------------
#define NICROM_PIN p30
#define NICROM_PIN2 p23
Nicrom nicrom(NICROM_PIN);
Nicrom nicrom2(NICROM_PIN2);

int mainParaSepaFlag = 0;
Timeout mainParaSeparationTimer;//メインパラシュート放出タイマー

void mainParaSeparation(){
    if(fastPressureFlag == 0){
        mainParaSepaFlag = 1;
    }
    return;   
}

//------------------------------------------------------------------------------
//   フライトピン用外部割り込み設定 
//------------------------------------------------------------------------------

#define FLIGHT_PIN p24 
static volatile int flightPinFlag = 0;
static volatile int flightPinOKFlag = 0;

void flightPinCallFunc(){//この関数内でニクロム線に電流を流してもいいかなと考えたりもした
    flightPinFlag = 1;
    return;   
}
//InterruptIn flightPin(FLIGHT_PIN);
ExtTrigger flightPin(FLIGHT_PIN, FALL, &flightPinCallFunc, 0.2f);//ピン番号，立ち下がり，呼び出す関数，チャタリング待ち時間 

//------------------------------------------------------------------------------
//   分離検知
//------------------------------------------------------------------------------
#define DETECT_SEPARATION_PIN1 p12
static int detectSepaFlag1 = 0;
void detectDrogueSepa(){
    detectSepaFlag1 = 1;
    return;   
}
ExtTrigger DetectDrogueSeparation(DETECT_SEPARATION_PIN1, RISE, &detectDrogueSepa, 0.2f);

//------------------------------------------------------------------------------
//   バッテリー電圧チェック
//------------------------------------------------------------------------------
#define MAIN_BATTERY_PIN p18
#define EXT_BATTERY_PIN p19
#define USING_MAIN_PIN p20
battery_monitoring  bt(2.6118, 2.9647, MAIN_BATTERY_PIN );
battery_monitoring  ext_bt(2.6118, 2.9647, EXT_BATTERY_PIN);
battery_monitoring  usingMainbt(2.6118, 2.9647, USING_MAIN_PIN);

//------------------------------------------------------------------------------
//   GPS 関連 
//------------------------------------------------------------------------------
Serial mygps(p28, p27, 115200);
GPS_interrupt gps(&mygps);
/*
static double K = 0;
static double I = 0;*/
static double KI[2] = {0.0, 0.0};
static int satellitesNumber = 0;//捕捉衛星数
static double altitude = 0;//高度
static double speed_knot = 0;//対地速度[m/s]
static double head = 0;//進行方角，北から左回り正，0~359[°]

//------------------------------------------------------------------------------
//   microSDカードの設定
//------------------------------------------------------------------------------
#define MOSI p5
#define MISO p6
#define SCK p7
#define CS_SD p8
SDFileSystem sd(MOSI, MISO, SCK, CS_SD, "sd");
static double sdBuff[14] = {0.0};
//------------------------------------------------------------------------------
//   割り込みでMadgwickFilterを回す
//------------------------------------------------------------------------------
static double imu[6] = {0};//角速度xyz，加速度xyz
static double mag[3] = {0};//地磁気xyz
static Quaternion myQ(1.0f,0.0f,0.0f,0.0f);//姿勢のクォータニオン



//------------------------------------------------------------------------------
//   LPS25H
//------------------------------------------------------------------------------
#include "LPS25H.h"
MyLPS25H lps25h(i2cBus);

double pressure = 0;
uint32_t pressureRaw = 0;
int fallingFlag = 0;

//------------------------------------------------------------------------------
//   ダウンリンク
//------------------------------------------------------------------------------
// パケット受信時に呼ばれてほしい関数のプロトタイプ宣言    Length: データ長   *message: データ
void OnXbeeDataReceived(int Length, char *message);

XbeeApi xbeeAPI(p13, p14, 115200, OnXbeeDataReceived);
static uint64_t downlinkCount = 0;
char flightPinSeparationFlag = 0b00000000;
inline void downLink(){

    if(gps.gps_readable){
        KI[0] = (double)gps.Longitude();
        KI[1] = (double)gps.Latitude();
        satellitesNumber = gps.Number();
        altitude = gps.Height();
        speed_knot = gps.Knot();
        head = gps.Degree();        
    }
    else{
        KI[0]= 0.0; KI[1] = 0.0; satellitesNumber = 0; altitude = 0.0; speed_knot = 0.0; head = 0.0;
    }
    
    if(phaseNumber == 1){//フライトピンが抜けるまで
            if(downlinkCount % 20 == 0){
                xbeeAPI.StartPacket(0x11);
                xbeeAPI.Write((float)bt.debug()*9.9f);
                xbeeAPI.Write((float)ext_bt.debug()*9.90f);
                xbeeAPI.Write((float)usingMainbt.debug()*9.90f);
                xbeeAPI.Write((float)altitude);
                xbeeAPI.Write((double)pressure);//気圧
                xbeeAPI.Write((char)flightPinSeparationFlag);
                xbeeAPI.EndPacket();  
            }
            else{
                xbeeAPI.StartPacket(0x10);
                xbeeAPI.Write((float)myQ.x);//姿勢クォータニオン
                xbeeAPI.Write((float)myQ.y);
                xbeeAPI.Write((float)myQ.z);
                xbeeAPI.Write((float)myQ.w);
                xbeeAPI.Write((float)(KI[0] - 139.0));//130.901722222);//gps.Longitude());//GPS座標
                xbeeAPI.Write((float)(KI[1] - 34.0));//30.4103833333);//gps.Latitude()); 
                xbeeAPI.EndPacket();  
            }
    }
    else if(phaseNumber == 2){//飛行中
            if(downlinkCount % 10 == 0){
                
                xbeeAPI.StartPacket(0x12);
                xbeeAPI.Write((int)innerTime.read_us());
                xbeeAPI.Write((float)altitude);//100.0f);//(float)gps.Height());
                xbeeAPI.Write((float)speed_knot);//80.0f);//(float)gps.Knot());
                xbeeAPI.Write((float)head);//270.0f);//(float)gps.Degree());
                xbeeAPI.Write((double)pressure);//気圧
                xbeeAPI.Write(flightPinSeparationFlag);
                xbeeAPI.EndPacket();
            }
            else{
                xbeeAPI.StartPacket(0x10);
                xbeeAPI.Write((float)myQ.x);//姿勢クォータニオン
                xbeeAPI.Write((float)myQ.y);
                xbeeAPI.Write((float)myQ.z);
                xbeeAPI.Write((float)myQ.w);
                xbeeAPI.Write((float)(KI[0] - 139.0));//gps.Longitude());//GPS座標
                xbeeAPI.Write((float)(KI[1] - 34.0));//gps.Latitude());
                xbeeAPI.EndPacket();          
                //pc.printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\r\n",innerTime.read(),myQ.x,myQ.y,myQ.z,myQ.w,gps.Longitude(),gps.Latitude(),pressure);         
           }
    }
    else if(phaseNumber == 3){//回収待機中
            xbeeAPI.StartPacket(0x13);
            xbeeAPI.Write((float)(KI[0] - 139.0));//130.901722222);//gps.Longitude());//GPS座標
            xbeeAPI.Write((float)(KI[1] - 34.0));//30.4103833333);//gps.Latitude());
            //xbeeAPI.Write((double)pressure);
            xbeeAPI.Write((float)bt.debug()*9.9f);
            xbeeAPI.Write((float)ext_bt.debug()*9.90f);
            xbeeAPI.Write((float)usingMainbt.debug()*9.90f);
            xbeeAPI.EndPacket(); 
    } 
    downlinkCount++;  
}

inline void Print(char* str){
    //pc.printf("%s\r\n", str);
    string s(str);
    xbeeAPI.StartPacket(0x50);
    xbeeAPI.Write(s);//文字列
    xbeeAPI.EndPacket();  
    return;   
}
// パケット受信時の処理    Length: データ長   *message: データ
void OnXbeeDataReceived(int Length, char *message){
    pc.printf("%d\t%s\r\n",Length, message);
    if(message[0] == 0x58){//'X'サーボ
        if(message[1] == 0xFF){
            zeroAngle = currentAngle;
            Print("Set Zero Angle");
        }
        else{
            currentAngle = (double)(message[1] - 90);
            servo = currentAngle;
            servo2 = currentAngle;
            char mojiretu[32] = "";
            sprintf(mojiretu, "Set Servo: %d", (int)message[1]); 
            Print(mojiretu);
        }
    }
    else if(message[0] == 0x46){//'F' フライトピン待機開始許可フラグ
        if((flightPin.read() == 1) && (message[1] == 0x4F)){//'O' Flightpin OK
            flightPinOKFlag = 1;   
            Print("Start Wait Flight Pin");
        }
        else if(message[1] == 0x4E){
            flightPinOKFlag = 0;
            Print("Flight Pin Reset!!\r\n");   
        }
    }
    //指令分離
    else if((flightPinFlag == 1) && (Length > 5) && (message[0] == 'J')){//J B K C M D
        if(message[1] == 'B'){
            if(message[2] == 'K'){
                if(message[3] == 'C'){
                    if(message[4] == 'M'){
                        if(message[5] == 'D'){
                            mainParaSepaFlag = 1; 
                        }   
                    }   
                }   
            }
        }
    }
}

//------------------------------------------------------------------------------
//   スケジュール割り込み関数
//------------------------------------------------------------------------------
const int CALL_PERIOD = 50000;//us

Ticker ScheduleCallTimer;//schedule
int sdSaveFlag = 0;
void autoCallFunc(){
    
    //static uint64_t timingCount = 0;
    sdSaveFlag = 1;
    downLink();
    //timingCount++;
}
/**
*  0.2秒ごとに増えていく
*/
int sepaCount = 0;
Ticker SepaCountTimer;
void SepaCountFunc(){
    sepaCount++;
    return;   
}

//------------------------------------------------------------------------------
//   1段目分離からの2段目分離タイマー
//------------------------------------------------------------------------------
Timeout secondSepaDelayTimer;
void secondSepaDelayFunc(){
    mainParaSepaFlag = 1;
    return;   
}
//------------------------------------------------------------------------------
//   フライトピンが抜けてから6秒は分離しないというセーフティ的な何か
//------------------------------------------------------------------------------
int safetyFlag = 0;
Timeout safetyTimer;
void safetyFunc(){
    safetyFlag = 1;
    return;   
}

//------------------------------------------------------------------------------
//  0.04秒ごとに気圧取得許可フラグを立てる
//------------------------------------------------------------------------------
Ticker PressureGetTimer;
int pressureGetFlag = 0;
void pressureGetFunc(){
    pressureGetFlag = 1;
    return;
}

int main() {
    //setPress();
    //XBee初期設定
    // 0番目の宛先アドレス
    xbeeAPI.SetAddress(0, 0x0013A200, 0x40A88F02);
    // 1番目の宛先アドレス
    xbeeAPI.SetAddress(1, 0x0013A200, 0x40E7D43E);
    // 2番目の宛先アドレス
    xbeeAPI.SetAddress(2, 0x0013A200, 0x40DC2538);
    //3番目の宛先アドレス
    xbeeAPI.SetAddress(3, 0x0013A200, 0x40BABC20);
   
    flightPinSeparationFlag = 0b00000000;
   
    Print("start\r\n");
    Print("-----------------------------------------");
    Print("Boot Phase:");  
    pc.printf("start\r\n");
    pc.printf("\r\n-----------------------------------------\r\n");
    pc.printf("Boot Phase:\r\n");  
    for(char i = 0;i < 100;i++){
        pc.printf("Loading: %3d [%%]\r", i);
        wait_ms(30); 
    } 
    Print("Loading: 100 [%%] -> finished!!");
    Print("-----------------------------------------");
    pc.printf("Loading: 100 [%%] -> finished!!\r\n");
    pc.printf("\r\n-----------------------------------------\r\n");

    /*==========================================================================
        フェーズⅠ   < 発射待機・システムチェック > 
    ===========================================================================*/
    phaseNumber = 1;
    int temp_time = 0;
    Print("----------------");
    Print("Phase I START");
    pc.printf("\r\n----------------\r\n");
    pc.printf("Phase I START\r\n");
    
    Print("SD check ->");
    pc.printf("SD check -> ");
    fp = fopen("/sd/check.txt", "w");
    if(fp == NULL){
        while(1){
            Print("SD ERROR");
            error("SD ERROR\r\n");   
        }   
    }
    else{
        pc.printf("OK\r\n");
        Print("\tOK");
        fprintf(fp,"this file is made by checking sd card program.\r\n");
        fclose(fp);   
    }
    
    while(1){
        static int fileCount = 0;
        sprintf(datafile, "/sd/datafile%d.txt", fileCount);
        fp = fopen(datafile, "rb");
        if(fp != NULL){
            fclose(fp);
            fileCount++;   
        }
        else{
            fp = fopen(datafile, "wb");
            if(fp == NULL){//ファイルオープン失敗
                while(1){
                    Print("file open error\r\n");
                    pc.printf("file open error\r\n");
                    wait(1.0);
                }
            }
            fclose(fp);
            fp = fopen(datafile, "wb");
            if(fp != NULL){
                fclose(fp);   
            }
            break;   
        }
    }
    
    
    //内部タイマースタート
    innerTime.reset();
    innerTime.start();
    lps25h.SetPress();

    //9軸センサ 初期設定
    nine.setAccLPF(SET_ACC_LPF);
    nine.setGyro(SET_GYRO);
    nine.setAcc(SET_ACC);
    //9軸センサ オフセット設定
    nine.setOffset(nine_offset[0], nine_offset[1], nine_offset[2],
                   nine_offset[3], nine_offset[4], nine_offset[5],
                   nine_offset[6], nine_offset[7], nine_offset[8]);
    
    Print("finished mpu9250 setting");
    pc.printf("finished mpu9250 setting");
    
    //割り込みセンサーアクセス関数セット
    MadgwickFilter attitude(0.1);
    ScheduleCallTimer.attach_us(&autoCallFunc, CALL_PERIOD);//&downLink, 50000);//
    PressureGetTimer.attach(&pressureGetFunc, 0.04f);   
 
    
    //フライトピンをさすループ
waitFlightPinLabel:
    flightPin.clear();
    flightPinOKFlag = 0;
    Print("wait for connect Flight Pin");
    pc.printf("wait for connect Flight Pin");
    
    flightPinSeparationFlag = 0b10000000;//7bit目を0に
    
    while(1){
        nine.getGyroAcc(imu);//慣性データ取得
        nine.getMag(mag);//地磁気データ取得
        attitude.MadgwickAHRSupdate(imu[0]*DEG_TO_RAD, imu[1]*DEG_TO_RAD, imu[2]*DEG_TO_RAD, imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]);//姿勢推定
        attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
        if(pressureGetFlag){
            pressureGetFlag = 0;
            lps25h.GetPress(&pressure, &pressureRaw);
        }
        pc.printf("Attitude:%f,%f,%f,%f\r\n", myQ.x, myQ.y, myQ.z, myQ.w);  
        pc.printf("%f\r\n",pressure);
        //pc.printf("%f\t%f\r\n", KI[0], KI[1]);
        if(flightPin.read() == 1){
            flightPinSeparationFlag = flightPinSeparationFlag & 0b01111111;//7bit目を0に
            if(flightPinOKFlag == 1){
                break;   
            }   
            else if(pc.readable()){//デバッグ
                char flightChar = pc.getc();
                if(flightChar == 'F'){
                    flightPinOKFlag = 1;
                    break;
                }   
            }
        }
        else flightPinSeparationFlag |= 0b10000000;
        
        if(abs(temp_time - innerTime.read_ms()) > 500){
           // Print("wait for connect Flight Pin and send F.");
            pc.printf("wait for connect Flight Pin and send F.");
            temp_time = innerTime.read_ms();
        }
        
        //分離機構が作動してしまった時の例のやつ
        if(DetectDrogueSeparation.read() == 1){
            flightPinSeparationFlag = flightPinSeparationFlag | 0b00100000;   
        }
        else{
            flightPinSeparationFlag = flightPinSeparationFlag & 0b11011111;   
        }
    }

    //フライトピン待機
    temp_time = innerTime.read_ms();
    flightPinFlag = 0;
    
    flightPinSeparationFlag = flightPinSeparationFlag | 0b01000000;//6bit目を1に
    
    while(flightPinFlag == 0){
        
        nine.getGyroAcc(imu);//慣性データ取得
        nine.getMag(mag);//地磁気データ取得
        attitude.MadgwickAHRSupdate(imu[0]*DEG_TO_RAD, imu[1]*DEG_TO_RAD, imu[2]*DEG_TO_RAD, imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]);//姿勢推定
        attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
         pc.printf("Attitude:%f,%f,%f,%f\r\n", myQ.x, myQ.y, myQ.z, myQ.w);  
        if(pressureGetFlag){
            pressureGetFlag = 0;
            lps25h.GetPress(&pressure, &pressureRaw);
        } 
        if(abs(temp_time - innerTime.read_ms()) > 5000){
            pc.printf("Wait for Flight Pin Left");
            temp_time = innerTime.read_ms();
        }
        if(flightPinOKFlag == 0){
            goto waitFlightPinLabel;
        }
        
        //分離機構が作動してしまった時の例のやつ
        if(DetectDrogueSeparation.read() == 1){
            flightPinSeparationFlag = flightPinSeparationFlag | 0b00100000;   
        }
        else{
            flightPinSeparationFlag = flightPinSeparationFlag & 0b11011111;   
        }
    }
    
    flightPinSeparationFlag = flightPinSeparationFlag | 0b10000000;//7bit目を1に 0b11000000になっているはず
    Print("Detect Flight Pin!!");
    pc.printf("Detect Flight Pin!!");
    Print("Launch!!!!Launch!!!!");
    Print("Phase I END");
    pc.printf("Launch!!!!Launch!!!!");
    pc.printf("Phase I END");
    /*==========================================================================
        フェーズII  < 飛行中 >
    ==========================================================================*/
    Print("Phase II START");
    pc.printf("Phase II START");
    //色々初期化
    innerTime.reset();//内部時間
    phaseNumber = 2;//フェーズナンバー
    downlinkCount = 0;//
    detectSepaFlag1 = 0;
    safetyFlag = 0;
    safetyTimer.attach(&safetyFunc, SAFETY_TIME);
    
    drogueSeparationTimer.attach(&drogueSeparation, DROGUE_WAIT_TIME);
    mainParaSeparationTimer.attach(&mainParaSeparation, MAIN_PARA_WAIT_TIME);
    
    static int SepaStartFlag = 0;
    double oldPressure = (double)lps25h.GetPress();
    double deltaP = 0;   
    int pressureCount = 0;
    
    while(1){
        
        //分離機構が作動してしまった時の例のやつ
        if(DetectDrogueSeparation.read() == 1)  flightPinSeparationFlag = flightPinSeparationFlag | 0b00100000;   
        else flightPinSeparationFlag = flightPinSeparationFlag & 0b11011111;   
        
        temp_time = innerTime.read_ms();
        nine.getGyroAcc(imu);//慣性データ取得
        nine.getMag(mag);//地磁気データ取得
        attitude.MadgwickAHRSupdate(imu[0]*DEG_TO_RAD, imu[1]*DEG_TO_RAD, imu[2]*DEG_TO_RAD, imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]);//姿勢推定
        attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
        
        if(pressureGetFlag){//0.04秒ごとに気圧取得&比較
            pressureGetFlag = 0;
            lps25h.GetPress(&pressure, &pressureRaw);//気圧取得
            
            //pc.printf("%f\t%f\r\n",innerTime.read(), pressure);
            
            //気圧による分離条件式
            deltaP = pressure - oldPressure;//気圧の変化勾配
            if((deltaP > FALLING_THRESHOLD) && (safetyFlag == 1) && (pressure != 0.0)){//気圧差 > -0.04 かつ 6秒以上経過
                pressureCount++;
                if(pressureCount > 5){//ドラッグシュート分離
                    //fastPressureFlag = 1;
                    drogueSepaFlag = 1;
                    secondSepaDelayTimer.attach(&secondSepaDelayFunc, SECOND_SEPA_DELAY_TIME);   
                }
            }
            else{
               pressureCount = 0;   
            }
            oldPressure = pressure;
        }
        
        if(drogueSepaFlag == 1 ){//ドラッグシュート分離機構作動
               
            if(SepaStartFlag == 0){
                pc.printf("Separation %f [s]\r\n", innerTime.read());
                //ビットフラグ更新
                flightPinSeparationFlag = (flightPinSeparationFlag & 0b11000111) | 0b00010000;//3bit目を1に, 作動させたがまだわからない
                SepaStartFlag = 1;
                SepaCountTimer.attach(&SepaCountFunc, 0.2f);
                sepaCount = 0;   
                Print("Try Drogue Separation!!");
                pc.printf("Try Drogue Separation!!\r\n");
                servo = 30.0 + zeroAngle;
                servo2 = 30.0 + zeroAngle;
            }
            else{
                if(sepaCount != 0){
                    if(sepaCount % 2 == 0){
                        servo = 35.0 + zeroAngle;
                        servo2 = 35.0 + zeroAngle;                                    
                    }
                    else if(sepaCount % 2 == 1){
                        servo = 25.0 + zeroAngle;
                        servo2 = 25.0 + zeroAngle;
                    }
                }
                //ドラッグシュート分離機構が作動したか (これは常に確認する
                /*if(detectSepaFlag1 == 1){
                        flightPinSeparationFlag = (flightPinSeparationFlag & 0b11000111) | 0b00111000;//2bit目を1に
                        Print("Succeeded Drogue Separation!!\r\n");
                        Print("Finished Controling Servo Motor\r\n"); 
                        pc.printf("Succeeded Drogue Separation!!\r\n");
                        pc.printf("Finished Controling Servo Motor\r\n"); 
                }*/
                
                if(sepaCount > 24){//2秒間経過 
                    mainParaSepaFlag = 1;
                    drogueSepaFlag = 0;//ドラッグシュート分離を停止．
                    /*if(detectSepaFlag1 == 0){//2秒間サーボを動かして，だめだったとき
                        //pc.printf("Separation2 %f [s]\r\n", innerTime.read());
                        flightPinSeparationFlag = (flightPinSeparationFlag & 0b11000111) | 0b00110000;
                        drogueSepaFlag = 0;
                        Print("Failed Drogue Separation...\r\n");
                        pc.printf("Failed Drogue Separation...\r\n");
                        //mainParaSepaFlag = 1;//強制メインパラ分離
                        Print("Finished Controling Servo Motor\r\n"); 
                        pc.printf("Finished Controling Servo Motor\r\n"); 
                    }*/
                    pc.printf("Finished Controling Servo Motor\r\n");
                }
            }
        }
        
        if(mainParaSepaFlag == 1){
            flightPinSeparationFlag = flightPinSeparationFlag | 0b00000111;
            pc.printf("Separation2 %f [s]\r\n", innerTime.read());
            Print("Try Main Separation!!");
            pc.printf("Try Main Separation!!\r\n");
            nicrom.autoStop(5.0f);
            mainParaSepaFlag = 0;
            Print("Finished Nicrom Control");
            pc.printf("Finished Nicrom Control\r\n");
            break;
        }
        
        //SD保存
        
        if(sdSaveFlag == 1){
            sdWriteLed = 1;
            sdSaveFlag = 0;
            temp_time = innerTime.read_us();
            fp = fopen(datafile, "ab");//14
            if(fp != NULL){
                for(int i = 0; i < 6;i++){
                    sdBuff[i] = imu[i];   
                }
                for(int i = 0;i < 3;i++){
                    sdBuff[i+6] = mag[i];    
                }
                sdBuff[9] = myQ.x; sdBuff[10] = myQ.y; sdBuff[11] = myQ.z; sdBuff[12] = myQ.w; sdBuff[13] = pressure;
                fwrite(&temp_time, sizeof(int), 1, fp);
                fwrite(&flightPinSeparationFlag, sizeof(char), 1, fp);
                fwrite(sdBuff, sizeof(double), 14, fp);
                fclose(fp);
            }
            sdWriteLed = 0;
        }
    }   
    temp_time = innerTime.read_ms();
    while(1){
        nine.getGyroAcc(imu);//慣性データ取得
        nine.getMag(mag);//地磁気データ取得
        attitude.MadgwickAHRSupdate(imu[0]*DEG_TO_RAD, imu[1]*DEG_TO_RAD, imu[2]*DEG_TO_RAD, imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]);//姿勢推定
        attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
        if(pressureGetFlag){
            pressureGetFlag = 0;
            lps25h.GetPress(&pressure, &pressureRaw);   
        }
        if(abs(innerTime.read_ms() - temp_time) > 30000){
            break;   
        }
    }
    Print("Phase II END");
    pc.printf("Phase II END\r\n");
    /*==========================================================================
        フェーズⅢ  < 回収待機 >
    ==========================================================================*/
    Print("Phase III START");
    pc.printf("Phase III START");
    phaseNumber = 3;
    downlinkCount = 0;
   
    temp_time = innerTime.read_ms();
    while(1){
        nine.getGyroAcc(imu);//慣性データ取得
        nine.getMag(mag);//地磁気データ取得
        attitude.MadgwickAHRSupdate(imu[0]*DEG_TO_RAD, imu[1]*DEG_TO_RAD, imu[2]*DEG_TO_RAD, imu[3], imu[4], imu[5], mag[0], mag[1], mag[2]);//姿勢推定
        attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
        if(pressureGetFlag == 1){
            pressureGetFlag = 0;
            lps25h.GetPress(&pressure, &pressureRaw);
        }
        if(abs(innerTime.read_ms() - temp_time) > 5000){
            pc.printf("%f\t%f\t%f\t%f\r\n",innerTime.read(), gps.Longitude(), gps.Latitude(), pressure);
            temp_time = innerTime.read_ms();
        }
    }
}