File content as of revision 2:104839501493:

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

#include "PQADXL375.h"
#include "PQAEGPS.h"
#include "PQEEPROM.h"
#include "PQES920LR.h"
#include "PQINA226.h"
#include "PQLPS22HB.h"
#include "PQMPU9250.h"

#define BURN_TIME  3.0f
#define T_SEP      7.0f
#define T_RECOVERY 14.0f

#define DEBUG_RATE    1
#define DATA_RATE     20
#define LOG_RATE      1
#define DOWNLINK_RATE 1

#define LOG_HEADER      0x11
#define DOWNLINK_HEADER 0x12
#define UPLINK_HEADER   0x13

#define TIME_LSB    54.931640625
#define VOLTAGE_LSB 1.25
#define CURRENT_LSB 1.0986328125
#define PRESS_LSB   0.0384521484375
#define TEMP_LSB    0.002593994140625
#define ACCEL_LSB   0.00048828125
#define GYRO_LSB    0.06103515625
#define MAG_LSB     0.146484375

Serial pc(USBTX, USBRX, 115200);

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

I2C i2c(p28, p27);

SDFileSystem sd_file_system(p5, p6, p7, p8, "sd");

ES920LR es(es_serial);

AEGPS gps(gps_serial);

ADXL375 adxl(i2c, ADXL375::ALT_ADDRESS_HIGH);
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_HIGH);
MPU9250 mpu(i2c, MPU9250::AD0_HIGH);

EEPROM eeprom(i2c);

Timer mission_timer;
Timer flight_timer;
Timer sd_timer;

Ticker debug_ticker;
Ticker log_ticker;
Ticker downlink_ticker;

DigitalIn liftoff_pin(p18);
DigitalIn ems_pin(p19);
DigitalIn flight_pin(p26);

DigitalOut sep(p25);
DigitalOut buzzer(p22);

BusOut led(LED1, LED2, LED3, LED4);

void init();
void read();
void command_handler(char *command);
void downlink();
void log();
void debug();

FILE *sd;

char file_name[64];

bool launched = false;
bool burning = false;
bool apogee = false;
int t = 0;

int addr;

char mission_timer_reset;
int mission_time;
int flight_time;
char f_sd;
char f_gps;
char f_adxl;
char f_ina_in;
char f_ina_ex;
char f_lps;
char f_mpu;
char f_mpu_ak;
enum {
    SAFETY,
    READY,
    FLIGHT,
    SEP,
    EMERGENCY,
    RECOVERY
} phase;
int utc_hour;
int utc_min;
float utc_sec;
float lat;
float lon;
int sat;
int fix;
float hdop;
float alt;
float geoid;
float high_accel[3];
float voltage_in;
float current_in;
float voltage_ex;
float current_ex;
float press;
float temp;
float accel[3];
float gyro[3];
float mag[3];

float coef = 0.01;
float press_prev_LPF;
float press_LPF;
float press_LPF_prev;
float press_LPF_diff;

short mission_time_bits;
short flight_time_bits;
char flags;
short voltage_in_bits;
short current_in_bits;
short voltage_ex_bits;
short current_ex_bits;
short press_bits;
short temp_bits;
short accel_bits[3];
short gyro_bits[3];
short mag_bits[3];

int main()
{
    mission_timer.start();
    init();

    while(1) {
        read();
        led = phase;
        switch(phase) {
            case SAFETY:
                break;
            case READY:
                if(flight_pin || liftoff_pin) phase = FLIGHT;
                break;
            case FLIGHT:
                if(!launched) {
                    flight_timer.reset();
                    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_ms() - t) > 500) {
                        press_LPF_diff = press_LPF - press_LPF_prev;
                        press_LPF_prev = press_LPF;
                        if(press_LPF_diff > 0.0f) {
                            apogee = true;
                        }
                    } else {
                        t = flight_timer.read_ms();
                    }
                    if(!burning && (apogee || flight_timer.read() > T_SEP)) {
                        phase = SEP;
                    }
                }
                break;
            case SEP:
                buzzer = 1;
                sep = 1;
                if(flight_timer.read() > T_RECOVERY) phase = RECOVERY;
                break;
            case EMERGENCY:
                buzzer = 0;
                sep = 0;
                break;
            case RECOVERY:
                buzzer = 1;
                sep = 0;
                break;
        }
    }
}

