File content as of revision 6:d951f75ce5e8:

 /*
 * ハイブリッドロケット（2021年3月伊豆大島共同打上実験）アビオニクスメインモジュール
 */

#include "mbed.h"
#include "SDFileSystem.h"

#include "PQ_ADXL375.h"
#include "PQ_GPS.h"
#include "PQ_EEPROM.h"
#include "PQ_ES920.h"
#include "PQ_INA226.h"
#include "PQ_LPS22HB.h"
#include "PQ_MPU9250.h"

const float BURN_TIME = 3.27f;  // エンジン燃焼時間[s]
const float T_APOGEE = 13.44f; // 離床検知から弾道頂点までの時間[s]
const float T_SEP = 12.0f;      // 分離にかける時間[s]
const float SAMPLING_FREQUENCY = 10.0f; // データのサンプリングレート[Hz]
const float DOWNLINK_RATE = 2.0f;       // ダウンリンクのレート[Hz]

Serial pc(USBTX, USBRX, 115200);
Serial gps_serial(p9, p10, 115200);
Serial es_serial(p13, p14, 115200);

I2C i2c(p28, p27);

ES920 es(es_serial);
GPS gps(gps_serial);

ADXL375 adxl(i2c, ADXL375::ALT_ADDRESS_HIGH); // 3軸高加速度
INA226 ina_in(i2c, INA226::A0_VS, INA226::A1_VS);   // 電圧，電流（バッテリー）
INA226 ina_ex(i2c, INA226::A0_GND, INA226::A1_GND); // 電圧，電流（外部電源）
//消す！！
LPS22HB lps(i2c, LPS22HB::SA0_LOW); // 気圧，温度
//LPS22HB lps(i2c, LPS22HB::SA0_HIGH); // 気圧，温度
MPU9250 mpu(i2c, MPU9250::AD0_HIGH); // 3軸加速度，3軸角速度，3軸地磁気

SDFileSystem sd(p5, p6, p7, p8, "sd");
EEPROM eeprom(i2c); // 24FC1025*4（0x000000~0x07FFFF）

Timer mission_timer; // 電源投入からの時間
Timer flight_timer;  // 離床検知からの時間
Timer sd_timer;      // SD用（fcloseのため）
Timer sep_timer;     // 分離時間

Ticker downlink_ticker; // ダウンリンク
Ticker record_ticker;   // データ保存

DigitalIn flight(p20); // 離床検知用フライトピン（外部プルアップ）
DigitalIn signal(p24); // ミッション基板の生存信号

DigitalOut mission(p18); // ミッション基板の動作制御
DigitalOut relay(p19);   // ミッション基板のリレースイッチング
DigitalOut sep(p21);     // 分離機構のニクロム線制御
DigitalOut buzzer(p23);  // 捜索用ブザー

void init(); // 初期化処理
void read(); // タイマー，GPS，センサの読み取り
void command_handler(char *command); // アップリンクコマンドの処理
void downlink(); // データのダウンリンク
void record(); // EEPROM，SDへの記録

char file_name[64];
FILE *fp;

bool launched = false;  // 打ちあがっているか：true=離床済み
bool burning = false;   // エンジンが燃焼中か：true=燃焼中
bool use_apogee = true; // 頂点検知を行うか：true=行う
bool apogee = false;    // 頂点検知したか：true=頂点検知済み
bool separated = false; // 分離したか：true=分離済み
bool landed = false;    // 着水したか：true=着水済み
float t = 0; // 頂点検知用

int addr; // EEPROMの書き込みアドレス

