File content as of revision 35:3f9e6d691f33:

#pragma O3

#include "IM920.h"
#include "mbed.h"
#include "SDFileSystem.h"
#include "BMP085.h"
#include "L3GD20.h"
#include "LSM303DLHC.h"
#include "24LCXXX.h"
#include "GPS.h"
#include "Servo.h"
#include "BME280.h"
//発射検知でLED1の点灯、頂点通過検知でLED2の点灯、パラシュート展開でLED3の点灯、水密構造のLED4の点灯
//im920ではECIOコマンドを使って、送信データのアスキーコード逆変換が行われるようにすること
//-----------------------------------------ロケットの制御にかかわる設定
#define launch_pressure_threshold 990.//発射したとみなす気圧（おおよそ地上気圧-80.4hpa)
#define launch_acc_threshold 2.5 //発射時の加速度(G) 
#define acc_axis 0//上方向の軸を決定x;0,y;1,z;2 
#define pre_sd 0.00037//気圧センサの測定誤差(事前に測定して計算)
#define size_dif 3//気圧の変位を保存する数
#define time_limit_launch_and_fall_ms 10000//発射から頂点に至るまでの予測時間(msec)
#define fall_detect_lock 2000//加速度で発射検知したときに頂点検知にロックをかける時間(msec)
#define servo_deg_initial 20//サーボの初期角度
#define servo_deg_close 90//サーボの水密構造を閉じる時の角度

//-----------------------------------------センサの測定レンジなどの設定
#define Acc_range 2.//加速度センサの測定レンジ
#define para_switch p22//パラシュート展開用のデジタル出力ピン番号
#define time_between_para_opening_ms 4000//パラシュート展開用の電熱線の加熱時間(msec) 
#define conv2Gravity 10./(32768./Acc_range)/9.8//加速度センサの値の単位をGに変換
#define AT24C1024_address 0x50
#define eeprom_byte_size 10000
#define im920_serial_baudrate 19200

DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);

DigitalOut Para(para_switch);//パラシュート展開用のクラス

Timer t;

AnalogIn analog_rand(p20);//電圧読み取り用のピン

double pres_dif[size_dif]= {0}; //気圧の変化をsize_difの数だけ保存
unsigned long launch_detect_time=0;//発射検知の時間(ms)
unsigned long fall_detect_time=0;//頂点検知の時間(ms)
unsigned long para_open_time=0;//パラシュート展開の時間(ms)

SDFileSystem sd(p5, p6, p7, p8, "sd");//SDカードのオブジェクト作成
BMP085 BMP(p9,p10);//気圧、温度センサのクラス
BME280 BME(p9,p10);//気圧センサ（外部）クラス
L3GD20 L3GD(p9,p10);//ジャイロセンサのクラス
I2C L(p9,p10);
LSM303DLHC LSM(&L);//加速度センサのクラス
I2C E(p9,p10);
_24LCXXX eeprom(&E,AT24C1024_address);//eepromのクラス
GPS sGPS(p13,p14);//GPSのクラス
Serial pc(USBTX, USBRX);
IM920 Im920(p28,p27,p16,p15,im920_serial_baudrate);//無線通信用のserialクラス
Servo myservo(p21);//サーボ用のクラス
int status=0;

class Rocket_Data_Get_And_Save//ロケットの測定データを管理するクラス
{
private:
    int check_eeprom;//eepromへの書き込みができたかどうかのフラグ
    unsigned long time_ms_now;
    char buf_eeprom[20];//書き込みデータの一時保存先
    inline void LSM_updata() {
        LSM.getAccel();
        acc[0]=conv2Gravity*LSM.accelX();
        acc[1]=conv2Gravity*LSM.accelY();
        acc[2]=conv2Gravity*LSM.accelZ();
        LSM.getMagnet();
        mag[0]=LSM.magnetX();
        mag[1]=LSM.magnetX();
        mag[2]=LSM.magnetX();
    }

public:
    double acc[3];//x,y,z
    float gyro[3];
    double mag[3];
    double latitude,longitude;
    double pres,temp;
    uint32_t eeprom_add;
    int  result_num;
    char num_buf[30];