void init()
{
    char file_name_format[] = "/sd/IZU2020_AVIONICS_%d.dat";
    int file_number = 1;
    while(1) {
        sprintf(file_name, file_name_format, file_number);
        sd = fopen(file_name, "r");
        if(sd != NULL) {
            fclose(sd);
            file_number++;
        } else {
            sprintf(file_name, "/sd/IZU2020_AVIONICS_%d.dat", file_number);
            break;
        }
    }
    sd = fopen(file_name, "w");
    sd_timer.start();

    if(sd) {
        fprintf(sd, "mission_timer_reset,");
        fprintf(sd, "mission_time,");
        fprintf(sd, "flight_time,");
        fprintf(sd, "phase,");
        fprintf(sd, "f_sd,");
        fprintf(sd, "f_gps,");
        fprintf(sd, "f_adxl,");
        fprintf(sd, "f_ina_in,");
        fprintf(sd, "f_ina_ex,");
        fprintf(sd, "f_lps,");
        fprintf(sd, "f_mpu,");
        fprintf(sd, "f_mpu_ak,");
        fprintf(sd, "lat,");
        fprintf(sd, "lon,");
        fprintf(sd, "sat,");
        fprintf(sd, "fix,");
        fprintf(sd, "hdop,");
        fprintf(sd, "alt,");
        fprintf(sd, "geoid,");
        fprintf(sd, "high_accel_x,");
        fprintf(sd, "high_accel_y,");
        fprintf(sd, "high_accel_z,");
        fprintf(sd, "voltage_in,");
        fprintf(sd, "current_in,");
        fprintf(sd, "voltage_ex,");
        fprintf(sd, "current_ex,");
        fprintf(sd, "press,");
        fprintf(sd, "temp,");
        fprintf(sd, "accel_x,");
        fprintf(sd, "accel_y,");
        fprintf(sd, "accel_z,");
        fprintf(sd, "gyro_x,");
        fprintf(sd, "gyro_y,");
        fprintf(sd, "gyro_z,");
        fprintf(sd, "mag_x,");
        fprintf(sd, "mag_y,");
        fprintf(sd, "mag_z,");
        fprintf(sd, "\r\n");
    }

    liftoff_pin.mode(PullDown);
    ems_pin.mode(PullDown);
    flight_pin.mode(PullUp);

    debug_ticker.attach(debug, 1.0f / DEBUG_RATE);
    log_ticker.attach(log, 1.0f / LOG_RATE);
    downlink_ticker.attach(downlink, 1.0f / DOWNLINK_RATE);

    es.attach(command_handler);

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

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

    utc_hour  = gps.get_hour();
    utc_min   = gps.get_min();
    utc_sec   = gps.get_sec();
    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)sd;

    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_LPF = press * 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);
    }

    f_mpu_ak = mpu.test_AK8963();
    if(f_mpu_ak) {
        mpu.read_mag(mag);
    }
}