int mission_timer_reset; // mission_timerをリセットした回数
float mission_time; // ミッション開始からの時間
float flight_time; // 離床検知からの時間
// 運用フェーズ
enum {
    SAFETY, // 動作無し（READYへコマンドで移行）
    READY, // 離床検知待機（SAFETY，FLIGHTへコマンドで移行）
    FLIGHT, // 離床検知（フライトピン）から分離まで（コマンドでSEP，EMERGENCYへ移行）
    SEP, // パラシュート分離
    EMERGENCY, // 離床失敗した場合の緊急停止（パラシュート分離の禁止，コマンドによる）
    RECOVERY // 分離終了から回収まで
} phase;
// 動作フラグ：1=有効，0=無効
char f_sd; // SDカード
char f_gps; // GPS
char f_adxl; // ADXL375
char f_ina_in; // INA226（バッテリー）
char f_ina_ex; // INA226（外部電源）
char f_lps; // LPS22HB
char f_mpu; // MPU9250
float lat; // 緯度[deg]
float lon; // 経度[deg]
int sat; // 衛星数
int fix; // 位置特定品質
float hdop; // 水平精度低下率
float alt; // 海抜高度[m]
float geoid; // ジオイド[m]
float high_accel[3]; // 高加速度[G}
float voltage_in; // バッテリー電圧[mV]
float current_in; // バッテリー電流[mA]
float voltage_ex; // 外部電源電圧[mV]
float current_ex; // 外部電源電流[mA]
float press; // 気圧[hPa]
float temp; // 温度[C]
float accel[3]; // 加速度[G]
float gyro[3]; // 角速度[rad/s]

char sound = 1;

// 移動中央値
float press_buf[10];
int press_count;
float press_median;

// ローパスフィルタ
float coef = 0.01;
float press_prev_LPF;
float press_LPF;
float press_LPF_prev;
float press_LPF_diff;
float press_LPF_ground;

int main()
{
    wait(2); // ES920LRのブートローダー起動待ち
    init();

    while(1) {
        read();
        switch(phase) {
            case SAFETY:
                mission = 0;
                relay = 1;
                sep = 0;
                buzzer = 0;
                break;
            case READY:
                mission = 0;
                relay = 1;
                sep = 0;
                buzzer = 0;
                press_LPF_ground = press_LPF;
                if(flight.read() == 1) phase = FLIGHT;
                break;
            case FLIGHT:
                mission = 1;
                relay = 1;
                sep = 0;
                buzzer = 0;
                if(!launched) {
                    flight_timer.start();
                    launched = true;
                    burning = true;
                }
                if(flight_timer.read() > BURN_TIME) {
                    if(burning) {
                        burning = false;
                        press_LPF_prev = press_LPF;
                    }
                    if(flight_timer.read() - t > 0.25) {
                        if(press_LPF - press_LPF_prev > 0.0f) {
                            if(use_apogee){
                                apogee = true;
                            }
                        }
                        press_LPF_prev = press_LPF;
                        t = flight_timer.read();
                    }
                    if(!burning && (apogee || flight_timer.read() > T_APOGEE)) {
                        phase = SEP;
                    }
                }
                break;
            case SEP:
                mission = 0;
                relay = 1;
                sep = 1;
                buzzer = 1;
                if(!separated) {
                    sep_timer.start();
                    separated = true;
                }
                if(sep_timer.read() > T_SEP) {
                    phase = RECOVERY;
                }
                break;
            case EMERGENCY:
                mission = 1;
                relay = 1;
                sep = 0;
                buzzer = 0;
                break;
            case RECOVERY:
                mission = 0;
                if(!landed && press_LPF > press_LPF_ground) {
                        relay = 0;
                        landed = true;
                }
                sep = 0;
                buzzer = sound;
                break;
        }
    }
}

void init()
{
    // 連番のファイルを作成
    char file_name_format[] = "/sd/IZU2021_AVIONICS_%d.dat";
    int file_number = 1;
    while(1) {
        sprintf(file_name, file_name_format, file_number);
        fp = fopen(file_name, "r");
        if(fp != NULL) {
            fclose(fp);
            file_number++;
        } else {
            sprintf(file_name, file_name_format, file_number);
            break;
        }
    }
    fp = fopen(file_name, "w");

    sd_timer.start();

    if(fp) {
        fprintf(fp, "mission_time,flight_time,phase,");
        fprintf(fp, "use_apogee,apogee,landed,signal,mission,relay,sep,buzzer,");
        fprintf(fp, "flight,f_sd,f_gps,f_adxl,f_ina_in,f_ina_ex,f_lps,f_mpu,");
        fprintf(fp, "lat,lon,sat,fix,hdop,alt,geoid,");
        fprintf(fp, "high_accel_x,high_accel_y,high_accel_z,");
        fprintf(fp, "voltage_in,current_in,");
        fprintf(fp, "voltage_ex,current_ex,");
        fprintf(fp, "press,temp,");
        fprintf(fp, "accel_x,accel_y,accel_z,");
        fprintf(fp, "gyro_x,gyro_y,gyro_z,");
        fprintf(fp, "mag_x,mag_y,mag_z");
        fprintf(fp, "\r\n");
    }

    record_ticker.attach(record, 1.0f / SAMPLING_FREQUENCY);
    downlink_ticker.attach(downlink, 1.0f / DOWNLINK_RATE);

    // ミッション基板のリセット
    mission = 1;
    relay = 0;

    sep = 0;
    buzzer = 0;

    es.attach(command_handler);

    adxl.begin();
    ina_in.begin();
    ina_ex.begin();
    lps.begin();
    mpu.begin();

    mission_timer.start();
}

