FTE-denshi / Mbed 2 deprecated FTE-06

東北大学学友会準加盟団体 From The Earth の高高度ロケットFTE-06（通称：海豚）にて使用したソフトウェアです．ご自由にお使いください．このプログラムによって生じた損害について当団体は一切責任を負いません．また，各モジュールのライブラリは当団体が作成したものではないので再配布は禁止します．

Dependencies:   mbed FATFileSystem

Fork of FTE-06 by Tetsushi Amano

main.cpp@18:f6ecd5ea145f, 2017-08-05 (annotated)

Committer:
mizuki_akaike
Date:
Sat Aug 05 12:36:29 2017 +0000
Revision:
18:f6ecd5ea145f
Parent:
15:792eb5103e9e 
eeprom???????;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
moai26 0:9c3cc716cbb3 1 #pragma O3
moai26 0:9c3cc716cbb3 2
moai26 0:9c3cc716cbb3 3 #include "mbed.h"
kohei_yamamoto 2:3f81e5511138 4 #include "SDFileSystem.h"
mizuki_akaike 7:bc4710d2c7d8 5 #include "BMP085.h"
mizuki_akaike 8:4d2e785cb951 6 #include "L3GD20.h"
mizuki_akaike 9:bcc7dbc2d185 7 #include "LSM303DLHC.h"
mizuki_akaike 10:ef6fb2d8e2c8 8 #include "24LCXXX.h"
mizuki_akaike 13:c3c7524d2440 9 #include "GPS.h"
mizuki_akaike 13:c3c7524d2440 10 #include "Servo.h"
moai26 0:9c3cc716cbb3 11 //発射検知でLED1の点灯、頂点通過検知でLED2の点灯、パラシュート展開でLED3の点灯、水密構造のLED4の点灯
mizuki_akaike 13:c3c7524d2440 12 //im920ではECIOコマンドを使って、送信データのアスキーコード逆変換が行われるようにすること
mizuki_akaike 9:bcc7dbc2d185 13 //-----------------------------------------ロケットの制御にかかわる設定
mizuki_akaike 9:bcc7dbc2d185 14 #define launch_pressure_threshold 990.//発射したとみなす気圧（おおよそ地上気圧-80.4hpa)
mizuki_akaike 9:bcc7dbc2d185 15 #define launch_acc_threshold 5.//発射時の加速度(G)
moai26 0:9c3cc716cbb3 16 #define acc_axis 1//上方向の軸を決定x;0,y;1,z;2
mizuki_akaike 3:983e03f4e146 17 #define pre_sd 0.00037//気圧センサの測定誤差(事前に測定して計算)
moai26 0:9c3cc716cbb3 18 #define size_dif 3//気圧の変位を保存する数
moai26 0:9c3cc716cbb3 19 #define time_limit_launch_and_fall_ms 20000//発射から頂点に至るまでの予測時間(msec)
moai26 0:9c3cc716cbb3 20 #define fall_detect_lock 2000//加速度で発射検知したときに頂点検知にロックをかける時間(msec)
mizuki_akaike 13:c3c7524d2440 21 #define servo_deg_initial 20//サーボの初期角度
mizuki_akaike 13:c3c7524d2440 22 #define servo_deg_close 90//サーボの水密構造を閉じる時の角度
moai26 0:9c3cc716cbb3 23
mizuki_akaike 9:bcc7dbc2d185 24 //-----------------------------------------センサの測定レンジなどの設定
mizuki_akaike 14:b902e73a5404 25 #define Acc_range 2.//加速度センサの測定レンジ
mizuki_akaike 14:b902e73a5404 26 #define para_switch p10//パラシュート展開用のデジタル出力ピン番号
mizuki_akaike 14:b902e73a5404 27 #define time_between_para_opening_ms 4000//パラシュート展開用の電熱線の加熱時間(msec)
mizuki_akaike 14:b902e73a5404 28 #define conv2Gravity 1./(32768./Acc_range)//加速度センサの値の単位をGに変換
mizuki_akaike 10:ef6fb2d8e2c8 29 #define AT24C1024_address 0x50
mizuki_akaike 18:f6ecd5ea145f 30 #define eeprom_byte_size 100000
mizuki_akaike 9:bcc7dbc2d185 31
moai26 0:9c3cc716cbb3 32 DigitalOut myled1(LED1);
moai26 0:9c3cc716cbb3 33 DigitalOut myled2(LED2);
moai26 0:9c3cc716cbb3 34 DigitalOut myled3(LED3);
moai26 0:9c3cc716cbb3 35 DigitalOut myled4(LED4);
mizuki_akaike 14:b902e73a5404 36
mizuki_akaike 14:b902e73a5404 37 DigitalOut Para(para_switch);//パラシュート展開用のクラス
mizuki_akaike 14:b902e73a5404 38
moai26 0:9c3cc716cbb3 39 Timer t;
moai26 0:9c3cc716cbb3 40
kohei_yamamoto 2:3f81e5511138 41 AnalogIn analog_rand(p20);//電圧読み取り用のピン
moai26 0:9c3cc716cbb3 42
mizuki_akaike 11:c43040ddcfb8 43 double pres_dif[size_dif]= {0}; //気圧の変化をsize_difの数だけ保存
mizuki_akaike 14:b902e73a5404 44 uint32_t launch_detect_time=0;//発射検知の時間(ms)
mizuki_akaike 14:b902e73a5404 45 uint32_t fall_detect_time=0;//頂点検知の時間(ms)
mizuki_akaike 14:b902e73a5404 46 uint32_t para_open_time=0;//パラシュート展開の時間(ms)
moai26 0:9c3cc716cbb3 47
kohei_yamamoto 2:3f81e5511138 48 SDFileSystem sd(p5, p6, p7, p8, "sd");//SDカードのオブジェクト作成
mizuki_akaike 9:bcc7dbc2d185 49 BMP085 BMP(p9,p10);//気圧、温度センサのクラス
mizuki_akaike 9:bcc7dbc2d185 50 L3GD20 L3GD(p9,p10);//ジャイロセンサのクラス
mizuki_akaike 9:bcc7dbc2d185 51 I2C L(p9,p10);
mizuki_akaike 9:bcc7dbc2d185 52 LSM303DLHC LSM(&L);//加速度センサのクラス
mizuki_akaike 10:ef6fb2d8e2c8 53 I2C E(p9,p10);
mizuki_akaike 10:ef6fb2d8e2c8 54 _24LCXXX eeprom(&E,AT24C1024_address);//eepromのクラス
mizuki_akaike 13:c3c7524d2440 55 GPS sGPS(p28,p27);//GPSのクラス
mizuki_akaike 13:c3c7524d2440 56 Serial Im920(p28,p27);//無線通信用のserialクラス
mizuki_akaike 13:c3c7524d2440 57 Servo myservo(p21);//サーボ用のクラス
mizuki_akaike 13:c3c7524d2440 58 int status=0;
moai26 0:9c3cc716cbb3 59
moai26 0:9c3cc716cbb3 60 class Rocket_Data_Get_And_Save//ロケットの測定データを管理するクラス
moai26 0:9c3cc716cbb3 61 {
mizuki_akaike 6:f4d022422ba6 62 private:
mizuki_akaike 11:c43040ddcfb8 63 int check_eeprom;//eepromへの書き込みができたかどうかのフラグ
mizuki_akaike 6:f4d022422ba6 64 uint32_t time_ms_now;
mizuki_akaike 11:c43040ddcfb8 65 char buf_eeprom[20];//書き込みデータの一時保存先
mizuki_akaike 9:bcc7dbc2d185 66 inline void LSM_updata() {
mizuki_akaike 9:bcc7dbc2d185 67 LSM.getAccel();
mizuki_akaike 9:bcc7dbc2d185 68 acc[0]=conv2Gravity*LSM.accelX();
mizuki_akaike 9:bcc7dbc2d185 69 acc[1]=conv2Gravity*LSM.accelY();
mizuki_akaike 9:bcc7dbc2d185 70 acc[2]=conv2Gravity*LSM.accelZ();
mizuki_akaike 9:bcc7dbc2d185 71
mizuki_akaike 9:bcc7dbc2d185 72 }
mizuki_akaike 9:bcc7dbc2d185 73
moai26 0:9c3cc716cbb3 74 public:
moai26 0:9c3cc716cbb3 75 double acc[3];//x,y,z
mizuki_akaike 9:bcc7dbc2d185 76 float gyro[3];
moai26 0:9c3cc716cbb3 77 double mag[3];
moai26 0:9c3cc716cbb3 78 double latitude,longitude;
moai26 0:9c3cc716cbb3 79 double pres,temp;
moai26 0:9c3cc716cbb3 80 uint32_t eeprom_add;
kohei_yamamoto 2:3f81e5511138 81 double val;
kohei_yamamoto 2:3f81e5511138 82 unsigned int seed;
kohei_yamamoto 2:3f81e5511138 83 char filename[100];
kohei_yamamoto 2:3f81e5511138 84 int result_num;
moai26 0:9c3cc716cbb3 85
moai26 0:9c3cc716cbb3 86 Rocket_Data_Get_And_Save() {//初期化子
mizuki_akaike 10:ef6fb2d8e2c8 87 //--------------変数の初期化
moai26 0:9c3cc716cbb3 88 for(uint32_t j=0; j<3; j++) {
moai26 0:9c3cc716cbb3 89 acc[j]=gyro[j]=mag[j]=0;
moai26 0:9c3cc716cbb3 90 }
mizuki_akaike 10:ef6fb2d8e2c8 91 pres=0;
mizuki_akaike 10:ef6fb2d8e2c8 92 temp=0;
moai26 0:9c3cc716cbb3 93 eeprom_add=0;
mizuki_akaike 10:ef6fb2d8e2c8 94 check_eeprom=true;
moai26 0:9c3cc716cbb3 95 time_ms_now=0;
moai26 0:9c3cc716cbb3 96 latitude=0;
moai26 0:9c3cc716cbb3 97 longitude=0;
mizuki_akaike 9:bcc7dbc2d185 98
mizuki_akaike 10:ef6fb2d8e2c8 99 //---------------------------
mizuki_akaike 13:c3c7524d2440 100 myservo.position(servo_deg_initial);//サーボ位置の初期化
kohei_yamamoto 2:3f81e5511138 101 /*analogpinの入力電圧を用いてrandomなfile作成*/
kohei_yamamoto 2:3f81e5511138 102 val = analog_rand.read();//p20の電圧読み取り
kohei_yamamoto 2:3f81e5511138 103 seed = val * 10000;
mizuki_akaike 9:bcc7dbc2d185 104 do {
mizuki_akaike 9:bcc7dbc2d185 105 seed %= 1000;
mizuki_akaike 9:bcc7dbc2d185 106 } while (seed > 1000);//0~999のunsigned_int型に変換
kohei_yamamoto 2:3f81e5511138 107 srand(seed);
kohei_yamamoto 2:3f81e5511138 108 result_num = rand();
mizuki_akaike 18:f6ecd5ea145f 109 sprintf(filename, "result_%d.csv", result_num);
mizuki_akaike 5:80e841579008 110 mkdir("/sd/mydir", 0777);
moai26 0:9c3cc716cbb3 111 }
moai26 0:9c3cc716cbb3 112
kohei_yamamoto 2:3f81e5511138 113
moai26 0:9c3cc716cbb3 114 void save_data() { //sdカードへのデータの保存
mizuki_akaike 9:bcc7dbc2d185 115
mizuki_akaike 9:bcc7dbc2d185 116 /*
mizuki_akaike 9:bcc7dbc2d185 117 データのセーブについて
mizuki_akaike 9:bcc7dbc2d185 118 データを上書きされないように乱数を使って
mizuki_akaike 9:bcc7dbc2d185 119 データ名を毎回変更する
mizuki_akaike 9:bcc7dbc2d185 120 クラスの初期化子でファイル名を決めてしまうといいかも
mizuki_akaike 9:bcc7dbc2d185 121 乱数のseedは起動するたびに変更する必要があるので
mizuki_akaike 9:bcc7dbc2d185 122 何もつながっていないanalogpinの入力電圧とか使うといいかも
mizuki_akaike 9:bcc7dbc2d185 123 乱数の幅は0~999までとかに制限すること
mizuki_akaike 9:bcc7dbc2d185 124 データはresult_(乱数).datに保存すること
mizuki_akaike 9:bcc7dbc2d185 125 データの書式は
mizuki_akaike 9:bcc7dbc2d185 126 "time,temperature,pressure,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,..."
mizuki_akaike 9:bcc7dbc2d185 127 という書式にすること
mizuki_akaike 9:bcc7dbc2d185 128 下手に単位とかデータの名前をいれるとデータ分析で時間がかかるので、数値を","で区切るだけにすること
mizuki_akaike 9:bcc7dbc2d185 129 */
mizuki_akaike 9:bcc7dbc2d185 130
mizuki_akaike 9:bcc7dbc2d185 131 /*SDカードに書き込み*/
mizuki_akaike 9:bcc7dbc2d185 132
mizuki_akaike 18:f6ecd5ea145f 133 FILE *fp = fopen("/sd/mydir/data.csv", "a");
mizuki_akaike 9:bcc7dbc2d185 134 if (fp == NULL)error("Could not open SD Card for write\n");
mizuki_akaike 18:f6ecd5ea145f 135 fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", val, temp, pres, acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2], mag[0], mag[1], mag[2], latitude, longitude);
mizuki_akaike 9:bcc7dbc2d185 136 fclose(fp);
mizuki_akaike 9:bcc7dbc2d185 137
moai26 0:9c3cc716cbb3 138 }
mizuki_akaike 9:bcc7dbc2d185 139
moai26 0:9c3cc716cbb3 140 void get_data() {
moai26 0:9c3cc716cbb3 141 time_ms_now=t.read_ms();//取得時の時間を保存
mizuki_akaike 7:bc4710d2c7d8 142 BMP.update();
mizuki_akaike 7:bc4710d2c7d8 143 pres=BMP.get_pressure();
mizuki_akaike 7:bc4710d2c7d8 144 temp=BMP.get_temperature();
mizuki_akaike 8:4d2e785cb951 145 L3GD.read(&gyro[0],&gyro[1],&gyro[2]);
mizuki_akaike 9:bcc7dbc2d185 146 LSM_updata();
moai26 0:9c3cc716cbb3 147 save_data();//データの保存
mizuki_akaike 11:c43040ddcfb8 148 write_data2eeprom();//eepromへの保存
moai26 0:9c3cc716cbb3 149 }
moai26 0:9c3cc716cbb3 150
moai26 0:9c3cc716cbb3 151 void write_data2eeprom() {//eepromへの書き込み
mizuki_akaike 10:ef6fb2d8e2c8 152 //最低限のデータのみ保存
mizuki_akaike 11:c43040ddcfb8 153 sprintf(buf_eeprom,"%d,%f.4\n",time,pres);
mizuki_akaike 11:c43040ddcfb8 154 char write_byte=buf_eeprom[0];
mizuki_akaike 12:a26df6ca5585 155 for(uint32_t n=0; write_byte!='\0'&&write_byte!=NULL; n++) {
mizuki_akaike 11:c43040ddcfb8 156 write_byte=buf_eeprom[n];
mizuki_akaike 11:c43040ddcfb8 157 check_eeprom=eeprom.byte_write(eeprom_add++,write_byte);
mizuki_akaike 12:a26df6ca5585 158 }
mizuki_akaike 10:ef6fb2d8e2c8 159 }
mizuki_akaike 10:ef6fb2d8e2c8 160
mizuki_akaike 10:ef6fb2d8e2c8 161 void delete_eeprom() { //eepromの中身を全消去
mizuki_akaike 18:f6ecd5ea145f 162 char *del;
mizuki_akaike 18:f6ecd5ea145f 163 char emp='0';//eepromの初期化は"0"
mizuki_akaike 18:f6ecd5ea145f 164 eeprom.nbyte_read(0,del,1);
mizuki_akaike 18:f6ecd5ea145f 165 for(int32_t n=0;del[0]!='\0'&&n<eeprom_byte_size;n++){//終端文字か指定バイトの数まで初期化
mizuki_akaike 18:f6ecd5ea145f 166 eeprom.byte_write(n,emp);
mizuki_akaike 18:f6ecd5ea145f 167 eeprom.nbyte_read(n+1,del,1);
mizuki_akaike 18:f6ecd5ea145f 168 }
moai26 0:9c3cc716cbb3 169
moai26 0:9c3cc716cbb3 170 }
moai26 0:9c3cc716cbb3 171
moai26 0:9c3cc716cbb3 172 void get_GPS() {//GPSデータの取得
mizuki_akaike 13:c3c7524d2440 173 if(sGPS.sample()) {
mizuki_akaike 13:c3c7524d2440 174 latitude=sGPS.latitude;
mizuki_akaike 13:c3c7524d2440 175 longitude=sGPS.longitude;
mizuki_akaike 13:c3c7524d2440 176 } else {
mizuki_akaike 13:c3c7524d2440 177 latitude=0;
mizuki_akaike 13:c3c7524d2440 178 longitude=0;
mizuki_akaike 13:c3c7524d2440 179 }
moai26 0:9c3cc716cbb3 180 }
moai26 0:9c3cc716cbb3 181
mizuki_akaike 13:c3c7524d2440 182 void send_GPS_and_status() {//GPSデータの送信
mizuki_akaike 13:c3c7524d2440 183 char send_data[100];
mizuki_akaike 13:c3c7524d2440 184 sprintf(send_data,"TXDT %f,%f\n",latitude,longitude);
mizuki_akaike 13:c3c7524d2440 185 Im920.printf(send_data);
mizuki_akaike 13:c3c7524d2440 186 wait_ms(5);
mizuki_akaike 13:c3c7524d2440 187 sprintf(send_data,"TXDT %d\n",status);
mizuki_akaike 13:c3c7524d2440 188 Im920.printf(send_data);
mizuki_akaike 13:c3c7524d2440 189 }
moai26 0:9c3cc716cbb3 190
mizuki_akaike 15:792eb5103e9e 191 void send_data(const char msg[]) {
mizuki_akaike 15:792eb5103e9e 192 char send_data[256];
mizuki_akaike 15:792eb5103e9e 193 sprintf(send_data,"TXDT %c\n",msg);
mizuki_akaike 15:792eb5103e9e 194 Im920.printf(send_data);
mizuki_akaike 15:792eb5103e9e 195 wait_ms(5);
mizuki_akaike 15:792eb5103e9e 196
mizuki_akaike 15:792eb5103e9e 197 }
mizuki_akaike 15:792eb5103e9e 198
mizuki_akaike 14:b902e73a5404 199 void servo_move() { //サーボの動作に関する関数
mizuki_akaike 13:c3c7524d2440 200 myservo.position(servo_deg_close);
mizuki_akaike 14:b902e73a5404 201 }
mizuki_akaike 6:f4d022422ba6 202
moai26 0:9c3cc716cbb3 203 };
moai26 0:9c3cc716cbb3 204
moai26 0:9c3cc716cbb3 205 Rocket_Data_Get_And_Save rocket_data;
moai26 0:9c3cc716cbb3 206
moai26 0:9c3cc716cbb3 207 uint32_t pressure_status_check()
moai26 0:9c3cc716cbb3 208 {
moai26 0:9c3cc716cbb3 209 //気圧が上昇なら2,減少なら1,変化していないなら0を返す
moai26 0:9c3cc716cbb3 210 //つまり、機体が上昇なら1、下降なら2を返す
moai26 0:9c3cc716cbb3 211 uint32_t pressure_status[size_dif]= {0};
moai26 0:9c3cc716cbb3 212 //pres_dif[]は気圧の変動を保存
moai26 0:9c3cc716cbb3 213 for(uint32_t num=0; num<size_dif; num++) {
moai26 0:9c3cc716cbb3 214 if(pres_dif[num]>3*pre_sd) {//pre_sdは気圧センサの測定誤差
moai26 0:9c3cc716cbb3 215 pressure_status[num]=2;
moai26 0:9c3cc716cbb3 216 } else if(pres_dif[num]<-3*pre_sd) {
moai26 0:9c3cc716cbb3 217 pressure_status[num]=1;
moai26 0:9c3cc716cbb3 218 } else {
moai26 0:9c3cc716cbb3 219 pressure_status[num]=0;
moai26 0:9c3cc716cbb3 220 }
moai26 0:9c3cc716cbb3 221 }
moai26 0:9c3cc716cbb3 222 uint32_t msg=0;
moai26 0:9c3cc716cbb3 223 for(uint32_t num=2; num<size_dif; num++) {
moai26 0:9c3cc716cbb3 224 if(pressure_status[num]==1&&pressure_status[num-1]==1&&pressure_status[num-2]==1) {//3回連続気圧減少なら、機体が上昇中
moai26 0:9c3cc716cbb3 225 msg=1;
moai26 0:9c3cc716cbb3 226 break;
moai26 0:9c3cc716cbb3 227 } else if(pressure_status[num]==2&&pressure_status[num-1]==2&&pressure_status[num-2]==2) {//3回連続気圧上昇なら、機体が下降中
moai26 0:9c3cc716cbb3 228 msg=2;
moai26 0:9c3cc716cbb3 229 break;
moai26 0:9c3cc716cbb3 230 } else { //それ以外は安定状態判定
moai26 0:9c3cc716cbb3 231 continue;
moai26 0:9c3cc716cbb3 232 }
moai26 0:9c3cc716cbb3 233
moai26 0:9c3cc716cbb3 234 }
moai26 0:9c3cc716cbb3 235
moai26 0:9c3cc716cbb3 236 return msg;
moai26 0:9c3cc716cbb3 237
moai26 0:9c3cc716cbb3 238 }
moai26 0:9c3cc716cbb3 239
mizuki_akaike 14:b902e73a5404 240 void Setup() //SDカードの確認
moai26 0:9c3cc716cbb3 241 {
moai26 0:9c3cc716cbb3 242 /*
moai26 0:9c3cc716cbb3 243 起動時に４つのLEDを一旦全部点灯
moai26 0:9c3cc716cbb3 244 SDカードの接続とかセンサーがつながっていることが確認されたら、LEDをすべて消灯
moai26 0:9c3cc716cbb3 245 確認終了後break
moai26 0:9c3cc716cbb3 246 */
moai26 0:9c3cc716cbb3 247
mizuki_akaike 14:b902e73a5404 248 Para=0;//パラシュート展開用のトランジスタをoff
moai26 0:9c3cc716cbb3 249 myled1=myled2=myled3=myled4=1;
moai26 0:9c3cc716cbb3 250 wait_ms(1000);
moai26 0:9c3cc716cbb3 251 myled1=myled2=myled3=myled4=0;
mizuki_akaike 15:792eb5103e9e 252 rocket_data.send_data("setup_ok");
moai26 0:9c3cc716cbb3 253 }
moai26 0:9c3cc716cbb3 254
moai26 0:9c3cc716cbb3 255 uint32_t Launch_detect() //発射検知
moai26 0:9c3cc716cbb3 256 {
moai26 0:9c3cc716cbb3 257 /*
moai26 0:9c3cc716cbb3 258 加速度センサーで検知するタイプと気圧センサで検知するタイプの二つを用意
moai26 0:9c3cc716cbb3 259
moai26 0:9c3cc716cbb3 260 加速度検知した場合は検知後、数秒間は頂点検知にロックをかける。
moai26 0:9c3cc716cbb3 261 検知したらbreak
moai26 0:9c3cc716cbb3 262 検知したらLED1を点灯
moai26 0:9c3cc716cbb3 263 加速度センサで検知したら1,気圧センサで検知したら0を返す
moai26 0:9c3cc716cbb3 264 */
moai26 0:9c3cc716cbb3 265
moai26 0:9c3cc716cbb3 266 //センサの初期データを取得
moai26 0:9c3cc716cbb3 267 double pre_pres=0;
moai26 0:9c3cc716cbb3 268 rocket_data.get_data();
moai26 0:9c3cc716cbb3 269 for(uint32_t j=0; j<size_dif; j++) {
moai26 0:9c3cc716cbb3 270 pre_pres=rocket_data.pres;
moai26 0:9c3cc716cbb3 271 rocket_data.get_data();
moai26 0:9c3cc716cbb3 272 pres_dif[j]=rocket_data.pres-pre_pres;
moai26 0:9c3cc716cbb3 273 }
moai26 0:9c3cc716cbb3 274
moai26 0:9c3cc716cbb3 275
moai26 0:9c3cc716cbb3 276 uint32_t msg;
moai26 0:9c3cc716cbb3 277 uint32_t cycle=0;
moai26 0:9c3cc716cbb3 278 rocket_data.get_data();
moai26 0:9c3cc716cbb3 279 pre_pres=rocket_data.pres;
moai26 0:9c3cc716cbb3 280 while(true) {
moai26 0:9c3cc716cbb3 281 rocket_data.get_data();
moai26 0:9c3cc716cbb3 282 pres_dif[cycle%size_dif]=rocket_data.pres-pre_pres;//差分を計算
moai26 0:9c3cc716cbb3 283 //加速度で検知
moai26 0:9c3cc716cbb3 284 if(fabs(rocket_data.acc[acc_axis])>launch_acc_threshold) {
moai26 0:9c3cc716cbb3 285 launch_detect_time=t.read_ms();
moai26 0:9c3cc716cbb3 286 msg=1;
moai26 0:9c3cc716cbb3 287 break;
moai26 0:9c3cc716cbb3 288 //気圧で検知
mizuki_akaike 5:80e841579008 289 } else if(rocket_data.pres<=launch_pressure_threshold) {//基準気圧を下回ったら発射を検知
moai26 0:9c3cc716cbb3 290 launch_detect_time=t.read_ms();
moai26 0:9c3cc716cbb3 291 msg=0;
moai26 0:9c3cc716cbb3 292 break;
moai26 0:9c3cc716cbb3 293 }
moai26 0:9c3cc716cbb3 294 cycle++;
moai26 0:9c3cc716cbb3 295 pre_pres=rocket_data.pres;//更新
moai26 0:9c3cc716cbb3 296 }
moai26 0:9c3cc716cbb3 297 myled1=1;
moai26 0:9c3cc716cbb3 298 return msg;
moai26 0:9c3cc716cbb3 299 }
moai26 0:9c3cc716cbb3 300
moai26 0:9c3cc716cbb3 301 void fall_detect(uint32_t launch_detect_msg) //頂点検知
moai26 0:9c3cc716cbb3 302 {
moai26 0:9c3cc716cbb3 303 /*
moai26 0:9c3cc716cbb3 304 加速度センサで発射検知された場合には、頂点検知ができなかったときのために時間の制限をかける。
moai26 0:9c3cc716cbb3 305 加速度検知のときは頂点検知に2,3秒ロックをかける
moai26 0:9c3cc716cbb3 306 気圧センサで発射検知された場合には、時間の制限はかけない。
moai26 0:9c3cc716cbb3 307 検知したらbreak
moai26 0:9c3cc716cbb3 308 検知したらLED2を点灯
moai26 0:9c3cc716cbb3 309 */
moai26 0:9c3cc716cbb3 310 double pre_pres=0;
moai26 0:9c3cc716cbb3 311 uint32_t cycle=0;
moai26 0:9c3cc716cbb3 312 //加速度検知で発射検知したとき
moai26 0:9c3cc716cbb3 313 if(launch_detect_msg==1) {
moai26 0:9c3cc716cbb3 314 rocket_data.get_data();
moai26 0:9c3cc716cbb3 315 pre_pres=rocket_data.pres;
moai26 0:9c3cc716cbb3 316 while(true) {
moai26 0:9c3cc716cbb3 317 rocket_data.get_data();
moai26 0:9c3cc716cbb3 318 pres_dif[cycle%size_dif]=rocket_data.pres-pre_pres;
moai26 0:9c3cc716cbb3 319 //頂点検知にロックをかける
moai26 0:9c3cc716cbb3 320 if((t.read_ms()-launch_detect_time)>fall_detect_lock) {
moai26 0:9c3cc716cbb3 321 //気圧変動の判定を取得
kohei_yamamoto 2:3f81e5511138 322 if(pressure_status_check()==2) {
moai26 0:9c3cc716cbb3 323 fall_detect_time=t.read_ms();
moai26 0:9c3cc716cbb3 324 break;
moai26 0:9c3cc716cbb3 325 //やばいときのための時間制限
moai26 0:9c3cc716cbb3 326 } else if((t.read_ms()-launch_detect_time)>time_limit_launch_and_fall_ms) {
moai26 0:9c3cc716cbb3 327 fall_detect_time=t.read_ms();
moai26 0:9c3cc716cbb3 328 break;
moai26 0:9c3cc716cbb3 329 }
moai26 0:9c3cc716cbb3 330 }
moai26 0:9c3cc716cbb3 331 cycle++;
kohei_yamamoto 2:3f81e5511138 332 pre_pres=rocket_data.pres;
moai26 0:9c3cc716cbb3 333 }
moai26 0:9c3cc716cbb3 334 //気圧で発射検知したとき
moai26 0:9c3cc716cbb3 335 } else {
moai26 0:9c3cc716cbb3 336 rocket_data.get_data();
moai26 0:9c3cc716cbb3 337 pre_pres=rocket_data.pres;
moai26 0:9c3cc716cbb3 338 while(true) {
moai26 0:9c3cc716cbb3 339 rocket_data.get_data();
moai26 0:9c3cc716cbb3 340 pres_dif[cycle%size_dif]=rocket_data.pres-pre_pres;
kohei_yamamoto 2:3f81e5511138 341 if(pressure_status_check()==2) {
moai26 0:9c3cc716cbb3 342 fall_detect_time=t.read_ms();
moai26 0:9c3cc716cbb3 343 break;
moai26 0:9c3cc716cbb3 344 }
moai26 0:9c3cc716cbb3 345 cycle++;
kohei_yamamoto 2:3f81e5511138 346 pre_pres=rocket_data.pres;
moai26 0:9c3cc716cbb3 347 }
moai26 0:9c3cc716cbb3 348 }
moai26 0:9c3cc716cbb3 349
moai26 0:9c3cc716cbb3 350 myled2=1;
moai26 0:9c3cc716cbb3 351 }
moai26 0:9c3cc716cbb3 352
moai26 0:9c3cc716cbb3 353
moai26 0:9c3cc716cbb3 354
moai26 0:9c3cc716cbb3 355 void para_open() //パラシュート展開
moai26 0:9c3cc716cbb3 356 {
moai26 0:9c3cc716cbb3 357 /*
moai26 0:9c3cc716cbb3 358 fall_detectを抜けたら、パラシュート展開
moai26 0:9c3cc716cbb3 359 展開高度の制限が必要になったら、waitを挟む
moai26 0:9c3cc716cbb3 360 展開命令後LED3点灯
moai26 0:9c3cc716cbb3 361 */
mizuki_akaike 14:b902e73a5404 362 Para=1;
mizuki_akaike 14:b902e73a5404 363 wait_ms(time_between_para_opening_ms);
mizuki_akaike 14:b902e73a5404 364 Para=0;
mizuki_akaike 14:b902e73a5404 365 para_open_time=t.read_ms();
moai26 0:9c3cc716cbb3 366 myled3=1;
moai26 0:9c3cc716cbb3 367 }
moai26 0:9c3cc716cbb3 368
moai26 0:9c3cc716cbb3 369 void bottle_close() //水密構造稼働
moai26 0:9c3cc716cbb3 370 {
moai26 0:9c3cc716cbb3 371 /*
moai26 0:9c3cc716cbb3 372 パラシュート展開完了後、水密構造を起動
moai26 0:9c3cc716cbb3 373 水密構造稼働ごLED4点灯
moai26 0:9c3cc716cbb3 374 */
mizuki_akaike 13:c3c7524d2440 375 rocket_data.servo_move();
moai26 0:9c3cc716cbb3 376 wait_ms(1000);
moai26 0:9c3cc716cbb3 377 myled4=1;
moai26 0:9c3cc716cbb3 378 }
moai26 0:9c3cc716cbb3 379
moai26 0:9c3cc716cbb3 380 int main()
moai26 0:9c3cc716cbb3 381 {
moai26 0:9c3cc716cbb3 382 t.start();
mizuki_akaike 14:b902e73a5404 383 Setup();
mizuki_akaike 13:c3c7524d2440 384 status=1;
moai26 0:9c3cc716cbb3 385 uint32_t msg=Launch_detect();
mizuki_akaike 13:c3c7524d2440 386 status=2;
moai26 0:9c3cc716cbb3 387 fall_detect(msg);
mizuki_akaike 13:c3c7524d2440 388 status=3;
moai26 0:9c3cc716cbb3 389 para_open();
mizuki_akaike 13:c3c7524d2440 390 status=4;
moai26 0:9c3cc716cbb3 391 bottle_close();
mizuki_akaike 13:c3c7524d2440 392 status=5;
moai26 0:9c3cc716cbb3 393
kohei_yamamoto 2:3f81e5511138 394 }