Diff: main.cpp

Revision:

0:9c3cc716cbb3

Child:

1:6d05a3f76ac5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 23 06:06:12 2017 +0000
@@ -0,0 +1,282 @@
+//for FTE-06
+#pragma O3
+
+#include "mbed.h"
+
+
+
+//発射検知でLED1の点灯、頂点通過検知でLED2の点灯、パラシュート展開でLED3の点灯、水密構造のLED4の点灯
+
+
+
+
+#define launch_acc_threshold 5//発射時の加速度(G)
+#define launch_pressure_threshold 990//発射したとみなす気圧（おおよそ地上気圧-80.4hpa)
+#define acc_axis 1//上方向の軸を決定x;0,y;1,z;2 
+#define pre_sd 0.037//気圧センサの測定誤差(事前に測定して計算)
+#define size_dif 3//気圧の変位を保存する数
+#define time_limit_launch_and_fall_ms 20000//発射から頂点に至るまでの予測時間(msec)
+#define fall_detect_lock 2000//加速度で発射検知したときに頂点検知にロックをかける時間(msec)
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+Timer t;
+
+
+
+double pres_dif[size_dif]= {0}; //気圧の変化をsize_difの数だけ保存
+uint32_t launch_detect_time=0;//発射検知の時間
+uint32_t fall_detect_time=0;//頂点検知の時間
+
+
+
+class Rocket_Data_Get_And_Save//ロケットの測定データを管理するクラス
+{
+public:
+    double acc[3];//x,y,z
+    double gyro[3];
+    double mag[3];
+    double latitude,longitude;
+    double pres,temp;
+    uint32_t eeprom_add;
+
+
+    Rocket_Data_Get_And_Save() {//初期化子
+        for(uint32_t j=0; j<3; j++) {
+            acc[j]=gyro[j]=mag[j]=0;
+            pres=temp=0;
+        }
+        eeprom_add=0;
+        time_ms_now=0;
+        latitude=0;
+        longitude=0;
+    }
+
+    void save_data() { //sdカードへのデータの保存
+        /*
+        データのセーブについて
+        データを上書きされないように乱数を使って
+        データ名を毎回変更する
+        クラスの初期化子でファイル名を決めてしまうといいかも
+        乱数のseedは起動するたびに変更する必要があるので
+        何もつながっていないanalogpinの入力電圧とか使うといいかも
+        乱数の幅は0~999までとかに制限すること
+        データはresult_(乱数).datに保存すること
+        データの書式は
+        "time,temperature,pressure,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,..."
+        という書式にすること
+        下手に単位とかデータの名前をいれるとデータ分析で時間がかかるので、数値を","で区切るだけにすること
+        */
+
+    }
+    void get_data() {
+        time_ms_now=t.read_ms();//取得時の時間を保存
+
+        save_data();//データの保存
+    }
+
+    void write_data2eeprom() {//eepromへの書き込み
+        //1文字書き込むごとにwait_ms(1)が必要
+
+
+    }
+
+    void get_GPS() {//GPSデータの取得
+
+
+    }
+
+    void send_GPS() {//GPSデータの送信
+
+
+    }
+private:
+    uint32_t time_ms_now;
+};
+
+Rocket_Data_Get_And_Save rocket_data;
+
+uint32_t pressure_status_check()
+{
+    //気圧が上昇なら2,減少なら1,変化していないなら0を返す
+    //つまり、機体が上昇なら1、下降なら2を返す
+    uint32_t pressure_status[size_dif]= {0};
+    //pres_dif[]は気圧の変動を保存
+    for(uint32_t num=0; num<size_dif; num++) {
+        if(pres_dif[num]>3*pre_sd) {//pre_sdは気圧センサの測定誤差
+            pressure_status[num]=2;
+        } else if(pres_dif[num]<-3*pre_sd) {
+            pressure_status[num]=1;
+        } else {
+            pressure_status[num]=0;
+        }
+    }
+    uint32_t msg=0;
+    for(uint32_t num=2; num<size_dif; num++) {
+        if(pressure_status[num]==1&&pressure_status[num-1]==1&&pressure_status[num-2]==1) {//3回連続気圧減少なら、機体が上昇中
+            msg=1;
+            break;
+        } else if(pressure_status[num]==2&&pressure_status[num-1]==2&&pressure_status[num-2]==2) {//3回連続気圧上昇なら、機体が下降中
+            msg=2;
+            break;
+        } else { //それ以外は安定状態判定
+            continue;
+        }
+
+    }
+
+    return msg;
+
+}
+
+void SD_check() //SDカードの確認
+{
+    /*
+    起動時に４つのLEDを一旦全部点灯
+    SDカードの接続とかセンサーがつながっていることが確認されたら、LEDをすべて消灯
+    確認終了後break
+    */
+
+
+    myled1=myled2=myled3=myled4=1;
+    wait_ms(1000);
+    myled1=myled2=myled3=myled4=0;
+}
+
+uint32_t Launch_detect() //発射検知
+{
+    /*
+    加速度センサーで検知するタイプと気圧センサで検知するタイプの二つを用意
+
+    加速度検知した場合は検知後、数秒間は頂点検知にロックをかける。
+    検知したらbreak
+    検知したらLED1を点灯
+    加速度センサで検知したら1,気圧センサで検知したら0を返す
+    */
+
+//センサの初期データを取得
+    double pre_pres=0;
+    rocket_data.get_data();
+    for(uint32_t j=0; j<size_dif; j++) {
+        pre_pres=rocket_data.pres;
+        rocket_data.get_data();
+        pres_dif[j]=rocket_data.pres-pre_pres;
+    }
+
+
+    uint32_t msg;
+    uint32_t cycle=0;
+    rocket_data.get_data();
+    pre_pres=rocket_data.pres;
+    while(true) {
+        rocket_data.get_data();
+        pres_dif[cycle%size_dif]=rocket_data.pres-pre_pres;//差分を計算
+
+
+//加速度で検知
+        if(fabs(rocket_data.acc[acc_axis])>launch_acc_threshold) {
+            launch_detect_time=t.read_ms();
+            msg=1;
+            break;
+//気圧で検知
+        } else if(rocket_data.pres<=launch_pressure_threshold) {//基準気圧を下回ったら発射を検知
+            launch_detect_time=t.read_ms();
+            msg=0;
+            break;
+        }
+        cycle++;
+        pre_pres=rocket_data.pres;//更新
+    }
+    myled1=1;
+    return msg;
+}
+
+void fall_detect(uint32_t launch_detect_msg) //頂点検知
+{
+    /*
+    加速度センサで発射検知された場合には、頂点検知ができなかったときのために時間の制限をかける。
+    加速度検知のときは頂点検知に2,3秒ロックをかける
+    気圧センサで発射検知された場合には、時間の制限はかけない。
+    検知したらbreak
+    検知したらLED2を点灯
+    */
+    double pre_pres=0;
+    uint32_t cycle=0;
+//加速度検知で発射検知したとき
+    if(launch_detect_msg==1) {
+        rocket_data.get_data();
+        pre_pres=rocket_data.pres;
+        while(true) {
+            rocket_data.get_data();
+            pres_dif[cycle%size_dif]=rocket_data.pres-pre_pres;
+            //頂点検知にロックをかける
+            if((t.read_ms()-launch_detect_time)>fall_detect_lock) {
+                //気圧変動の判定を取得
+                if(pressure_status_check()!=1) {//上昇しなくなったと検知したら頂点検知
+                    fall_detect_time=t.read_ms();
+                    break;
+                    //やばいときのための時間制限
+                } else if((t.read_ms()-launch_detect_time)>time_limit_launch_and_fall_ms) {
+                    fall_detect_time=t.read_ms();
+                    break;
+                }
+            }
+            cycle++;
+            pre_pres=rocket_data.pres;//更新
+        }
+//気圧で発射検知したとき
+    } else {
+        rocket_data.get_data();
+        pre_pres=rocket_data.pres;
+        while(true) {
+            rocket_data.get_data();
+            pres_dif[cycle%size_dif]=rocket_data.pres-pre_pres;
+            if(pressure_status_check()!=1) {//上昇しなくなったら頂点検知
+                fall_detect_time=t.read_ms();
+                break;
+            }
+            cycle++;
+            pre_pres=rocket_data.pres;//更新
+        }
+    }
+
+    myled2=1;
+}
+
+
+
+void para_open() //パラシュート展開
+{
+    /*
+    fall_detectを抜けたら、パラシュート展開
+    展開高度の制限が必要になったら、waitを挟む
+    展開命令後LED3点灯
+    */
+    wait_ms(1000);
+    myled3=1;
+}
+
+void bottle_close() //水密構造稼働
+{
+    /*
+    パラシュート展開完了後、水密構造を起動
+    水密構造稼働ごLED4点灯
+    */
+    wait_ms(1000);
+    myled4=1;
+}
+
+int main()
+{
+    t.start();
+    SD_check();
+    uint32_t msg=Launch_detect();
+    fall_detect(msg);
+    para_open();
+    bottle_close();
+
+
+}
+