PQ_Hybrid_Electrical_Equipment_Team
main program
Dependencies: PQMPU9250 PQINA226 mbed PQAEGPS PQEEPROM PQADXL375 SDFileSystem PQLPS22HB PQES920LR
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "SDFileSystem.h" 00003 00004 #include "PQADXL375.h" 00005 #include "PQAEGPS.h" 00006 #include "PQEEPROM.h" 00007 #include "PQES920LR.h" 00008 #include "PQINA226.h" 00009 #include "PQLPS22HB.h" 00010 #include "PQMPU9250.h" 00011 00012 #define BURN_TIME 3.0f 00013 #define T_SEP 7.0f 00014 #define T_RECOVERY 14.0f 00015 00016 #define DEBUG_RATE 1 00017 #define DATA_RATE 20 00018 #define LOG_RATE 1 00019 #define DOWNLINK_RATE 1 00020 00021 #define LOG_HEADER 0x11 00022 #define DOWNLINK_HEADER 0x12 00023 #define UPLINK_HEADER 0x13 00024 00025 #define TIME_LSB 54.931640625 00026 #define VOLTAGE_LSB 1.25 00027 #define CURRENT_LSB 1.0986328125 00028 #define PRESS_LSB 0.0384521484375 00029 #define TEMP_LSB 0.002593994140625 00030 #define ACCEL_LSB 0.00048828125 00031 #define GYRO_LSB 0.06103515625 00032 #define MAG_LSB 0.146484375 00033 00034 Serial pc(USBTX, USBRX, 115200); 00035 00036 Serial gps_serial(p9, p10, 115200); 00037 Serial es_serial(p13, p14, 115200); 00038 00039 I2C i2c(p28, p27); 00040 00041 SDFileSystem sd_file_system(p5, p6, p7, p8, "sd"); 00042 00043 ES920LR es(es_serial); 00044 00045 AEGPS gps(gps_serial); 00046 00047 ADXL375 adxl(i2c, ADXL375::ALT_ADDRESS_HIGH); 00048 INA226 ina_in(i2c, INA226::A0_VS, INA226::A1_VS); 00049 INA226 ina_ex(i2c, INA226::A0_GND, INA226::A1_GND); 00050 LPS22HB lps(i2c, LPS22HB::SA0_HIGH); 00051 MPU9250 mpu(i2c, MPU9250::AD0_HIGH); 00052 00053 EEPROM eeprom(i2c); 00054 00055 Timer mission_timer; 00056 Timer flight_timer; 00057 Timer sd_timer; 00058 00059 Ticker downlink_ticker; 00060 Ticker log_ticker; 00061 Ticker debug_ticker; 00062 00063 DigitalIn debug_pin(p17); 00064 DigitalIn liftoff_pin(p18); 00065 DigitalIn ems_pin(p19); 00066 DigitalIn flight_pin(p20); 00067 00068 DigitalOut sep(p21); 00069 DigitalOut buzzer(p23); 00070 DigitalOut sig(p24); 00071 00072 BusOut led(LED1, LED2, LED3, LED4); 00073 00074 void init(); 00075 void read(); 00076 void command_handler(char *command); 00077 void downlink(); 00078 void log(); 00079 void debug(); 00080 00081 FILE *sd; 00082 00083 char file_name[64]; 00084 00085 bool launched = false; 00086 bool burning = false; 00087 bool apogee = false; 00088 int t = 0; 00089 00090 int addr; 00091 00092 char mission_timer_reset; 00093 int mission_time; 00094 int flight_time; 00095 char f_sd; 00096 char f_gps; 00097 char f_adxl; 00098 char f_ina_in; 00099 char f_ina_ex; 00100 char f_lps; 00101 char f_mpu; 00102 char f_mpu_ak; 00103 enum { 00104 SAFETY, 00105 READY, 00106 FLIGHT, 00107 SEP, 00108 EMERGENCY, 00109 RECOVERY 00110 } phase; 00111 int utc_hour; 00112 int utc_min; 00113 float utc_sec; 00114 float lat; 00115 float lon; 00116 int sat; 00117 int fix; 00118 float hdop; 00119 float alt; 00120 float geoid; 00121 float high_accel[3]; 00122 float voltage_in; 00123 float current_in; 00124 float voltage_ex; 00125 float current_ex; 00126 float press; 00127 float temp; 00128 float accel[3]; 00129 float gyro[3]; 00130 float mag[3]; 00131 00132 float coef = 0.01; 00133 float press_prev_LPF; 00134 float press_LPF; 00135 float press_LPF_prev; 00136 float press_LPF_diff; 00137 00138 short mission_time_bits; 00139 short flight_time_bits; 00140 char flags; 00141 short voltage_in_bits; 00142 short current_in_bits; 00143 short voltage_ex_bits; 00144 short current_ex_bits; 00145 short press_bits; 00146 short temp_bits; 00147 short accel_bits[3]; 00148 short gyro_bits[3]; 00149 short mag_bits[3]; 00150 00151 int main() 00152 { 00153 mission_timer.start(); 00154 init(); 00155 00156 while(1) { 00157 read(); 00158 led = phase; 00159 if(ems_pin) phase = EMERGENCY; 00160 switch(phase) { 00161 case SAFETY: 00162 break; 00163 case READY: 00164 if(flight_pin || liftoff_pin) phase = FLIGHT; 00165 break; 00166 case FLIGHT: 00167 if(!launched) { 00168 flight_timer.reset(); 00169 flight_timer.start(); 00170 launched = true; 00171 burning = true; 00172 } 00173 if(flight_timer.read() > BURN_TIME) { 00174 if(burning) { 00175 burning = false; 00176 press_LPF_prev = press_LPF; 00177 } 00178 if((flight_timer.read_ms() - t) > 500) { 00179 press_LPF_diff = press_LPF - press_LPF_prev; 00180 press_LPF_prev = press_LPF; 00181 if(press_LPF_diff > 0.0f) { 00182 apogee = true; 00183 } else { 00184 t = flight_timer.read_ms(); 00185 } 00186 } 00187 if(!burning && (apogee || flight_timer.read() > T_SEP)) { 00188 phase = SEP; 00189 } 00190 } 00191 break; 00192 case SEP: 00193 buzzer = 1; 00194 sep = 1; 00195 if(flight_timer.read() > T_RECOVERY) phase = RECOVERY; 00196 break; 00197 case EMERGENCY: 00198 buzzer = 0; 00199 sep = 0; 00200 break; 00201 case RECOVERY: 00202 buzzer = 1; 00203 sep = 0; 00204 break; 00205 } 00206 } 00207 } 00208 00209 void init() 00210 { 00211 char file_name_format[] = "/sd/IZU2020_AVIONICS_%d.dat"; 00212 int file_number = 1; 00213 while(1) { 00214 sprintf(file_name, file_name_format, file_number); 00215 sd = fopen(file_name, "r"); 00216 if(sd != NULL) { 00217 fclose(sd); 00218 file_number++; 00219 } else { 00220 sprintf(file_name, "/sd/IZU2020_AVIONICS_%d.dat", file_number); 00221 break; 00222 } 00223 } 00224 sd = fopen(file_name, "w"); 00225 sd_timer.start(); 00226 00227 if(sd) { 00228 fprintf(sd, "mission_timer_reset,"); 00229 fprintf(sd, "mission_time,"); 00230 fprintf(sd, "flight_time,"); 00231 fprintf(sd, "phase,"); 00232 fprintf(sd, "f_sd,"); 00233 fprintf(sd, "f_gps,"); 00234 fprintf(sd, "f_adxl,"); 00235 fprintf(sd, "f_ina_in,"); 00236 fprintf(sd, "f_ina_ex,"); 00237 fprintf(sd, "f_lps,"); 00238 fprintf(sd, "f_mpu,"); 00239 fprintf(sd, "f_mpu_ak,"); 00240 fprintf(sd, "lat,"); 00241 fprintf(sd, "lon,"); 00242 fprintf(sd, "sat,"); 00243 fprintf(sd, "fix,"); 00244 fprintf(sd, "hdop,"); 00245 fprintf(sd, "alt,"); 00246 fprintf(sd, "geoid,"); 00247 fprintf(sd, "high_accel_x,"); 00248 fprintf(sd, "high_accel_y,"); 00249 fprintf(sd, "high_accel_z,"); 00250 fprintf(sd, "voltage_in,"); 00251 fprintf(sd, "current_in,"); 00252 fprintf(sd, "voltage_ex,"); 00253 fprintf(sd, "current_ex,"); 00254 fprintf(sd, "press,"); 00255 fprintf(sd, "temp,"); 00256 fprintf(sd, "accel_x,"); 00257 fprintf(sd, "accel_y,"); 00258 fprintf(sd, "accel_z,"); 00259 fprintf(sd, "gyro_x,"); 00260 fprintf(sd, "gyro_y,"); 00261 fprintf(sd, "gyro_z,"); 00262 fprintf(sd, "mag_x,"); 00263 fprintf(sd, "mag_y,"); 00264 fprintf(sd, "mag_z,"); 00265 fprintf(sd, "\r\n"); 00266 } 00267 00268 debug_pin.mode(PullUp); 00269 liftoff_pin.mode(PullDown); 00270 ems_pin.mode(PullDown); 00271 flight_pin.mode(PullUp); 00272 00273 debug_ticker.attach(debug, 1.0f / DEBUG_RATE); 00274 log_ticker.attach(log, 1.0f / LOG_RATE); 00275 downlink_ticker.attach(downlink, 1.0f / DOWNLINK_RATE); 00276 00277 es.attach(command_handler); 00278 00279 adxl.begin(); 00280 ina_in.begin(); 00281 ina_ex.begin(); 00282 lps.begin(); 00283 mpu.begin(); 00284 } 00285 00286 void read() 00287 { 00288 if(mission_timer.read_ms() >= 30*60*1000) { 00289 mission_timer.reset(); 00290 mission_timer_reset++; 00291 } 00292 mission_time = mission_timer.read_ms(); 00293 flight_time = flight_timer.read_ms(); 00294 00295 utc_hour = gps.get_hour(); 00296 utc_min = gps.get_min(); 00297 utc_sec = gps.get_sec(); 00298 lat = gps.get_lat(); 00299 lon = gps.get_lon(); 00300 sat = gps.get_sat(); 00301 fix = gps.get_fix(); 00302 hdop = gps.get_hdop(); 00303 alt = gps.get_alt(); 00304 geoid = gps.get_geoid(); 00305 00306 f_sd = (bool)sd; 00307 00308 f_gps = (bool)fix; 00309 00310 f_adxl = adxl.test(); 00311 if(f_adxl) { 00312 adxl.read(high_accel); 00313 } 00314 00315 f_ina_in = ina_in.test(); 00316 if(f_ina_in) { 00317 ina_in.read_voltage(&voltage_in); 00318 ina_in.read_current(¤t_in); 00319 } 00320 00321 f_ina_ex = ina_ex.test(); 00322 if(f_ina_ex) { 00323 ina_ex.read_voltage(&voltage_ex); 00324 ina_ex.read_current(¤t_ex); 00325 } 00326 00327 f_lps = lps.test(); 00328 if(f_lps) { 00329 lps.read_press(&press); 00330 lps.read_temp(&temp); 00331 00332 press_LPF = press * coef + press_prev_LPF * (1 - coef); 00333 press_prev_LPF = press_LPF; 00334 } 00335 00336 f_mpu = mpu.test(); 00337 if(f_mpu) { 00338 mpu.read_accel(accel); 00339 mpu.read_gyro(gyro); 00340 } 00341 00342 f_mpu_ak = mpu.test_AK8963(); 00343 if(f_mpu_ak) { 00344 mpu.read_mag(mag); 00345 } 00346 } 00347 00348 void command_handler(char *command) 00349 { 00350 switch(command[0]) { 00351 case 0x80: 00352 break; 00353 case 0x81: 00354 break; 00355 case 0x82: 00356 break; 00357 case 0x83: 00358 break; 00359 case 0x84: 00360 break; 00361 case 0x85: 00362 break; 00363 case 0x86: 00364 break; 00365 case 0x87: 00366 break; 00367 case 0x88: 00368 break; 00369 case 0x89: 00370 break; 00371 case 0x8A: 00372 break; 00373 case 0x8B: 00374 break; 00375 case 0x8C: 00376 break; 00377 case 0x8D: 00378 break; 00379 case 0x8E: 00380 break; 00381 case 0x8F: 00382 break; 00383 case 0x90: 00384 break; 00385 case 0x91: 00386 break; 00387 case 0x92: 00388 break; 00389 case 0x93: 00390 break; 00391 case 0x94: 00392 break; 00393 case 0x95: 00394 break; 00395 case 0x96: 00396 break; 00397 case 0x97: 00398 break; 00399 case 0x98: 00400 break; 00401 case 0x99: 00402 break; 00403 case 0x9A: 00404 break; 00405 case 0x9B: 00406 break; 00407 case 0x9C: 00408 break; 00409 case 0x9D: 00410 break; 00411 case 0x9E: 00412 break; 00413 case 0x9F: 00414 break; 00415 case 0xA0: 00416 break; 00417 case 0xA1: 00418 break; 00419 case 0xA2: 00420 break; 00421 case 0xA3: 00422 break; 00423 case 0xA4: 00424 break; 00425 case 0xA5: 00426 break; 00427 case 0xA6: 00428 break; 00429 case 0xA7: 00430 break; 00431 case 0xA8: 00432 break; 00433 case 0xA9: 00434 break; 00435 case 0xAA: 00436 break; 00437 case 0xAB: 00438 break; 00439 case 0xAC: 00440 break; 00441 case 0xAD: 00442 break; 00443 case 0xAE: 00444 break; 00445 case 0xAF: 00446 break; 00447 case 0xB0: 00448 if(phase == READY) phase = SAFETY; 00449 break; 00450 case 0xB1: 00451 if(phase == SAFETY) phase = READY; 00452 break; 00453 case 0xB2: 00454 if(phase == READY) phase = FLIGHT; 00455 break; 00456 case 0xB3: 00457 if(!burning && phase == FLIGHT) phase = SEP; 00458 break; 00459 case 0xB4: 00460 if(phase >= FLIGHT && phase <= SEP) phase = EMERGENCY; 00461 break; 00462 case 0xB5: 00463 if(phase >= SEP && phase <= EMERGENCY) phase = RECOVERY; 00464 break; 00465 case 0xB6: 00466 break; 00467 case 0xB7: 00468 break; 00469 case 0xB8: 00470 break; 00471 case 0xB9: 00472 break; 00473 case 0xBA: 00474 break; 00475 case 0xBB: 00476 break; 00477 case 0xBC: 00478 break; 00479 case 0xBD: 00480 break; 00481 case 0xBE: 00482 break; 00483 case 0xBF: 00484 break; 00485 case 0xC0: 00486 break; 00487 case 0xC1: 00488 break; 00489 case 0xC2: 00490 break; 00491 case 0xC3: 00492 break; 00493 case 0xC4: 00494 break; 00495 case 0xC5: 00496 break; 00497 case 0xC6: 00498 break; 00499 case 0xC7: 00500 break; 00501 case 0xC8: 00502 break; 00503 case 0xC9: 00504 break; 00505 case 0xCA: 00506 break; 00507 case 0xCB: 00508 break; 00509 case 0xCC: 00510 break; 00511 case 0xCD: 00512 break; 00513 case 0xCE: 00514 break; 00515 case 0xCF: 00516 break; 00517 case 0xD0: 00518 break; 00519 case 0xD1: 00520 break; 00521 case 0xD2: 00522 break; 00523 case 0xD3: 00524 break; 00525 case 0xD4: 00526 break; 00527 case 0xD5: 00528 break; 00529 case 0xD6: 00530 break; 00531 case 0xD7: 00532 break; 00533 case 0xD8: 00534 break; 00535 case 0xD9: 00536 break; 00537 case 0xDA: 00538 break; 00539 case 0xDB: 00540 break; 00541 case 0xDC: 00542 break; 00543 case 0xDD: 00544 break; 00545 case 0xDE: 00546 break; 00547 case 0xDF: 00548 break; 00549 case 0xE0: 00550 break; 00551 case 0xE1: 00552 adxl.begin(); 00553 break; 00554 case 0xE2: 00555 break; 00556 case 0xE3: 00557 break; 00558 case 0xE4: 00559 break; 00560 case 0xE5: 00561 ina_ex.begin(); 00562 break; 00563 case 0xE6: 00564 break; 00565 case 0xE7: 00566 break; 00567 case 0xE8: 00568 break; 00569 case 0xE9: 00570 ina_in.begin(); 00571 break; 00572 case 0xEA: 00573 break; 00574 case 0xEB: 00575 break; 00576 case 0xEC: 00577 lps.begin(); 00578 break; 00579 case 0xED: 00580 mpu.begin(); 00581 break; 00582 case 0xEE: 00583 break; 00584 case 0xEF: 00585 break; 00586 case 0xF0: 00587 break; 00588 case 0xF1: 00589 break; 00590 case 0xF2: 00591 break; 00592 case 0xF3: 00593 break; 00594 case 0xF4: 00595 break; 00596 case 0xF5: 00597 break; 00598 case 0xF6: 00599 break; 00600 case 0xF7: 00601 break; 00602 case 0xF8: 00603 break; 00604 case 0xF9: 00605 break; 00606 case 0xFA: 00607 break; 00608 case 0xFB: 00609 break; 00610 case 0xFC: 00611 break; 00612 case 0xFD: 00613 break; 00614 case 0xFE: 00615 break; 00616 case 0xFF: 00617 NVIC_SystemReset(); 00618 break; 00619 } 00620 } 00621 00622 void downlink() 00623 { 00624 mission_time_bits = (short)(mission_time / TIME_LSB); 00625 flight_time_bits = (short)(flight_time / TIME_LSB); 00626 flags = 0; 00627 flags |= f_sd << 7; 00628 flags |= f_gps << 6; 00629 flags |= f_adxl << 5; 00630 flags |= f_ina_in << 4; 00631 flags |= f_ina_ex << 3; 00632 flags |= f_lps << 2; 00633 flags |= f_mpu << 1; 00634 flags |= f_mpu_ak; 00635 voltage_in_bits = (short)(voltage_in / VOLTAGE_LSB); 00636 current_in_bits = (short)(current_in / CURRENT_LSB); 00637 voltage_ex_bits = (short)(voltage_ex / VOLTAGE_LSB); 00638 current_ex_bits = (short)(current_ex / CURRENT_LSB); 00639 press_bits = (short)(press / PRESS_LSB); 00640 temp_bits = (short)(temp / TEMP_LSB); 00641 for(int i = 0; i < 3; i++) { 00642 accel_bits[i] = (short)(accel[i] / ACCEL_LSB); 00643 } 00644 for(int i = 0; i < 3; i++) { 00645 gyro_bits[i] = (short)(gyro[i] / GYRO_LSB); 00646 } 00647 for(int i = 0; i < 3; i++) { 00648 mag_bits[i] = (short)(mag[i] / MAG_LSB); 00649 } 00650 00651 char data[50]; 00652 data[0] = DOWNLINK_HEADER; 00653 data[1] = mission_timer_reset; 00654 data[2] = *(char*)&mission_time_bits+0; 00655 data[3] = ((char*)&mission_time_bits)[1]; 00656 data[4] = ((char*)&flight_time_bits)[0]; 00657 data[5] = ((char*)&flight_time_bits)[1]; 00658 data[6] = flags; 00659 data[7] = phase; 00660 data[8] = ((char*)&lat)[0]; 00661 data[9] = ((char*)&lat)[1]; 00662 data[10] = ((char*)&lat)[2]; 00663 data[11] = ((char*)&lat)[3]; 00664 data[12] = ((char*)&lon)[0]; 00665 data[13] = ((char*)&lon)[1]; 00666 data[14] = ((char*)&lon)[2]; 00667 data[15] = ((char*)&lon)[3]; 00668 data[16] = ((char*)&alt)[0]; 00669 data[17] = ((char*)&alt)[1]; 00670 data[18] = ((char*)&alt)[2]; 00671 data[19] = ((char*)&alt)[3]; 00672 data[20] = ((char*)&voltage_in_bits)[0]; 00673 data[21] = ((char*)&voltage_in_bits)[1]; 00674 data[22] = ((char*)¤t_in_bits)[0]; 00675 data[23] = ((char*)¤t_in_bits)[1]; 00676 data[24] = ((char*)&voltage_ex_bits)[0]; 00677 data[25] = ((char*)&voltage_ex_bits)[1]; 00678 data[26] = ((char*)¤t_ex_bits)[0]; 00679 data[27] = ((char*)¤t_ex_bits)[1]; 00680 data[28] = ((char*)&press_bits)[0]; 00681 data[29] = ((char*)&press_bits)[1]; 00682 data[30] = ((char*)&temp_bits)[0]; 00683 data[31] = ((char*)&temp_bits)[1]; 00684 data[32] = ((char*)&accel_bits[0])[0]; 00685 data[33] = ((char*)&accel_bits[0])[1]; 00686 data[34] = ((char*)&accel_bits[1])[0]; 00687 data[35] = ((char*)&accel_bits[1])[1]; 00688 data[36] = ((char*)&accel_bits[2])[0]; 00689 data[37] = ((char*)&accel_bits[2])[1]; 00690 data[38] = ((char*)&gyro_bits[0])[0]; 00691 data[39] = ((char*)&gyro_bits[0])[1]; 00692 data[40] = ((char*)&gyro_bits[1])[0]; 00693 data[41] = ((char*)&gyro_bits[1])[1]; 00694 data[42] = ((char*)&gyro_bits[2])[0]; 00695 data[43] = ((char*)&gyro_bits[2])[1]; 00696 data[44] = ((char*)&mag_bits[0])[0]; 00697 data[45] = ((char*)&mag_bits[0])[1]; 00698 data[46] = ((char*)&mag_bits[1])[0]; 00699 data[47] = ((char*)&mag_bits[1])[1]; 00700 data[48] = ((char*)&mag_bits[2])[0]; 00701 data[49] = ((char*)&mag_bits[2])[1]; 00702 00703 es.send(data, 50); 00704 } 00705 00706 void log() 00707 { 00708 if(phase >= FLIGHT && phase <= SEP) { 00709 char data[128]; 00710 data[0] = LOG_HEADER; 00711 data[1] = ((char*)&mission_timer_reset)[0]; 00712 data[2] = ((char*)&mission_time)[0]; 00713 data[3] = ((char*)&mission_time)[1]; 00714 data[4] = ((char*)&mission_time)[2]; 00715 data[5] = ((char*)&mission_time)[3]; 00716 data[6] = ((char*)&flight_time)[0]; 00717 data[7] = ((char*)&flight_time)[1]; 00718 data[8] = ((char*)&flight_time)[2]; 00719 data[9] = ((char*)&flight_time)[3]; 00720 data[10] = ((char*)&f_sd)[0]; 00721 data[11] = ((char*)&f_gps)[0]; 00722 data[12] = ((char*)&f_adxl)[0]; 00723 data[13] = ((char*)&f_ina_in)[0]; 00724 data[14] = ((char*)&f_ina_ex)[0]; 00725 data[15] = ((char*)&f_lps)[0]; 00726 data[16] = ((char*)&f_mpu)[0]; 00727 data[17] = ((char*)&f_mpu_ak)[0]; 00728 data[18] = ((char*)&phase)[0]; 00729 data[19] = ((char*)&lat)[0]; 00730 data[20] = ((char*)&lat)[1]; 00731 data[21] = ((char*)&lat)[2]; 00732 data[22] = ((char*)&lat)[3]; 00733 data[23] = ((char*)&lon)[0]; 00734 data[24] = ((char*)&lon)[1]; 00735 data[25] = ((char*)&lon)[2]; 00736 data[26] = ((char*)&lon)[3]; 00737 data[27] = ((char*)&sat)[0]; 00738 data[28] = ((char*)&sat)[1]; 00739 data[29] = ((char*)&sat)[2]; 00740 data[30] = ((char*)&sat)[3]; 00741 data[31] = ((char*)&fix)[0]; 00742 data[32] = ((char*)&fix)[1]; 00743 data[33] = ((char*)&fix)[2]; 00744 data[34] = ((char*)&fix)[3]; 00745 data[35] = ((char*)&hdop)[0]; 00746 data[36] = ((char*)&hdop)[1]; 00747 data[37] = ((char*)&hdop)[2]; 00748 data[38] = ((char*)&hdop)[3]; 00749 data[39] = ((char*)&alt)[0]; 00750 data[40] = ((char*)&alt)[1]; 00751 data[41] = ((char*)&alt)[2]; 00752 data[42] = ((char*)&alt)[3]; 00753 data[43] = ((char*)&geoid)[0]; 00754 data[44] = ((char*)&geoid)[1]; 00755 data[45] = ((char*)&geoid)[2]; 00756 data[46] = ((char*)&geoid)[3]; 00757 data[47] = ((char*)&high_accel[0])[0]; 00758 data[48] = ((char*)&high_accel[0])[1]; 00759 data[49] = ((char*)&high_accel[0])[2]; 00760 data[50] = ((char*)&high_accel[0])[3]; 00761 data[51] = ((char*)&high_accel[1])[0]; 00762 data[52] = ((char*)&high_accel[1])[1]; 00763 data[53] = ((char*)&high_accel[1])[2]; 00764 data[54] = ((char*)&high_accel[1])[3]; 00765 data[55] = ((char*)&high_accel[2])[0]; 00766 data[56] = ((char*)&high_accel[2])[1]; 00767 data[57] = ((char*)&high_accel[2])[2]; 00768 data[58] = ((char*)&high_accel[2])[3]; 00769 data[59] = ((char*)&voltage_in)[0]; 00770 data[60] = ((char*)&voltage_in)[1]; 00771 data[61] = ((char*)&voltage_in)[2]; 00772 data[62] = ((char*)&voltage_in)[3]; 00773 data[63] = ((char*)¤t_in)[0]; 00774 data[64] = ((char*)¤t_in)[1]; 00775 data[65] = ((char*)¤t_in)[2]; 00776 data[66] = ((char*)¤t_in)[3]; 00777 data[67] = ((char*)&voltage_ex)[0]; 00778 data[68] = ((char*)&voltage_ex)[1]; 00779 data[69] = ((char*)&voltage_ex)[2]; 00780 data[70] = ((char*)&voltage_ex)[3]; 00781 data[71] = ((char*)¤t_ex)[0]; 00782 data[72] = ((char*)¤t_ex)[1]; 00783 data[73] = ((char*)¤t_ex)[2]; 00784 data[74] = ((char*)¤t_ex)[3]; 00785 data[75] = ((char*)&press)[0]; 00786 data[76] = ((char*)&press)[1]; 00787 data[77] = ((char*)&press)[2]; 00788 data[78] = ((char*)&press)[3]; 00789 data[79] = ((char*)&temp)[0]; 00790 data[80] = ((char*)&temp)[1]; 00791 data[81] = ((char*)&temp)[2]; 00792 data[82] = ((char*)&temp)[3]; 00793 data[83] = ((char*)&accel[0])[0]; 00794 data[84] = ((char*)&accel[0])[1]; 00795 data[85] = ((char*)&accel[0])[2]; 00796 data[86] = ((char*)&accel[0])[3]; 00797 data[87] = ((char*)&accel[1])[0]; 00798 data[88] = ((char*)&accel[1])[1]; 00799 data[89] = ((char*)&accel[1])[2]; 00800 data[90] = ((char*)&accel[1])[3]; 00801 data[91] = ((char*)&accel[2])[0]; 00802 data[92] = ((char*)&accel[2])[1]; 00803 data[93] = ((char*)&accel[2])[2]; 00804 data[94] = ((char*)&accel[2])[3]; 00805 data[95] = ((char*)&gyro[0])[0]; 00806 data[96] = ((char*)&gyro[0])[1]; 00807 data[97] = ((char*)&gyro[0])[2]; 00808 data[98] = ((char*)&gyro[0])[3]; 00809 data[99] = ((char*)&gyro[1])[0]; 00810 data[100] = ((char*)&gyro[1])[1]; 00811 data[101] = ((char*)&gyro[1])[2]; 00812 data[102] = ((char*)&gyro[1])[3]; 00813 data[103] = ((char*)&gyro[2])[0]; 00814 data[104] = ((char*)&gyro[2])[1]; 00815 data[105] = ((char*)&gyro[2])[2]; 00816 data[106] = ((char*)&gyro[2])[3]; 00817 data[107] = ((char*)&mag[0])[0]; 00818 data[108] = ((char*)&mag[0])[1]; 00819 data[109] = ((char*)&mag[0])[2]; 00820 data[110] = ((char*)&mag[0])[3]; 00821 data[111] = ((char*)&mag[1])[0]; 00822 data[112] = ((char*)&mag[1])[1]; 00823 data[113] = ((char*)&mag[1])[2]; 00824 data[114] = ((char*)&mag[1])[3]; 00825 data[115] = ((char*)&mag[2])[0]; 00826 data[116] = ((char*)&mag[2])[1]; 00827 data[117] = ((char*)&mag[2])[2]; 00828 data[118] = ((char*)&mag[2])[3]; 00829 data[119] = 0x00; 00830 data[120] = 0x00; 00831 data[121] = 0x00; 00832 data[122] = 0x00; 00833 data[123] = 0x00; 00834 data[124] = 0x00; 00835 data[125] = 0x00; 00836 data[126] = 0x00; 00837 data[127] = 0x00; 00838 00839 eeprom.write(addr, data, 128); 00840 addr += 0x80; 00841 } 00842 00843 if(sd) { 00844 fprintf(sd, "%d,", mission_timer_reset); 00845 fprintf(sd, "%d,", mission_time); 00846 fprintf(sd, "%d,", flight_time); 00847 fprintf(sd, "%d,", phase); 00848 fprintf(sd, "%d,", f_sd); 00849 fprintf(sd, "%d,", f_gps); 00850 fprintf(sd, "%d,", f_adxl); 00851 fprintf(sd, "%d,", f_ina_in); 00852 fprintf(sd, "%d,", f_ina_ex); 00853 fprintf(sd, "%d,", f_lps); 00854 fprintf(sd, "%d,", f_mpu); 00855 fprintf(sd, "%d,", f_mpu_ak); 00856 fprintf(sd, "%f,", lat); 00857 fprintf(sd, "%f,", lon); 00858 fprintf(sd, "%d,", sat); 00859 fprintf(sd, "%d,", fix); 00860 fprintf(sd, "%f,", hdop); 00861 fprintf(sd, "%f,", alt); 00862 fprintf(sd, "%f,", geoid); 00863 fprintf(sd, "%f,", high_accel[0]); 00864 fprintf(sd, "%f,", high_accel[1]); 00865 fprintf(sd, "%f,", high_accel[2]); 00866 fprintf(sd, "%f,", voltage_in); 00867 fprintf(sd, "%f,", current_in); 00868 fprintf(sd, "%f,", voltage_ex); 00869 fprintf(sd, "%f,", current_ex); 00870 fprintf(sd, "%f,", press); 00871 fprintf(sd, "%f,", temp); 00872 fprintf(sd, "%f,", accel[0]); 00873 fprintf(sd, "%f,", accel[1]); 00874 fprintf(sd, "%f,", accel[2]); 00875 fprintf(sd, "%f,", gyro[0]); 00876 fprintf(sd, "%f,", gyro[1]); 00877 fprintf(sd, "%f,", gyro[2]); 00878 fprintf(sd, "%f,", mag[0]); 00879 fprintf(sd, "%f,", mag[1]); 00880 fprintf(sd, "%f,", mag[2]); 00881 fprintf(sd, "\r\n"); 00882 } 00883 00884 if(sd_timer.read_ms() >= 60*1000) { 00885 sd_timer.reset(); 00886 sd_timer.start(); 00887 if(sd) { 00888 fclose(sd); 00889 sd = fopen(file_name, "a"); 00890 } 00891 } 00892 } 00893 00894 void debug() 00895 { 00896 if(!debug_pin) { 00897 pc.printf("mission_timer_reset: %d\r\n", mission_timer_reset); 00898 pc.printf("mission_time: %d\r\n", mission_time); 00899 pc.printf("flight_time: %d\r\n", flight_time); 00900 pc.printf("phase: %d\r\n", phase); 00901 pc.printf("f_sd: %d\r\n", f_sd); 00902 pc.printf("f_gps: %d\r\n", f_gps); 00903 pc.printf("f_adxl: %d\r\n", f_adxl); 00904 pc.printf("f_ina_in: %d\r\n", f_ina_in); 00905 pc.printf("f_ina_ex: %d\r\n", f_ina_ex); 00906 pc.printf("f_lps: %d\r\n", f_lps); 00907 pc.printf("f_mpu: %d\r\n", f_mpu); 00908 pc.printf("f_mpu_ak: %d\r\n", f_mpu_ak); 00909 pc.printf("lat: %f\r\n", lat); 00910 pc.printf("lon: %f\r\n", lon); 00911 pc.printf("sat: %d\r\n", sat); 00912 pc.printf("fix: %d\r\n", fix); 00913 pc.printf("hdop: %f\r\n", hdop); 00914 pc.printf("alt: %f\r\n", alt); 00915 pc.printf("geoid: %f\r\n", geoid); 00916 pc.printf("high_accel_x: %f\r\n", high_accel[0]); 00917 pc.printf("high_accel_y: %f\r\n", high_accel[1]); 00918 pc.printf("high_accel_z: %f\r\n", high_accel[2]); 00919 pc.printf("voltage_in: %f\r\n", voltage_in); 00920 pc.printf("current_in: %f\r\n", current_in); 00921 pc.printf("voltage_ex: %f\r\n", voltage_ex); 00922 pc.printf("current_ex: %f\r\n", current_ex); 00923 pc.printf("press: %f\r\n", press); 00924 pc.printf("temp: %f\r\n", temp); 00925 pc.printf("accel_x: %f\r\n", accel[0]); 00926 pc.printf("accel_y: %f\r\n", accel[1]); 00927 pc.printf("accel_z: %f\r\n", accel[2]); 00928 pc.printf("gyro_x: %f\r\n", gyro[0]); 00929 pc.printf("gyro_y: %f\r\n", gyro[1]); 00930 pc.printf("gyro_z: %f\r\n", gyro[2]); 00931 pc.printf("mag_x: %f\r\n", mag[0]); 00932 pc.printf("mag_y: %f\r\n", mag[1]); 00933 pc.printf("mag_z: %f\r\n", mag[2]); 00934 } 00935 }
Generated on Sat Jul 16 2022 02:23:16 by
1.7.2