PQ_Hybrid_Electrical_Equipment_Team
高高度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
--- 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");