Diff: main.cpp

Revision:

2:104839501493

Parent:

1:5c46289e3bd1

Child:

3:84d2520b9079

--- a/main.cpp	Sat Feb 15 03:35:10 2020 +0000
+++ b/main.cpp	Thu Feb 20 04:14:33 2020 +0000
@@ -9,41 +9,35 @@
 #include "PQLPS22HB.h"
 #include "PQMPU9250.h"
 
-#define DEBUG 1
-
-#define BURN_TIME 3.0f
-#define T_SEP 7.0f
+#define BURN_TIME  3.0f
+#define T_SEP      7.0f
 #define T_RECOVERY 14.0f
 
-#define DATA_RATE 20
-#define LOG_RATE 1
+#define DEBUG_RATE    1
+#define DATA_RATE     20
+#define LOG_RATE      1
 #define DOWNLINK_RATE 1
 
-#define LOG_HEADER 0x11
+#define LOG_HEADER      0x11
 #define DOWNLINK_HEADER 0x12
-#define UPLINK_HEADER 0x13
+#define UPLINK_HEADER   0x13
 
-#define TIME_LSB 54.9
-#define LAT_LSB 0.00275
-#define LON_LSB 0.00549
-#define ALT_LSB 15.3
-#define HIGH_ACCEL_LSB 0.0061
+#define TIME_LSB    54.931640625
 #define VOLTAGE_LSB 1.25
-#define CURRENT_LSB 1.1
-#define PRESS_LSB 0.0385
-#define TEMP_LSB 0.00259
-#define ACCEL_LSB 0.000488
-#define GYRO_LSB 0.061
-#define MAG_LSB 0.146
+#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);
-Serial es_serial(p9, p10, 115200);
 
 I2C i2c(p28, p27);
 
-LocalFileSystem local_file_system("local");
 SDFileSystem sd_file_system(p5, p6, p7, p8, "sd");
 
 ES920LR es(es_serial);
@@ -62,9 +56,6 @@
 Timer flight_timer;
 Timer sd_timer;
 
-Timeout sep_timeout;
-Timeout recovery_timeout;
-
 Ticker debug_ticker;
 Ticker log_ticker;
 Ticker downlink_ticker;
@@ -76,23 +67,25 @@
 DigitalOut sep(p25);
 DigitalOut buzzer(p22);
 
-BusOut led(LED4, LED3, LED2, LED1);
+BusOut led(LED1, LED2, LED3, LED4);
 
 void init();
 void read();
-void log();
+void command_handler(char *command);
 void downlink();
-void command_handler(char *command);
+void log();
+void debug();
 
-int addr;
-
-FILE *local;
 FILE *sd;
 
 char file_name[64];
 
-int launched = 0;
-int burning = 0;
+bool launched = false;
+bool burning = false;
+bool apogee = false;
+int t = 0;
+
+int addr;
 
 char mission_timer_reset;
 int mission_time;
@@ -134,16 +127,15 @@
 float gyro[3];
 float mag[3];
 
-float press_prev;
-float press_diff;
+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 lat_bits;
-short lon_bits;
-short alt_bits;
-short high_accel_bits[3];
 short voltage_in_bits;
 short current_in_bits;
 short voltage_ex_bits;
@@ -159,8 +151,6 @@
     mission_timer.start();
     init();
 
-    sd_timer.start();
-
     while(1) {
         read();
         led = phase;
@@ -168,18 +158,30 @@
             case SAFETY:
                 break;
             case READY:
-                if(flight_pin | liftoff_pin) phase = FLIGHT;
+                if(flight_pin || liftoff_pin) phase = FLIGHT;
                 break;
             case FLIGHT:
                 if(!launched) {
                     flight_timer.reset();
                     flight_timer.start();
-                    launched = 1;
-                    burning = 1;
+                    launched = true;
+                    burning = true;
                 }
-                if(flight_timer.read() > T_SEP) {
-                    burning = 0;
-                    if(!burning & ( press_diff > 0.0f | flight_timer.read() > T_SEP ) ) {
+                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;
                     }
                 }
@@ -190,16 +192,14 @@
                 if(flight_timer.read() > T_RECOVERY) phase = RECOVERY;
                 break;
             case EMERGENCY:
+                buzzer = 0;
                 sep = 0;
                 break;
             case RECOVERY:
+                buzzer = 1;
                 sep = 0;
                 break;
         }
-        if(mission_timer.read_ms() >= 30*60*1000) {
-            mission_timer.reset();
-            mission_timer_reset++;
-        }
     }
 }
 
@@ -219,6 +219,7 @@
         }
     }
     sd = fopen(file_name, "w");
+    sd_timer.start();
 
     if(sd) {
         fprintf(sd, "mission_timer_reset,");
@@ -264,8 +265,9 @@
     liftoff_pin.mode(PullDown);
     ems_pin.mode(PullDown);
     flight_pin.mode(PullUp);
-    
-    log_ticker.attach(log, LOG_RATE);
+
+    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);
@@ -279,6 +281,10 @@
 
 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();
 
@@ -295,8 +301,12 @@
 
     f_sd = (bool)sd;
 
+    f_gps = (bool)fix;
+
     f_adxl = adxl.test();
