高高度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
Diff: main.cpp
- Revision:
- 7:3a2d0474f1ca
- Parent:
- 6:f2016544b9e4
- Child:
- 8:2ce2c7eebb92
diff -r f2016544b9e4 -r 3a2d0474f1ca main.cpp --- a/main.cpp Sat Feb 23 11:50:05 2019 +0000 +++ b/main.cpp Tue Mar 12 14:12:31 2019 +0000 @@ -18,14 +18,14 @@ 設定値 投下前に必ず確認!! *******************************************************************************/ -const float START_CONTROLL_TIME = 1.0f; +const float START_CONTROLL_TIME = 10.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; +bool wait_GPS = false; +bool ok_PC = true; /******************************************************************************* コンストラクタ @@ -107,6 +107,7 @@ char CanSat_phase; bool do_first = false; bool controll_yn = false; +bool start_controll_once = false; bool es920_using = false; char es920_uplink_command = '-'; @@ -152,14 +153,14 @@ /******************************************************************************* 定数 *******************************************************************************/ -const float ES920_MAX_2 = 0.0000305180; -const float ES920_MAX_5 = 0.0000762951; -const float ES920_MAX_20 = 0.0003051804; -const float ES920_MAX_50 = 0.0007629511; -const float ES920_MAX_100 = 0.0015259022; -const float ES920_MAX_200 = 0.0030518044; -const float ES920_MAX_500 = 0.0076295109; -const float ES920_MAX_1500 = 0.0228885328; +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; /******************************************************************************* @@ -187,7 +188,8 @@ // 0x02 : save_fast // 0x04 : controll_yn // 0x08 : flight_pin -// 0x10 : buzzer +// 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 @@ -314,11 +316,12 @@ do_first = true; } - if(alt - alt_flight_start > START_CONTROLL_ALT){ + 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){ + if(speed_ave > RECOVERY_SPEED && alt < 5.0f){ timeout_recovery.detach(); changePhase_RECOVERY(); } @@ -442,7 +445,7 @@ speed_time_old = flight_time_read; } - if(CanSat_phase <= CANSAT_WAIT){ + if(CanSat_phase < CANSAT_WAIT){ pres_0 = pres; temp_0 = temp; } @@ -644,22 +647,24 @@ if(!es920_using){ es920 << HEADER_DATA; - es920 << (unsigned short)time_reset; - es920 << (unsigned short)(time_read / 1000); + es920 << (short)time_reset; + es920 << (short)(time_read / 1000); - es920 << (unsigned short)flight_time_reset; - es920 << (unsigned short)(flight_time_read / 1000); + es920 << (short)flight_time_reset; + es920 << (short)(flight_time_read / 1000); - es920 << CanSat_phase; + 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(Buzzer.status) status |= (char)0x10; + 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; @@ -668,27 +673,26 @@ es920 << (float)gps_lat; es920 << (float)gps_lon; - es920 << (unsigned short)(gps_alt / ES920_MAX_1500); - es920 << (unsigned short)(gps_knot / ES920_MAX_200); - es920 << (unsigned short)(gps_deg / ES920_MAX_500); - es920 << (unsigned short)gps_sat; + 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 << (unsigned short)(ina_v / ES920_MAX_20); - es920 << (unsigned short)(ina_i / ES920_MAX_20); + es920 << (short)(ina_v / ES920_MAX_20); + es920 << (short)(ina_i / ES920_MAX_20); - es920 << (unsigned short)(pres / ES920_MAX_1500); - es920 << (unsigned short)(temp / ES920_MAX_100); + 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_5); - es920 << (short)(euler[1] / ES920_MAX_5); - es920 << (short)(euler[2] / ES920_MAX_5); + es920 << (short)(euler[0] / ES920_MAX_360); + es920 << (short)(euler[1] / ES920_MAX_360); + es920 << (short)(euler[2] / ES920_MAX_360); - es920 << (unsigned short)CO2; - es920 << (unsigned short)TVOCs; + es920 << (short)CO2; - es920 << (unsigned short)(bottle_pres / ES920_MAX_2); + es920 << (short)(bottle_pres / ES920_MAX_2); es920.send(); } @@ -791,7 +795,7 @@ 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, "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],"); @@ -827,106 +831,108 @@ データの書き込み *******************************************************************************/ void recData(){ - 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; + 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); - case CANSAT_SAFETY: - fprintf(fp, "SAFETY,"); - break; + 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); - case CANSAT_WAIT: - fprintf(fp, "WAIT,"); - break; - - case CANSAT_FLIGHT: - fprintf(fp, "FLIGHT,"); - break; - - case CANSAT_RECOVERY: - fprintf(fp, "RECOVERY,"); - break; + /////////////////////////////////////////////////////////////////////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(); } - - 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((unsigned short)time_reset); - EEPROM.chargeBuff((int)time_read); - EEPROM.chargeBuff((unsigned 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((unsigned 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_50)); - EEPROM.chargeBuff((short)(amg[1] / ES920_MAX_50)); - EEPROM.chargeBuff((short)(amg[2] / ES920_MAX_50)); - EEPROM.chargeBuff((short)(amg[3] / ES920_MAX_50)); - EEPROM.chargeBuff((short)(amg[4] / ES920_MAX_50)); - EEPROM.chargeBuff((short)(amg[5] / ES920_MAX_50)); - EEPROM.chargeBuff((short)(amg[6] / ES920_MAX_50)); - EEPROM.chargeBuff((short)(amg[7] / ES920_MAX_50)); - EEPROM.chargeBuff((short)(amg[8] / ES920_MAX_50)); - 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_500)); - EEPROM.chargeBuff((short)(euler[1] / ES920_MAX_500)); - EEPROM.chargeBuff((short)(euler[2] / ES920_MAX_500)); - EEPROM.chargeBuff((unsigned short)CO2); - EEPROM.chargeBuff((unsigned short)TVOCs); - EEPROM.chargeBuff((float)bottle_pres); - EEPROM.writeBuff(); - eeprom_ptr = EEPROM.setNextPage(); } @@ -938,7 +944,7 @@ Buzzer.fire_off(); } else{ - Buzzer.fire_off(); + Buzzer.fire_on(); } } @@ -1001,7 +1007,7 @@ //////////////////////////////////////////////////BNO055 BNO055.setOperationMode(BNO055_lib::CONFIG); BNO055.setAccRange(BNO055_lib::_16G); - BNO055.setAxis(BNO055_lib::Z, BNO055_lib::Y, BNO055_lib::X); + 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");