FTE-denshi / Mbed 2 deprecated FTE-06

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

Dependencies:   mbed FATFileSystem

Fork of FTE-06 by Tetsushi Amano

main.cpp@1:6d05a3f76ac5, 2017-05-23 (annotated)

Committer:
moai26
Date:
Tue May 23 13:09:31 2017 +0000
Revision:
1:6d05a3f76ac5
Parent:
0:9c3cc716cbb3
Child:
2:3f81e5511138
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
moai26 0:9c3cc716cbb3 1 //for FTE-06
moai26 1:6d05a3f76ac5 2
moai26 1:6d05a3f76ac5 3 //testtest
moai26 0:9c3cc716cbb3 4 #pragma O3
moai26 0:9c3cc716cbb3 5
moai26 0:9c3cc716cbb3 6 #include "mbed.h"
moai26 0:9c3cc716cbb3 7
moai26 0:9c3cc716cbb3 8
moai26 0:9c3cc716cbb3 9
moai26 0:9c3cc716cbb3 10 //発射検知でLED1の点灯、頂点通過検知でLED2の点灯、パラシュート展開でLED3の点灯、水密構造のLED4の点灯
moai26 0:9c3cc716cbb3 11
moai26 0:9c3cc716cbb3 12
moai26 0:9c3cc716cbb3 13
moai26 0:9c3cc716cbb3 14
moai26 0:9c3cc716cbb3 15 #define launch_acc_threshold 5//発射時の加速度(G)
moai26 0:9c3cc716cbb3 16 #define launch_pressure_threshold 990//発射したとみなす気圧（おおよそ地上気圧-80.4hpa)
moai26 0:9c3cc716cbb3 17 #define acc_axis 1//上方向の軸を決定x;0,y;1,z;2
moai26 0:9c3cc716cbb3 18 #define pre_sd 0.037//気圧センサの測定誤差(事前に測定して計算)
moai26 0:9c3cc716cbb3 19 #define size_dif 3//気圧の変位を保存する数
moai26 0:9c3cc716cbb3 20 #define time_limit_launch_and_fall_ms 20000//発射から頂点に至るまでの予測時間(msec)
moai26 0:9c3cc716cbb3 21 #define fall_detect_lock 2000//加速度で発射検知したときに頂点検知にロックをかける時間(msec)
moai26 0:9c3cc716cbb3 22
moai26 0:9c3cc716cbb3 23 DigitalOut myled1(LED1);
moai26 0:9c3cc716cbb3 24 DigitalOut myled2(LED2);
moai26 0:9c3cc716cbb3 25 DigitalOut myled3(LED3);
moai26 0:9c3cc716cbb3 26 DigitalOut myled4(LED4);
moai26 0:9c3cc716cbb3 27 Timer t;
moai26 0:9c3cc716cbb3 28
moai26 0:9c3cc716cbb3 29
moai26 0:9c3cc716cbb3 30
moai26 0:9c3cc716cbb3 31 double pres_dif[size_dif]= {0}; //気圧の変化をsize_difの数だけ保存
moai26 0:9c3cc716cbb3 32 uint32_t launch_detect_time=0;//発射検知の時間
moai26 0:9c3cc716cbb3 33 uint32_t fall_detect_time=0;//頂点検知の時間
moai26 0:9c3cc716cbb3 34
moai26 0:9c3cc716cbb3 35
moai26 0:9c3cc716cbb3 36
moai26 0:9c3cc716cbb3 37 class Rocket_Data_Get_And_Save//ロケットの測定データを管理するクラス
moai26 0:9c3cc716cbb3 38 {
moai26 0:9c3cc716cbb3 39 public:
moai26 0:9c3cc716cbb3 40 double acc[3];//x,y,z
moai26 0:9c3cc716cbb3 41 double gyro[3];
moai26 0:9c3cc716cbb3 42 double mag[3];
moai26 0:9c3cc716cbb3 43 double latitude,longitude;
moai26 0:9c3cc716cbb3 44 double pres,temp;
moai26 0:9c3cc716cbb3 45 uint32_t eeprom_add;
moai26 0:9c3cc716cbb3 46
moai26 0:9c3cc716cbb3 47
moai26 0:9c3cc716cbb3 48 Rocket_Data_Get_And_Save() {//初期化子
moai26 0:9c3cc716cbb3 49 for(uint32_t j=0; j<3; j++) {
moai26 0:9c3cc716cbb3 50 acc[j]=gyro[j]=mag[j]=0;
moai26 0:9c3cc716cbb3 51 pres=temp=0;
moai26 0:9c3cc716cbb3 52 }
moai26 0:9c3cc716cbb3 53 eeprom_add=0;
moai26 0:9c3cc716cbb3 54 time_ms_now=0;
moai26 0:9c3cc716cbb3 55 latitude=0;
moai26 0:9c3cc716cbb3 56 longitude=0;
moai26 0:9c3cc716cbb3 57 }
moai26 0:9c3cc716cbb3 58
moai26 0:9c3cc716cbb3 59 void save_data() { //sdカードへのデータの保存
moai26 0:9c3cc716cbb3 60 /*
moai26 0:9c3cc716cbb3 61 データのセーブについて
moai26 0:9c3cc716cbb3 62 データを上書きされないように乱数を使って
moai26 0:9c3cc716cbb3 63 データ名を毎回変更する
moai26 0:9c3cc716cbb3 64 クラスの初期化子でファイル名を決めてしまうといいかも
moai26 0:9c3cc716cbb3 65 乱数のseedは起動するたびに変更する必要があるので
moai26 0:9c3cc716cbb3 66 何もつながっていないanalogpinの入力電圧とか使うといいかも
moai26 0:9c3cc716cbb3 67 乱数の幅は0~999までとかに制限すること
moai26 0:9c3cc716cbb3 68 データはresult_(乱数).datに保存すること
moai26 0:9c3cc716cbb3 69 データの書式は
moai26 0:9c3cc716cbb3 70 "time,temperature,pressure,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,..."
moai26 0:9c3cc716cbb3 71 という書式にすること
moai26 0:9c3cc716cbb3 72 下手に単位とかデータの名前をいれるとデータ分析で時間がかかるので、数値を","で区切るだけにすること
moai26 0:9c3cc716cbb3 73 */
moai26 0:9c3cc716cbb3 74
moai26 0:9c3cc716cbb3 75 }
moai26 0:9c3cc716cbb3 76 void get_data() {
moai26 0:9c3cc716cbb3 77 time_ms_now=t.read_ms();//取得時の時間を保存
moai26 0:9c3cc716cbb3 78
moai26 0:9c3cc716cbb3 79 save_data();//データの保存
moai26 0:9c3cc716cbb3 80 }
moai26 0:9c3cc716cbb3 81
moai26 0:9c3cc716cbb3 82 void write_data2eeprom() {//eepromへの書き込み
moai26 0:9c3cc716cbb3 83 //1文字書き込むごとにwait_ms(1)が必要
moai26 0:9c3cc716cbb3 84
moai26 0:9c3cc716cbb3 85
moai26 0:9c3cc716cbb3 86 }
moai26 0:9c3cc716cbb3 87
moai26 0:9c3cc716cbb3 88 void get_GPS() {//GPSデータの取得
moai26 0:9c3cc716cbb3 89
moai26 0:9c3cc716cbb3 90
moai26 0:9c3cc716cbb3 91 }
moai26 0:9c3cc716cbb3 92
moai26 0:9c3cc716cbb3 93 void send_GPS() {//GPSデータの送信
moai26 0:9c3cc716cbb3 94
moai26 0:9c3cc716cbb3 95
moai26 0:9c3cc716cbb3 96 }
moai26 0:9c3cc716cbb3 97 private:
moai26 0:9c3cc716cbb3 98 uint32_t time_ms_now;
moai26 0:9c3cc716cbb3 99 };
moai26 0:9c3cc716cbb3 100
moai26 0:9c3cc716cbb3 101 Rocket_Data_Get_And_Save rocket_data;
moai26 0:9c3cc716cbb3 102
moai26 0:9c3cc716cbb3 103 uint32_t pressure_status_check()
moai26 0:9c3cc716cbb3 104 {
moai26 0:9c3cc716cbb3 105 //気圧が上昇なら2,減少なら1,変化していないなら0を返す
moai26 0:9c3cc716cbb3 106 //つまり、機体が上昇なら1、下降なら2を返す
moai26 0:9c3cc716cbb3 107 uint32_t pressure_status[size_dif]= {0};
moai26 0:9c3cc716cbb3 108 //pres_dif[]は気圧の変動を保存
moai26 0:9c3cc716cbb3 109 for(uint32_t num=0; num<size_dif; num++) {
moai26 0:9c3cc716cbb3 110 if(pres_dif[num]>3*pre_sd) {//pre_sdは気圧センサの測定誤差
moai26 0:9c3cc716cbb3 111 pressure_status[num]=2;
moai26 0:9c3cc716cbb3 112 } else if(pres_dif[num]<-3*pre_sd) {
moai26 0:9c3cc716cbb3 113 pressure_status[num]=1;
moai26 0:9c3cc716cbb3 114 } else {
moai26 0:9c3cc716cbb3 115 pressure_status[num]=0;
moai26 0:9c3cc716cbb3 116 }
moai26 0:9c3cc716cbb3 117 }
moai26 0:9c3cc716cbb3 118 uint32_t msg=0;
moai26 0:9c3cc716cbb3 119 for(uint32_t num=2; num<size_dif; num++) {
moai26 0:9c3cc716cbb3 120 if(pressure_status[num]==1&&pressure_status[num-1]==1&&pressure_status[num-2]==1) {//3回連続気圧減少なら、機体が上昇中
moai26 0:9c3cc716cbb3 121 msg=1;
moai26 0:9c3cc716cbb3 122 break;
moai26 0:9c3cc716cbb3 123 } else if(pressure_status[num]==2&&pressure_status[num-1]==2&&pressure_status[num-2]==2) {//3回連続気圧上昇なら、機体が下降中
moai26 0:9c3cc716cbb3 124 msg=2;
moai26 0:9c3cc716cbb3 125 break;
moai26 0:9c3cc716cbb3 126 } else { //それ以外は安定状態判定
moai26 0:9c3cc716cbb3 127 continue;
moai26 0:9c3cc716cbb3 128 }
moai26 0:9c3cc716cbb3 129
moai26 0:9c3cc716cbb3 130 }
moai26 0:9c3cc716cbb3 131
moai26 0:9c3cc716cbb3 132 return msg;
moai26 0:9c3cc716cbb3 133
moai26 0:9c3cc716cbb3 134 }
moai26 0:9c3cc716cbb3 135
moai26 0:9c3cc716cbb3 136 void SD_check() //SDカードの確認
moai26 0:9c3cc716cbb3 137 {
moai26 0:9c3cc716cbb3 138 /*
moai26 0:9c3cc716cbb3 139 起動時に４つのLEDを一旦全部点灯
moai26 0:9c3cc716cbb3 140 SDカードの接続とかセンサーがつながっていることが確認されたら、LEDをすべて消灯
moai26 0:9c3cc716cbb3 141 確認終了後break
moai26 0:9c3cc716cbb3 142 */
moai26 0:9c3cc716cbb3 143
moai26 0:9c3cc716cbb3 144
moai26 0:9c3cc716cbb3 145 myled1=myled2=myled3=myled4=1;
moai26 0:9c3cc716cbb3 146 wait_ms(1000);
moai26 0:9c3cc716cbb3 147 myled1=myled2=myled3=myled4=0;
moai26 0:9c3cc716cbb3 148 }
moai26 0:9c3cc716cbb3 149
moai26 0:9c3cc716cbb3 150 uint32_t Launch_detect() //発射検知
moai26 0:9c3cc716cbb3 151 {
moai26 0:9c3cc716cbb3 152 /*
moai26 0:9c3cc716cbb3 153 加速度センサーで検知するタイプと気圧センサで検知するタイプの二つを用意
moai26 0:9c3cc716cbb3 154
moai26 0:9c3cc716cbb3 155 加速度検知した場合は検知後、数秒間は頂点検知にロックをかける。
moai26 0:9c3cc716cbb3 156 検知したらbreak
moai26 0:9c3cc716cbb3 157 検知したらLED1を点灯
moai26 0:9c3cc716cbb3 158 加速度センサで検知したら1,気圧センサで検知したら0を返す
moai26 0:9c3cc716cbb3 159 */
moai26 0:9c3cc716cbb3 160
moai26 0:9c3cc716cbb3 161 //センサの初期データを取得
moai26 0:9c3cc716cbb3 162 double pre_pres=0;
moai26 0:9c3cc716cbb3 163 rocket_data.get_data();
moai26 0:9c3cc716cbb3 164 for(uint32_t j=0; j<size_dif; j++) {
moai26 0:9c3cc716cbb3 165 pre_pres=rocket_data.pres;
moai26 0:9c3cc716cbb3 166 rocket_data.get_data();
moai26 0:9c3cc716cbb3 167 pres_dif[j]=rocket_data.pres-pre_pres;
moai26 0:9c3cc716cbb3 168 }
moai26 0:9c3cc716cbb3 169
moai26 0:9c3cc716cbb3 170
moai26 0:9c3cc716cbb3 171 uint32_t msg;
moai26 0:9c3cc716cbb3 172 uint32_t cycle=0;
moai26 0:9c3cc716cbb3 173 rocket_data.get_data();
moai26 0:9c3cc716cbb3 174 pre_pres=rocket_data.pres;
moai26 0:9c3cc716cbb3 175 while(true) {
moai26 0:9c3cc716cbb3 176 rocket_data.get_data();
moai26 0:9c3cc716cbb3 177 pres_dif[cycle%size_dif]=rocket_data.pres-pre_pres;//差分を計算
moai26 0:9c3cc716cbb3 178
moai26 0:9c3cc716cbb3 179
moai26 0:9c3cc716cbb3 180 //加速度で検知
moai26 0:9c3cc716cbb3 181 if(fabs(rocket_data.acc[acc_axis])>launch_acc_threshold) {
moai26 0:9c3cc716cbb3 182 launch_detect_time=t.read_ms();
moai26 0:9c3cc716cbb3 183 msg=1;
moai26 0:9c3cc716cbb3 184 break;
moai26 0:9c3cc716cbb3 185 //気圧で検知
moai26 0:9c3cc716cbb3 186 } else if(rocket_data.pres<=launch_pressure_threshold) {//基準気圧を下回ったら発射を検知
moai26 0:9c3cc716cbb3 187 launch_detect_time=t.read_ms();
moai26 0:9c3cc716cbb3 188 msg=0;
moai26 0:9c3cc716cbb3 189 break;
moai26 0:9c3cc716cbb3 190 }
moai26 0:9c3cc716cbb3 191 cycle++;
moai26 0:9c3cc716cbb3 192 pre_pres=rocket_data.pres;//更新
moai26 0:9c3cc716cbb3 193 }
moai26 0:9c3cc716cbb3 194 myled1=1;
moai26 0:9c3cc716cbb3 195 return msg;
moai26 0:9c3cc716cbb3 196 }
moai26 0:9c3cc716cbb3 197
moai26 0:9c3cc716cbb3 198 void fall_detect(uint32_t launch_detect_msg) //頂点検知
moai26 0:9c3cc716cbb3 199 {
moai26 0:9c3cc716cbb3 200 /*
moai26 0:9c3cc716cbb3 201 加速度センサで発射検知された場合には、頂点検知ができなかったときのために時間の制限をかける。
moai26 0:9c3cc716cbb3 202 加速度検知のときは頂点検知に2,3秒ロックをかける
moai26 0:9c3cc716cbb3 203 気圧センサで発射検知された場合には、時間の制限はかけない。
moai26 0:9c3cc716cbb3 204 検知したらbreak
moai26 0:9c3cc716cbb3 205 検知したらLED2を点灯
moai26 0:9c3cc716cbb3 206 */
moai26 0:9c3cc716cbb3 207 double pre_pres=0;
moai26 0:9c3cc716cbb3 208 uint32_t cycle=0;
moai26 0:9c3cc716cbb3 209 //加速度検知で発射検知したとき
moai26 0:9c3cc716cbb3 210 if(launch_detect_msg==1) {
moai26 0:9c3cc716cbb3 211 rocket_data.get_data();
moai26 0:9c3cc716cbb3 212 pre_pres=rocket_data.pres;
moai26 0:9c3cc716cbb3 213 while(true) {
moai26 0:9c3cc716cbb3 214 rocket_data.get_data();
moai26 0:9c3cc716cbb3 215 pres_dif[cycle%size_dif]=rocket_data.pres-pre_pres;
moai26 0:9c3cc716cbb3 216 //頂点検知にロックをかける
moai26 0:9c3cc716cbb3 217 if((t.read_ms()-launch_detect_time)>fall_detect_lock) {
moai26 0:9c3cc716cbb3 218 //気圧変動の判定を取得
moai26 0:9c3cc716cbb3 219 if(pressure_status_check()!=1) {//上昇しなくなったと検知したら頂点検知
moai26 0:9c3cc716cbb3 220 fall_detect_time=t.read_ms();
moai26 0:9c3cc716cbb3 221 break;
moai26 0:9c3cc716cbb3 222 //やばいときのための時間制限
moai26 0:9c3cc716cbb3 223 } else if((t.read_ms()-launch_detect_time)>time_limit_launch_and_fall_ms) {
moai26 0:9c3cc716cbb3 224 fall_detect_time=t.read_ms();
moai26 0:9c3cc716cbb3 225 break;
moai26 0:9c3cc716cbb3 226 }
moai26 0:9c3cc716cbb3 227 }
moai26 0:9c3cc716cbb3 228 cycle++;
moai26 0:9c3cc716cbb3 229 pre_pres=rocket_data.pres;//更新
moai26 0:9c3cc716cbb3 230 }
moai26 0:9c3cc716cbb3 231 //気圧で発射検知したとき
moai26 0:9c3cc716cbb3 232 } else {
moai26 0:9c3cc716cbb3 233 rocket_data.get_data();
moai26 0:9c3cc716cbb3 234 pre_pres=rocket_data.pres;
moai26 0:9c3cc716cbb3 235 while(true) {
moai26 0:9c3cc716cbb3 236 rocket_data.get_data();
moai26 0:9c3cc716cbb3 237 pres_dif[cycle%size_dif]=rocket_data.pres-pre_pres;
moai26 0:9c3cc716cbb3 238 if(pressure_status_check()!=1) {//上昇しなくなったら頂点検知
moai26 0:9c3cc716cbb3 239 fall_detect_time=t.read_ms();
moai26 0:9c3cc716cbb3 240 break;
moai26 0:9c3cc716cbb3 241 }
moai26 0:9c3cc716cbb3 242 cycle++;
moai26 0:9c3cc716cbb3 243 pre_pres=rocket_data.pres;//更新
moai26 0:9c3cc716cbb3 244 }
moai26 0:9c3cc716cbb3 245 }
moai26 0:9c3cc716cbb3 246
moai26 0:9c3cc716cbb3 247 myled2=1;
moai26 0:9c3cc716cbb3 248 }
moai26 0:9c3cc716cbb3 249
moai26 0:9c3cc716cbb3 250
moai26 0:9c3cc716cbb3 251
moai26 0:9c3cc716cbb3 252 void para_open() //パラシュート展開
moai26 0:9c3cc716cbb3 253 {
moai26 0:9c3cc716cbb3 254 /*
moai26 0:9c3cc716cbb3 255 fall_detectを抜けたら、パラシュート展開
moai26 0:9c3cc716cbb3 256 展開高度の制限が必要になったら、waitを挟む
moai26 0:9c3cc716cbb3 257 展開命令後LED3点灯
moai26 0:9c3cc716cbb3 258 */
moai26 0:9c3cc716cbb3 259 wait_ms(1000);
moai26 0:9c3cc716cbb3 260 myled3=1;
moai26 0:9c3cc716cbb3 261 }
moai26 0:9c3cc716cbb3 262
moai26 0:9c3cc716cbb3 263 void bottle_close() //水密構造稼働
moai26 0:9c3cc716cbb3 264 {
moai26 0:9c3cc716cbb3 265 /*
moai26 0:9c3cc716cbb3 266 パラシュート展開完了後、水密構造を起動
moai26 0:9c3cc716cbb3 267 水密構造稼働ごLED4点灯
moai26 0:9c3cc716cbb3 268 */
moai26 0:9c3cc716cbb3 269 wait_ms(1000);
moai26 0:9c3cc716cbb3 270 myled4=1;
moai26 0:9c3cc716cbb3 271 }
moai26 0:9c3cc716cbb3 272
moai26 0:9c3cc716cbb3 273 int main()
moai26 0:9c3cc716cbb3 274 {
moai26 0:9c3cc716cbb3 275 t.start();
moai26 0:9c3cc716cbb3 276 SD_check();
moai26 0:9c3cc716cbb3 277 uint32_t msg=Launch_detect();
moai26 0:9c3cc716cbb3 278 fall_detect(msg);
moai26 0:9c3cc716cbb3 279 para_open();
moai26 0:9c3cc716cbb3 280 bottle_close();
moai26 0:9c3cc716cbb3 281
moai26 0:9c3cc716cbb3 282
moai26 0:9c3cc716cbb3 283 }
moai26 0:9c3cc716cbb3 284