-    if(f_adxl) adxl.read(high_accel);
+    if(f_adxl) {
+        adxl.read(high_accel);
+    }
 
     f_ina_in = ina_in.test();
     if(f_ina_in) {
@@ -313,9 +323,10 @@
     f_lps = lps.test();
     if(f_lps) {
         lps.read_press(&press);
-        press_diff = press - press_prev;
-        press_prev = press;
         lps.read_temp(&temp);
+
+        press_LPF = press * coef + press_prev_LPF * (1 - coef);
+        press_prev_LPF = press_LPF;
     }
 
     f_mpu = mpu.test();
@@ -332,7 +343,6 @@
 
 void command_handler(char *command)
 {
-    pc.printf("COMMAND: %02x", command[0]);
     switch(command[0]) {
         case 0x80:
             break;
@@ -440,7 +450,7 @@
             if(phase == READY) phase = FLIGHT;
             break;
         case 0xB3:
-            if(phase == FLIGHT) phase = SEP;
+            if(!burning & phase == FLIGHT) phase = SEP;
             break;
         case 0xB4:
             if(phase >= FLIGHT & phase <= SEP) phase = EMERGENCY;
@@ -617,12 +627,6 @@
     flags |= f_lps     << 2;
     flags |= f_mpu     << 1;
     flags |= f_mpu_ak;
-    lat_bits = (short)(lat / LAT_LSB);
-    lon_bits = (short)(lon / LON_LSB);
-    alt_bits = (short)(alt / ALT_LSB);
-    for(int i = 0; i < 3; i++) {
-        high_accel_bits[i] = (short)(high_accel[i] / HIGH_ACCEL_LSB);
-    }
     voltage_in_bits = (short)(voltage_in / VOLTAGE_LSB);
     current_in_bits = (short)(current_in / CURRENT_LSB);
     voltage_ex_bits = (short)(voltage_ex / VOLTAGE_LSB);
@@ -648,18 +652,18 @@
     data[5] = ((char*)&flight_time_bits)[1];
     data[6] = flags;
     data[7] = phase;
-    data[8] = ((char*)&lat_bits)[0];
-    data[9] = ((char*)&lat_bits)[1];
-    data[10] = ((char*)&lon_bits)[0];
-    data[11] = ((char*)&lon_bits)[1];
-    data[12] = ((char*)&alt_bits)[0];
-    data[13] = ((char*)&alt_bits)[1];
-    data[14] = ((char*)&high_accel_bits[0])[0];
-    data[15] = ((char*)&high_accel_bits[0])[1];
-    data[16] = ((char*)&high_accel_bits[1])[0];
-    data[17] = ((char*)&high_accel_bits[1])[1];
-    data[18] = ((char*)&high_accel_bits[2])[0];
-    data[19] = ((char*)&high_accel_bits[2])[1];
+    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];
@@ -694,183 +698,142 @@
     es.send(data, 50);
 }
 
-void debug()
-{
-    pc.printf("{");
-    pc.printf("\"mission_timer_reset\": %d,", mission_timer_reset);
-    pc.printf("\"mission_time\": %d,", mission_time);
-    pc.printf("\"flight_time\": %d,", flight_time);
-    pc.printf("\"phase\": %d,", phase);
-    pc.printf("\"f_sd\": %d,", f_sd);
-    pc.printf("\"f_gps\": %d,", f_gps);
-    pc.printf("\"f_adxl\": %d,", f_adxl);
-    pc.printf("\"f_ina_in\": %d,", f_ina_in);
-    pc.printf("\"f_ina_ex\": %d,", f_ina_ex);
-    pc.printf("\"f_lps\": %d,", f_lps);
-    pc.printf("\"f_mpu\": %d,", f_mpu);
-    pc.printf("\"f_mpu_ak\": %d,", f_mpu_ak);
-    pc.printf("\"lat\": %f,", lat);
-    pc.printf("\"lon\": %f,", lon);
-    pc.printf("\"sat\": %d,", sat);
-    pc.printf("\"fix\": %d,", fix);
-    pc.printf("\"hdop\": %f,", hdop);
-    pc.printf("\"alt\": %f,", alt);
-    pc.printf("\"geoid\": %f,", geoid);
-    pc.printf("\"high_accel_x\": %f,", high_accel[0]);
-    pc.printf("\"high_accel_y\": %f,", high_accel[1]);
-    pc.printf("\"high_accel_z\": %f,", high_accel[2]);
-    pc.printf("\"voltage_in\": %f,", voltage_in);
-    pc.printf("\"current_in\": %f,", current_in);
-    pc.printf("\"voltage_ex\": %f,", voltage_ex);
-    pc.printf("\"current_ex\": %f,", current_ex);
-    pc.printf("\"press\": %f,", press);
-    pc.printf("\"temp\": %f,", temp);
-    pc.printf("\"accel_x\": %f,", accel[0]);
-    pc.printf("\"accel_y\": %f,", accel[1]);
-    pc.printf("\"accel_z\": %f,", accel[2]);
-    pc.printf("\"gyro_x\": %f,", gyro[0]);
-    pc.printf("\"gyro_y\": %f,", gyro[1]);
-    pc.printf("\"gyro_z\": %f,", gyro[2]);
-    pc.printf("\"mag_x\": %f,", mag[0]);
-    pc.printf("\"mag_y\": %f,", mag[1]);
-    pc.printf("\"mag_z\": %f", mag[2]);
-    pc.printf("}\r\n");
-}
-
 void log()
 {
-    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(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);
@@ -913,7 +876,7 @@
         fprintf(sd, "\r\n");
     }
 
-    if(sd_timer.read_ms() >= 30 * 1000) {
+    if(sd_timer.read_ms() >= 60*1000) {
         sd_timer.reset();
         sd_timer.start();
         if(sd) {
@@ -921,4 +884,45 @@
             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]);
 }
\ No newline at end of file