File content as of revision 1:d9208e6f6952:

#include "mbed.h"
#include "math.h"
#include "EEPROM.h"
#include "LPS33HW.h"
#include "P7100.h"
#include "MPU9250.h"
#include "GMS6_CR6.h"


Timer t;                                        //時間計測
Serial pc(USBTX, USBRX);
LPS33HW lps(p9, p10);
EEPROM rom(p9, p10);
MPU9250 mpu(p9, p10);
P7100 p7100(p5, p6, p7, p8);
Serial twe(p28, p27);                           //EFM:p28, p27
GMS6_CR6 gps(p13, p14);                         //EFM:p13, p14

DigitalIn Flight_Pin(p30);
DigitalOut ThrustValve(p21);
DigitalOut DepressValve(p22);
DigitalOut LED_1(p23);
DigitalOut LED_2(p24);
DigitalOut LED_3(p25);
DigitalOut LED_4(p26);

void get_save_low();                            //自作関数プロトタイプ宣言
float get_save_high();                          //自作関数プロトタイプ宣言
void recovery();                                //自作関数プロトタイプ宣言
void check();

char s[64];                                 //データ取得配列
float accel[3], gyro[3];                    //MPU9250data
float AX, AY, AZ, A, GX, GY, GZ;               //MPU9250data
float T;                                     //時刻データ
float P;                                     //気圧データ取得
float V;                                     //電圧取得
float TP = 0.25 * V - 0.125;                //タンク圧力換算

int pointer_high = 0;                          //EEPROM保存アドレス(high)
int pointer_low = 0;                          //EEPROM保存アドレス(low)

float lat_north;                            //GPS緯度
float lon_east;                             //GPS経度
float GPS[2];                               //GPSデータ格納配列