void read()
{
    if(mission_timer.read() >= 30*60) {
        mission_timer.reset();
        mission_timer_reset++;
    }
    mission_time = mission_timer.read() + mission_timer_reset*30*60;
    flight_time = flight_timer.read();

    lat   = gps.get_lat();
    lon   = gps.get_lon();
    sat   = gps.get_sat();
    fix   = gps.get_fix();
    hdop  = gps.get_hdop();
    alt   = gps.get_alt();
    geoid = gps.get_geoid();

    f_sd = (bool)fp;

    f_gps = (bool)fix;

    f_adxl = adxl.test();
    if(f_adxl) {
        adxl.read(high_accel);
    }

    f_ina_in = ina_in.test();
    if(f_ina_in) {
        ina_in.read_voltage(&voltage_in);
        ina_in.read_current(&current_in);
    }

    f_ina_ex = ina_ex.test();
    if(f_ina_ex) {
        ina_ex.read_voltage(&voltage_ex);
        ina_ex.read_current(&current_ex);
    }

    f_lps = lps.test();
    if(f_lps) {
        lps.read_press(&press);
        lps.read_temp(&temp);

        // 移動中央値を求める
        press_buf[press_count] = press;
        press_count++;
        if(press_count > 10) {
            press_count = 0;
        }

        // 配列のコピー
        float buf[10];
        for(int i = 0; i < 10; i++) {
            buf[i] = press_buf[i];
        }

        // バブルソート
        for(int i = 0; i < 9; i++) {
            for(int j = 9; j > i; j--) {
                if(buf[j] < buf[j - 1]) {
                    float temp = buf[j];
                    buf[j] = buf[j - 1];
                    buf[j - 1] = temp;
                }
            }
        }

        // 中央値
        press_median = (buf[4] + buf[5]) / 2;

        // ローパスフィルタ
        press_LPF = press_median * coef + press_prev_LPF * (1 - coef);
        press_prev_LPF = press_LPF;
    }

    f_mpu = mpu.test();
    if(f_mpu) {
        mpu.read_accel(accel);
        mpu.read_gyro(gyro);
    }
}

void command_handler(char *command)
{
    switch(command[0]) {
        case 0xB0: // '0'
            if(phase == READY) phase = SAFETY;
            break;
        case 0xB1: // '1'
            if(phase == SAFETY) phase = READY;
            break;
        case 0xB2: // '2'
            if(phase == READY) phase = FLIGHT;
            break;
        case 0xB3: // '3'
            if(!burning && phase == FLIGHT) phase = SEP;
            break;
        case 0xB4: // '4'
            if(phase == FLIGHT) phase = EMERGENCY;
            break;
        case 0xB5: // '5'
            if(phase == SEP) phase = RECOVERY;
            break;
        case 0xE1: // 'a'
            use_apogee = !use_apogee;
            break;
        case 0xE2: // 'b'
            sound = !sound;
            break;
        case 0xFF: // 'DEL'
            NVIC_SystemReset();
            break;
    }
}

