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:
- 2:a443df6115bc
- Parent:
- 1:6dea30c8b406
- Child:
- 3:4571111a5996
--- a/main.cpp Tue Feb 19 15:56:55 2019 +0000
+++ b/main.cpp Wed Feb 20 13:33:16 2019 +0000
@@ -19,6 +19,7 @@
投下前に必ず確認！！
*******************************************************************************/
bool wait_GPS = true;
+bool ok_PC = false;
/*******************************************************************************
コンストラクタ
@@ -45,10 +46,10 @@
Adafruit_ADS1115 ADS1115(&i2c_bus, ADS1015_ADDRESS);
-DigitalOut valve1(p23);
-DigitalOut valve2(p24);
-DigitalOut valve3(p24);
-DigitalOut valve4(p25);
+DigitalOut Valve1(p23);
+DigitalOut Valve2(p24);
+DigitalOut Valve3(p24);
+DigitalOut Valve4(p25);
DigitalOut Buzzer(p22);
DigitalIn FlightPin(p15);
@@ -72,6 +73,8 @@
void modeChange(); //無線あり
+void controllAttitude();
+
void readSensor();
void readGPS();
@@ -87,12 +90,18 @@
void stopRec();
void recData();
+void buzzerChange();
+
/*******************************************************************************
変数の宣言
*******************************************************************************/
char CanSat_phase;
bool do_first = false;
+bool controll_yn = false;
+
+bool valve1 = false, valve2 = false, valve3 = false, valve4 = false;
+bool buzzer = false;
bool es920_using = false;
char es920_uplink_command = '-';
@@ -135,6 +144,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;
/*******************************************************************************
@@ -204,6 +221,7 @@
readSensor();
recData();
modeChange(); //状態遷移の判断
+ controllAttitude();
}
}
@@ -213,6 +231,37 @@
無線あり：HEADER_START
*******************************************************************************/
void modeChange(){
+ switch(CanSat_phase){
+ case CANSAT_SAFETY:
+ if(!do_first){
+
+ }
+ break;
+
+ case CANSAT_WAIT:
+ if(!do_first){
+
+ }
+ break;
+
+ case CANSAT_FLIGHT:
+ if(!do_first){
+
+ }
+ break;
+
+ case CANSAT_RECOVERY:
+ if(!do_first){
+
+ }
+ break;
+ }
+}
+
+/*******************************************************************************
+姿勢制御
+*******************************************************************************/
+void controllAttitude(){
}
@@ -308,6 +357,8 @@
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;
}
@@ -337,7 +388,58 @@
データ表示
*******************************************************************************/
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, valve2, valve3, valve4);
+ pc.printf("Buzzer : %d\r\n", buzzer);
+
+ 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");
+ }
}
@@ -345,7 +447,71 @@
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;
+ }
+ }
}
@@ -353,7 +519,21 @@
ヘルプ表示
*******************************************************************************/
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);
}
@@ -362,13 +542,63 @@
無線あり：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 << 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 |= (char)0x10;
+ es920 << status;
+
+ if(valve1) status |= (char)0x01;
+ if(valve2) status |= (char)0x02;
+ if(valve3) status |= (char)0x04;
+ if(valve4) status |= (char)0x08;
+ es920 << status;
+
+ 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 << (short)gps_sat;
+
+ es920 << (unsigned short)(ina_v / ES920_MAX_20);
+ es920 << (unsigned short)(ina_i / ES920_MAX_20);
+
+ es920 << (unsigned short)(pres / ES920_MAX_1500);
+ es920 << (unsigned short)(temp / ES920_MAX_100);
+ es920 << (unsigned short)(alt / ES920_MAX_500);
+ es920 << (unsigned 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 << (unsigned short)CO2;
+ es920 << (unsigned short)TVOCs;
+
+ es920 << (unsigned short)(bottle_pres / ES920_MAX_2);
+
+ es920.send();
+ }
}
/*******************************************************************************
アップリンク受信
-無線あり：HEADER＿COMMAND
+無線あり：HEADER_COMMAND
*******************************************************************************/
void readUpLink(){
@@ -408,6 +638,14 @@
/*******************************************************************************
+ブザーのオンオフ切り替え
+*******************************************************************************/
+void buzzerChange(){
+
+}
+
+
+/*******************************************************************************
セットアップ（最初に1回実行）
無線あり：HEADER_SETUP_SENSOR
無線あり：HEADER_GPS_SETUP