FTE-denshi / Mbed 2 deprecated FTE-06

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

Dependencies:   mbed FATFileSystem

Fork of FTE-06 by Tetsushi Amano

main.cpp@56:91ba7f8bfd0a, 2017-08-22 (annotated)

Committer:
mizuki_akaike
Date:
Tue Aug 22 15:08:39 2017 +0000
Revision:
56:91ba7f8bfd0a
Parent:
55:a158b2b4b970
Child:
57:d78497a079cf
?????????

Who changed what in which revision?

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