File content as of revision 0:106e97338223:

#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 DEBUG 1

#define TIME_SEP1 7.0f
#define TIME_SEP2 5.0f
#define TIME_RECOVERY 30.0f
#define ALT_SEP1 500.0f

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

I2C i2c(p28, p27);

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

AEGPS gps(gps_serial);
ES920LR es(es_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;

Timeout sep1_timeout;
Timeout sep2_timeout;
Timeout recovery_timeout;

Ticker log_ticker;
Ticker read_ticker;

DigitalIn flight_pin(p24);

DigitalOut sep1(p21);
DigitalOut sep2(p22);
DigitalOut buzzer(p23);

AnalogIn pitot(p20);

void init();
void read();
void log();
void separate1();
void separate2();
void recovery();
void downlink();
void command_handler(char *command);

int addr;

FILE *fp;

typedef enum {
    INIT,
    SAFETY,
    WAIT,
    FLIGHT,
    SEP1,
    SEP2,
    RECOVERY
} Phase_t;

struct {
    int mission_timer_reset;
    int mission_time;
    int flight_timer_reset;
    int flight_time;
    Phase_t phase;
    char f_sd;
    char f_adxl;
    char f_ina_in;
    char f_ina_ex;
    char f_lps;
    char f_mpu;
    char f_mpu_ak;
    int hour;
    int min;
    float sec;
    float lat;
    float lon;
    int sat;
    int fix;
    float hdop;
    float alt;
    float geoid;
    float high_accel[3];
    float ina_in_v;
    float ina_in_i;
    float ina_ex_v;
    float ina_ex_i;
    float press;
    float temp;
    float accel[3];
    float gyro[3];
    float mag[3];
} rocket;

int main()
{
    flight_pin.mode(PullUp);
    mission_timer.start();
    sd_timer.start();
    log_ticker.attach(&log, 1.0f);

    es.attach(command_handler);

    while(1) {
        read();
        if(rocket.phase == INIT) {
            init();
            rocket.phase = SAFETY;
        } else if(rocket.phase == SAFETY) {

        } else if(rocket.phase == WAIT) {
            if(flight_pin) {
                sep1_timeout.attach(&separate1, TIME_SEP1);
                recovery_timeout.attach(&recovery, TIME_RECOVERY);
                rocket.phase = FLIGHT;
            }
        } else if(rocket.phase == FLIGHT) {
            flight_timer.start();
            if(rocket.alt > SEP1) {
                sep2_timeout.attach(&separate2, TIME_SEP2);
                rocket.phase = SEP1;
            }
        } else if(rocket.phase == SEP1) {
            if(false/*rocket.alt < ALT_SEP1*/) {
                rocket.phase = RECOVERY;
            }
        } else if(rocket.phase == SEP2) {
        } else if(rocket.phase == RECOVERY) {
        }
        if(mission_timer.read_ms() >= 30*60*1000) {
            mission_timer.reset();
            rocket.mission_timer_reset++;
        }
    }
}

void separate1()
{
    rocket.phase = SEP1;
    sep1_timeout.detach();
}

void separate2()
{
    rocket.phase = SEP2;
    sep2_timeout.detach();
}

void recovery()
{
    rocket.phase = RECOVERY;
    recovery_timeout.detach();
}

void init()
{
    fp = fopen("/sd/IZU2020_AVIONICS.txt", "a");

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

void read()
{
    rocket.f_sd     = (char)(bool)fp;
    rocket.f_adxl   = (char)adxl.test();
    rocket.f_ina_in = (char)ina_in.test();
    rocket.f_ina_ex = (char)ina_ex.test();
    rocket.f_lps    = (char)lps.test();
    rocket.f_mpu    = (char)mpu.test();
    rocket.f_mpu_ak = (char)mpu.test_AK8963();

    rocket.mission_time = mission_timer.read_ms();
    rocket.flight_time = flight_timer.read_ms();

    rocket.hour  = gps.get_hour();
    rocket.min   = gps.get_min();
    rocket.sec   = gps.get_sec();
    rocket.lat   = gps.get_lat();
    rocket.lon   = gps.get_lon();
    rocket.sat   = gps.get_sat();
    rocket.fix   = gps.get_fix();
    rocket.hdop  = gps.get_hdop();
    rocket.alt   = gps.get_alt();
    rocket.geoid = gps.get_geoid();

    adxl.read(rocket.high_accel);

    ina_in.read_voltage(&rocket.ina_in_v);
    ina_in.read_current(&rocket.ina_in_i);
    ina_ex.read_voltage(&rocket.ina_ex_v);
    ina_ex.read_current(&rocket.ina_ex_i);

    lps.read_press(&rocket.press);
    lps.read_temp(&rocket.temp);

    mpu.read_accel(rocket.accel);
    mpu.read_gyro(rocket.gyro);
    mpu.read_mag(rocket.mag);
}

void command_handler(char *command)
{
    pc.printf("received:%x\r\n", command[0]);
    switch(command[0]) {
        case 0x01:
            if(rocket.phase == WAIT) {
                rocket.phase = SAFETY;
            }
            break;

        case 0x02:
            if(rocket.phase == SAFETY) {
                rocket.phase = WAIT;
            }
            break;

        case 0x03:
            if(rocket.phase == WAIT) {
                rocket.phase = FLIGHT;
            }
            break;

        case 0x04:
            if(rocket.phase == FLIGHT) {
                rocket.phase = SEP1;
            }
            break;

        case 0x05:
            if(rocket.phase == SEP1) {
                rocket.phase = SEP2;
            }
            break;

        case 0x06:
            if(rocket.phase == SEP2) {
                rocket.phase = RECOVERY;
            }
            break;

        default:
            break;
    }
}

void downlink()
{
    //es.send(avio.byte, 128);
}

void log()
{
    char data[128];
    memset(data, 0x00, 128);
    memcpy(&data[0],          &rocket.phase,               1);
    memcpy(&data[1],   (char*)&rocket.mission_timer_reset, 2);
    memcpy(&data[3],   (char*)&rocket.mission_time,        4);
    memcpy(&data[7],   (char*)&rocket.flight_timer_reset,  2);
    memcpy(&data[9],   (char*)&rocket.flight_time,         4);
    memcpy(&data[13],  (char*)&rocket.lat,                 4);
    memcpy(&data[17],  (char*)&rocket.lon,                 4);
    memcpy(&data[21],  (char*)&rocket.sat,                 4);
    memcpy(&data[25],  (char*)&rocket.fix,                 4);
    memcpy(&data[29],  (char*)&rocket.hdop,                4);
    memcpy(&data[33],  (char*)&rocket.alt,                 4);
    memcpy(&data[37],  (char*)&rocket.geoid,               4);
    memcpy(&data[41],  (char*)&rocket.high_accel[0],       4);
    memcpy(&data[45],  (char*)&rocket.high_accel[1],       4);
    memcpy(&data[49],  (char*)&rocket.high_accel[2],       4);
    memcpy(&data[53],  (char*)&rocket.ina_in_v,            4);
    memcpy(&data[57],  (char*)&rocket.ina_in_i,            4);
    memcpy(&data[61],  (char*)&rocket.ina_ex_v,            4);
    memcpy(&data[65],  (char*)&rocket.ina_ex_i,            4);
    memcpy(&data[69],  (char*)&rocket.press,               4);
    memcpy(&data[73],  (char*)&rocket.temp,                4);
    memcpy(&data[77],  (char*)&rocket.accel[0],            4);
    memcpy(&data[81],  (char*)&rocket.accel[1],            4);
    memcpy(&data[85],  (char*)&rocket.accel[2],            4);
    memcpy(&data[89],  (char*)&rocket.gyro[0],            4);
    memcpy(&data[93],  (char*)&rocket.gyro[1],             4);
    memcpy(&data[97],  (char*)&rocket.gyro[2],             4);
    memcpy(&data[101], (char*)&rocket.mag[0],              4);
    memcpy(&data[105], (char*)&rocket.mag[1],              4);
    memcpy(&data[109], (char*)&rocket.mag[2],              4);

    eeprom.write(addr, data, 128);
    char test[] = {'A', 'V', 'I', 'O', '\r', '\n'};
    es.send(test, 7);
    addr += 0x80;
    /*for(int i = 0; i < 128 ; i++) {
        pc.printf("%x ", data[i]);
    }*/
    /*for(int i = 0; i < 128 ; i++) {
        fprintf(fp, "%02x ", (int)data[i]);
    }*/

    fprintf(fp, "%d,%d,%d,", rocket.mission_time, rocket.flight_time, rocket.phase);
    fprintf(fp, "%d,%d,%d,%d,%d,%d,%d,", rocket.f_sd, rocket.f_adxl, rocket.f_ina_in, rocket.f_ina_ex, rocket.f_lps, rocket.f_mpu, rocket.f_mpu_ak);
    fprintf(fp, "%d,", flight_pin.read());
    fprintf(fp, "%d,%d,%.1f,%.6f,%.6f,%d,%d,%.2f,%.1f,%.1f,", rocket.hour, rocket.min, rocket.sec, rocket.lat, rocket.lon, rocket.sat, rocket.fix, rocket.hdop, rocket.alt, rocket.geoid);
    fprintf(fp, "%.2f,%.2f,%.2f,", rocket.high_accel[0], rocket.high_accel[1], rocket.high_accel[2]);
    fprintf(fp, "%.1f,%.1f,", rocket.press, rocket.temp);
    fprintf(fp, "%.2f,%.2f,%.2f,%.2f,", rocket.ina_in_v, rocket.ina_in_i, rocket.ina_ex_v, rocket.ina_ex_i);
    fprintf(fp, "%.1f,%.1f,%.1f,", rocket.accel[0], rocket.accel[1], rocket.accel[2]);
    fprintf(fp, "%.1f,%.1f,%.1f,", rocket.gyro[0], rocket.gyro[1], rocket.gyro[2]);
    fprintf(fp, "%.1f,%.1f,%.1f,", rocket.mag[0], rocket.mag[1], rocket.mag[2]);
    fprintf(fp, "\r\n");

    if(sd_timer.read_ms() >= 30 * 1000) {
        sd_timer.reset();
        sd_timer.start();
        if(fp) {
            fclose(fp);
            fp = fopen("/sd/IZU2020_AVIONICS.txt", "a");
        }
    }
    //fwrite(data, sizeof(char), 128, fp);

#if DEBUG
    pc.printf("mission time: %d[sec]\r\n", rocket.mission_time);
    pc.printf("flight time: %d[sec]\r\n", rocket.flight_time);
    pc.printf("phase: %d\r\n", rocket.phase);
    pc.printf("sd: %d\t\tadxl: %d\t\tina_in: %d\t\tina_ex: %d\r\n", rocket.f_sd, rocket.f_adxl, rocket.f_ina_in, rocket.f_ina_ex);
    pc.printf("lps: %d\t\tmpu: %d\t\tmpu_ak: %d\r\n", rocket.f_lps, rocket.f_mpu, rocket.f_mpu_ak);
    pc.printf("flight pin: %d\r\n\n", flight_pin.read());
    pc.printf("gps\tutc: %d:%d:%.1f\r\n", rocket.hour, rocket.min, rocket.sec);
    pc.printf("\tlat:%.6f, lon:%.6f\r\n", rocket.lat, rocket.lon);
    pc.printf("\tsat:%d, fix:%d\r\n", rocket.sat, rocket.fix);
    pc.printf("\thdop:%.2f, alt:%.1f, geoid:%.1f\r\n\n", rocket.hdop, rocket.alt, rocket.geoid);
    pc.printf("high_accel\tx:\t%.2f[G]\ty:\t%.2f[G]\tz:\t%.2f[G]\r\n\n", rocket.high_accel[0], rocket.high_accel[1], rocket.high_accel[2]);
    pc.printf("press:\t%.1f[hPa]\ttemp:\t%.1f[C]\r\n\n", rocket.press, rocket.temp);
    pc.printf("internal:\t%.2f[mV]\t%.2f[mA]\r\n", rocket.ina_in_v, rocket.ina_in_i);
    pc.printf("external:\t%.2f[mV]\t%.2f[mA]\r\n\n", rocket.ina_ex_v, rocket.ina_ex_i);
    pc.printf("accel\tx:\t%.1f[G]\t\ty:\t%.1f[G]\t\tz:\t%.1f[G]\r\n", rocket.accel[0], rocket.accel[1], rocket.accel[2]);
    pc.printf("gyro\tx:\t%.1f[rad/s]\ty:\t%.1f[rad/s]\tz:\t%.1f[rad/s]\r\n", rocket.gyro[0], rocket.gyro[1], rocket.gyro[2]);
    pc.printf("mag\tx:\t%.1f[H]\t\ty:\t%.1f[H]\t\tz:\t%.1f[H]\r\n\n", rocket.mag[0], rocket.mag[1], rocket.mag[2]);
#endif

}