void command_handler(char *command)
{
    switch(command[0]) {
        case 0x80:
            break;
        case 0x81:
            break;
        case 0x82:
            break;
        case 0x83:
            break;
        case 0x84:
            break;
        case 0x85:
            break;
        case 0x86:
            break;
        case 0x87:
            break;
        case 0x88:
            break;
        case 0x89:
            break;
        case 0x8A:
            break;
        case 0x8B:
            break;
        case 0x8C:
            break;
        case 0x8D:
            break;
        case 0x8E:
            break;
        case 0x8F:
            break;
        case 0x90:
            break;
        case 0x91:
            break;
        case 0x92:
            break;
        case 0x93:
            break;
        case 0x94:
            break;
        case 0x95:
            break;
        case 0x96:
            break;
        case 0x97:
            break;
        case 0x98:
            break;
        case 0x99:
            break;
        case 0x9A:
            break;
        case 0x9B:
            break;
        case 0x9C:
            break;
        case 0x9D:
            break;
        case 0x9E:
            break;
        case 0x9F:
            break;
        case 0xA0:
            break;
        case 0xA1:
            break;
        case 0xA2:
            break;
        case 0xA3:
            break;
        case 0xA4:
            break;
        case 0xA5:
            break;
        case 0xA6:
            break;
        case 0xA7:
            break;
        case 0xA8:
            break;
        case 0xA9:
            break;
        case 0xAA:
            break;
        case 0xAB:
            break;
        case 0xAC:
            break;
        case 0xAD:
            break;
        case 0xAE:
            break;
        case 0xAF:
            break;
        case 0xB0:
            if(phase == READY) phase = SAFETY;
            break;
        case 0xB1:
            if(phase == SAFETY) phase = READY;
            break;
        case 0xB2:
            if(phase == READY) phase = FLIGHT;
            break;
        case 0xB3:
            if(!burning & phase == FLIGHT) phase = SEP;
            break;
        case 0xB4:
            if(phase >= FLIGHT & phase <= SEP) phase = EMERGENCY;
            break;
        case 0xB5:
            if(phase == SEP) phase = RECOVERY;
            break;
        case 0xB6:
            break;
        case 0xB7:
            break;
        case 0xB8:
            break;
        case 0xB9:
            break;
        case 0xBA:
            break;
        case 0xBB:
            break;
        case 0xBC:
            break;
        case 0xBD:
            break;
        case 0xBE:
            break;
        case 0xBF:
            break;
        case 0xC0:
            break;
        case 0xC1:
            break;
        case 0xC2:
            break;
        case 0xC3:
            break;
        case 0xC4:
            break;
        case 0xC5:
            break;
        case 0xC6:
            break;
        case 0xC7:
            break;
        case 0xC8:
            break;
        case 0xC9:
            break;
        case 0xCA:
            break;
        case 0xCB:
            break;
        case 0xCC:
            break;
        case 0xCD:
            break;
        case 0xCE:
            break;
        case 0xCF:
            break;
        case 0xD0:
            break;
        case 0xD1:
            break;
        case 0xD2:
            break;
        case 0xD3:
            break;
        case 0xD4:
            break;
        case 0xD5:
            break;
        case 0xD6:
            break;
        case 0xD7:
            break;
        case 0xD8:
            break;
        case 0xD9:
            break;
        case 0xDA:
            break;
        case 0xDB:
            break;
        case 0xDC:
            break;
        case 0xDD:
            break;
        case 0xDE:
            break;
        case 0xDF:
            break;
        case 0xE0:
            break;
        case 0xE1:
            adxl.begin();
            break;
        case 0xE2:
            break;
        case 0xE3:
            break;
        case 0xE4:
            break;
        case 0xE5:
            ina_ex.begin();
            break;
        case 0xE6:
            break;
        case 0xE7:
            break;
        case 0xE8:
            break;
        case 0xE9:
            ina_in.begin();
            break;
        case 0xEA:
            break;
        case 0xEB:
            break;
        case 0xEC:
            lps.begin();
            break;
        case 0xED:
            mpu.begin();
            break;
        case 0xEE:
            break;
        case 0xEF:
            break;
        case 0xF0:
            break;
        case 0xF1:
            break;
        case 0xF2:
            break;
        case 0xF3:
            break;
        case 0xF4:
            break;
        case 0xF5:
            break;
        case 0xF6:
            break;
        case 0xF7:
            break;
        case 0xF8:
            break;
        case 0xF9:
            break;
        case 0xFA:
            break;
        case 0xFB:
            break;
        case 0xFC:
            break;
        case 0xFD:
            break;
        case 0xFE:
            break;
        case 0xFF:
            NVIC_SystemReset();
            break;
    }
}

