Diff: main.cpp

Revision:

0:106e97338223

Child:

1:5c46289e3bd1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jan 07 10:50:29 2020 +0000
@@ -0,0 +1,363 @@
+#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
+
+}
\ No newline at end of file