![](/media/cache/group/icon1_qbDJ87a.png.50x50_q85.png)
高高度CanSat Space SWANの投下時に使用したプログラム.(IZU2019)
Dependencies: Nichrome_lib mbed ads1115_test BNO055_lib ADXL375_i2c ES920LR CCS811_lib SDFileSystem BME280_lib INA226_lib EEPROM_lib GPS_interrupt
main.cpp
- Committer:
- Sigma884
- Date:
- 2019-04-21
- Revision:
- 8:2ce2c7eebb92
- Parent:
- 7:3a2d0474f1ca
File content as of revision 8:2ce2c7eebb92:
#include "mbed.h" #include "math.h" #include "Adafruit_ADS1015.h" #include "ADXL375_i2c.h" #include "BME280_lib.h" #include "BNO055_lib.h" #include "CCS811_lib.h" #include "EEPROM_lib.h" #include "ES920LR.hpp" #include "GPS_interrupt.h" #include "INA226.h" #include "Nichrome_lib.h" #include "SDFileSystem.h" /******************************************************************************* 設定値 投下前に必ず確認!! *******************************************************************************/ const float START_CONTROLL_TIME = 1.0f; const float START_CONTROLL_ALT = -5.0f; const float RECOVERY_TIME = 120.0f; const float RECOVERY_SPEED = -2.0f; bool wait_GPS = true; bool ok_PC = false; /******************************************************************************* コンストラクタ *******************************************************************************/ RawSerial pc(USBTX, USBRX, 115200); RawSerial serial_es920(p9, p10); ES920LR es920(serial_es920, pc, 115200); Serial GPS_serial(p13, p14, 9600); GPS_interrupt GPS(&GPS_serial); float GPS_freq = 1; SDFileSystem sd(p5, p6, p7, p8, "sd"); const char file_name[64] = "/sd/Space_SWAN_LOG.txt"; I2C i2c_bus(p28, p27); ADXL375_i2c ADXL375(i2c_bus, ADXL375_i2c::ALT_ADDRESS_HIGH); myINA226 INA226(i2c_bus, myINA226::A1_VDD, myINA226::A0_VDD); BME280_lib BME280(i2c_bus, BME280_lib::AD0_LOW); BNO055_lib BNO055(i2c_bus, BNO055_lib::AD0_HIGH); CCS811_lib CCS811(i2c_bus, CCS811_lib::AD0_LOW); EEPROM_lib EEPROM(i2c_bus, 4); Adafruit_ADS1115 ADS1115(&i2c_bus, ADS1015_ADDRESS); Nichrome_lib Valve1(p23); Nichrome_lib Valve2(p24); Nichrome_lib Valve3(p25); Nichrome_lib Valve4(p26); Nichrome_lib Buzzer(p22); DigitalIn FlightPin(p15); Timeout timeout_start_controll; Timeout timeout_recovery; Timer time_main; Timer time_flight; Ticker tick_gps; Ticker tick_print; Ticker tick_save; Ticker tick_header_data; /******************************************************************************* 関数のプロトタイプ宣言 *******************************************************************************/ void setup(); //無線あり void modeChange(); //無線あり void changePhase_RECOVERY(); void startControllAttitude(); void controllAttitude(); void readSensor(); void readGPS(); void printData(); void readPC(); void printHelp(); void sendDownLink();//無線あり void readUpLink(); //無線あり void startRecSlow(); void startRecFast(); void stopRec(); void recData(); void buzzerChange(); /******************************************************************************* 変数の宣言 *******************************************************************************/ char CanSat_phase; bool do_first = false; bool controll_yn = false; bool start_controll_once = false; bool es920_using = false; char es920_uplink_command = '-'; float adxl_acc[3]; float ina_v, ina_i; double amg[9], quart[4], euler[3]; float pres, temp, hum, alt, pres_0, temp_0; float pres_now; int speed_time_old; float alt_buf[10], alt_ave, alt_ave_old, speed, speed_buf[10], speed_ave; int alt_count = 0, speed_count = 0; float alt_flight_start; int CO2, TVOCs; int16_t bottle_pres_bit; float bottle_pres; float gps_lat, gps_lon, gps_alt, gps_utc[6], gps_knot, gps_deg; int gps_sat; bool gps_yn; bool flight_pin; FILE *fp; bool save_slow = false; bool save_fast = false; int save_c = 0; int eeprom_ptr; int time_read, time_reset = 0; int time_min, time_sec, time_ms; int flight_time_read, flight_time_read_old, flight_time_reset = 0; int flight_time_min, flight_time_sec, flight_time_ms; /******************************************************************************* 定数 *******************************************************************************/ const float ES920_MAX_2 = 0.0000610370; const float ES920_MAX_20 = 0.0006103702; const float ES920_MAX_50 = 0.0015259255; const float ES920_MAX_100 = 0.0030518509; const float ES920_MAX_200 = 0.0061037019; const float ES920_MAX_360 = 0.0109866634; const float ES920_MAX_500 = 0.0152592547; const float ES920_MAX_1500 = 0.0457777642; /******************************************************************************* 無線のヘッダまとめ(CANSAT -> GND) *******************************************************************************/ const char HEADER_SENSOR_SETUP = 0x01; // 0x01, ADXL, INA, BME, BNO, CCS, PRES, SD // 0 1 1 1 1 1 1 1 // 0 1 2 3 4 5 6 const char HEADER_GPS_SETUP = 0x02; // 0x02, readable, wait_count // 0 1 4 // 0 1 // 0x00 : NG // 0x01 : OK // 0xAA : Finish // 0xFF : Ignore const char HEADER_DATA = 0x10; // 0x10, 起動時間, フライト時間, フェーズ, status, valve, lat, lon, alt, knot, deg, sat, // 0 4(2,2) 4(2,2) 1 1 1 4 4 2 2 2 2 // 0 2 4 6 8 9 10 11 15 19 21 23 25 // 0x01 : save_slow // 0x02 : save_fast // 0x04 : controll_yn // 0x08 : flight_pin // 0x10 : gps_yn // 0x20 : Buzzer // V, I, pres, temp, alt, speed, eulerX3, CO2, TVOCs, bottle // 2 2 2 2 2 2 6(2,2,2) 2 2 2 // 27 29 31 33 35 37 394143 45 47 49 const char HEADER_START = 0x11; // 0x11, What // 0 1 // 'W' : 投下検知モード // 'S' : 安全モード // 'F' : フライトモード // 'R' : 回収モード /******************************************************************************* 無線のヘッダまとめ(GND -> CANSAT) *******************************************************************************/ const char HEADER_COMMAND = 0xcd; // 0xcd, command // 0 1 // 0 /******************************************************************************* CanSatのフェーズ *******************************************************************************/ const char CANSAT_SETUP = 0x00; //セットアップ中 const char CANSAT_SAFETY = 0x01; //安全モード const char CANSAT_WAIT = 0x02; //待機モード const char CANSAT_FLIGHT = 0x03; //フライトモード const char CANSAT_RECOVERY = 0x04; //回収モード /******************************************************************************* メイン関数 *******************************************************************************/ int main() { CanSat_phase = CANSAT_SETUP; setup(); pc.attach(&readPC, Serial::RxIrq); tick_gps.attach(&readGPS, 1.0/GPS_freq); tick_print.attach(&printData, 1.0f); tick_header_data.attach(&sendDownLink, 1.0f); es920.attach(&readUpLink, HEADER_COMMAND); FlightPin.mode(PullUp); time_main.reset(); time_main.start(); startRecSlow(); CanSat_phase = CANSAT_SAFETY; while(1) { readSensor(); modeChange(); //状態遷移の判断 controllAttitude(); if(save_fast){ recData(); } } } /******************************************************************************* 状態遷移の判断 無線あり:HEADER_START *******************************************************************************/ void modeChange(){ switch(CanSat_phase){ case CANSAT_SAFETY:////////////////////安全モード if(!do_first){ es920_using = true; es920 << HEADER_START; es920 << 'S'; es920.send(); es920_using = false; do_first = true; } break; case CANSAT_WAIT:////////////////////待機モード if(!do_first){ es920_using = true; es920 << HEADER_START; es920 << 'W'; es920.send(); es920_using = false; do_first = true; } if(flight_pin){ CanSat_phase = CANSAT_FLIGHT; do_first = false; } break; case CANSAT_FLIGHT:////////////////////フライトモード if(!do_first){ es920_using = true; es920 << HEADER_START; es920 << 'F'; es920.send(); es920_using = false; if(save_slow && !save_fast){ stopRec(); startRecFast(); } else if(!save_slow && !save_fast){ startRecFast(); } time_flight.reset(); time_flight.start(); alt_flight_start = alt; timeout_start_controll.attach(&startControllAttitude, START_CONTROLL_TIME); timeout_recovery.attach(&changePhase_RECOVERY, RECOVERY_TIME); do_first = true; } if(alt - alt_flight_start < START_CONTROLL_ALT && !start_controll_once){ timeout_start_controll.detach(); startControllAttitude(); start_controll_once = true; } /* if(speed_ave > RECOVERY_SPEED && alt < 5.0f){ timeout_recovery.detach(); changePhase_RECOVERY(); } */ break; case CANSAT_RECOVERY: if(!do_first){ es920_using = true; es920 << HEADER_START; es920 << 'R'; es920.send(); es920_using = false; stopRec(); startRecSlow(); controll_yn = false; Buzzer.fire_on(); do_first = true; } break; } } void changePhase_RECOVERY(){ CanSat_phase = CANSAT_RECOVERY; do_first = false; } void startControllAttitude(){ controll_yn = true; } /******************************************************************************* 姿勢制御 *******************************************************************************/ void controllAttitude(){ if(controll_yn){ if(euler[1] > 10){ Valve1.fire_on(); Valve3.fire_off(); } else if(euler[1] < -10){ Valve1.fire_off(); Valve3.fire_on(); } else{ Valve1.fire_off(); Valve3.fire_off(); } } else{ Valve1.fire_off(); Valve2.fire_off(); Valve3.fire_off(); Valve4.fire_off(); } } /******************************************************************************* センサー読み取り *******************************************************************************/ void readSensor(){ time_read = time_main.read_ms(); time_min = time_reset * 30 + floor((double)time_read / (60 * 1000)); time_sec = time_read % (60 * 1000); time_sec = floor((double)time_sec / 1000); time_ms = time_read % 1000; if(time_read >= 30*60*1000){ time_main.reset(); time_reset ++; } if(CanSat_phase >= CANSAT_FLIGHT){ flight_time_read_old = flight_time_read; flight_time_read = time_flight.read_ms(); } else{ flight_time_read = 0; } flight_time_min = flight_time_reset * 30 + floor((double)flight_time_read / (60 * 1000)); flight_time_sec = flight_time_read % (60 * 1000); flight_time_sec = floor((double)flight_time_sec / 1000); flight_time_ms = flight_time_read % 1000; if(time_read >= 30*60*1000){ time_flight.reset(); flight_time_reset ++; } ADXL375.getOutput(adxl_acc); INA226.get_Voltage_current(&ina_v, &ina_i); ina_v = ina_v / 1000; ina_i = ina_i / 1000; if(ina_i > 64){ ina_i = 0.0f; } BME280.getData(&temp, &pres_now, &hum); if(pres_now != pres){ pres = pres_now; alt = BME280.getAlt2(pres_0, temp_0); alt_buf[alt_count] = alt; alt_count ++; if(alt_count > 10){ alt_count = 0; } float alt_sum = 0; for(int i = 0; i < 10; i ++){ alt_sum += alt_buf[i]; } alt_ave_old = alt_ave; alt_ave = alt_sum / 10; if(fabs(alt_ave - alt_ave_old) > 0.01){ speed = (alt_ave - alt_ave_old) / (((float)flight_time_read - (float)speed_time_old) / 1000); } if(CanSat_phase <= CANSAT_WAIT){ speed = 0.0f; } speed_buf[speed_count] = speed; speed_count ++; if(speed_count > 10){ speed_count = 0; } float speed_sum = 0; for(int i = 0; i < 10; i ++){ speed_sum = speed_buf[i]; } speed_ave = speed_sum / 10; speed_time_old = flight_time_read; } if(CanSat_phase < CANSAT_WAIT){ pres_0 = pres; temp_0 = temp; } BNO055.getAMG(amg); BNO055.getQuart(quart); BNO055.getEulerFromQ(euler); CCS811.getData(&CO2, &TVOCs); bottle_pres_bit = ADS1115.readADC_SingleEnded(0); bottle_pres_bit = (float)bottle_pres_bit * 0.125f * 3.0f / 2.0f; bottle_pres = (float)bottle_pres_bit/1000.0f / 4.0f - 0.25f; flight_pin = FlightPin; } /******************************************************************************* GPS読み取り *******************************************************************************/ void readGPS(){ gps_yn = GPS.gps_readable; if(gps_yn){ gps_lat = GPS.Latitude(); gps_lon = GPS.Longitude(); gps_alt = GPS.Height(); GPS.getUTC(gps_utc); gps_utc[3] += 9; if(gps_utc[3] >= 24){ gps_utc[3] -= 24; gps_utc[2] += 1; } gps_knot = GPS.Knot(); gps_deg = GPS.Degree(); gps_sat = GPS.Number(); } } /******************************************************************************* データ表示 *******************************************************************************/ void printData(){ if(ok_PC){ pc.printf("Time : %d:%02d\r\n", time_min, time_sec); pc.printf("FlightTime : %d:%02d\r\n", flight_time_min, flight_time_sec); pc.printf("Phase : "); switch(CanSat_phase){ case CANSAT_SETUP: pc.printf("SETUP\r\n"); break; case CANSAT_SAFETY: pc.printf("SAFETY\r\n"); break; case CANSAT_WAIT: pc.printf("WAIT\r\n"); break; case CANSAT_FLIGHT: pc.printf("FLIGHT\r\n"); break; case CANSAT_RECOVERY: pc.printf("RECOVERY\r\n"); break; } pc.printf("SAVE : "); if(save_slow){ pc.printf("SLOW\r\n"); } else if(save_fast){ pc.printf("FAST\r\n"); } else{ pc.printf("STOP\r\n"); } pc.printf("Controll : %d\r\n", controll_yn); pc.printf("Flight Pin : %d\r\n", flight_pin); pc.printf("Valve: %d, %d, %d, %d\r\n", Valve1.status, Valve2.status, Valve3.status, Valve4.status); pc.printf("Buzzer : %d\r\n", Buzzer.status); pc.printf("GPS : %3.7f, %3.7f, %.2f\r\n", gps_lat, gps_lon, gps_alt); pc.printf("Move : %.2f[knot], %.2f[deg]\r\n", gps_knot, gps_deg); pc.printf("Sats : %d\r\n", gps_sat); pc.printf("INA226 : %.2f[V], %.2f[A]\r\n", ina_v, ina_i); pc.printf("BME280 : %.2f[hPa], %.2f[degC], %.2f[%%], %.2f[m], %.2f[m/s]\r\n", pres, temp, hum, alt_ave, speed_ave); pc.printf("BNO055 : %f, %f, %f\r\n", euler[0], euler[1], euler[2]); pc.printf("CCS811 : %d[ppm], %d[ppm]\r\n", CO2, TVOCs); pc.printf("Bottle : %f[MPa]\r\n", bottle_pres); pc.printf("\n\n\n"); } } /******************************************************************************* PC読み取り *******************************************************************************/ void readPC(){ if(ok_PC){ char command = pc.getc(); pc.printf("PC Command : %c\r\n", command); switch(command){ case '?': //ヘルプ表示 printHelp(); break; case 'W': //投下待機モードへ移行 if(CanSat_phase == CANSAT_SAFETY){ CanSat_phase = CANSAT_WAIT; do_first = false; } break; case 'S': //安全モードへ移行 if(CanSat_phase == CANSAT_WAIT || CanSat_phase == CANSAT_FLIGHT){ CanSat_phase = CANSAT_SAFETY; do_first = false; } break; case 'F': //フライトモードへ移行 if(CanSat_phase == CANSAT_WAIT){ CanSat_phase = CANSAT_FLIGHT; do_first = false; } break; case 'C': //記録停止 if((CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY) && (save_slow || save_fast)){ stopRec(); } break; case 'o': //低速記録開始 if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){ startRecSlow(); } break; case 'O': //高速記録開始 if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){ startRecFast(); } break; case 'B': //ブザー切り替え buzzerChange(); break; case '1': //姿勢制御再開 if(CanSat_phase == CANSAT_FLIGHT && !controll_yn){ controll_yn = true; } break; case '0': //姿勢制御停止 if(CanSat_phase == CANSAT_FLIGHT && controll_yn){ controll_yn = false; } break; } } } /******************************************************************************* ヘルプ表示 *******************************************************************************/ void printHelp(){ pc.printf("\r\n********************\r\n"); pc.printf("Commands\r\n"); pc.printf("W : Safety -> Wait\r\n"); pc.printf("S : Wait -> Safety\r\n"); pc.printf("F : Wait -> Flight\r\n"); pc.printf("C : Stop Recording\r\n"); pc.printf("o(small o) : Start Recording (Slow)\r\n"); pc.printf("O(large o) : Start Recording (Fast)\r\n"); pc.printf("B : Buzzer Change\r\n"); pc.printf("1(one) : Restart Controll Attitude\r\n"); pc.printf("0(zero) : Stop Controll Attitude\r\n"); pc.printf("********************\r\n\n"); wait(1.0f); } /******************************************************************************* ダウンリンク送信 無線あり:HEADER_DATA *******************************************************************************/ void sendDownLink(){ if(!es920_using){ es920 << HEADER_DATA; es920 << (short)time_reset; es920 << (short)(time_read / 1000); es920 << (short)flight_time_reset; es920 << (short)(flight_time_read / 1000); es920 << (char)CanSat_phase; char status = 0x00; if(save_slow) status |= (char)0x01; if(save_fast) status |= (char)0x02; if(controll_yn) status |= (char)0x04; if(flight_pin) status |= (char)0x08; if(gps_yn) status |= (char)0x10; if(Buzzer.status) status |= (char)0x20; es920 << status; status = 0x00; if(Valve1.status) status |= (char)0x01; if(Valve2.status) status |= (char)0x02; if(Valve3.status) status |= (char)0x04; if(Valve4.status) status |= (char)0x08; es920 << status; es920 << (float)gps_lat; es920 << (float)gps_lon; es920 << (short)(gps_alt / ES920_MAX_1500); es920 << (short)(gps_knot / ES920_MAX_200); es920 << (short)(gps_deg / ES920_MAX_360); es920 << (short)gps_sat; es920 << (short)(ina_v / ES920_MAX_20); es920 << (short)(ina_i / ES920_MAX_20); es920 << (short)(pres / ES920_MAX_1500); es920 << (short)(temp / ES920_MAX_100); es920 << (short)(alt / ES920_MAX_500); es920 << (short)(speed / ES920_MAX_50); es920 << (short)(euler[0] / ES920_MAX_360); es920 << (short)(euler[1] / ES920_MAX_360); es920 << (short)(euler[2] / ES920_MAX_360); es920 << (short)CO2; es920 << (short)(bottle_pres / ES920_MAX_2); es920.send(); } } /******************************************************************************* アップリンク解析 無線あり:HEADER_COMMAND(受信) *******************************************************************************/ void readUpLink(){ es920_uplink_command = es920.data[0]; pc.printf("GND Command = %c\r\n", es920_uplink_command); switch(es920_uplink_command){ case 'W': //投下待機モードへ移行 if(CanSat_phase == CANSAT_SAFETY){ CanSat_phase = CANSAT_WAIT; do_first = false; } break; case 'S': //安全モードへ移行 if(CanSat_phase == CANSAT_WAIT || CanSat_phase == CANSAT_FLIGHT){ CanSat_phase = CANSAT_SAFETY; do_first = false; } break; case 'F': //フライトモードへ移行 if(CanSat_phase == CANSAT_WAIT){ CanSat_phase = CANSAT_FLIGHT; do_first = false; } break; case 'C': //記録停止 if((CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY) && (save_slow || save_fast)){ stopRec(); } break; case 'o': //低速記録開始 if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){ startRecSlow(); } break; case 'O': //高速記録開始 if(CanSat_phase == CANSAT_RECOVERY || CanSat_phase == CANSAT_SAFETY && (!save_slow && !save_fast)){ startRecFast(); } break; case 'B': //ブザー切り替え buzzerChange(); break; case '1': //姿勢制御再開 if(CanSat_phase == CANSAT_FLIGHT && !controll_yn){ controll_yn = true; } break; case '0': //姿勢制御停止 if(CanSat_phase == CANSAT_FLIGHT && controll_yn){ controll_yn = false; } break; } } /******************************************************************************* データを1秒ごとに書き込み開始 *******************************************************************************/ void startRecSlow(){ if(!save_slow && !save_fast){ fp = fopen(file_name, "a"); fprintf(fp, "Time,Flight Time,Phase,Controll,Flight Pin,"); fprintf(fp, "Valve1,Valve2,Valve3,Valve4,Buzzer,"); fprintf(fp, "GPS Y/N,GPS Time,Latitude,Longitude,GPS Alt,Knot,Deg,Sattelite,"); fprintf(fp, "ADXL_x[G],ADXL_y[G],ADXL_z[G],Battery_V[V],Battery_I[A],"); fprintf(fp, "Pres[hPa],Temp[degC],Hum[%%],Alt[m],Alt_ave[m],Speed[m/s],Speed_ave[m/s],"); fprintf(fp, "Acc_x[m/s^2],Acc_y[m/s^2],Acc_z[m/s^2],Mag_x[uT],Mag_y[uT],Mag_z[uT],Gyro_x[deg/s],Gyro_y[deg/s],Gyro_z[deg/s],"); fprintf(fp, "Q_w,Q_x,Q_y,Q_z,Yaw[deg],Roll[deg],Pitch[deg],"); fprintf(fp, "CO2[ppm],TVOCs[ppm],"); fprintf(fp, "Bottle Pres[MPa]\r\n"); tick_save.detach(); tick_save.attach(&recData, 1.0f); save_slow = true; } } /******************************************************************************* データを最速で書き込み開始 *******************************************************************************/ void startRecFast(){ if(!save_slow && !save_fast){ fp = fopen(file_name, "a"); fprintf(fp, "Time,Flight Time,Phase,Controll,Flight Pin,"); fprintf(fp, "Valve1,Valve2,Valve3,Valve4,Buzzer,Uplink"); fprintf(fp, "GPS Y/N,GPS Time,Latitude,Longitude,GPS Alt,Knot,Deg,Sattelite,"); fprintf(fp, "ADXL_x[G],ADXL_y[G],ADXL_z[G],Battery_V[V],Battery_I[A],"); fprintf(fp, "Pres[hPa],Temp[degC],Hum[%%],Alt[m],Alt_ave[m],Speed[m/s],Speed_ave[m/s],"); fprintf(fp, "Acc_x[m/s^2],Acc_y[m/s^2],Acc_z[m/s^2],Mag_x[uT],Mag_y[uT],Mag_z[uT],Gyro_x[deg/s],Gyro_y[deg/s],Gyro_z[deg/s],"); fprintf(fp, "Q_w,Q_x,Q_y,Q_z,Yaw[deg],Roll[deg],Pitch[deg],"); fprintf(fp, "CO2[ppm],TVOCs[ppm],"); fprintf(fp, "Bottle Pres[MPa]\r\n"); tick_save.detach(); save_fast = true; } } /******************************************************************************* データ記録停止 *******************************************************************************/ void stopRec(){ if(save_slow){ save_slow = false; tick_save.detach(); fprintf(fp, "\r\n\n"); fclose(fp); } else if(save_fast){ save_fast = false; fprintf(fp, "\r\n\n"); fclose(fp); } } /******************************************************************************* データの書き込み *******************************************************************************/ void recData(){ if(save_slow || save_fast){ fprintf(fp, "%d:%02d.%03d,", time_min, time_sec, time_ms); fprintf(fp, "%d:%02d.%03d,", flight_time_min, flight_time_sec, flight_time_ms); switch(CanSat_phase){ case CANSAT_SETUP: fprintf(fp, "SETUP,"); break; case CANSAT_SAFETY: fprintf(fp, "SAFETY,"); break; case CANSAT_WAIT: fprintf(fp, "WAIT,"); break; case CANSAT_FLIGHT: fprintf(fp, "FLIGHT,"); break; case CANSAT_RECOVERY: fprintf(fp, "RECOVERY,"); break; } fprintf(fp, "%d,", controll_yn); fprintf(fp, "%d,", flight_pin); fprintf(fp, "%d,%d,%d,%d,%d,", Valve1.status, Valve2.status, Valve3.status, Valve4.status, Buzzer.status); fprintf(fp, "%c,", es920_uplink_command); es920_uplink_command = '-'; fprintf(fp, "%d,", gps_yn); fprintf(fp, "%d/%d/%d ", (int)gps_utc[0], (int)gps_utc[1], (int)gps_utc[2]); //日付 fprintf(fp, "%d:%02d:%02.2f,", (int)gps_utc[3], (int)gps_utc[4], gps_utc[5]); //時間 fprintf(fp, "%3.7f,%3.7f,%.2f,", gps_lat, gps_lon, gps_alt); //GPS座標 fprintf(fp, "%.2f,%.2f,", gps_knot, gps_deg); //GPS速度 fprintf(fp, "%d,", gps_sat); //GPS衛星数 fprintf(fp, "%.2f,%.2f,%.2f,%f,%f,", adxl_acc[0], adxl_acc[1], adxl_acc[2], ina_v, ina_i); fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,", pres, temp, hum, alt, alt_ave, speed, speed_ave); fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f,", amg[0], amg[1], amg[2], amg[3], amg[4], amg[5], amg[6], amg[7], amg[8]); fprintf(fp, "%f,%f,%f,%f,", quart[0], quart[1], quart[2], quart[3]); fprintf(fp, "%.2f,%.2f,%.2f,", euler[0], euler[1], euler[2]); fprintf(fp, "%d,%d,", CO2, TVOCs); fprintf(fp, "%f\r\n", bottle_pres); /////////////////////////////////////////////////////////////////////EEPROM EEPROM.chargeBuff((short)time_reset); EEPROM.chargeBuff((int)time_read); EEPROM.chargeBuff((short)flight_time_reset); EEPROM.chargeBuff((int)flight_time_read); EEPROM.chargeBuff((char)CanSat_phase); char status = 0x00; if(controll_yn) status |= 0x01; if(flight_pin) status |= 0x02; if(Valve1.status) status |= 0x04; if(Valve2.status) status |= 0x08; if(Valve3.status) status |= 0x10; if(Valve4.status) status |= 0x20; if(Buzzer.status) status |= 0x40; EEPROM.chargeBuff((char)status); EEPROM.chargeBuff((char)es920_uplink_command); EEPROM.chargeBuff((char)gps_yn); EEPROM.chargeBuff((float)gps_lat); EEPROM.chargeBuff((float)gps_lon); EEPROM.chargeBuff((float)gps_alt); EEPROM.chargeBuff((float)gps_knot); EEPROM.chargeBuff((float)gps_deg); EEPROM.chargeBuff((short)gps_sat); EEPROM.chargeBuff((float)adxl_acc[0]); EEPROM.chargeBuff((float)adxl_acc[1]); EEPROM.chargeBuff((float)adxl_acc[2]); EEPROM.chargeBuff((float)ina_v); EEPROM.chargeBuff((float)ina_i); EEPROM.chargeBuff((float)pres); EEPROM.chargeBuff((float)temp); EEPROM.chargeBuff((float)hum); EEPROM.chargeBuff((float)alt); EEPROM.chargeBuff((float)speed); EEPROM.chargeBuff((short)(amg[0] / ES920_MAX_20)); EEPROM.chargeBuff((short)(amg[1] / ES920_MAX_20)); EEPROM.chargeBuff((short)(amg[2] / ES920_MAX_20)); EEPROM.chargeBuff((short)(amg[3] / ES920_MAX_20)); EEPROM.chargeBuff((short)(amg[4] / ES920_MAX_20)); EEPROM.chargeBuff((short)(amg[5] / ES920_MAX_20)); EEPROM.chargeBuff((short)(amg[6] / ES920_MAX_1500)); EEPROM.chargeBuff((short)(amg[7] / ES920_MAX_1500)); EEPROM.chargeBuff((short)(amg[8] / ES920_MAX_1500)); EEPROM.chargeBuff((float)quart[0]); EEPROM.chargeBuff((float)quart[1]); EEPROM.chargeBuff((float)quart[2]); EEPROM.chargeBuff((float)quart[3]); EEPROM.chargeBuff((short)(euler[0] / ES920_MAX_360)); EEPROM.chargeBuff((short)(euler[1] / ES920_MAX_360)); EEPROM.chargeBuff((short)(euler[2] / ES920_MAX_360)); EEPROM.chargeBuff((short)CO2); EEPROM.chargeBuff((short)TVOCs); EEPROM.chargeBuff((float)bottle_pres); EEPROM.writeBuff(); eeprom_ptr = EEPROM.setNextPage(); } } /******************************************************************************* ブザーのオンオフ切り替え *******************************************************************************/ void buzzerChange(){ if(Buzzer.status){ Buzzer.fire_off(); } else{ Buzzer.fire_on(); } } /******************************************************************************* セットアップ(最初に1回実行) 無線あり:HEADER_SETUP_SENSOR 無線あり:HEADER_GPS_SETUP *******************************************************************************/ void setup(){ wait(1.0f); char setup_string[1024]; pc.printf("\r\n******************************\r\n"); pc.printf("Sensor Setting Start!!\r\n"); strcat(setup_string, "Sensor Setting Start!!\r\n"); es920 << HEADER_SENSOR_SETUP; //////////////////////////////////////////////////ADXL375 ADXL375.setDataRate(ADXL375_100HZ); if(ADXL375.whoAmI() == 1){ pc.printf("ADXL375 : OK\r\n"); strcat(setup_string, "ADXL375 : OK\r\n"); es920 << (char)0x01; } else{ pc.printf("ADXL375 : NG\r\n"); strcat(setup_string, "ADXL375 : NG\r\n"); es920 << (char)0x00; } ADXL375.offset(0.0f, 0.0f, 0.0f); //////////////////////////////////////////////////INA226 INA226.set_callibretion(); if(INA226.Connection_check() == 0){ pc.printf("INA226 : OK\r\n"); strcat(setup_string, "INA226 : OK!!\r\n"); es920 << (char)0x01; } else{ pc.printf("INA226 : NG\r\n"); strcat(setup_string, "INA226 : NG\r\n"); es920 << (char)0x00; } //////////////////////////////////////////////////BME280 BME280.configMeasure(BME280_lib::NORMAL, 1, 2, 1); BME280.configFilter(4); if(BME280.connectCheck() == 1){ pc.printf("BME280 OK!!\r\n"); strcat(setup_string, "BME280 : OK!!\r\n"); es920 << (char)0x01; } else{ pc.printf("BME280 NG\r\n"); strcat(setup_string, "BME280 : NG\r\n"); es920 << (char)0x00; } //////////////////////////////////////////////////BNO055 BNO055.setOperationMode(BNO055_lib::CONFIG); BNO055.setAccRange(BNO055_lib::_16G); BNO055.setAxis(BNO055_lib::Z, BNO055_lib::X, BNO055_lib::Y); BNO055.setAxisPM(1, 1, -1); if(BNO055.connectCheck() == 1){ pc.printf("BNO055 OK!!\r\n"); strcat(setup_string, "BNO055 : OK!!\r\n"); es920 << (char)0x01; } else{ pc.printf("BNO055 NG\r\n"); strcat(setup_string, "BNO055 : NG\r\n"); es920 << (char)0x00; } BNO055.setOperationMode(BNO055_lib::NDOF); //////////////////////////////////////////////////CCS811 if(CCS811.connectCheck() == 1){ pc.printf("CCS811 OK!!\r\n"); strcat(setup_string, "CCS811 : OK!!\r\n"); es920 << (char)0x01; } else{ pc.printf("CCS811 NG\r\n"); strcat(setup_string, "CCS811 : NG\r\n"); es920 << (char)0x00; } //////////////////////////////////////////////////圧力センサ pc.printf("PRES : "); ADS1115.setGain(GAIN_TWOTHIRDS); pc.printf("OK!!\r\n"); strcat(setup_string, "PRES : OK!!\r\n"); es920 << (char)0x01; //////////////////////////////////////////////////SD fp = fopen("/sd/Space_SWAN_setup.txt", "r"); if(fp != NULL){ pc.printf("SD : OK!!\r\n"); strcat(setup_string, "SD : OK!!\r\n"); es920 << (char)0x01; fclose(fp); } else{ pc.printf("SD : NG...\r\n"); strcat(setup_string, "SD : NG...\r\n"); es920 << (char)0x00; } pc.printf("Sensor Setting Finished!!\r\n"); pc.printf("******************************\r\n"); strcat(setup_string, "Sensor Setting Finished!!\r\n"); es920.send(); wait(1.0f); pc.printf("\r\n******************************\r\n"); if(wait_GPS){ pc.printf("GPS Setting Start!!\r\n"); strcat(setup_string, "GPS Setting Start!!\r\n"); pc.printf("Waiting : 0"); strcat(setup_string, "Wait : "); int gps_wait_count = 0; while(!GPS.gps_readable){ pc.printf("\rWaiting : %d", gps_wait_count);// es920 << HEADER_GPS_SETUP; es920 << (char)GPS.gps_readable; es920 << (int)gps_wait_count; es920.send(); wait(1.0f); gps_wait_count ++; } char ss[8]; sprintf(ss, "%d", gps_wait_count); strcat(setup_string, ss); pc.printf(" OK!!\r\n"); strcat(setup_string, " OK!!\r\n"); pc.printf("GPS Setting Finished!!\r\n"); strcat(setup_string, "GPS Setting Finished!!\r\n"); es920 << HEADER_GPS_SETUP; es920 << (char)0xAA; es920 << (int)0; es920.send(); } else{ pc.printf("GPS Setting Ignore...\r\n"); strcat(setup_string, "GPS Setting Ignore...\r\n"); es920 << HEADER_GPS_SETUP; es920 << (char)0xFF; es920 << (int)0; es920.send(); } pc.printf("******************************\r\n"); fp = fopen(file_name, "a"); fprintf(fp, setup_string); fclose(fp); setup_string[0] = NULL; EEPROM.setWriteAddr(1, 0, 0x00, 0x00); printHelp(); wait(1.0f); }