void downlink()
{
    mission_time_bits = (short)(mission_time / TIME_LSB);
    flight_time_bits = (short)(flight_time / TIME_LSB);
    flags |= f_sd      << 7;
    flags |= f_gps     << 6;
    flags |= f_adxl    << 5;
    flags |= f_ina_in  << 4;
    flags |= f_ina_ex  << 3;
    flags |= f_lps     << 2;
    flags |= f_mpu     << 1;
    flags |= f_mpu_ak;
    voltage_in_bits = (short)(voltage_in / VOLTAGE_LSB);
    current_in_bits = (short)(current_in / CURRENT_LSB);
    voltage_ex_bits = (short)(voltage_ex / VOLTAGE_LSB);
    current_ex_bits = (short)(current_ex / CURRENT_LSB);
    press_bits = (short)(press / PRESS_LSB);
    temp_bits = (short)(temp / TEMP_LSB);
    for(int i = 0; i < 3; i++) {
        accel_bits[i] = (short)(accel[i] / ACCEL_LSB);
    }
    for(int i = 0; i < 3; i++) {
        gyro_bits[i] = (short)(gyro[i] / GYRO_LSB);
    }
    for(int i = 0; i < 3; i++) {
        mag_bits[i] = (short)(mag[i] / MAG_LSB);
    }

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

    es.send(data, 50);
}

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

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

    if(sd) {
        fprintf(sd, "%d,", mission_timer_reset);
        fprintf(sd, "%d,", mission_time);
        fprintf(sd, "%d,", flight_time);
        fprintf(sd, "%d,", phase);
        fprintf(sd, "%d,", f_sd);
        fprintf(sd, "%d,", f_gps);
        fprintf(sd, "%d,", f_adxl);
        fprintf(sd, "%d,", f_ina_in);
        fprintf(sd, "%d,", f_ina_ex);
        fprintf(sd, "%d,", f_lps);
        fprintf(sd, "%d,", f_mpu);
        fprintf(sd, "%d,", f_mpu_ak);
        fprintf(sd, "%f,", lat);
        fprintf(sd, "%f,", lon);
        fprintf(sd, "%d,", sat);
        fprintf(sd, "%d,", fix);
        fprintf(sd, "%f,", hdop);
        fprintf(sd, "%f,", alt);
        fprintf(sd, "%f,", geoid);
        fprintf(sd, "%f,", high_accel[0]);
        fprintf(sd, "%f,", high_accel[1]);
        fprintf(sd, "%f,", high_accel[2]);
        fprintf(sd, "%f,", voltage_in);
        fprintf(sd, "%f,", current_in);
        fprintf(sd, "%f,", voltage_ex);
        fprintf(sd, "%f,", current_ex);
        fprintf(sd, "%f,", press);
        fprintf(sd, "%f,", temp);
        fprintf(sd, "%f,", accel[0]);
        fprintf(sd, "%f,", accel[1]);
        fprintf(sd, "%f,", accel[2]);
        fprintf(sd, "%f,", gyro[0]);
        fprintf(sd, "%f,", gyro[1]);
        fprintf(sd, "%f,", gyro[2]);
        fprintf(sd, "%f,", mag[0]);
        fprintf(sd, "%f,", mag[1]);
        fprintf(sd, "%f,", mag[2]);
        fprintf(sd, "\r\n");
    }

    if(sd_timer.read_ms() >= 60*1000) {
        sd_timer.reset();
        sd_timer.start();
        if(sd) {
            fclose(sd);
            sd = fopen(file_name, "a");
        }
    }
}

void debug()
{
    pc.printf("mission_timer_reset: %d\r\n", mission_timer_reset);
    pc.printf("mission_time: %d\r\n", mission_time);
    pc.printf("flight_time: %d\r\n", flight_time);
    pc.printf("phase: %d\r\n", phase);
    pc.printf("f_sd: %d\r\n", f_sd);
    pc.printf("f_gps: %d\r\n", f_gps);
    pc.printf("f_adxl: %d\r\n", f_adxl);
    pc.printf("f_ina_in: %d\r\n", f_ina_in);
    pc.printf("f_ina_ex: %d\r\n", f_ina_ex);
    pc.printf("f_lps: %d\r\n", f_lps);
    pc.printf("f_mpu: %d\r\n", f_mpu);
    pc.printf("f_mpu_ak: %d\r\n", f_mpu_ak);
    pc.printf("lat: %f\r\n", lat);
    pc.printf("lon: %f\r\n", lon);
    pc.printf("sat: %d\r\n", sat);
    pc.printf("fix: %d\r\n", fix);
    pc.printf("hdop: %f\r\n", hdop);
    pc.printf("alt: %f\r\n", alt);
    pc.printf("geoid: %f\r\n", geoid);
    pc.printf("high_accel_x: %f\r\n", high_accel[0]);
    pc.printf("high_accel_y: %f\r\n", high_accel[1]);
    pc.printf("high_accel_z: %f\r\n", high_accel[2]);
    pc.printf("voltage_in: %f\r\n", voltage_in);
    pc.printf("current_in: %f\r\n", current_in);
    pc.printf("voltage_ex: %f\r\n", voltage_ex);
    pc.printf("current_ex: %f\r\n", current_ex);
    pc.printf("press: %f\r\n", press);
    pc.printf("temp: %f\r\n", temp);
    pc.printf("accel_x: %f\r\n", accel[0]);
    pc.printf("accel_y: %f\r\n", accel[1]);
    pc.printf("accel_z: %f\r\n", accel[2]);
    pc.printf("gyro_x: %f\r\n", gyro[0]);
    pc.printf("gyro_y: %f\r\n", gyro[1]);
    pc.printf("gyro_z: %f\r\n", gyro[2]);
    pc.printf("mag_x: %f\r\n", mag[0]);
    pc.printf("mag_y: %f\r\n", mag[1]);
    pc.printf("mag_z: %f\r\n", mag[2]);
}