//main
int main()
{
    pc.baud(460800);                           //ボーレート変更
    twe.baud(230400);                          //tweボーレート変更

    ThrustValve = 0;                            //DigitalOut初期化
    DepressValve  = 0;
    LED_1 = 0;
    LED_2 = 0;
    LED_3 = 0;
    LED_4 = 0;

    //判定用変数
    double groundP = 0;                           //地上の気圧データ
    double heightP = 0;                          //高度TBDmの気圧
    double judgeP[5];                           //判定用気圧データ
    double pastP = 0;                            //一つ前の気圧データ
    double dP[5];                                //気圧データの差異
    int judge = 0;                              //判定結果
    int i = 0;                                  //差異配列の番号
    memset(judgeP, 0, 5);
    memset(dP, 1, 5);

    int n_dvalve = 0;                            //脱圧バルブの開閉回数

    t.start();                                //timer Start

    //sensor Setup
    lps.start(1);                           //気圧センサスタート
    lps.start(1);                           //気圧センサスタート
    wait(0.1);
    mpu.start();                            //MPU9250 start&setup
    mpu.accelsetup(3);
    mpu.gyrosetup(3);
    mpu.AKsetup(1);
    wait(0.2);

    pc.printf("Power ON!\r\n");
    twe.printf("Power ON!\r\n");
    pc.printf("Flight Test!\r\n");
    twe.printf("Flight Test!\r\n");
    wait(2);

    //Check mode
    pc.printf("Check mode Start!\r\n");
    twe.printf("Check mode Start!\r\n");
    //valve check
    DepressValve  = 1;wait(1);DepressValve  = 0;
    wait(3);
    DepressValve  = 1;wait(1);DepressValve  = 0;
    wait(3);
    ThrustValve = 1;wait(1);ThrustValve = 0;
    wait(1);
    ThrustValve = 1;wait(1);ThrustValve = 0;
    wait(2);
    
    for(int c = 0; c < 80 ; c++) {
        LED_1 = 1;
        LED_2 = 1;
        LED_3 = 1;
        LED_4 = 1;
        check();
        LED_1 = 0;
        LED_2 = 0;
        LED_3 = 0;
        LED_4 = 0;
        wait(0.5);
    }



    //地上の気圧取得&計算
    pc.printf("Calibration Start!\r\n");
    twe.printf("Calibration Start!\r\n");
    for(int a = 0; a < 1000; a++) {
        groundP = groundP + lps.data_read();
        wait_ms(50);
    }
    groundP = groundP/1000;                   //地上の気圧データ
    heightP = 0.999517419 * groundP;             //高度4.0m@10度の気圧を計算
    wait(2);
    pc.printf("groundP:%f, heightP:%f\r\n", groundP, heightP);
    twe.printf("groundP:%f, heightP:%f\r\n", groundP, heightP);
    wait(2);
    twe.printf("groundP:%f, heightP:%f\r\n", groundP, heightP);

    //Wait mode
    pc.printf("Wait mode Start!\r\n");
    twe.printf("Wait mode Start!\r\n");
    LED_1 = 1;
    while(Flight_Pin == 0) {
        get_save_low();                         //data get&save(&downlink)
        wait_ms(950);
    }
    pc.printf("Wait mode Finish!\r\n");
    twe.printf("Wait mode Finish!\r\n");
    LED_1 = 0;

    //Flight mode
    pc.printf("Flight mode Start!\r\n");
    twe.printf("Flight mode Start!\r\n");
    LED_2 = 1;
    while(1) {
        judgeP[i] = 0;                                     //初期化

        for(int n = 0; n < 2; n++) {                    //2回に一回送信
            judgeP[i] = judgeP[i] + get_save_high();                    //data get&save
        }
        twe.printf("\r\n");                                //送信トリガー

        //判定
        judgeP[i] = judgeP[i]/2;                        //判定用気圧を計算
        dP[i] = judgeP[i] - pastP;                      //差異を取得
        pastP = judgeP[i];                              //判定用気圧値を過去データに置き換える
        i++;
        if(i == 5) {
            i = 0;
        }

        if(judgeP[0] > heightP && judgeP[1] > heightP && judgeP[2] > heightP && judgeP[3] > heightP && judgeP[4] > heightP) {         //高度判定
            judge = 1;
            break;
        }
        else if(dP[0] <= 0 && dP[1] <= 0 && dP[2] <= 0 && dP[3] <= 0 && dP[4] <= 0 ) {
            judge = 1;
            break;
        }
    }
    pc.printf("Flight mode Finish!\r\n");
    twe.printf("Flight mode Finish!\r\n");
    LED_2 = 0;



    //thruster mode
    if(judge == 1) {
        pc.printf("Thruster mode Start!\r\n");
        twe.printf("Thruster mode Start!\r\n");
        LED_3 = 1;
        ThrustValve = 1;                                                //スラスタ電磁弁開
        for(int k = 0; k < 250; k++) {
            for(int n = 0; n < 2; n++) {                                    //2回に一回送信
                get_save_high();                    //data get&save
            }
            twe.printf("\r\n");                                            //送信トリガー
        }
        ThrustValve = 0;                                                //スラスタ電磁弁閉
        pc.printf("Thruster mode Finish!\r\n");
        twe.printf("Thruster mode Finish!\r\n");
    }



    //depress mode
    pc.printf("Depress mode Start!\r\n");
    twe.printf("Depress mode Start!\r\n");
    LED_4 = 1;
    memset(judgeP, 0, 5);                                   //判定初期化
    memset(dP, 1, 5);
    pastP = 0;

    while(1) {
        DepressValve = 1;                                   //脱圧電磁弁開

        judgeP[i] = 0;                                      //初期化

        for(int k = 0; k < 150; k++) {
            for(int n = 0; n < 2; n++) {                    //2回に一回送信
                judgeP[i] = judgeP[i] + get_save_high();                    //data get&save
            }
            twe.printf("\r\n");                                //送信トリガー

            //判定
            judgeP[i] = judgeP[i]/2;                        //判定用気圧を計算
            dP[i] = judgeP[i] - pastP;                      //差異を取得
            pastP = judgeP[i];                              //判定用気圧値を過去データに置き換える

            if(dP[0] <= 0 && dP[1] <= 0 && dP[2] <= 0 && dP[3] <= 0 && dP[4] <= 0 && n_dvalve > 2 || n_dvalve == 4) {               //着地判定
                judge = 3;
                break;
            }
            i++;
            if(i == 5) {
                i = 0;
            }
        }

        DepressValve = 0;                                  //脱圧電磁弁閉
        wait_ms(100);
        n_dvalve++;

        if(judge == 3) {
            break;   //地上判定を通過したらループを抜ける
        }
    }
    pc.printf("Depress mode Finish!\r\n");
    twe.printf("Depress mode Finish!\r\n");
    LED_4 = 0;



    //recovery mode
    pc.printf("Recovery mode Start!\r\n");
    twe.printf("Recovery mode Start!\r\n");
    LED_1 = 1;
    LED_2 = 1;
    LED_4 = 1;
    while(1) {
        recovery();
    }
    pc.printf("Recovery mode Finish!\r\n");
    twe.printf("Recovery mode Finish!\r\n");
}