void downlink()
{
    short mission_time_bits = (short)mission_time;;
    short flight_time_bits = (short)(flight_time * 10);
    char flags1 = 0;
    flags1 |= (char)use_apogee << 7;
    flags1 |= (char)apogee     << 6;
    flags1 |= (char)landed     << 5;
    flags1 |= signal.read()    << 4;
    flags1 |= mission.read()   << 3;
    flags1 |= relay.read()     << 2;
    flags1 |= sep.read()       << 1;
    flags1 |= buzzer.read()    << 0;
    char flags2 = 0;
    flags2 |= flight.read() << 7;
    flags2 |= f_sd          << 6;
    flags2 |= f_gps         << 5;
    flags2 |= f_adxl        << 4;
    flags2 |= f_ina_in      << 3;
    flags2 |= f_ina_ex      << 2;
    flags2 |= f_lps         << 1;
    flags2 |= f_mpu         << 0;
    int lat_bits = *((int*)&lat);
    int lon_bits = *((int*)&lon);
    int alt_bits = *((int*)&alt);
    short high_accel_bits[3];
    for(int i = 0; i < 3; i++) {
        high_accel_bits[i] = (short)(high_accel[i] / ADXL375_LSB);
    }
    short voltage_in_bits = (short)(voltage_in / INA226_VOLTAGE_LSB);
    short current_in_bits = (short)(current_in / INA226_CURRENT_LSB);
    short voltage_ex_bits = (short)(voltage_ex / INA226_VOLTAGE_LSB);
    short current_ex_bits = (short)(current_ex / INA226_CURRENT_LSB);
    int press_bits = (int)(press * LPS22HB_PRESS_LSB);
    short temp_bits = (short)(temp / LPS22HB_TEMP_LSB);
    short accel_bits[3];
    for(int i = 0; i < 3; i++) {
        accel_bits[i] = (short)(accel[i] / (MPU9250_ACCEL_LSB * 8));
    }
    short gyro_bits[3];
    for(int i = 0; i < 3; i++) {
        gyro_bits[i] = (short)(gyro[i] / (MPU9250_GYRO_LSB * 8));
    }

    char data[50];
    data[0] = ((char*)&mission_time_bits)[0];
    data[1] = ((char*)&mission_time_bits)[1];
    data[2] = ((char*)&flight_time_bits)[0];
    data[3] = ((char*)&flight_time_bits)[1];
    data[4] = phase;
    data[5] = flags1;
    data[6] = flags2;
    data[7] = ((char*)&lat_bits)[0];
    data[8] = ((char*)&lat_bits)[1];
    data[9] = ((char*)&lat_bits)[2];
    data[10] = ((char*)&lat_bits)[3];
    data[11] = ((char*)&lon_bits)[0];
    data[12] = ((char*)&lon_bits)[1];
    data[13] = ((char*)&lon_bits)[2];
    data[14] = ((char*)&lon_bits)[3];
    data[15] = ((char*)&alt_bits)[0];
    data[16] = ((char*)&alt_bits)[1];
    data[17] = ((char*)&alt_bits)[2];
    data[18] = ((char*)&alt_bits)[3];
    data[19] = ((char*)&high_accel_bits[0])[0];
    data[20] = ((char*)&high_accel_bits[0])[1];
    data[21] = ((char*)&high_accel_bits[1])[0];
    data[22] = ((char*)&high_accel_bits[1])[1];
    data[23] = ((char*)&high_accel_bits[2])[0];
    data[24] = ((char*)&high_accel_bits[2])[1];
    data[25] = ((char*)&voltage_in_bits)[0];
    data[26] = ((char*)&voltage_in_bits)[1];
    data[27] = ((char*)&current_in_bits)[0];
    data[28] = ((char*)&current_in_bits)[1];
    data[29] = ((char*)&voltage_ex_bits)[0];
    data[30] = ((char*)&voltage_ex_bits)[1];
    data[31] = ((char*)&current_ex_bits)[0];
    data[32] = ((char*)&current_ex_bits)[1];
    data[33] = ((char*)&press_bits)[0];
    data[34] = ((char*)&press_bits)[1];
    data[35] = ((char*)&press_bits)[2];
    data[36] = ((char*)&temp_bits)[0];
    data[37] = ((char*)&temp_bits)[1];
    data[38] = ((char*)&accel_bits[0])[0];
    data[39] = ((char*)&accel_bits[0])[1];
    data[40] = ((char*)&accel_bits[1])[0];
    data[41] = ((char*)&accel_bits[1])[1];
    data[42] = ((char*)&accel_bits[2])[0];
    data[43] = ((char*)&accel_bits[2])[1];
    data[44] = ((char*)&gyro_bits[0])[0];
    data[45] = ((char*)&gyro_bits[0])[1];
    data[46] = ((char*)&gyro_bits[1])[0];
    data[47] = ((char*)&gyro_bits[1])[1];
    data[48] = ((char*)&gyro_bits[2])[0];
    data[49] = ((char*)&gyro_bits[2])[1];

    es.send(data, 50);
}

