PQ_Hybrid_Electrical_Equipment_Team
2016年度伊豆大島共同打ち上げ実験で使用したプログラム
Dependencies: Battery_monitoring ExternalTrigger GPS_interrupt LPS25H_ver2 MadgwickFilter Nicrom Quaternion SDFileSystem Servo XbeeApiLib mbed mpu9250_i2c
Diff: Hybrid_main_FirstEdtion.cpp
- Revision:
- 0:3e847864d796
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Hybrid_main_FirstEdtion.cpp Mon Jul 17 10:09:15 2017 +0000
@@ -0,0 +1,760 @@
+#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();
+ }
+ }
+}