//自作関数
void get_save_low()
{
    T = t.read();                         //時刻データ取得
    P = lps.data_read();                  //気圧データ取得
    V = p7100.v_read();                   //電圧取得
    TP = 0.25 * V - 0.125;                //タンク圧力換算

    mpu.accel_read(3, accel);
    mpu.gyro_read(3, gyro);
    AX = accel[0];
    AY = accel[1];
    AZ = accel[2];
    A = sqrtf(AX*AX + AY*AY + AZ*AZ);
    GX = gyro[0];
    GY = gyro[1];
    GZ = gyro[2];

    sprintf(s, "%8.3f %8.3f %6.3f %5.3f %5.2f %5.2f %5.2f %5.2f %5.2f\r\n", T, P, TP, AX, AY, AZ, GX, GY, GZ);    //floatからchar*へ変換

    rom.write_low(pointer_low, s, 64);        // write tha data
    twe.printf("%8.3f %8.3f %6.3f %5.2f\r\n", T, P, TP, A);                        //無線機送信
    
    pointer_low = pointer_low + 64;          //アドレスずらし

    pc.printf("%s\r\n", s);         //check用

    memset(s, '\0', 64 );                          //初期化
}

float get_save_high()
{
    T = t.read();                         //時刻データ取得
    P = lps.data_read();                  //気圧データ取得
    V = p7100.v_read();                   //電圧取得
    TP = 0.25 * V - 0.125;                //タンク圧力換算

    mpu.accel_read(3, accel);
    mpu.gyro_read(3, gyro);
    AX = accel[0];
    AY = accel[1];
    AZ = accel[2];
    A = sqrtf(AX*AX + AY*AY + AZ*AZ);
    GX = gyro[0];
    GY = gyro[1];
    GZ = gyro[2];

    sprintf(s, "%8.3f %8.3f %6.3f %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f\r\n", T, P, TP, AX, AY, AZ, GX, GY, GZ);    //floatからchar*へ変換

    rom.write_high(pointer_high, s, 64);        // write tha data

    twe.printf("%8.3f %8.3f %6.3f %5.2f,", T, P, TP, A);                        //無線機送信

    pointer_high = pointer_high + 64;          //アドレスずらし

    pc.printf("%s\r\n", s);         //check用

    memset(s, '\0', 64 );                          //初期化

    return P;
}

void recovery()
{
    T = t.read();                         //時刻データ取得
    V = p7100.v_read();                   //電圧取得
    TP = 0.25 * V - 0.125;                //タンク圧力換算

    sprintf(s, "%8.3f %6.3f\r\n", T, TP);          //floatからchar*へ変換

    rom.write_low(pointer_low, s, 64);             // write tha data
    pointer_low = pointer_low + 64;                //アドレスずらし

    pc.printf("%s\r\n", s);                     //check用
    twe.printf("%8.3f %6.3f, ", T, TP);                        //無線機送信
    memset(s, '\0', 64 );                          //初期化

    gps.read_solid(GPS);                                                                //測位データ取得
    lat_north = GPS[0];
    lon_east = GPS[1];

    if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) {         //緯度が正常な値のときのみ送信
        twe.printf("%f,%f", lat_north, lon_east);                                 //送信
        pc.printf("%f,%f\r\n", lat_north, lon_east);                                  //check用
    }
    twe.printf("\r\n");                                             //無線機トリガー
    memset(GPS,'\0', 2);
}

void check()
{
    T = t.read();                         //時刻データ取得
    P = lps.data_read();                  //気圧データ取得
    V = p7100.v_read();                   //電圧取得
    TP = 0.25 * V - 0.125;                //タンク圧力換算

    mpu.accel_read(3, accel);
    mpu.gyro_read(3, gyro);
    AX = accel[0];
    AY = accel[1];
    AZ = accel[2];
    A = sqrtf(AX*AX + AY*AY + AZ*AZ);
    GX = gyro[0];
    GY = gyro[1];
    GZ = gyro[2];

    sprintf(s, "%8.3f %8.3f %6.3f %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", T, P, TP, AX, AY, AZ, A, GX, GY, GZ);    //floatからchar*へ変換
    twe.printf("%s\r\n", s);                //送信
    pc.printf("%s\r\n\r\n", s);                     //check用
    memset(s, '\0', 64 );                          //初期化

    gps.read_solid(GPS);                                                                //測位データ取得
    lat_north = GPS[0];
    lon_east = GPS[1];

    if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) {         //緯度が正常な値のときのみ送信
        twe.printf("%f,%f\r\n", lat_north, lon_east);                                 //送信
        pc.printf("%f,%f\r\n", lat_north, lon_east);                                  //check用
    }
    memset(GPS,'\0', 2);
}