void record()
{
    if(phase >= FLIGHT && !landed) {
        char data[128];
        data[0] = ((char*)&mission_time)[0];
        data[1] = ((char*)&mission_time)[1];
        data[2] = ((char*)&mission_time)[2];
        data[3] = ((char*)&mission_time)[3];
        data[4] = ((char*)&flight_time)[0];
        data[5] = ((char*)&flight_time)[1];
        data[6] = ((char*)&flight_time)[2];
        data[7] = ((char*)&flight_time)[3];
        data[8] = phase;
        data[9] = use_apogee;
        data[10] = apogee;
        data[11] = landed;
        data[12] = signal.read();
        data[13] = mission.read();
        data[14] = relay.read();
        data[15] = sep.read();
        data[16] = buzzer.read();
        data[17] = flight.read();
        data[18] = f_sd;
        data[19] = f_gps;
        data[20] = f_adxl;
        data[21] = f_ina_in;
        data[22] = f_ina_ex;
        data[23] = f_lps;
        data[24] = f_mpu;
        data[25] = ((char*)&lat)[0];
        data[26] = ((char*)&lat)[1];
        data[27] = ((char*)&lat)[2];
        data[28] = ((char*)&lat)[3];
        data[29] = ((char*)&lon)[0];
        data[30] = ((char*)&lon)[1];
        data[31] = ((char*)&lon)[2];
        data[32] = ((char*)&lon)[3];
        data[33] = ((char*)&sat)[0];
        data[34] = ((char*)&sat)[1];
        data[35] = ((char*)&sat)[2];
        data[36] = ((char*)&sat)[3];
        data[37] = ((char*)&fix)[0];
        data[38] = ((char*)&fix)[1];
        data[39] = ((char*)&fix)[2];
        data[40] = ((char*)&fix)[3];
        data[41] = ((char*)&hdop)[0];
        data[42] = ((char*)&hdop)[1];
        data[43] = ((char*)&hdop)[2];
        data[44] = ((char*)&hdop)[3];
        data[45] = ((char*)&alt)[0];
        data[46] = ((char*)&alt)[1];
        data[47] = ((char*)&alt)[2];
        data[48] = ((char*)&alt)[3];
        data[49] = ((char*)&geoid)[0];
        data[50] = ((char*)&geoid)[1];
        data[51] = ((char*)&geoid)[2];
        data[52] = ((char*)&geoid)[3];
        data[53] = ((char*)&high_accel[0])[0];
        data[54] = ((char*)&high_accel[0])[1];
        data[55] = ((char*)&high_accel[0])[2];
        data[56] = ((char*)&high_accel[0])[3];
        data[57] = ((char*)&high_accel[1])[0];
        data[58] = ((char*)&high_accel[1])[1];
        data[59] = ((char*)&high_accel[1])[2];
        data[60] = ((char*)&high_accel[1])[3];
        data[61] = ((char*)&high_accel[2])[0];
        data[62] = ((char*)&high_accel[2])[1];
        data[63] = ((char*)&high_accel[2])[2];
        data[64] = ((char*)&high_accel[2])[3];
        data[65] = ((char*)&voltage_in)[0];
        data[66] = ((char*)&voltage_in)[1];
        data[67] = ((char*)&voltage_in)[2];
        data[68] = ((char*)&voltage_in)[3];
        data[69] = ((char*)&current_in)[0];
        data[70] = ((char*)&current_in)[1];
        data[71] = ((char*)&current_in)[2];
        data[72] = ((char*)&current_in)[3];
        data[73] = ((char*)&voltage_ex)[0];
        data[74] = ((char*)&voltage_ex)[1];
        data[75] = ((char*)&voltage_ex)[2];
        data[76] = ((char*)&voltage_ex)[3];
        data[77] = ((char*)&current_ex)[0];
        data[78] = ((char*)&current_ex)[1];
        data[79] = ((char*)&current_ex)[2];
        data[80] = ((char*)&current_ex)[3];
        data[81] = ((char*)&press)[0];
        data[82] = ((char*)&press)[1];
        data[83] = ((char*)&press)[2];
        data[84] = ((char*)&press)[3];
        data[85] = ((char*)&temp)[0];
        data[86] = ((char*)&temp)[1];
        data[87] = ((char*)&temp)[2];
        data[88] = ((char*)&temp)[3];
        data[89] = ((char*)&accel[0])[0];
        data[90] = ((char*)&accel[0])[1];
        data[91] = ((char*)&accel[0])[2];
        data[92] = ((char*)&accel[0])[3];
        data[93] = ((char*)&accel[1])[0];
        data[94] = ((char*)&accel[1])[1];
        data[95] = ((char*)&accel[1])[2];
        data[96] = ((char*)&accel[1])[3];
        data[97] = ((char*)&accel[2])[0];
        data[98] = ((char*)&accel[2])[1];
        data[99] = ((char*)&accel[2])[2];
        data[100] = ((char*)&accel[2])[3];
        data[101] = ((char*)&gyro[0])[0];
        data[102] = ((char*)&gyro[0])[1];
        data[103] = ((char*)&gyro[0])[2];
        data[104] = ((char*)&gyro[0])[3];
        data[105] = ((char*)&gyro[1])[0];
        data[106] = ((char*)&gyro[1])[1];
        data[107] = ((char*)&gyro[1])[2];
        data[108] = ((char*)&gyro[1])[3];
        data[109] = ((char*)&gyro[2])[0];
        data[110] = ((char*)&gyro[2])[1];
        data[111] = ((char*)&gyro[2])[2];
        data[112] = ((char*)&gyro[2])[3];
        data[113] = 0;
        data[114] = 0;
        data[115] = 0;
        data[116] = 0;
        data[117] = 0;
        data[118] = 0;
        data[119] = 0;
        data[120] = 0;
        data[121] = 0;
        data[122] = 0;
        data[123] = 0;
        data[124] = 0;
        data[125] = 0;
        data[126] = 0;
        data[127] = 0;

        eeprom.write(addr, data, 128);
        addr += 0x80;
    }

    if(fp) {
        char *phase_names[] =  {"SAFETY", "READY", "FLIGHT", "SEP", "EMERGENCY", "RECOVERY"};
        fprintf(fp, "%.3f,%.3f,%s,", mission_time, flight_time, phase_names[phase]);
        fprintf(fp, "%d,%d,%d,%d,%d,%d,%d,%d,", use_apogee, apogee, landed, signal.read(), mission.read(), relay.read(), sep.read(), buzzer.read());
        fprintf(fp, "%d,%d,%d,%d,%d,%d,%d,%d,", flight.read(), f_sd, f_gps, f_adxl, f_ina_in, f_ina_ex, f_lps, f_mpu);
        fprintf(fp, "%.6f,%.6f,%d,%d,%f,%f,%f,", lat, lon, sat, fix, hdop, alt, geoid);
        fprintf(fp, "%f,%f,%f,", high_accel[0], high_accel[1], high_accel[2]);
        fprintf(fp, "%f,%f,", voltage_in, current_in);
        fprintf(fp, "%f,%f,", voltage_ex, current_ex);
        fprintf(fp, "%f,%f,", press, temp);
        fprintf(fp, "%f,%f,%f,", accel[0], accel[1], accel[2]);
        fprintf(fp, "%f,%f,%f", gyro[0], gyro[1], gyro[2]);
        fprintf(fp, "\r\n");
    }

    if(sd_timer.read() >= 20) {
        sd_timer.reset();
        if(fp) {
            fclose(fp);
            fp = fopen(file_name, "a");
        }
    }
}