PQ_Hybrid_Electrical_Equipment_Team / Mbed 2 deprecated Hybrid_main_FirstEdtion

2016年度伊豆大島共同打ち上げ実験で使用したプログラム

Dependencies:   Battery_monitoring ExternalTrigger GPS_interrupt LPS25H_ver2 MadgwickFilter Nicrom Quaternion SDFileSystem Servo XbeeApiLib mbed mpu9250_i2c

Hybrid_main_FirstEdtion.cpp@0:3e847864d796, 2017-07-17 (annotated)

Committer:
Gaku0606
Date:
Mon Jul 17 10:09:15 2017 +0000
Revision:
0:3e847864d796
www

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gaku0606 0:3e847864d796 1 #include "mbed.h"
Gaku0606 0:3e847864d796 2 #include "SDFileSystem.h"
Gaku0606 0:3e847864d796 3 #include "Quaternion.hpp"
Gaku0606 0:3e847864d796 4 #include "MadgwickFilter.hpp"
Gaku0606 0:3e847864d796 5 #include "mpu9250_i2c.h"
Gaku0606 0:3e847864d796 6 #include "GPS_interrupt.h"
Gaku0606 0:3e847864d796 7 #include "ExternalTrigger.hpp"
Gaku0606 0:3e847864d796 8 #include "nicrom.hpp"
Gaku0606 0:3e847864d796 9 #include "battery_monitoring.hpp"
Gaku0606 0:3e847864d796 10 #include "XbeeApiLib.h" // XbeeApiLib のインクルード
Gaku0606 0:3e847864d796 11 #include "Servo.h"
Gaku0606 0:3e847864d796 12 #include <string>
Gaku0606 0:3e847864d796 13 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 14 // なんかこう計算を速くしたり，よく使うもの
Gaku0606 0:3e847864d796 15 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 16 const double PI = 3.1415926535897932384626433832795;//4.0*atan(1.0);
Gaku0606 0:3e847864d796 17 const double DEG_TO_RAD = 0.01745329251994329576923690768489;//PI / 180.0;
Gaku0606 0:3e847864d796 18 const double RAD_TO_DEG = 57.295779513082320876798154814105;//180.0 / PI;
Gaku0606 0:3e847864d796 19 const double ACC_LPF_COEF = 0.95;
Gaku0606 0:3e847864d796 20 const double GYRO_LPF_COEF = 0.5f;
Gaku0606 0:3e847864d796 21 const double MAG_LPF_COEF = 0.95;
Gaku0606 0:3e847864d796 22 #define EARTH_EQUATOR_RADIUS 6378136.6//地球の赤道半径[m]
Gaku0606 0:3e847864d796 23 #define EARTH_POLAR_RADIUS 6356751.9//地球の極半径[m]
Gaku0606 0:3e847864d796 24 static int phaseNumber = 0;
Gaku0606 0:3e847864d796 25 static unsigned long system_count = 0;
Gaku0606 0:3e847864d796 26 FILE *fp;
Gaku0606 0:3e847864d796 27 char datafile[64] = "datafile0.txt";
Gaku0606 0:3e847864d796 28 //#include <new>
Gaku0606 0:3e847864d796 29
Gaku0606 0:3e847864d796 30 DigitalOut sdWriteLed(LED3);
Gaku0606 0:3e847864d796 31 /*
Gaku0606 0:3e847864d796 32 void no_memory () {
Gaku0606 0:3e847864d796 33 printf("panic: can't allocate to memory!\r\n");
Gaku0606 0:3e847864d796 34 exit(-1);
Gaku0606 0:3e847864d796 35 }
Gaku0606 0:3e847864d796 36 //
Gaku0606 0:3e847864d796 37 extern "C" void HardFault_Handler() {
Gaku0606 0:3e847864d796 38 printf("Hard Fault!\n");
Gaku0606 0:3e847864d796 39 while(1);
Gaku0606 0:3e847864d796 40 }*/
Gaku0606 0:3e847864d796 41 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 42 // すぐに変更できたらいいもの
Gaku0606 0:3e847864d796 43 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 44 #define SET_ACC_LPF NO_USE
Gaku0606 0:3e847864d796 45 #define SET_GYRO _1000DPS
Gaku0606 0:3e847864d796 46 #define SET_ACC _16G
Gaku0606 0:3e847864d796 47 const double nine_offset[9] = {0.32766334f, 3.067240359f, -0.326759467f,
Gaku0606 0:3e847864d796 48 0.0052546f, -0.009152758f, 0.142725298f,
Gaku0606 0:3e847864d796 49 10.875, -24, 41.55f};
Gaku0606 0:3e847864d796 50 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 51 // 諸々のパラメータ
Gaku0606 0:3e847864d796 52 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 53 #define SECOND_SEPA_DELAY_TIME 5.0f//[s] 気圧で1段目分離機構を作動させてから2段目を起動させるまでの時間
Gaku0606 0:3e847864d796 54 #define MAIN_PARA_WAIT_TIME 15.0f//フライトピンが抜けてから分離するまでの時間[s]
Gaku0606 0:3e847864d796 55 #define DROGUE_WAIT_TIME 10.0f
Gaku0606 0:3e847864d796 56 #define FALLING_THRESHOLD -0.04//hPa 落下しているとみなせる気圧差
Gaku0606 0:3e847864d796 57 const float SAFETY_TIME = 6.0f;//フライトピンが抜けてから分離しない時間
Gaku0606 0:3e847864d796 58
Gaku0606 0:3e847864d796 59 const int FALLING_COUNT = 100;//この回数，気圧が連続で上昇したら落下とする
Gaku0606 0:3e847864d796 60
Gaku0606 0:3e847864d796 61 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 62 // ダウンリンク系のインスタンス生成
Gaku0606 0:3e847864d796 63 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 64 Serial pc(USBTX,USBRX, 115200);
Gaku0606 0:3e847864d796 65 void downLink();
Gaku0606 0:3e847864d796 66 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 67 // 内部時間タイマー
Gaku0606 0:3e847864d796 68 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 69 Timer innerTime;
Gaku0606 0:3e847864d796 70
Gaku0606 0:3e847864d796 71 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 72 // 9軸センサー
Gaku0606 0:3e847864d796 73 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 74 #define NINE_SDA p9
Gaku0606 0:3e847864d796 75 #define NINE_SCL p10
Gaku0606 0:3e847864d796 76
Gaku0606 0:3e847864d796 77 I2C i2cBus(NINE_SDA, NINE_SCL);
Gaku0606 0:3e847864d796 78 mpu9250 nine(i2cBus, AD0_HIGH);
Gaku0606 0:3e847864d796 79
Gaku0606 0:3e847864d796 80 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 81 // サーボモーター ドラッグシュート放出用分離機構
Gaku0606 0:3e847864d796 82 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 83 int fastPressureFlag = 0;//気圧による分離命令がタイマーより先だったとき 1 になる．
Gaku0606 0:3e847864d796 84 #define MAX_PULSE 0.00245//[s]
Gaku0606 0:3e847864d796 85 #define MIN_PULSE 0.00065//[s]
Gaku0606 0:3e847864d796 86 #define SERVO_PIN p26
Gaku0606 0:3e847864d796 87 #define SERVO_PIN2 p25
Gaku0606 0:3e847864d796 88 Servo servo(SERVO_PIN, MAX_PULSE, MIN_PULSE);
Gaku0606 0:3e847864d796 89 Servo servo2(SERVO_PIN2, MAX_PULSE, MIN_PULSE);
Gaku0606 0:3e847864d796 90 volatile int drogueSepaFlag = 0;//ドラッグシュート放出フラグ
Gaku0606 0:3e847864d796 91 Timeout drogueSeparationTimer;//ドラッグシュート放出タイマー
Gaku0606 0:3e847864d796 92 float currentAngle = 0;
Gaku0606 0:3e847864d796 93 float zeroAngle = 0;
Gaku0606 0:3e847864d796 94
Gaku0606 0:3e847864d796 95 void drogueSeparation(){
Gaku0606 0:3e847864d796 96 if(fastPressureFlag == 0){
Gaku0606 0:3e847864d796 97 drogueSepaFlag = 1;
Gaku0606 0:3e847864d796 98 }
Gaku0606 0:3e847864d796 99 return;
Gaku0606 0:3e847864d796 100 }
Gaku0606 0:3e847864d796 101 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 102 // ニクロム線 メインパラシュート放出分離機構
Gaku0606 0:3e847864d796 103 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 104 #define NICROM_PIN p30
Gaku0606 0:3e847864d796 105 #define NICROM_PIN2 p23
Gaku0606 0:3e847864d796 106 Nicrom nicrom(NICROM_PIN);
Gaku0606 0:3e847864d796 107 Nicrom nicrom2(NICROM_PIN2);
Gaku0606 0:3e847864d796 108
Gaku0606 0:3e847864d796 109 int mainParaSepaFlag = 0;
Gaku0606 0:3e847864d796 110 Timeout mainParaSeparationTimer;//メインパラシュート放出タイマー
Gaku0606 0:3e847864d796 111
Gaku0606 0:3e847864d796 112 void mainParaSeparation(){
Gaku0606 0:3e847864d796 113 if(fastPressureFlag == 0){
Gaku0606 0:3e847864d796 114 mainParaSepaFlag = 1;
Gaku0606 0:3e847864d796 115 }
Gaku0606 0:3e847864d796 116 return;
Gaku0606 0:3e847864d796 117 }
Gaku0606 0:3e847864d796 118
Gaku0606 0:3e847864d796 119 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 120 // フライトピン用外部割り込み設定
Gaku0606 0:3e847864d796 121 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 122
Gaku0606 0:3e847864d796 123 #define FLIGHT_PIN p24
Gaku0606 0:3e847864d796 124 static volatile int flightPinFlag = 0;
Gaku0606 0:3e847864d796 125 static volatile int flightPinOKFlag = 0;
Gaku0606 0:3e847864d796 126
Gaku0606 0:3e847864d796 127 void flightPinCallFunc(){//この関数内でニクロム線に電流を流してもいいかなと考えたりもした
Gaku0606 0:3e847864d796 128 flightPinFlag = 1;
Gaku0606 0:3e847864d796 129 return;
Gaku0606 0:3e847864d796 130 }
Gaku0606 0:3e847864d796 131 //InterruptIn flightPin(FLIGHT_PIN);
Gaku0606 0:3e847864d796 132 ExtTrigger flightPin(FLIGHT_PIN, FALL, &flightPinCallFunc, 0.2f);//ピン番号，立ち下がり，呼び出す関数，チャタリング待ち時間
Gaku0606 0:3e847864d796 133
Gaku0606 0:3e847864d796 134 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 135 // 分離検知
Gaku0606 0:3e847864d796 136 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 137 #define DETECT_SEPARATION_PIN1 p12
Gaku0606 0:3e847864d796 138 static int detectSepaFlag1 = 0;
Gaku0606 0:3e847864d796 139 void detectDrogueSepa(){
Gaku0606 0:3e847864d796 140 detectSepaFlag1 = 1;
Gaku0606 0:3e847864d796 141 return;
Gaku0606 0:3e847864d796 142 }
Gaku0606 0:3e847864d796 143 ExtTrigger DetectDrogueSeparation(DETECT_SEPARATION_PIN1, RISE, &detectDrogueSepa, 0.2f);
Gaku0606 0:3e847864d796 144
Gaku0606 0:3e847864d796 145 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 146 // バッテリー電圧チェック
Gaku0606 0:3e847864d796 147 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 148 #define MAIN_BATTERY_PIN p18
Gaku0606 0:3e847864d796 149 #define EXT_BATTERY_PIN p19
Gaku0606 0:3e847864d796 150 #define USING_MAIN_PIN p20
Gaku0606 0:3e847864d796 151 battery_monitoring bt(2.6118, 2.9647, MAIN_BATTERY_PIN );
Gaku0606 0:3e847864d796 152 battery_monitoring ext_bt(2.6118, 2.9647, EXT_BATTERY_PIN);
Gaku0606 0:3e847864d796 153 battery_monitoring usingMainbt(2.6118, 2.9647, USING_MAIN_PIN);
Gaku0606 0:3e847864d796 154
Gaku0606 0:3e847864d796 155 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 156 // GPS 関連
Gaku0606 0:3e847864d796 157 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 158 Serial mygps(p28, p27, 115200);
Gaku0606 0:3e847864d796 159 GPS_interrupt gps(&mygps);
Gaku0606 0:3e847864d796 160 /*
Gaku0606 0:3e847864d796 161 static double K = 0;
Gaku0606 0:3e847864d796 162 static double I = 0;*/
Gaku0606 0:3e847864d796 163 static double KI[2] = {0.0, 0.0};
Gaku0606 0:3e847864d796 164 static int satellitesNumber = 0;//捕捉衛星数
Gaku0606 0:3e847864d796 165 static double altitude = 0;//高度
Gaku0606 0:3e847864d796 166 static double speed_knot = 0;//対地速度[m/s]
Gaku0606 0:3e847864d796 167 static double head = 0;//進行方角，北から左回り正，0~359[°]
Gaku0606 0:3e847864d796 168
Gaku0606 0:3e847864d796 169 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 170 // microSDカードの設定
Gaku0606 0:3e847864d796 171 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 172 #define MOSI p5
Gaku0606 0:3e847864d796 173 #define MISO p6
Gaku0606 0:3e847864d796 174 #define SCK p7
Gaku0606 0:3e847864d796 175 #define CS_SD p8
Gaku0606 0:3e847864d796 176 SDFileSystem sd(MOSI, MISO, SCK, CS_SD, "sd");
Gaku0606 0:3e847864d796 177 static double sdBuff[14] = {0.0};
Gaku0606 0:3e847864d796 178 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 179 // 割り込みでMadgwickFilterを回す
Gaku0606 0:3e847864d796 180 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 181 static double imu[6] = {0};//角速度xyz，加速度xyz
Gaku0606 0:3e847864d796 182 static double mag[3] = {0};//地磁気xyz
Gaku0606 0:3e847864d796 183 static Quaternion myQ(1.0f,0.0f,0.0f,0.0f);//姿勢のクォータニオン
Gaku0606 0:3e847864d796 184
Gaku0606 0:3e847864d796 185
Gaku0606 0:3e847864d796 186
Gaku0606 0:3e847864d796 187 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 188 // LPS25H
Gaku0606 0:3e847864d796 189 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 190 #include "LPS25H.h"
Gaku0606 0:3e847864d796 191 MyLPS25H lps25h(i2cBus);
Gaku0606 0:3e847864d796 192
Gaku0606 0:3e847864d796 193 double pressure = 0;
Gaku0606 0:3e847864d796 194 uint32_t pressureRaw = 0;
Gaku0606 0:3e847864d796 195 int fallingFlag = 0;
Gaku0606 0:3e847864d796 196
Gaku0606 0:3e847864d796 197 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 198 // ダウンリンク
Gaku0606 0:3e847864d796 199 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 200 // パケット受信時に呼ばれてほしい関数のプロトタイプ宣言 Length: データ長 *message: データ
Gaku0606 0:3e847864d796 201 void OnXbeeDataReceived(int Length, char *message);
Gaku0606 0:3e847864d796 202
Gaku0606 0:3e847864d796 203 XbeeApi xbeeAPI(p13, p14, 115200, OnXbeeDataReceived);
Gaku0606 0:3e847864d796 204 static uint64_t downlinkCount = 0;
Gaku0606 0:3e847864d796 205 char flightPinSeparationFlag = 0b00000000;
Gaku0606 0:3e847864d796 206 inline void downLink(){
Gaku0606 0:3e847864d796 207
Gaku0606 0:3e847864d796 208 if(gps.gps_readable){
Gaku0606 0:3e847864d796 209 KI[0] = (double)gps.Longitude();
Gaku0606 0:3e847864d796 210 KI[1] = (double)gps.Latitude();
Gaku0606 0:3e847864d796 211 satellitesNumber = gps.Number();
Gaku0606 0:3e847864d796 212 altitude = gps.Height();
Gaku0606 0:3e847864d796 213 speed_knot = gps.Knot();
Gaku0606 0:3e847864d796 214 head = gps.Degree();
Gaku0606 0:3e847864d796 215 }
Gaku0606 0:3e847864d796 216 else{
Gaku0606 0:3e847864d796 217 KI[0]= 0.0; KI[1] = 0.0; satellitesNumber = 0; altitude = 0.0; speed_knot = 0.0; head = 0.0;
Gaku0606 0:3e847864d796 218 }
Gaku0606 0:3e847864d796 219
Gaku0606 0:3e847864d796 220 if(phaseNumber == 1){//フライトピンが抜けるまで
Gaku0606 0:3e847864d796 221 if(downlinkCount % 20 == 0){
Gaku0606 0:3e847864d796 222 xbeeAPI.StartPacket(0x11);
Gaku0606 0:3e847864d796 223 xbeeAPI.Write((float)bt.debug()*9.9f);
Gaku0606 0:3e847864d796 224 xbeeAPI.Write((float)ext_bt.debug()*9.90f);
Gaku0606 0:3e847864d796 225 xbeeAPI.Write((float)usingMainbt.debug()*9.90f);
Gaku0606 0:3e847864d796 226 xbeeAPI.Write((float)altitude);
Gaku0606 0:3e847864d796 227 xbeeAPI.Write((double)pressure);//気圧
Gaku0606 0:3e847864d796 228 xbeeAPI.Write((char)flightPinSeparationFlag);
Gaku0606 0:3e847864d796 229 xbeeAPI.EndPacket();
Gaku0606 0:3e847864d796 230 }
Gaku0606 0:3e847864d796 231 else{
Gaku0606 0:3e847864d796 232 xbeeAPI.StartPacket(0x10);
Gaku0606 0:3e847864d796 233 xbeeAPI.Write((float)myQ.x);//姿勢クォータニオン
Gaku0606 0:3e847864d796 234 xbeeAPI.Write((float)myQ.y);
Gaku0606 0:3e847864d796 235 xbeeAPI.Write((float)myQ.z);
Gaku0606 0:3e847864d796 236 xbeeAPI.Write((float)myQ.w);
Gaku0606 0:3e847864d796 237 xbeeAPI.Write((float)(KI[0] - 139.0));//130.901722222);//gps.Longitude());//GPS座標
Gaku0606 0:3e847864d796 238 xbeeAPI.Write((float)(KI[1] - 34.0));//30.4103833333);//gps.Latitude());
Gaku0606 0:3e847864d796 239 xbeeAPI.EndPacket();
Gaku0606 0:3e847864d796 240 }
Gaku0606 0:3e847864d796 241 }
Gaku0606 0:3e847864d796 242 else if(phaseNumber == 2){//飛行中
Gaku0606 0:3e847864d796 243 if(downlinkCount % 10 == 0){
Gaku0606 0:3e847864d796 244
Gaku0606 0:3e847864d796 245 xbeeAPI.StartPacket(0x12);
Gaku0606 0:3e847864d796 246 xbeeAPI.Write((int)innerTime.read_us());
Gaku0606 0:3e847864d796 247 xbeeAPI.Write((float)altitude);//100.0f);//(float)gps.Height());
Gaku0606 0:3e847864d796 248 xbeeAPI.Write((float)speed_knot);//80.0f);//(float)gps.Knot());
Gaku0606 0:3e847864d796 249 xbeeAPI.Write((float)head);//270.0f);//(float)gps.Degree());
Gaku0606 0:3e847864d796 250 xbeeAPI.Write((double)pressure);//気圧
Gaku0606 0:3e847864d796 251 xbeeAPI.Write(flightPinSeparationFlag);
Gaku0606 0:3e847864d796 252 xbeeAPI.EndPacket();
Gaku0606 0:3e847864d796 253 }
Gaku0606 0:3e847864d796 254 else{
Gaku0606 0:3e847864d796 255 xbeeAPI.StartPacket(0x10);
Gaku0606 0:3e847864d796 256 xbeeAPI.Write((float)myQ.x);//姿勢クォータニオン
Gaku0606 0:3e847864d796 257 xbeeAPI.Write((float)myQ.y);
Gaku0606 0:3e847864d796 258 xbeeAPI.Write((float)myQ.z);
Gaku0606 0:3e847864d796 259 xbeeAPI.Write((float)myQ.w);
Gaku0606 0:3e847864d796 260 xbeeAPI.Write((float)(KI[0] - 139.0));//gps.Longitude());//GPS座標
Gaku0606 0:3e847864d796 261 xbeeAPI.Write((float)(KI[1] - 34.0));//gps.Latitude());
Gaku0606 0:3e847864d796 262 xbeeAPI.EndPacket();
Gaku0606 0:3e847864d796 263 //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);
Gaku0606 0:3e847864d796 264 }
Gaku0606 0:3e847864d796 265 }
Gaku0606 0:3e847864d796 266 else if(phaseNumber == 3){//回収待機中
Gaku0606 0:3e847864d796 267 xbeeAPI.StartPacket(0x13);
Gaku0606 0:3e847864d796 268 xbeeAPI.Write((float)(KI[0] - 139.0));//130.901722222);//gps.Longitude());//GPS座標
Gaku0606 0:3e847864d796 269 xbeeAPI.Write((float)(KI[1] - 34.0));//30.4103833333);//gps.Latitude());
Gaku0606 0:3e847864d796 270 //xbeeAPI.Write((double)pressure);
Gaku0606 0:3e847864d796 271 xbeeAPI.Write((float)bt.debug()*9.9f);
Gaku0606 0:3e847864d796 272 xbeeAPI.Write((float)ext_bt.debug()*9.90f);
Gaku0606 0:3e847864d796 273 xbeeAPI.Write((float)usingMainbt.debug()*9.90f);
Gaku0606 0:3e847864d796 274 xbeeAPI.EndPacket();
Gaku0606 0:3e847864d796 275 }
Gaku0606 0:3e847864d796 276 downlinkCount++;
Gaku0606 0:3e847864d796 277 }
Gaku0606 0:3e847864d796 278
Gaku0606 0:3e847864d796 279 inline void Print(char* str){
Gaku0606 0:3e847864d796 280 //pc.printf("%s\r\n", str);
Gaku0606 0:3e847864d796 281 string s(str);
Gaku0606 0:3e847864d796 282 xbeeAPI.StartPacket(0x50);
Gaku0606 0:3e847864d796 283 xbeeAPI.Write(s);//文字列
Gaku0606 0:3e847864d796 284 xbeeAPI.EndPacket();
Gaku0606 0:3e847864d796 285 return;
Gaku0606 0:3e847864d796 286 }
Gaku0606 0:3e847864d796 287 // パケット受信時の処理 Length: データ長 *message: データ
Gaku0606 0:3e847864d796 288 void OnXbeeDataReceived(int Length, char *message){
Gaku0606 0:3e847864d796 289 pc.printf("%d\t%s\r\n",Length, message);
Gaku0606 0:3e847864d796 290 if(message[0] == 0x58){//'X'サーボ
Gaku0606 0:3e847864d796 291 if(message[1] == 0xFF){
Gaku0606 0:3e847864d796 292 zeroAngle = currentAngle;
Gaku0606 0:3e847864d796 293 Print("Set Zero Angle");
Gaku0606 0:3e847864d796 294 }
Gaku0606 0:3e847864d796 295 else{
Gaku0606 0:3e847864d796 296 currentAngle = (double)(message[1] - 90);
Gaku0606 0:3e847864d796 297 servo = currentAngle;
Gaku0606 0:3e847864d796 298 servo2 = currentAngle;
Gaku0606 0:3e847864d796 299 char mojiretu[32] = "";
Gaku0606 0:3e847864d796 300 sprintf(mojiretu, "Set Servo: %d", (int)message[1]);
Gaku0606 0:3e847864d796 301 Print(mojiretu);
Gaku0606 0:3e847864d796 302 }
Gaku0606 0:3e847864d796 303 }
Gaku0606 0:3e847864d796 304 else if(message[0] == 0x46){//'F' フライトピン待機開始許可フラグ
Gaku0606 0:3e847864d796 305 if((flightPin.read() == 1) && (message[1] == 0x4F)){//'O' Flightpin OK
Gaku0606 0:3e847864d796 306 flightPinOKFlag = 1;
Gaku0606 0:3e847864d796 307 Print("Start Wait Flight Pin");
Gaku0606 0:3e847864d796 308 }
Gaku0606 0:3e847864d796 309 else if(message[1] == 0x4E){
Gaku0606 0:3e847864d796 310 flightPinOKFlag = 0;
Gaku0606 0:3e847864d796 311 Print("Flight Pin Reset!!\r\n");
Gaku0606 0:3e847864d796 312 }
Gaku0606 0:3e847864d796 313 }
Gaku0606 0:3e847864d796 314 //指令分離
Gaku0606 0:3e847864d796 315 else if((flightPinFlag == 1) && (Length > 5) && (message[0] == 'J')){//J B K C M D
Gaku0606 0:3e847864d796 316 if(message[1] == 'B'){
Gaku0606 0:3e847864d796 317 if(message[2] == 'K'){
Gaku0606 0:3e847864d796 318 if(message[3] == 'C'){
Gaku0606 0:3e847864d796 319 if(message[4] == 'M'){
Gaku0606 0:3e847864d796 320 if(message[5] == 'D'){
Gaku0606 0:3e847864d796 321 mainParaSepaFlag = 1;
Gaku0606 0:3e847864d796 322 }
Gaku0606 0:3e847864d796 323 }
Gaku0606 0:3e847864d796 324 }
Gaku0606 0:3e847864d796 325 }
Gaku0606 0:3e847864d796 326 }
Gaku0606 0:3e847864d796 327 }
Gaku0606 0:3e847864d796 328 }
Gaku0606 0:3e847864d796 329
Gaku0606 0:3e847864d796 330 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 331 // スケジュール割り込み関数
Gaku0606 0:3e847864d796 332 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 333 const int CALL_PERIOD = 50000;//us
Gaku0606 0:3e847864d796 334
Gaku0606 0:3e847864d796 335 Ticker ScheduleCallTimer;//schedule
Gaku0606 0:3e847864d796 336 int sdSaveFlag = 0;
Gaku0606 0:3e847864d796 337 void autoCallFunc(){
Gaku0606 0:3e847864d796 338
Gaku0606 0:3e847864d796 339 //static uint64_t timingCount = 0;
Gaku0606 0:3e847864d796 340 sdSaveFlag = 1;
Gaku0606 0:3e847864d796 341 downLink();
Gaku0606 0:3e847864d796 342 //timingCount++;
Gaku0606 0:3e847864d796 343 }
Gaku0606 0:3e847864d796 344 /**
Gaku0606 0:3e847864d796 345 * 0.2秒ごとに増えていく
Gaku0606 0:3e847864d796 346 */
Gaku0606 0:3e847864d796 347 int sepaCount = 0;
Gaku0606 0:3e847864d796 348 Ticker SepaCountTimer;
Gaku0606 0:3e847864d796 349 void SepaCountFunc(){
Gaku0606 0:3e847864d796 350 sepaCount++;
Gaku0606 0:3e847864d796 351 return;
Gaku0606 0:3e847864d796 352 }
Gaku0606 0:3e847864d796 353
Gaku0606 0:3e847864d796 354 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 355 // 1段目分離からの2段目分離タイマー
Gaku0606 0:3e847864d796 356 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 357 Timeout secondSepaDelayTimer;
Gaku0606 0:3e847864d796 358 void secondSepaDelayFunc(){
Gaku0606 0:3e847864d796 359 mainParaSepaFlag = 1;
Gaku0606 0:3e847864d796 360 return;
Gaku0606 0:3e847864d796 361 }
Gaku0606 0:3e847864d796 362 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 363 // フライトピンが抜けてから6秒は分離しないというセーフティ的な何か
Gaku0606 0:3e847864d796 364 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 365 int safetyFlag = 0;
Gaku0606 0:3e847864d796 366 Timeout safetyTimer;
Gaku0606 0:3e847864d796 367 void safetyFunc(){
Gaku0606 0:3e847864d796 368 safetyFlag = 1;
Gaku0606 0:3e847864d796 369 return;
Gaku0606 0:3e847864d796 370 }
Gaku0606 0:3e847864d796 371
Gaku0606 0:3e847864d796 372 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 373 // 0.04秒ごとに気圧取得許可フラグを立てる
Gaku0606 0:3e847864d796 374 //------------------------------------------------------------------------------
Gaku0606 0:3e847864d796 375 Ticker PressureGetTimer;
Gaku0606 0:3e847864d796 376 int pressureGetFlag = 0;
Gaku0606 0:3e847864d796 377 void pressureGetFunc(){
Gaku0606 0:3e847864d796 378 pressureGetFlag = 1;
Gaku0606 0:3e847864d796 379 return;
Gaku0606 0:3e847864d796 380 }
Gaku0606 0:3e847864d796 381
Gaku0606 0:3e847864d796 382 int main() {
Gaku0606 0:3e847864d796 383 //setPress();
Gaku0606 0:3e847864d796 384 //XBee初期設定
Gaku0606 0:3e847864d796 385 // 0番目の宛先アドレス
Gaku0606 0:3e847864d796 386 xbeeAPI.SetAddress(0, 0x0013A200, 0x40A88F02);
Gaku0606 0:3e847864d796 387 // 1番目の宛先アドレス
Gaku0606 0:3e847864d796 388 xbeeAPI.SetAddress(1, 0x0013A200, 0x40E7D43E);
Gaku0606 0:3e847864d796 389 // 2番目の宛先アドレス
Gaku0606 0:3e847864d796 390 xbeeAPI.SetAddress(2, 0x0013A200, 0x40DC2538);
Gaku0606 0:3e847864d796 391 //3番目の宛先アドレス
Gaku0606 0:3e847864d796 392 xbeeAPI.SetAddress(3, 0x0013A200, 0x40BABC20);
Gaku0606 0:3e847864d796 393
Gaku0606 0:3e847864d796 394 flightPinSeparationFlag = 0b00000000;
Gaku0606 0:3e847864d796 395
Gaku0606 0:3e847864d796 396 Print("start\r\n");
Gaku0606 0:3e847864d796 397 Print("-----------------------------------------");
Gaku0606 0:3e847864d796 398 Print("Boot Phase:");
Gaku0606 0:3e847864d796 399 pc.printf("start\r\n");
Gaku0606 0:3e847864d796 400 pc.printf("\r\n-----------------------------------------\r\n");
Gaku0606 0:3e847864d796 401 pc.printf("Boot Phase:\r\n");
Gaku0606 0:3e847864d796 402 for(char i = 0;i < 100;i++){
Gaku0606 0:3e847864d796 403 pc.printf("Loading: %3d [%%]\r", i);
Gaku0606 0:3e847864d796 404 wait_ms(30);
Gaku0606 0:3e847864d796 405 }
Gaku0606 0:3e847864d796 406 Print("Loading: 100 [%%] -> finished!!");
Gaku0606 0:3e847864d796 407 Print("-----------------------------------------");
Gaku0606 0:3e847864d796 408 pc.printf("Loading: 100 [%%] -> finished!!\r\n");
Gaku0606 0:3e847864d796 409 pc.printf("\r\n-----------------------------------------\r\n");
Gaku0606 0:3e847864d796 410
Gaku0606 0:3e847864d796 411 /*==========================================================================
Gaku0606 0:3e847864d796 412 フェーズⅠ < 発射待機・システムチェック >
Gaku0606 0:3e847864d796 413 ===========================================================================*/
Gaku0606 0:3e847864d796 414 phaseNumber = 1;
Gaku0606 0:3e847864d796 415 int temp_time = 0;
Gaku0606 0:3e847864d796 416 Print("----------------");
Gaku0606 0:3e847864d796 417 Print("Phase I START");
Gaku0606 0:3e847864d796 418 pc.printf("\r\n----------------\r\n");
Gaku0606 0:3e847864d796 419 pc.printf("Phase I START\r\n");
Gaku0606 0:3e847864d796 420
Gaku0606 0:3e847864d796 421 Print("SD check ->");
Gaku0606 0:3e847864d796 422 pc.printf("SD check -> ");
Gaku0606 0:3e847864d796 423 fp = fopen("/sd/check.txt", "w");
Gaku0606 0:3e847864d796 424 if(fp == NULL){
Gaku0606 0:3e847864d796 425 while(1){
Gaku0606 0:3e847864d796 426 Print("SD ERROR");
Gaku0606 0:3e847864d796 427 error("SD ERROR\r\n");
Gaku0606 0:3e847864d796 428 }
Gaku0606 0:3e847864d796 429 }
Gaku0606 0:3e847864d796 430 else{
Gaku0606 0:3e847864d796 431 pc.printf("OK\r\n");
Gaku0606 0:3e847864d796 432 Print("\tOK");
Gaku0606 0:3e847864d796 433 fprintf(fp,"this file is made by checking sd card program.\r\n");
Gaku0606 0:3e847864d796 434 fclose(fp);
Gaku0606 0:3e847864d796 435 }
Gaku0606 0:3e847864d796 436
Gaku0606 0:3e847864d796 437 while(1){
Gaku0606 0:3e847864d796 438 static int fileCount = 0;
Gaku0606 0:3e847864d796 439 sprintf(datafile, "/sd/datafile%d.txt", fileCount);
Gaku0606 0:3e847864d796 440 fp = fopen(datafile, "rb");
Gaku0606 0:3e847864d796 441 if(fp != NULL){
Gaku0606 0:3e847864d796 442 fclose(fp);
Gaku0606 0:3e847864d796 443 fileCount++;
Gaku0606 0:3e847864d796 444 }
Gaku0606 0:3e847864d796 445 else{
Gaku0606 0:3e847864d796 446 fp = fopen(datafile, "wb");
Gaku0606 0:3e847864d796 447 if(fp == NULL){//ファイルオープン失敗
Gaku0606 0:3e847864d796 448 while(1){
Gaku0606 0:3e847864d796 449 Print("file open error\r\n");
Gaku0606 0:3e847864d796 450 pc.printf("file open error\r\n");
Gaku0606 0:3e847864d796 451 wait(1.0);
Gaku0606 0:3e847864d796 452 }
Gaku0606 0:3e847864d796 453 }
Gaku0606 0:3e847864d796 454 fclose(fp);
Gaku0606 0:3e847864d796 455 fp = fopen(datafile, "wb");
Gaku0606 0:3e847864d796 456 if(fp != NULL){
Gaku0606 0:3e847864d796 457 fclose(fp);
Gaku0606 0:3e847864d796 458 }
Gaku0606 0:3e847864d796 459 break;
Gaku0606 0:3e847864d796 460 }
Gaku0606 0:3e847864d796 461 }
Gaku0606 0:3e847864d796 462
Gaku0606 0:3e847864d796 463
Gaku0606 0:3e847864d796 464 //内部タイマースタート
Gaku0606 0:3e847864d796 465 innerTime.reset();
Gaku0606 0:3e847864d796 466 innerTime.start();
Gaku0606 0:3e847864d796 467 lps25h.SetPress();
Gaku0606 0:3e847864d796 468
Gaku0606 0:3e847864d796 469 //9軸センサ 初期設定
Gaku0606 0:3e847864d796 470 nine.setAccLPF(SET_ACC_LPF);
Gaku0606 0:3e847864d796 471 nine.setGyro(SET_GYRO);
Gaku0606 0:3e847864d796 472 nine.setAcc(SET_ACC);
Gaku0606 0:3e847864d796 473 //9軸センサ オフセット設定
Gaku0606 0:3e847864d796 474 nine.setOffset(nine_offset[0], nine_offset[1], nine_offset[2],
Gaku0606 0:3e847864d796 475 nine_offset[3], nine_offset[4], nine_offset[5],
Gaku0606 0:3e847864d796 476 nine_offset[6], nine_offset[7], nine_offset[8]);
Gaku0606 0:3e847864d796 477
Gaku0606 0:3e847864d796 478 Print("finished mpu9250 setting");
Gaku0606 0:3e847864d796 479 pc.printf("finished mpu9250 setting");
Gaku0606 0:3e847864d796 480
Gaku0606 0:3e847864d796 481 //割り込みセンサーアクセス関数セット
Gaku0606 0:3e847864d796 482 MadgwickFilter attitude(0.1);
Gaku0606 0:3e847864d796 483 ScheduleCallTimer.attach_us(&autoCallFunc, CALL_PERIOD);//&downLink, 50000);//
Gaku0606 0:3e847864d796 484 PressureGetTimer.attach(&pressureGetFunc, 0.04f);
Gaku0606 0:3e847864d796 485
Gaku0606 0:3e847864d796 486
Gaku0606 0:3e847864d796 487 //フライトピンをさすループ
Gaku0606 0:3e847864d796 488 waitFlightPinLabel:
Gaku0606 0:3e847864d796 489 flightPin.clear();
Gaku0606 0:3e847864d796 490 flightPinOKFlag = 0;
Gaku0606 0:3e847864d796 491 Print("wait for connect Flight Pin");
Gaku0606 0:3e847864d796 492 pc.printf("wait for connect Flight Pin");
Gaku0606 0:3e847864d796 493
Gaku0606 0:3e847864d796 494 flightPinSeparationFlag = 0b10000000;//7bit目を0に
Gaku0606 0:3e847864d796 495
Gaku0606 0:3e847864d796 496 while(1){
Gaku0606 0:3e847864d796 497 nine.getGyroAcc(imu);//慣性データ取得
Gaku0606 0:3e847864d796 498 nine.getMag(mag);//地磁気データ取得
Gaku0606 0:3e847864d796 499 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]);//姿勢推定
Gaku0606 0:3e847864d796 500 attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
Gaku0606 0:3e847864d796 501 if(pressureGetFlag){
Gaku0606 0:3e847864d796 502 pressureGetFlag = 0;
Gaku0606 0:3e847864d796 503 lps25h.GetPress(&pressure, &pressureRaw);
Gaku0606 0:3e847864d796 504 }
Gaku0606 0:3e847864d796 505 pc.printf("Attitude:%f,%f,%f,%f\r\n", myQ.x, myQ.y, myQ.z, myQ.w);
Gaku0606 0:3e847864d796 506 pc.printf("%f\r\n",pressure);
Gaku0606 0:3e847864d796 507 //pc.printf("%f\t%f\r\n", KI[0], KI[1]);
Gaku0606 0:3e847864d796 508 if(flightPin.read() == 1){
Gaku0606 0:3e847864d796 509 flightPinSeparationFlag = flightPinSeparationFlag & 0b01111111;//7bit目を0に
Gaku0606 0:3e847864d796 510 if(flightPinOKFlag == 1){
Gaku0606 0:3e847864d796 511 break;
Gaku0606 0:3e847864d796 512 }
Gaku0606 0:3e847864d796 513 else if(pc.readable()){//デバッグ
Gaku0606 0:3e847864d796 514 char flightChar = pc.getc();
Gaku0606 0:3e847864d796 515 if(flightChar == 'F'){
Gaku0606 0:3e847864d796 516 flightPinOKFlag = 1;
Gaku0606 0:3e847864d796 517 break;
Gaku0606 0:3e847864d796 518 }
Gaku0606 0:3e847864d796 519 }
Gaku0606 0:3e847864d796 520 }
Gaku0606 0:3e847864d796 521 else flightPinSeparationFlag |= 0b10000000;
Gaku0606 0:3e847864d796 522
Gaku0606 0:3e847864d796 523 if(abs(temp_time - innerTime.read_ms()) > 500){
Gaku0606 0:3e847864d796 524 // Print("wait for connect Flight Pin and send F.");
Gaku0606 0:3e847864d796 525 pc.printf("wait for connect Flight Pin and send F.");
Gaku0606 0:3e847864d796 526 temp_time = innerTime.read_ms();
Gaku0606 0:3e847864d796 527 }
Gaku0606 0:3e847864d796 528
Gaku0606 0:3e847864d796 529 //分離機構が作動してしまった時の例のやつ
Gaku0606 0:3e847864d796 530 if(DetectDrogueSeparation.read() == 1){
Gaku0606 0:3e847864d796 531 flightPinSeparationFlag = flightPinSeparationFlag | 0b00100000;
Gaku0606 0:3e847864d796 532 }
Gaku0606 0:3e847864d796 533 else{
Gaku0606 0:3e847864d796 534 flightPinSeparationFlag = flightPinSeparationFlag & 0b11011111;
Gaku0606 0:3e847864d796 535 }
Gaku0606 0:3e847864d796 536 }
Gaku0606 0:3e847864d796 537
Gaku0606 0:3e847864d796 538 //フライトピン待機
Gaku0606 0:3e847864d796 539 temp_time = innerTime.read_ms();
Gaku0606 0:3e847864d796 540 flightPinFlag = 0;
Gaku0606 0:3e847864d796 541
Gaku0606 0:3e847864d796 542 flightPinSeparationFlag = flightPinSeparationFlag | 0b01000000;//6bit目を1に
Gaku0606 0:3e847864d796 543
Gaku0606 0:3e847864d796 544 while(flightPinFlag == 0){
Gaku0606 0:3e847864d796 545
Gaku0606 0:3e847864d796 546 nine.getGyroAcc(imu);//慣性データ取得
Gaku0606 0:3e847864d796 547 nine.getMag(mag);//地磁気データ取得
Gaku0606 0:3e847864d796 548 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]);//姿勢推定
Gaku0606 0:3e847864d796 549 attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
Gaku0606 0:3e847864d796 550 pc.printf("Attitude:%f,%f,%f,%f\r\n", myQ.x, myQ.y, myQ.z, myQ.w);
Gaku0606 0:3e847864d796 551 if(pressureGetFlag){
Gaku0606 0:3e847864d796 552 pressureGetFlag = 0;
Gaku0606 0:3e847864d796 553 lps25h.GetPress(&pressure, &pressureRaw);
Gaku0606 0:3e847864d796 554 }
Gaku0606 0:3e847864d796 555 if(abs(temp_time - innerTime.read_ms()) > 5000){
Gaku0606 0:3e847864d796 556 pc.printf("Wait for Flight Pin Left");
Gaku0606 0:3e847864d796 557 temp_time = innerTime.read_ms();
Gaku0606 0:3e847864d796 558 }
Gaku0606 0:3e847864d796 559 if(flightPinOKFlag == 0){
Gaku0606 0:3e847864d796 560 goto waitFlightPinLabel;
Gaku0606 0:3e847864d796 561 }
Gaku0606 0:3e847864d796 562
Gaku0606 0:3e847864d796 563 //分離機構が作動してしまった時の例のやつ
Gaku0606 0:3e847864d796 564 if(DetectDrogueSeparation.read() == 1){
Gaku0606 0:3e847864d796 565 flightPinSeparationFlag = flightPinSeparationFlag | 0b00100000;
Gaku0606 0:3e847864d796 566 }
Gaku0606 0:3e847864d796 567 else{
Gaku0606 0:3e847864d796 568 flightPinSeparationFlag = flightPinSeparationFlag & 0b11011111;
Gaku0606 0:3e847864d796 569 }
Gaku0606 0:3e847864d796 570 }
Gaku0606 0:3e847864d796 571
Gaku0606 0:3e847864d796 572 flightPinSeparationFlag = flightPinSeparationFlag | 0b10000000;//7bit目を1に 0b11000000になっているはず
Gaku0606 0:3e847864d796 573 Print("Detect Flight Pin!!");
Gaku0606 0:3e847864d796 574 pc.printf("Detect Flight Pin!!");
Gaku0606 0:3e847864d796 575 Print("Launch!!!!Launch!!!!");
Gaku0606 0:3e847864d796 576 Print("Phase I END");
Gaku0606 0:3e847864d796 577 pc.printf("Launch!!!!Launch!!!!");
Gaku0606 0:3e847864d796 578 pc.printf("Phase I END");
Gaku0606 0:3e847864d796 579 /*==========================================================================
Gaku0606 0:3e847864d796 580 フェーズII < 飛行中 >
Gaku0606 0:3e847864d796 581 ==========================================================================*/
Gaku0606 0:3e847864d796 582 Print("Phase II START");
Gaku0606 0:3e847864d796 583 pc.printf("Phase II START");
Gaku0606 0:3e847864d796 584 //色々初期化
Gaku0606 0:3e847864d796 585 innerTime.reset();//内部時間
Gaku0606 0:3e847864d796 586 phaseNumber = 2;//フェーズナンバー
Gaku0606 0:3e847864d796 587 downlinkCount = 0;//
Gaku0606 0:3e847864d796 588 detectSepaFlag1 = 0;
Gaku0606 0:3e847864d796 589 safetyFlag = 0;
Gaku0606 0:3e847864d796 590 safetyTimer.attach(&safetyFunc, SAFETY_TIME);
Gaku0606 0:3e847864d796 591
Gaku0606 0:3e847864d796 592 drogueSeparationTimer.attach(&drogueSeparation, DROGUE_WAIT_TIME);
Gaku0606 0:3e847864d796 593 mainParaSeparationTimer.attach(&mainParaSeparation, MAIN_PARA_WAIT_TIME);
Gaku0606 0:3e847864d796 594
Gaku0606 0:3e847864d796 595 static int SepaStartFlag = 0;
Gaku0606 0:3e847864d796 596 double oldPressure = (double)lps25h.GetPress();
Gaku0606 0:3e847864d796 597 double deltaP = 0;
Gaku0606 0:3e847864d796 598 int pressureCount = 0;
Gaku0606 0:3e847864d796 599
Gaku0606 0:3e847864d796 600 while(1){
Gaku0606 0:3e847864d796 601
Gaku0606 0:3e847864d796 602 //分離機構が作動してしまった時の例のやつ
Gaku0606 0:3e847864d796 603 if(DetectDrogueSeparation.read() == 1) flightPinSeparationFlag = flightPinSeparationFlag | 0b00100000;
Gaku0606 0:3e847864d796 604 else flightPinSeparationFlag = flightPinSeparationFlag & 0b11011111;
Gaku0606 0:3e847864d796 605
Gaku0606 0:3e847864d796 606 temp_time = innerTime.read_ms();
Gaku0606 0:3e847864d796 607 nine.getGyroAcc(imu);//慣性データ取得
Gaku0606 0:3e847864d796 608 nine.getMag(mag);//地磁気データ取得
Gaku0606 0:3e847864d796 609 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]);//姿勢推定
Gaku0606 0:3e847864d796 610 attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
Gaku0606 0:3e847864d796 611
Gaku0606 0:3e847864d796 612 if(pressureGetFlag){//0.04秒ごとに気圧取得&比較
Gaku0606 0:3e847864d796 613 pressureGetFlag = 0;
Gaku0606 0:3e847864d796 614 lps25h.GetPress(&pressure, &pressureRaw);//気圧取得
Gaku0606 0:3e847864d796 615
Gaku0606 0:3e847864d796 616 //pc.printf("%f\t%f\r\n",innerTime.read(), pressure);
Gaku0606 0:3e847864d796 617
Gaku0606 0:3e847864d796 618 //気圧による分離条件式
Gaku0606 0:3e847864d796 619 deltaP = pressure - oldPressure;//気圧の変化勾配
Gaku0606 0:3e847864d796 620 if((deltaP > FALLING_THRESHOLD) && (safetyFlag == 1) && (pressure != 0.0)){//気圧差 > -0.04 かつ 6秒以上経過
Gaku0606 0:3e847864d796 621 pressureCount++;
Gaku0606 0:3e847864d796 622 if(pressureCount > 5){//ドラッグシュート分離
Gaku0606 0:3e847864d796 623 //fastPressureFlag = 1;
Gaku0606 0:3e847864d796 624 drogueSepaFlag = 1;
Gaku0606 0:3e847864d796 625 secondSepaDelayTimer.attach(&secondSepaDelayFunc, SECOND_SEPA_DELAY_TIME);
Gaku0606 0:3e847864d796 626 }
Gaku0606 0:3e847864d796 627 }
Gaku0606 0:3e847864d796 628 else{
Gaku0606 0:3e847864d796 629 pressureCount = 0;
Gaku0606 0:3e847864d796 630 }
Gaku0606 0:3e847864d796 631 oldPressure = pressure;
Gaku0606 0:3e847864d796 632 }
Gaku0606 0:3e847864d796 633
Gaku0606 0:3e847864d796 634 if(drogueSepaFlag == 1 ){//ドラッグシュート分離機構作動
Gaku0606 0:3e847864d796 635
Gaku0606 0:3e847864d796 636 if(SepaStartFlag == 0){
Gaku0606 0:3e847864d796 637 pc.printf("Separation %f [s]\r\n", innerTime.read());
Gaku0606 0:3e847864d796 638 //ビットフラグ更新
Gaku0606 0:3e847864d796 639 flightPinSeparationFlag = (flightPinSeparationFlag & 0b11000111) | 0b00010000;//3bit目を1に, 作動させたがまだわからない
Gaku0606 0:3e847864d796 640 SepaStartFlag = 1;
Gaku0606 0:3e847864d796 641 SepaCountTimer.attach(&SepaCountFunc, 0.2f);
Gaku0606 0:3e847864d796 642 sepaCount = 0;
Gaku0606 0:3e847864d796 643 Print("Try Drogue Separation!!");
Gaku0606 0:3e847864d796 644 pc.printf("Try Drogue Separation!!\r\n");
Gaku0606 0:3e847864d796 645 servo = 30.0 + zeroAngle;
Gaku0606 0:3e847864d796 646 servo2 = 30.0 + zeroAngle;
Gaku0606 0:3e847864d796 647 }
Gaku0606 0:3e847864d796 648 else{
Gaku0606 0:3e847864d796 649 if(sepaCount != 0){
Gaku0606 0:3e847864d796 650 if(sepaCount % 2 == 0){
Gaku0606 0:3e847864d796 651 servo = 35.0 + zeroAngle;
Gaku0606 0:3e847864d796 652 servo2 = 35.0 + zeroAngle;
Gaku0606 0:3e847864d796 653 }
Gaku0606 0:3e847864d796 654 else if(sepaCount % 2 == 1){
Gaku0606 0:3e847864d796 655 servo = 25.0 + zeroAngle;
Gaku0606 0:3e847864d796 656 servo2 = 25.0 + zeroAngle;
Gaku0606 0:3e847864d796 657 }
Gaku0606 0:3e847864d796 658 }
Gaku0606 0:3e847864d796 659 //ドラッグシュート分離機構が作動したか (これは常に確認する
Gaku0606 0:3e847864d796 660 /*if(detectSepaFlag1 == 1){
Gaku0606 0:3e847864d796 661 flightPinSeparationFlag = (flightPinSeparationFlag & 0b11000111) | 0b00111000;//2bit目を1に
Gaku0606 0:3e847864d796 662 Print("Succeeded Drogue Separation!!\r\n");
Gaku0606 0:3e847864d796 663 Print("Finished Controling Servo Motor\r\n");
Gaku0606 0:3e847864d796 664 pc.printf("Succeeded Drogue Separation!!\r\n");
Gaku0606 0:3e847864d796 665 pc.printf("Finished Controling Servo Motor\r\n");
Gaku0606 0:3e847864d796 666 }*/
Gaku0606 0:3e847864d796 667
Gaku0606 0:3e847864d796 668 if(sepaCount > 24){//2秒間経過
Gaku0606 0:3e847864d796 669 mainParaSepaFlag = 1;
Gaku0606 0:3e847864d796 670 drogueSepaFlag = 0;//ドラッグシュート分離を停止．
Gaku0606 0:3e847864d796 671 /*if(detectSepaFlag1 == 0){//2秒間サーボを動かして，だめだったとき
Gaku0606 0:3e847864d796 672 //pc.printf("Separation2 %f [s]\r\n", innerTime.read());
Gaku0606 0:3e847864d796 673 flightPinSeparationFlag = (flightPinSeparationFlag & 0b11000111) | 0b00110000;
Gaku0606 0:3e847864d796 674 drogueSepaFlag = 0;
Gaku0606 0:3e847864d796 675 Print("Failed Drogue Separation...\r\n");
Gaku0606 0:3e847864d796 676 pc.printf("Failed Drogue Separation...\r\n");
Gaku0606 0:3e847864d796 677 //mainParaSepaFlag = 1;//強制メインパラ分離
Gaku0606 0:3e847864d796 678 Print("Finished Controling Servo Motor\r\n");
Gaku0606 0:3e847864d796 679 pc.printf("Finished Controling Servo Motor\r\n");
Gaku0606 0:3e847864d796 680 }*/
Gaku0606 0:3e847864d796 681 pc.printf("Finished Controling Servo Motor\r\n");
Gaku0606 0:3e847864d796 682 }
Gaku0606 0:3e847864d796 683 }
Gaku0606 0:3e847864d796 684 }
Gaku0606 0:3e847864d796 685
Gaku0606 0:3e847864d796 686 if(mainParaSepaFlag == 1){
Gaku0606 0:3e847864d796 687 flightPinSeparationFlag = flightPinSeparationFlag | 0b00000111;
Gaku0606 0:3e847864d796 688 pc.printf("Separation2 %f [s]\r\n", innerTime.read());
Gaku0606 0:3e847864d796 689 Print("Try Main Separation!!");
Gaku0606 0:3e847864d796 690 pc.printf("Try Main Separation!!\r\n");
Gaku0606 0:3e847864d796 691 nicrom.autoStop(5.0f);
Gaku0606 0:3e847864d796 692 mainParaSepaFlag = 0;
Gaku0606 0:3e847864d796 693 Print("Finished Nicrom Control");
Gaku0606 0:3e847864d796 694 pc.printf("Finished Nicrom Control\r\n");
Gaku0606 0:3e847864d796 695 break;
Gaku0606 0:3e847864d796 696 }
Gaku0606 0:3e847864d796 697
Gaku0606 0:3e847864d796 698 //SD保存
Gaku0606 0:3e847864d796 699
Gaku0606 0:3e847864d796 700 if(sdSaveFlag == 1){
Gaku0606 0:3e847864d796 701 sdWriteLed = 1;
Gaku0606 0:3e847864d796 702 sdSaveFlag = 0;
Gaku0606 0:3e847864d796 703 temp_time = innerTime.read_us();
Gaku0606 0:3e847864d796 704 fp = fopen(datafile, "ab");//14
Gaku0606 0:3e847864d796 705 if(fp != NULL){
Gaku0606 0:3e847864d796 706 for(int i = 0; i < 6;i++){
Gaku0606 0:3e847864d796 707 sdBuff[i] = imu[i];
Gaku0606 0:3e847864d796 708 }
Gaku0606 0:3e847864d796 709 for(int i = 0;i < 3;i++){
Gaku0606 0:3e847864d796 710 sdBuff[i+6] = mag[i];
Gaku0606 0:3e847864d796 711 }
Gaku0606 0:3e847864d796 712 sdBuff[9] = myQ.x; sdBuff[10] = myQ.y; sdBuff[11] = myQ.z; sdBuff[12] = myQ.w; sdBuff[13] = pressure;
Gaku0606 0:3e847864d796 713 fwrite(&temp_time, sizeof(int), 1, fp);
Gaku0606 0:3e847864d796 714 fwrite(&flightPinSeparationFlag, sizeof(char), 1, fp);
Gaku0606 0:3e847864d796 715 fwrite(sdBuff, sizeof(double), 14, fp);
Gaku0606 0:3e847864d796 716 fclose(fp);
Gaku0606 0:3e847864d796 717 }
Gaku0606 0:3e847864d796 718 sdWriteLed = 0;
Gaku0606 0:3e847864d796 719 }
Gaku0606 0:3e847864d796 720 }
Gaku0606 0:3e847864d796 721 temp_time = innerTime.read_ms();
Gaku0606 0:3e847864d796 722 while(1){
Gaku0606 0:3e847864d796 723 nine.getGyroAcc(imu);//慣性データ取得
Gaku0606 0:3e847864d796 724 nine.getMag(mag);//地磁気データ取得
Gaku0606 0:3e847864d796 725 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]);//姿勢推定
Gaku0606 0:3e847864d796 726 attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
Gaku0606 0:3e847864d796 727 if(pressureGetFlag){
Gaku0606 0:3e847864d796 728 pressureGetFlag = 0;
Gaku0606 0:3e847864d796 729 lps25h.GetPress(&pressure, &pressureRaw);
Gaku0606 0:3e847864d796 730 }
Gaku0606 0:3e847864d796 731 if(abs(innerTime.read_ms() - temp_time) > 30000){
Gaku0606 0:3e847864d796 732 break;
Gaku0606 0:3e847864d796 733 }
Gaku0606 0:3e847864d796 734 }
Gaku0606 0:3e847864d796 735 Print("Phase II END");
Gaku0606 0:3e847864d796 736 pc.printf("Phase II END\r\n");
Gaku0606 0:3e847864d796 737 /*==========================================================================
Gaku0606 0:3e847864d796 738 フェーズⅢ < 回収待機 >
Gaku0606 0:3e847864d796 739 ==========================================================================*/
Gaku0606 0:3e847864d796 740 Print("Phase III START");
Gaku0606 0:3e847864d796 741 pc.printf("Phase III START");
Gaku0606 0:3e847864d796 742 phaseNumber = 3;
Gaku0606 0:3e847864d796 743 downlinkCount = 0;
Gaku0606 0:3e847864d796 744
Gaku0606 0:3e847864d796 745 temp_time = innerTime.read_ms();
Gaku0606 0:3e847864d796 746 while(1){
Gaku0606 0:3e847864d796 747 nine.getGyroAcc(imu);//慣性データ取得
Gaku0606 0:3e847864d796 748 nine.getMag(mag);//地磁気データ取得
Gaku0606 0:3e847864d796 749 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]);//姿勢推定
Gaku0606 0:3e847864d796 750 attitude.getAttitude(&myQ); //姿勢をクォータニオンで取得
Gaku0606 0:3e847864d796 751 if(pressureGetFlag == 1){
Gaku0606 0:3e847864d796 752 pressureGetFlag = 0;
Gaku0606 0:3e847864d796 753 lps25h.GetPress(&pressure, &pressureRaw);
Gaku0606 0:3e847864d796 754 }
Gaku0606 0:3e847864d796 755 if(abs(innerTime.read_ms() - temp_time) > 5000){
Gaku0606 0:3e847864d796 756 pc.printf("%f\t%f\t%f\t%f\r\n",innerTime.read(), gps.Longitude(), gps.Latitude(), pressure);
Gaku0606 0:3e847864d796 757 temp_time = innerTime.read_ms();
Gaku0606 0:3e847864d796 758 }
Gaku0606 0:3e847864d796 759 }
Gaku0606 0:3e847864d796 760 }