Hybrid_main_FirstEdtion.cpp

00001 #include "mbed.h"
00002 #include "SDFileSystem.h"
00003 #include "Quaternion.hpp"
00004 #include "MadgwickFilter.hpp"
00005 #include "mpu9250_i2c.h"
00006 #include "GPS_interrupt.h"
00007 #include "ExternalTrigger.hpp"
00008 #include "nicrom.hpp"
00009 #include "battery_monitoring.hpp"
00010 #include "XbeeApiLib.h"     // XbeeApiLib のインクルード
00011 #include "Servo.h"
00012 #include <string>
00013 //------------------------------------------------------------------------------
00014 //   なんかこう計算を速くしたり，よく使うもの
00015 //------------------------------------------------------------------------------
00016 const double PI = 3.1415926535897932384626433832795;//4.0*atan(1.0);
00017 const double DEG_TO_RAD = 0.01745329251994329576923690768489;//PI / 180.0;
00018 const double RAD_TO_DEG = 57.295779513082320876798154814105;//180.0 / PI;
00019 const double ACC_LPF_COEF = 0.95;
00020 const double GYRO_LPF_COEF = 0.5f;
00021 const double MAG_LPF_COEF = 0.95;
00022 #define  EARTH_EQUATOR_RADIUS  6378136.6//地球の赤道半径[m]
00023 #define  EARTH_POLAR_RADIUS  6356751.9//地球の極半径[m]
00024 static int phaseNumber = 0; 
00025 static unsigned long system_count = 0;
00026 FILE *fp;
00027 char datafile[64] = "datafile0.txt";
00028 //#include <new>
00029 
00030 DigitalOut sdWriteLed(LED3);
00031 /*
00032 void no_memory () {
00033     printf("panic: can't allocate to memory!\r\n");
00034     exit(-1);
00035 }
00036 //
00037 extern "C" void HardFault_Handler() {
00038     printf("Hard Fault!\n");
00039     while(1);
00040 }*/
00041 //------------------------------------------------------------------------------
00042 //   すぐに変更できたらいいもの
00043 //------------------------------------------------------------------------------
00044 #define SET_ACC_LPF NO_USE
00045 #define SET_GYRO _1000DPS
00046 #define SET_ACC  _16G
00047 const double nine_offset[9] = {0.32766334f, 3.067240359f, -0.326759467f,
00048                                0.0052546f, -0.009152758f, 0.142725298f,
00049                                10.875, -24, 41.55f};
00050 //------------------------------------------------------------------------------
00051 //   諸々のパラメータ
00052 //------------------------------------------------------------------------------
00053 #define SECOND_SEPA_DELAY_TIME 5.0f//[s] 気圧で1段目分離機構を作動させてから2段目を起動させるまでの時間
00054 #define MAIN_PARA_WAIT_TIME 15.0f//フライトピンが抜けてから分離するまでの時間[s]
00055 #define DROGUE_WAIT_TIME 10.0f
00056 #define FALLING_THRESHOLD -0.04//hPa 落下しているとみなせる気圧差
00057 const float SAFETY_TIME = 6.0f;//フライトピンが抜けてから分離しない時間
00058 
00059 const int FALLING_COUNT = 100;//この回数，気圧が連続で上昇したら落下とする
00060 
00061 //------------------------------------------------------------------------------
00062 //   ダウンリンク系のインスタンス生成 
00063 //------------------------------------------------------------------------------
00064 Serial pc(USBTX,USBRX, 115200);
00065 void downLink();
00066 //------------------------------------------------------------------------------
00067 //   内部時間タイマー
00068 //------------------------------------------------------------------------------
00069 Timer innerTime;
00070 
00071 //------------------------------------------------------------------------------
00072 //   9軸センサー
00073 //------------------------------------------------------------------------------
00074 #define NINE_SDA p9
00075 #define NINE_SCL p10
00076 
00077 I2C i2cBus(NINE_SDA, NINE_SCL);
00078 mpu9250 nine(i2cBus, AD0_HIGH);
00079 
00080 //------------------------------------------------------------------------------
00081 //   サーボモーター ドラッグシュート放出用分離機構
00082 //------------------------------------------------------------------------------
00083 int fastPressureFlag = 0;//気圧による分離命令がタイマーより先だったとき 1 になる．
00084 #define MAX_PULSE 0.00245//[s]
00085 #define MIN_PULSE 0.00065//[s]
00086 #define SERVO_PIN p26
00087 #define SERVO_PIN2 p25
00088 Servo servo(SERVO_PIN, MAX_PULSE, MIN_PULSE);
00089 Servo servo2(SERVO_PIN2,  MAX_PULSE, MIN_PULSE);
00090 volatile int drogueSepaFlag = 0;//ドラッグシュート放出フラグ
00091 Timeout drogueSeparationTimer;//ドラッグシュート放出タイマー
00092 float currentAngle = 0;
00093 float zeroAngle = 0;
00094 
00095 void drogueSeparation(){
00096     if(fastPressureFlag == 0){
00097         drogueSepaFlag = 1;
00098     }
00099     return;   
00100 }
00101 //------------------------------------------------------------------------------
00102 //   ニクロム線 メインパラシュート放出分離機構
00103 //------------------------------------------------------------------------------
00104 #define NICROM_PIN p30
00105 #define NICROM_PIN2 p23
00106 Nicrom nicrom(NICROM_PIN);
00107 Nicrom nicrom2(NICROM_PIN2);
00108 
00109 int mainParaSepaFlag = 0;
00110 Timeout mainParaSeparationTimer;//メインパラシュート放出タイマー
00111 
00112 void mainParaSeparation(){
00113     if(fastPressureFlag == 0){
00114         mainParaSepaFlag = 1;
00115     }
00116     return;   
00117 }
00118 
00119 //------------------------------------------------------------------------------
00120 //   フライトピン用外部割り込み設定 
00121 //------------------------------------------------------------------------------
00122 
00123 #define FLIGHT_PIN p24 
00124 static volatile int flightPinFlag = 0;
00125 static volatile int flightPinOKFlag = 0;
00126 
00127 void flightPinCallFunc(){//この関数内でニクロム線に電流を流してもいいかなと考えたりもした
00128     flightPinFlag = 1;
00129     return;   
00130 }
00131 //InterruptIn flightPin(FLIGHT_PIN);
00132 ExtTrigger flightPin(FLIGHT_PIN, FALL, &flightPinCallFunc, 0.2f);//ピン番号，立ち下がり，呼び出す関数，チャタリング待ち時間 
00133 
00134 //------------------------------------------------------------------------------
00135 //   分離検知
00136 //------------------------------------------------------------------------------
00137 #define DETECT_SEPARATION_PIN1 p12
00138 static int detectSepaFlag1 = 0;
00139 void detectDrogueSepa(){
00140     detectSepaFlag1 = 1;
00141     return;   
00142 }
00143 ExtTrigger DetectDrogueSeparation(DETECT_SEPARATION_PIN1, RISE, &detectDrogueSepa, 0.2f);
00144 
00145 //------------------------------------------------------------------------------
00146 //   バッテリー電圧チェック
00147 //------------------------------------------------------------------------------
00148 #define MAIN_BATTERY_PIN p18
00149 #define EXT_BATTERY_PIN p19
00150 #define USING_MAIN_PIN p20
00151 battery_monitoring  bt(2.6118, 2.9647, MAIN_BATTERY_PIN );
00152 battery_monitoring  ext_bt(2.6118, 2.9647, EXT_BATTERY_PIN);
00153 battery_monitoring  usingMainbt(2.6118, 2.9647, USING_MAIN_PIN);
00154 
00155 //------------------------------------------------------------------------------
00156 //   GPS 関連 
00157 //------------------------------------------------------------------------------
00158 Serial mygps(p28, p27, 115200);
00159 GPS_interrupt gps(&mygps);
00160 /*
00161 static double K = 0;
00162 static double I = 0;*/
00163 static double KI[2] = {0.0, 0.0};
00164 static int satellitesNumber = 0;//捕捉衛星数
00165 static double altitude = 0;//高度
00166 static double speed_knot = 0;//対地速度[m/s]
00167 static double head = 0;//進行方角，北から左回り正，0~359[°]
00168 
00169 //------------------------------------------------------------------------------
00170 //   microSDカードの設定
00171 //------------------------------------------------------------------------------
00172 #define MOSI p5
00173 #define MISO p6
00174 #define SCK p7
00175 #define CS_SD p8
00176 SDFileSystem sd(MOSI, MISO, SCK, CS_SD, "sd");
00177 static double sdBuff[14] = {0.0};
00178 //------------------------------------------------------------------------------
00179 //   割り込みでMadgwickFilterを回す
00180 //------------------------------------------------------------------------------
00181 static double imu[6] = {0};//角速度xyz，加速度xyz
00182 static double mag[3] = {0};//地磁気xyz
00183 static Quaternion myQ(1.0f,0.0f,0.0f,0.0f);//姿勢のクォータニオン
00184 
00185 
00186 
00187 //------------------------------------------------------------------------------
00188 //   LPS25H
00189 //------------------------------------------------------------------------------
00190 #include "LPS25H.h"
00191 MyLPS25H lps25h(i2cBus);
00192 
00193 double pressure = 0;
00194 uint32_t pressureRaw = 0;
00195 int fallingFlag = 0;
00196 
00197 //------------------------------------------------------------------------------
00198 //   ダウンリンク
00199 //------------------------------------------------------------------------------
00200 // パケット受信時に呼ばれてほしい関数のプロトタイプ宣言    Length: データ長   *message: データ
00201 void OnXbeeDataReceived(int Length, char *message);
00202 
00203 XbeeApi xbeeAPI(p13, p14, 115200, OnXbeeDataReceived);
00204 static uint64_t downlinkCount = 0;
00205 char flightPinSeparationFlag = 0b00000000;
00206 inline void downLink(){
00207 
00208     if(gps.gps_readable){
00209         KI[0] = (double)gps.Longitude();
00210         KI[1] = (double)gps.Latitude();
00211         satellitesNumber = gps.Number();
00212         altitude = gps.Height();
00213         speed_knot = gps.Knot();
00214         head = gps.Degree();        
00215     }
00216     else{
00217         KI[0]= 0.0; KI[1] = 0.0; satellitesNumber = 0; altitude = 0.0; speed_knot = 0.0; head = 0.0;
00218     }
00219     
00220     if(phaseNumber == 1){//フライトピンが抜けるまで
00221             if(downlinkCount % 20 == 0){
00222                 xbeeAPI.StartPacket(0x11);
00223                 xbeeAPI.Write((float)bt.debug()*9.9f);
00224                 xbeeAPI.Write((float)ext_bt.debug()*9.90f);
00225                 xbeeAPI.Write((float)usingMainbt.debug()*9.90f);
00226                 xbeeAPI.Write((float)altitude);
00227                 xbeeAPI.Write((double)pressure);//気圧
00228                 xbeeAPI.Write((char)flightPinSeparationFlag);
00229                 xbeeAPI.EndPacket();  
00230             }
00231             else{
00232                 xbeeAPI.StartPacket(0x10);
00233                 xbeeAPI.Write((float)myQ.x);//姿勢クォータニオン
00234                 xbeeAPI.Write((float)myQ.y);
00235                 xbeeAPI.Write((float)myQ.z);
00236                 xbeeAPI.Write((float)myQ.w);
00237                 xbeeAPI.Write((float)(KI[0] - 139.0));//130.901722222);//gps.Longitude());//GPS座標
00238                 xbeeAPI.Write((float)(KI[1] - 34.0));//30.4103833333);//gps.Latitude()); 
00239                 xbeeAPI.EndPacket();  
00240             }
00241     }
00242     else if(phaseNumber == 2){//飛行中
00243             if(downlinkCount % 10 == 0){
00244                 
00245                 xbeeAPI.StartPacket(0x12);
00246                 xbeeAPI.Write((int)innerTime.read_us());
00247                 xbeeAPI.Write((float)altitude);//100.0f);//(float)gps.Height());
00248                 xbeeAPI.Write((float)speed_knot);//80.0f);//(float)gps.Knot());
00249                 xbeeAPI.Write((float)head);//270.0f);//(float)gps.Degree());
00250                 xbeeAPI.Write((double)pressure);//気圧
00251                 xbeeAPI.Write(flightPinSeparationFlag);
00252                 xbeeAPI.EndPacket();
00253             }
00254             else{
00255                 xbeeAPI.StartPacket(0x10);
00256                 xbeeAPI.Write((float)myQ.x);//姿勢クォータニオン
00257                 xbeeAPI.Write((float)myQ.y);
00258                 xbeeAPI.Write((float)myQ.z);
00259                 xbeeAPI.Write((float)myQ.w);
00260                 xbeeAPI.Write((float)(KI[0] - 139.0));//gps.Longitude());//GPS座標
00261                 xbeeAPI.Write((float)(KI[1] - 34.0));//gps.Latitude());
00262                 xbeeAPI.EndPacket();          
00263                 //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);         
00264            }
00265     }
00266     else if(phaseNumber == 3){//回収待機中
00267             xbeeAPI.StartPacket(0x13);
00268             xbeeAPI.Write((float)(KI[0] - 139.0));//130.901722222);//gps.Longitude());//GPS座標
00269             xbeeAPI.Write((float)(KI[1] - 34.0));//30.4103833333);//gps.Latitude());
00270             //xbeeAPI.Write((double)pressure);
00271             xbeeAPI.Write((float)bt.debug()*9.9f);
00272             xbeeAPI.Write((float)ext_bt.debug()*9.90f);
00273             xbeeAPI.Write((float)usingMainbt.debug()*9.90f);
00274             xbeeAPI.EndPacket(); 
00275     } 
00276     downlinkCount++;  
00277 }
00278 
00279 inline void Print(char* str){
00280     //pc.printf("%s\r\n", str);
00281     string s(str);
00282     xbeeAPI.StartPacket(0x50);
00283     xbeeAPI.Write(s);//文字列
00284     xbeeAPI.EndPacket();  
00285     return;   
00286 }
00287 // パケット受信時の処理    Length: データ長   *message: データ
00288 void OnXbeeDataReceived(int Length, char *message){
00289     pc.printf("%d\t%s\r\n",Length, message);
00290     if(message[0] == 0x58){//'X'サーボ
00291         if(message[1] == 0xFF){
00292             zeroAngle = currentAngle;
00293             Print("Set Zero Angle");
00294         }
00295         else{
00296             currentAngle = (double)(message[1] - 90);
00297             servo = currentAngle;
00298             servo2 = currentAngle;
00299             char mojiretu[32] = "";
00300             sprintf(mojiretu, "Set Servo: %d", (int)message[1]); 
00301             Print(mojiretu);
00302         }
00303     }
00304     else if(message[0] == 0x46){//'F' フライトピン待機開始許可フラグ
00305         if((flightPin.read() == 1) && (message[1] == 0x4F)){//'O' Flightpin OK
00306             flightPinOKFlag = 1;   
00307             Print("Start Wait Flight Pin");
00308         }
00309         else if(message[1] == 0x4E){
00310             flightPinOKFlag = 0;
00311             Print("Flight Pin Reset!!\r\n");   
00312         }
00313     }
00314     //指令分離
00315     else if((flightPinFlag == 1) && (Length > 5) && (message[0] == 'J')){//J B K C M D
00316         if(message[1] == 'B'){
00317             if(message[2] == 'K'){
00318                 if(message[3] == 'C'){
00319                     if(message[4] == 'M'){
00320                         if(message[5] == 'D'){
00321                             mainParaSepaFlag = 1; 
00322                         }   
00323                     }   
00324                 }   
00325             }
00326         }
00327     }
00328 }
00329 
00330 //------------------------------------------------------------------------------
00331 //   スケジュール割り込み関数
00332 //------------------------------------------------------------------------------
00333 const int CALL_PERIOD = 50000;//us
00334 
00335 Ticker ScheduleCallTimer;//schedule
00336 int sdSaveFlag = 0;
00337 void autoCallFunc(){
00338     
00339     //static uint64_t timingCount = 0;
00340     sdSaveFlag = 1;
00341     downLink();
00342     //timingCount++;
00343 }
00344 /**
00345 *  0.2秒ごとに増えていく
00346 */
00347 int sepaCount = 0;
00348 Ticker SepaCountTimer;
00349 void SepaCountFunc(){
00350     sepaCount++;
00351     return;   
00352 }
00353 
00354 //------------------------------------------------------------------------------
00355 //   1段目分離からの2段目分離タイマー
00356 //------------------------------------------------------------------------------
00357 Timeout secondSepaDelayTimer;
00358 void secondSepaDelayFunc(){
00359     mainParaSepaFlag = 1;
00360     return;   
00361 }
00362 //------------------------------------------------------------------------------
00363 //   フライトピンが抜けてから6秒は分離しないというセーフティ的な何か
00364 //------------------------------------------------------------------------------
00365 int safetyFlag = 0;
00366 Timeout safetyTimer;
00367 void safetyFunc(){
00368     safetyFlag = 1;
00369     return;   
00370 }
00371 
00372 //------------------------------------------------------------------------------
00373 //  0.04秒ごとに気圧取得許可フラグを立てる
00374 //------------------------------------------------------------------------------
00375 Ticker PressureGetTimer;
00376 int pressureGetFlag = 0;
00377 void pressureGetFunc(){
00378     pressureGetFlag = 1;
00379     return;
00380 }
00381 
00382 int main() {
00383     //setPress();
00384     //XBee初期設定
00385     // 0番目の宛先アドレス
00386     xbeeAPI.SetAddress(0, 0x0013A200, 0x40A88F02);
00387     // 1番目の宛先アドレス
00388     xbeeAPI.SetAddress(1, 0x0013A200, 0x40E7D43E);
00389     // 2番目の宛先アドレス
00390     xbeeAPI.SetAddress(2, 0x0013A200, 0x40DC2538);
00391     //3番目の宛先アドレス
00392     xbeeAPI.SetAddress(3, 0x0013A200, 0x40BABC20);
00393    
00394     flightPinSeparationFlag = 0b00000000;
00395    
00396     Print("start\r\n");
00397     Print("-----------------------------------------");
00398     Print("Boot Phase:");  
00399     pc.printf("start\r\n");
00400     pc.printf("\r\n-----------------------------------------\r\n");
00401     pc.printf("Boot Phase:\r\n");  
00402     for(char i = 0;i < 100;i++){
00403         pc.printf("Loading: %3d [%%]\r", i);
00404         wait_ms(30); 
00405     } 
00406     Print("Loading: 100 [%%] -> finished!!");
00407     Print("-----------------------------------------");
00408     pc.printf("Loading: 100 [%%] -> finished!!\r\n");
00409     pc.printf("\r\n-----------------------------------------\r\n");
00410 
00411     /*==========================================================================
00412         フェーズⅠ   < 発射待機・システムチェック > 
00413     ===========================================================================*/
00414     phaseNumber = 1;
00415     int temp_time = 0;
00416     Print("----------------");
00417     Print("Phase I START");
00418     pc.printf("\r\n----------------\r\n");
00419     pc.printf("Phase I START\r\n");
00420     
00421     Print("SD check ->");
00422     pc.printf("SD check -> ");
00423     fp = fopen("/sd/check.txt", "w");
00424     if(fp == NULL){
00425         while(1){
00426             Print("SD ERROR");
00427             error("SD ERROR\r\n");   
00428         }   
00429     }
00430     else{
00431         pc.printf("OK\r\n");
00432         Print("\tOK");
00433         fprintf(fp,"this file is made by checking sd card program.\r\n");
00434         fclose(fp);   
00435     }
00436     
00437     while(1){
00438         static int fileCount = 0;
00439         sprintf(datafile, "/sd/datafile%d.txt", fileCount);
00440         fp = fopen(datafile, "rb");
00441         if(fp != NULL){
00442             fclose(fp);
00443             fileCount++;   
00444         }
00445         else{
00446             fp = fopen(datafile, "wb");
00447             if(fp == NULL){//ファイルオープン失敗
00448                 while(1){
00449                     Print("file open error\r\n");
00450                     pc.printf("file open error\r\n");
00451                     wait(1.0);
00452                 }
00453             }
00454             fclose(fp);
00455             fp = fopen(datafile, "wb");
00456             if(fp != NULL){
00457                 fclose(fp);   
00458             }
00459             break;   
00460         }
00461     }
00462     
00463     
00464     //内部タイマースタート
00465     innerTime.reset();
00466     innerTime.start();
00467     lps25h.SetPress();
00468 
00469     //9軸センサ 初期設定
00470     nine.setAccLPF(SET_ACC_LPF);
00471     nine.setGyro(SET_GYRO);
00472     nine.setAcc(SET_ACC);
00473     //9軸センサ オフセット設定
00474     nine.setOffset(nine_offset[0], nine_offset[1], nine_offset[2],
00475                    nine_offset[3], nine_offset[4], nine_offset[5],
00476                    nine_offset[6], nine_offset[7], nine_offset[8]);
00477     
00478     Print("finished mpu9250 setting");
00479     pc.printf("finished mpu9250 setting");
00480     
00481     //割り込みセンサーアクセス関数セット
00482     MadgwickFilter attitude(0.1);
00483     ScheduleCallTimer.attach_us(&autoCallFunc, CALL_PERIOD);//&downLink, 50000);//
00484     PressureGetTimer.attach(&pressureGetFunc, 0.04f);   
00485  
00486     
00487     //フライトピンをさすループ
00488 waitFlightPinLabel:
00489     flightPin.clear();
00490     flightPinOKFlag = 0;
00491     Print("wait for connect Flight Pin");
00492     pc.printf("wait for connect Flight Pin");
00493     
00494     flightPinSeparationFlag = 0b10000000;//7bit目を0に
00495     
00496     while(1){
00497         nine.getGyroAcc(imu);//慣性データ取得
00498         nine.getMag(mag);//地磁気データ取得
00499         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]);//姿勢推定
00500         attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
00501         if(pressureGetFlag){
00502             pressureGetFlag = 0;
00503             lps25h.GetPress(&pressure, &pressureRaw);
00504         }
00505         pc.printf("Attitude:%f,%f,%f,%f\r\n", myQ.x, myQ.y, myQ.z, myQ.w);  
00506         pc.printf("%f\r\n",pressure);
00507         //pc.printf("%f\t%f\r\n", KI[0], KI[1]);
00508         if(flightPin.read() == 1){
00509             flightPinSeparationFlag = flightPinSeparationFlag & 0b01111111;//7bit目を0に
00510             if(flightPinOKFlag == 1){
00511                 break;   
00512             }   
00513             else if(pc.readable()){//デバッグ
00514                 char flightChar = pc.getc();
00515                 if(flightChar == 'F'){
00516                     flightPinOKFlag = 1;
00517                     break;
00518                 }   
00519             }
00520         }
00521         else flightPinSeparationFlag |= 0b10000000;
00522         
00523         if(abs(temp_time - innerTime.read_ms()) > 500){
00524            // Print("wait for connect Flight Pin and send F.");
00525             pc.printf("wait for connect Flight Pin and send F.");
00526             temp_time = innerTime.read_ms();
00527         }
00528         
00529         //分離機構が作動してしまった時の例のやつ
00530         if(DetectDrogueSeparation.read() == 1){
00531             flightPinSeparationFlag = flightPinSeparationFlag | 0b00100000;   
00532         }
00533         else{
00534             flightPinSeparationFlag = flightPinSeparationFlag & 0b11011111;   
00535         }
00536     }
00537 
00538     //フライトピン待機
00539     temp_time = innerTime.read_ms();
00540     flightPinFlag = 0;
00541     
00542     flightPinSeparationFlag = flightPinSeparationFlag | 0b01000000;//6bit目を1に
00543     
00544     while(flightPinFlag == 0){
00545         
00546         nine.getGyroAcc(imu);//慣性データ取得
00547         nine.getMag(mag);//地磁気データ取得
00548         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]);//姿勢推定
00549         attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
00550          pc.printf("Attitude:%f,%f,%f,%f\r\n", myQ.x, myQ.y, myQ.z, myQ.w);  
00551         if(pressureGetFlag){
00552             pressureGetFlag = 0;
00553             lps25h.GetPress(&pressure, &pressureRaw);
00554         } 
00555         if(abs(temp_time - innerTime.read_ms()) > 5000){
00556             pc.printf("Wait for Flight Pin Left");
00557             temp_time = innerTime.read_ms();
00558         }
00559         if(flightPinOKFlag == 0){
00560             goto waitFlightPinLabel;
00561         }
00562         
00563         //分離機構が作動してしまった時の例のやつ
00564         if(DetectDrogueSeparation.read() == 1){
00565             flightPinSeparationFlag = flightPinSeparationFlag | 0b00100000;   
00566         }
00567         else{
00568             flightPinSeparationFlag = flightPinSeparationFlag & 0b11011111;   
00569         }
00570     }
00571     
00572     flightPinSeparationFlag = flightPinSeparationFlag | 0b10000000;//7bit目を1に 0b11000000になっているはず
00573     Print("Detect Flight Pin!!");
00574     pc.printf("Detect Flight Pin!!");
00575     Print("Launch!!!!Launch!!!!");
00576     Print("Phase I END");
00577     pc.printf("Launch!!!!Launch!!!!");
00578     pc.printf("Phase I END");
00579     /*==========================================================================
00580         フェーズII  < 飛行中 >
00581     ==========================================================================*/
00582     Print("Phase II START");
00583     pc.printf("Phase II START");
00584     //色々初期化
00585     innerTime.reset();//内部時間
00586     phaseNumber = 2;//フェーズナンバー
00587     downlinkCount = 0;//
00588     detectSepaFlag1 = 0;
00589     safetyFlag = 0;
00590     safetyTimer.attach(&safetyFunc, SAFETY_TIME);
00591     
00592     drogueSeparationTimer.attach(&drogueSeparation, DROGUE_WAIT_TIME);
00593     mainParaSeparationTimer.attach(&mainParaSeparation, MAIN_PARA_WAIT_TIME);
00594     
00595     static int SepaStartFlag = 0;
00596     double oldPressure = (double)lps25h.GetPress();
00597     double deltaP = 0;   
00598     int pressureCount = 0;
00599     
00600     while(1){
00601         
00602         //分離機構が作動してしまった時の例のやつ
00603         if(DetectDrogueSeparation.read() == 1)  flightPinSeparationFlag = flightPinSeparationFlag | 0b00100000;   
00604         else flightPinSeparationFlag = flightPinSeparationFlag & 0b11011111;   
00605         
00606         temp_time = innerTime.read_ms();
00607         nine.getGyroAcc(imu);//慣性データ取得
00608         nine.getMag(mag);//地磁気データ取得
00609         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]);//姿勢推定
00610         attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
00611         
00612         if(pressureGetFlag){//0.04秒ごとに気圧取得&比較
00613             pressureGetFlag = 0;
00614             lps25h.GetPress(&pressure, &pressureRaw);//気圧取得
00615             
00616             //pc.printf("%f\t%f\r\n",innerTime.read(), pressure);
00617             
00618             //気圧による分離条件式
00619             deltaP = pressure - oldPressure;//気圧の変化勾配
00620             if((deltaP > FALLING_THRESHOLD) && (safetyFlag == 1) && (pressure != 0.0)){//気圧差 > -0.04 かつ 6秒以上経過
00621                 pressureCount++;
00622                 if(pressureCount > 5){//ドラッグシュート分離
00623                     //fastPressureFlag = 1;
00624                     drogueSepaFlag = 1;
00625                     secondSepaDelayTimer.attach(&secondSepaDelayFunc, SECOND_SEPA_DELAY_TIME);   
00626                 }
00627             }
00628             else{
00629                pressureCount = 0;   
00630             }
00631             oldPressure = pressure;
00632         }
00633         
00634         if(drogueSepaFlag == 1 ){//ドラッグシュート分離機構作動
00635                
00636             if(SepaStartFlag == 0){
00637                 pc.printf("Separation %f [s]\r\n", innerTime.read());
00638                 //ビットフラグ更新
00639                 flightPinSeparationFlag = (flightPinSeparationFlag & 0b11000111) | 0b00010000;//3bit目を1に, 作動させたがまだわからない
00640                 SepaStartFlag = 1;
00641                 SepaCountTimer.attach(&SepaCountFunc, 0.2f);
00642                 sepaCount = 0;   
00643                 Print("Try Drogue Separation!!");
00644                 pc.printf("Try Drogue Separation!!\r\n");
00645                 servo = 30.0 + zeroAngle;
00646                 servo2 = 30.0 + zeroAngle;
00647             }
00648             else{
00649                 if(sepaCount != 0){
00650                     if(sepaCount % 2 == 0){
00651                         servo = 35.0 + zeroAngle;
00652                         servo2 = 35.0 + zeroAngle;                                    
00653                     }
00654                     else if(sepaCount % 2 == 1){
00655                         servo = 25.0 + zeroAngle;
00656                         servo2 = 25.0 + zeroAngle;
00657                     }
00658                 }
00659                 //ドラッグシュート分離機構が作動したか (これは常に確認する
00660                 /*if(detectSepaFlag1 == 1){
00661                         flightPinSeparationFlag = (flightPinSeparationFlag & 0b11000111) | 0b00111000;//2bit目を1に
00662                         Print("Succeeded Drogue Separation!!\r\n");
00663                         Print("Finished Controling Servo Motor\r\n"); 
00664                         pc.printf("Succeeded Drogue Separation!!\r\n");
00665                         pc.printf("Finished Controling Servo Motor\r\n"); 
00666                 }*/
00667                 
00668                 if(sepaCount > 24){//2秒間経過 
00669                     mainParaSepaFlag = 1;
00670                     drogueSepaFlag = 0;//ドラッグシュート分離を停止．
00671                     /*if(detectSepaFlag1 == 0){//2秒間サーボを動かして，だめだったとき
00672                         //pc.printf("Separation2 %f [s]\r\n", innerTime.read());
00673                         flightPinSeparationFlag = (flightPinSeparationFlag & 0b11000111) | 0b00110000;
00674                         drogueSepaFlag = 0;
00675                         Print("Failed Drogue Separation...\r\n");
00676                         pc.printf("Failed Drogue Separation...\r\n");
00677                         //mainParaSepaFlag = 1;//強制メインパラ分離
00678                         Print("Finished Controling Servo Motor\r\n"); 
00679                         pc.printf("Finished Controling Servo Motor\r\n"); 
00680                     }*/
00681                     pc.printf("Finished Controling Servo Motor\r\n");
00682                 }
00683             }
00684         }
00685         
00686         if(mainParaSepaFlag == 1){
00687             flightPinSeparationFlag = flightPinSeparationFlag | 0b00000111;
00688             pc.printf("Separation2 %f [s]\r\n", innerTime.read());
00689             Print("Try Main Separation!!");
00690             pc.printf("Try Main Separation!!\r\n");
00691             nicrom.autoStop(5.0f);
00692             mainParaSepaFlag = 0;
00693             Print("Finished Nicrom Control");
00694             pc.printf("Finished Nicrom Control\r\n");
00695             break;
00696         }
00697         
00698         //SD保存
00699         
00700         if(sdSaveFlag == 1){
00701             sdWriteLed = 1;
00702             sdSaveFlag = 0;
00703             temp_time = innerTime.read_us();
00704             fp = fopen(datafile, "ab");//14
00705             if(fp != NULL){
00706                 for(int i = 0; i < 6;i++){
00707                     sdBuff[i] = imu[i];   
00708                 }
00709                 for(int i = 0;i < 3;i++){
00710                     sdBuff[i+6] = mag[i];    
00711                 }
00712                 sdBuff[9] = myQ.x; sdBuff[10] = myQ.y; sdBuff[11] = myQ.z; sdBuff[12] = myQ.w; sdBuff[13] = pressure;
00713                 fwrite(&temp_time, sizeof(int), 1, fp);
00714                 fwrite(&flightPinSeparationFlag, sizeof(char), 1, fp);
00715                 fwrite(sdBuff, sizeof(double), 14, fp);
00716                 fclose(fp);
00717             }
00718             sdWriteLed = 0;
00719         }
00720     }   
00721     temp_time = innerTime.read_ms();
00722     while(1){
00723         nine.getGyroAcc(imu);//慣性データ取得
00724         nine.getMag(mag);//地磁気データ取得
00725         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]);//姿勢推定
00726         attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
00727         if(pressureGetFlag){
00728             pressureGetFlag = 0;
00729             lps25h.GetPress(&pressure, &pressureRaw);   
00730         }
00731         if(abs(innerTime.read_ms() - temp_time) > 30000){
00732             break;   
00733         }
00734     }
00735     Print("Phase II END");
00736     pc.printf("Phase II END\r\n");
00737     /*==========================================================================
00738         フェーズⅢ  < 回収待機 >
00739     ==========================================================================*/
00740     Print("Phase III START");
00741     pc.printf("Phase III START");
00742     phaseNumber = 3;
00743     downlinkCount = 0;
00744    
00745     temp_time = innerTime.read_ms();
00746     while(1){
00747         nine.getGyroAcc(imu);//慣性データ取得
00748         nine.getMag(mag);//地磁気データ取得
00749         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]);//姿勢推定
00750         attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
00751         if(pressureGetFlag == 1){
00752             pressureGetFlag = 0;
00753             lps25h.GetPress(&pressure, &pressureRaw);
00754         }
00755         if(abs(innerTime.read_ms() - temp_time) > 5000){
00756             pc.printf("%f\t%f\t%f\t%f\r\n",innerTime.read(), gps.Longitude(), gps.Latitude(), pressure);
00757             temp_time = innerTime.read_ms();
00758         }
00759     }
00760 }