    Rocket_Data_Get_And_Save() {//初期化子
        //--------------変数の初期化
        for(uint32_t j=0; j<3; j++) {
            acc[j]=gyro[j]=mag[j]=0;
        }
        pres=0;
        temp=0;
        eeprom_add=0;
        check_eeprom=true;
        time_ms_now=0;
        latitude=0;
        longitude=0;

        //---------------------------
        myservo.position(servo_deg_initial);//サーボ位置の初期化
        /*randomなfile名作成*/
        /*
        result_num = rand()%1000;//0~999のランダムな数字
        snprintf(num_buf, 30, "%d", result_num);
        strcat("res", num_buf);*/
        mkdir("/sd/mydir", 0777);
    }


    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,..."
        という書式にすること
        下手に単位とかデータの名前をいれるとデータ分析で時間がかかるので、数値を","で区切るだけにすること
        */

        /*SDカードに書き込み*/

        FILE *fp = fopen("/sd/mydir/data.csv", "a");
        if (fp == NULL)error("Could not open SD Card for write\n");
        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);
        fclose(fp);

    }

    void get_data() {
        time_ms_now=t.read_ms();//取得時の時間を保存
        /*
        BMP.update();
        pres=BMP.get_pressure();
        temp=BMP.get_temperature();
        */
        get_GPS();
        pres=BME.getPressure();
        temp=BME.getTemperature();
        L3GD.read(&gyro[0],&gyro[1],&gyro[2]);
        LSM_updata();
        save_data();//データの保存
        send_GPS_and_status();
        write_data2eeprom();//eepromへの保存
    }

    void write_data2eeprom() {//eepromへの書き込み
        //最低限のデータのみ保存
        sprintf(buf_eeprom,"%d,%f.4\n",time,pres);
        char write_byte=buf_eeprom[0];
        for(uint32_t n=0; write_byte!='\0'&&write_byte!=NULL; n++) {
            write_byte=buf_eeprom[n];
            check_eeprom=eeprom.byte_write(eeprom_add++,write_byte);
        }
    }

    void delete_eeprom() { //eepromの中身を全消去
        char *del;
        char emp='0';//eepromの初期化は"0"
        eeprom.nbyte_read(0,del,1);
        for(int32_t n=0; del[0]!='\0'&&n<eeprom_byte_size; n++) { //終端文字か指定バイトの数まで初期化
            eeprom.byte_write(n,emp);
            eeprom.nbyte_read(n+1,del,1);
        }


    }

    void get_GPS() {//GPSデータの取得
        if(sGPS.sample()) {
            latitude=sGPS.latitude;
            longitude=sGPS.longitude;
        } else {
            latitude=0;
            longitude=0;
        }
    }

    void send_GPS_and_status() {//GPSデータの送信
        char send_data[100];
        sprintf(send_data,"%.7f%.7f",latitude,longitude);
        Im920.sendData(send_data,strlen(send_data));
        wait_ms(5);
        //sprintf(send_data,"%d",status);
        //Im920.sendData(send_data,strlen(send_data));
    }

    void send_data(const char msg[]) {
        Im920.sendData(msg,strlen(msg));
        wait_ms(5);

    }

    void servo_move() { //サーボの動作に関する関数
        myservo.position(servo_deg_close);
    }

};

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 Setup() //SDカードの確認
{
    /*
    起動時に４つのLEDを一旦全部点灯
    SDカードの接続とかセンサーがつながっていることが確認されたら、LEDをすべて消灯
    確認終了後break
    */
    BME.initialize();//BME280の初期化
    Para=0;//パラシュート展開用のトランジスタをoff
    myled1=myled2=myled3=myled4=1;
    wait_ms(1000);
    myled1=myled2=myled3=myled4=0;
    rocket_data.send_data("setup_ok");
}

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()==2) {
                    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()==2) {
                fall_detect_time=t.read_ms();
                break;
            }
            cycle++;
            pre_pres=rocket_data.pres;
        }
    }

    myled2=1;
}



void para_open() //パラシュート展開
{
    /*
    fall_detectを抜けたら、パラシュート展開
    展開高度の制限が必要になったら、waitを挟む
    展開命令後LED3点灯
    */
    Para=1;
    wait_ms(time_between_para_opening_ms);
    Para=0;
    para_open_time=t.read_ms();
    myled3=1;
}

void bottle_close() //水密構造稼働
{
    /*
    パラシュート展開完了後、水密構造を起動
    水密構造稼働ごLED4点灯
    */
    rocket_data.servo_move();
   // wait_ms(1000);
    myled4=1;
    while(true){
        rocket_data.get_data();
        }
    
}

int main()
{   
    t.start();
    Setup();
    status=1;
    uint32_t msg=Launch_detect();
    status=2;
    fall_detect(msg);
    status=3;
    para_open();
    status=4;
    bottle_close();
    status=5;
    
    
    
    

}