PQ_Hybrid_Electrical_Equipment_Team
PQ-013 Felix-Luminousの地上局プログラム ES920LR使用
Diff: main.cpp
- Revision:
- 0:d99384e36f64
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Nov 14 09:33:58 2018 +0000
@@ -0,0 +1,491 @@
+#include "mbed.h"
+#include "ES920LR.hpp"
+
+/*******************************************************************************
+コンストラクタ
+*******************************************************************************/
+RawSerial pc(USBTX, USBRX, 115200);
+RawSerial serial_es920(D1, D0);
+
+ES920LR es920(serial_es920, pc, 115200);
+
+/*******************************************************************************
+関数のプロトタイプ宣言
+*******************************************************************************/
+void readPC();
+void printHelp();
+
+void sendUpLink(char command);
+void readHeaderSensorSetup();
+void readHeaderGPSSetup();
+void readHeaderData();
+void readHeaderStart();
+
+/*******************************************************************************
+変数の宣言
+*******************************************************************************/
+int time_read, time_reset, time_min, time_sec;
+int flight_time_read, flight_time_reset, flight_time_min, flight_time_sec;
+
+char rocket_phase;
+
+bool ex_power;
+bool separate1, separate2;
+bool ok_up, ok_down, ok_top;
+bool save;
+bool flight_pin;
+
+float gps_lat, gps_lon, gps_alt, gps_knot, gps_deg;
+int gps_sat;
+
+float ina_in_v, ina_in_i, ina_ex_v, ina_ex_i;
+
+float pres33, alt33, speed33;
+
+float temp35, hum35;
+
+float pitot_speed;
+
+int gps_wait_count;
+
+
+/*******************************************************************************
+定数
+*******************************************************************************/
+const float ES920_MAX_20 = 0.000305176;
+const float ES920_MAX_100 = 0.001525879;
+const float ES920_MAX_200 = 0.003051758;
+const float ES920_MAX_500 = 0.007629395;
+const float ES920_MAX_1500 = 0.022888184;
+const float ES920_MAX_3000 = 0.045776367;
+
+
+/*******************************************************************************
+無線のヘッダまとめ(ROCKET -> GND)
+*******************************************************************************/
+const char HEADER_SENSOR_SETUP = 0x01;
+// 0x01, ADXL, INA_in, INA_ex, MPU, MPU_m, 22HB, 33HW, SHT, TSL, SD
+// 0 1 1 1 1 1 1 1 1 1 1
+// 0 1 2 3 4 5 6 7 8 9
+
+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;
+//起動時間, フライト時間, フェーズ, ex_pow&Sep1&Sep2&ok_up&ok_down&ok_top&gps_yn&save&flight_pin, lat, lon, alt, knot, deg, sat, in_v, in_i, ex_v, ex_i, pres33, alt33, speed33, temp35, hum35, pitot_speed
+//4(2,2) 4(2,2) 1 1 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2
+//0 2 4 6 8 9 10 12 14 16 18 20 22 24 26 28 30 32 34 36 38 40
+// 0x01 : ex_pow
+// 0x02 : Sep1
+// 0x04 : Sep2
+// 0x08 : ok_up
+// 0x10 : ok_down
+// 0x20 : ok_top
+// 0x40 : save
+// 0x80 : gps_yn
+
+const char HEADER_START = 0x11;
+// 0x11, What
+// 0 1
+// 0
+// 'W' : 安全モード->離床検知モード
+// 'S' : 離床検知モード->安全モード
+// 'F' : 離床検知モード->フライトモード
+// '1' : 1段目分離
+// '2' : 2段目分離
+
+
+/*******************************************************************************
+無線のヘッダまとめ(GND -> ROCKET)
+*******************************************************************************/
+const char HEADER_COMMAND = 0xcd;
+// 0xcd,コマンド
+// 0 1
+// 0 1
+
+
+/*******************************************************************************
+ロケットのフェーズ
+*******************************************************************************/
+const char ROCKET_SETUP = 0x00; //セットアップ中
+const char ROCKET_SAFETY = 0x01; //安全モード
+const char ROCKET_WAIT = 0x02; //待機モード
+const char ROCKET_FLIGHT = 0x03; //フライトモード
+const char ROCKET_SEP1 = 0x04; //分離1モード
+const char ROCKET_SEP2 = 0x05; //分離2モード
+const char ROCKET_RECOVERY = 0x06; //回収モード
+
+
+/*******************************************************************************
+メイン関数
+*******************************************************************************/
+int main(){
+ printHelp();
+
+ pc.attach(&readPC, Serial::RxIrq);
+
+ es920.attach(&readHeaderSensorSetup, HEADER_SENSOR_SETUP);
+ es920.attach(&readHeaderGPSSetup, HEADER_GPS_SETUP);
+ es920.attach(&readHeaderData, HEADER_DATA);
+ es920.attach(&readHeaderStart, HEADER_START);
+
+ while(1){
+ }
+}
+
+
+/*******************************************************************************
+PCからの読み取り
+*******************************************************************************/
+void readPC(){
+ char ch = pc.getc();
+ pc.printf("Input : %c\r\n", ch);
+ switch(ch){
+ case 'W':
+ case 'S':
+ case 'F':
+ case '1':
+ case '2':
+ case 'C':
+ case 'B':
+ case 'O':
+ sendUpLink(ch);
+ break;
+
+ case '?':
+ printHelp();
+ break;
+ }
+}
+
+
+/*******************************************************************************
+ヘルプを表示
+*******************************************************************************/
+void printHelp(){
+ for(int i = 0; i < 20; i ++){
+ pc.printf("*");
+ }
+
+ pc.printf("\r\nCommands\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(" 1(one) : 1st Separate 5[s]\r\n");
+ pc.printf(" 2(two) : 2nd Separate 5[s]\r\n");
+ pc.printf(" C : Stop Recording\r\n");
+ pc.printf(" O : Start Recording\r\n");
+ pc.printf(" B : Buzzer On/Off\r\n");
+ pc.printf(" ? : HELP\r\n");
+
+ for(int i = 0; i < 20; i ++){
+ pc.printf("*");
+ }
+ pc.printf("\r\n");
+ wait(1.0f);
+}
+
+
+/*******************************************************************************
+アップリンクを送信する
+*******************************************************************************/
+void sendUpLink(char command){
+ es920 << HEADER_COMMAND;
+ es920 << command;
+ es920.send();
+}
+
+
+/*******************************************************************************
+HEADER_SENSOR_SETUPを受信したら呼び出される
+*******************************************************************************/
+void readHeaderSensorSetup(){
+ pc.printf("\r\n**************************************************\r\n");
+ pc.printf("Sensor Setting Start!!\r\n");
+
+ ///////////////////////////////////////////ADXL375
+ if(es920.data[0] == 0x01){
+ pc.printf("ADXL375 : OK\r\n");
+ }
+ else{
+ pc.printf("ADXL375 : NG...\r\n");
+ }
+
+ ///////////////////////////////////////////INA226_in
+ if(es920.data[1] == 0x01){
+ pc.printf("INA226_in : OK!!\r\n");
+ }
+ else{
+ pc.printf("INA226_in : NG...\r\n");
+ }
+
+ ///////////////////////////////////////////INA226_ex
+ if(es920.data[2] == 0x01){
+ pc.printf("INA226_ex OK!!\r\n");
+ }
+ else{
+ pc.printf("INA226_ex NG...\r\n");
+ }
+
+ ///////////////////////////////////////////MPU9250
+ if(es920.data[3] == 0x01){
+ pc.printf("MPU9250 : OK!!\r\n");
+ }
+ else{
+ pc.printf("MPU9250 : NG...\r\n");
+ }
+ if(es920.data[4] == 0x01){
+ pc.printf("MPU9250 MAG : OK!!\r\n");
+ }
+ else{
+ pc.printf("MPU9250 MAG : NG...\r\n");
+ }
+
+ ///////////////////////////////////////////LPS22HB
+ if(es920.data[5] == 0x01){
+ pc.printf("LPS22HB : OK!!\r\n");
+ }
+ else{
+ pc.printf("LPS22HB : NG...\r\n");
+ }
+
+ ///////////////////////////////////////////LPS33HW
+ if(es920.data[6] == 0x01){
+ pc.printf("LPS33HW : OK!!\r\n");
+ }
+ else{
+ pc.printf("LPS33HW : NG...\r\n");
+ }
+
+ ///////////////////////////////////////////SHT35
+ if(es920.data[7] == 0x01){
+ pc.printf("SHT35 : OK!!\r\n");
+ }
+ else{
+ pc.printf("SHT35 : NG...\r\n");
+ }
+
+ ///////////////////////////////////////////TSL2561
+ if(es920.data[8] == 0x01){
+ pc.printf("TSL2561 : OK!!\r\n");
+ }
+ else{
+ pc.printf("SL2561 : NG...\r\n");
+ }
+
+ ///////////////////////////////////////////SD
+ if(es920.data[9] == 0x01){
+ pc.printf("SD : OK!!\r\n");
+ }
+ else{
+ pc.printf("SD : NG...\r\n");
+ }
+
+ pc.printf("Sensor Setting Finished!!\r\n");
+ pc.printf("**************************************************\r\n");
+}
+
+
+/*******************************************************************************
+HEADER_GPS_SETUPを受信したら呼び出される
+*******************************************************************************/
+void readHeaderGPSSetup(){
+ switch(es920.data[0]){
+ case 0x00:
+ gps_wait_count = es920.toInt(1);
+ pc.printf("GPS Wait : %d\r\n", gps_wait_count);
+ break;
+
+ case 0x01:
+ pc.printf("GPS Wait Complete!!\r\n");
+ break;
+
+ case 0xAA:
+ pc.printf("GPS Setting Finished!!\r\n");
+ pc.printf("\r\n**************************************************\r\n");
+ break;
+
+ case 0xFF:
+ pc.printf("GPS Setting Ignore...\r\n");
+ pc.printf("\r\n**************************************************\r\n");
+ break;
+ }
+}
+
+
+/*******************************************************************************
+HEADER_DATAを受信したら呼び出される
+*******************************************************************************/
+void readHeaderData(){
+ int i = 0; //0
+ time_reset = (int)es920.toShort(i);
+ i += 2; //2
+ time_read = (int)es920.toShort(i);
+ i += 2; //4
+ flight_time_reset = (int)es920.toShort(i);
+ i += 2; //6
+ flight_time_read = (int)es920.toShort(i);
+ i += 2; //8
+
+ rocket_phase = es920.data[i];
+ i += 1; //9
+
+ char rocket_status = es920.data[i];
+ i += 1; //10
+ ex_power = false;
+ separate1 = false;
+ separate2 = false;
+ ok_up = false;
+ ok_down = false;
+ ok_top = false;
+ save = false;
+ flight_pin = false;
+ if(rocket_status & 0x01)ex_power = true;
+ if(rocket_status & 0x02)separate1 = true;
+ if(rocket_status & 0x04)separate2 = true;
+ if(rocket_status & 0x08)ok_up = true;
+ if(rocket_status & 0x10)ok_down = true;
+ if(rocket_status & 0x20)ok_top = true;
+ if(rocket_status & 0x40)save = true;
+ if(rocket_status & 0x80)flight_pin = true;
+
+ gps_lat = (float)es920.toShort(i) * ES920_MAX_100;
+ i += 2; //12
+ gps_lon = (float)es920.toShort(i) * ES920_MAX_500;
+ i += 2; //14
+ gps_alt = (float)es920.toShort(i) * ES920_MAX_500;
+ i += 2; //16
+ gps_knot = (float)es920.toShort(i) * ES920_MAX_200;
+ i += 2; //18
+ gps_deg = (float)es920.toShort(i) * ES920_MAX_1500;
+ i += 2; //20
+ gps_sat = (int)es920.toShort(i);
+ i += 2; //22
+ ina_in_v = (float)es920.toShort(i) * ES920_MAX_20;
+ i += 2; //24
+ ina_in_i = (float)es920.toShort(i) * ES920_MAX_20;
+ i += 2; //26
+ ina_ex_v = (float)es920.toShort(i) * ES920_MAX_20;
+ i += 2; //28
+ ina_ex_i = (float)es920.toShort(i) * ES920_MAX_20;
+ i += 2; //30
+ pres33 = (float)es920.toShort(i) * ES920_MAX_3000;
+ i += 2; //32
+ alt33 = (float)es920.toShort(i) * ES920_MAX_500;
+ i += 2; //34
+ speed33 = (float)es920.toShort(i) * ES920_MAX_100;
+ i += 2; //36
+ temp35 = (float)es920.toShort(i) * ES920_MAX_100;
+ i += 2; //38
+ hum35 = (float)es920.toShort(i) * ES920_MAX_200;
+ i += 2; //40
+ pitot_speed = (float)es920.toShort(i) * ES920_MAX_200;
+
+
+ time_min = time_reset * 30 + (float)floor((double)time_read / 60);
+ time_sec = time_read % 60;
+ flight_time_min = flight_time_reset * 30 + floor((double)flight_time_read / 60);
+ flight_time_sec = flight_time_read % 60;
+
+ 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(rocket_phase){
+ case ROCKET_SETUP:
+ pc.printf("SETUP\r\n");
+ break;
+ case ROCKET_SAFETY:
+ pc.printf("SAFETY\r\n");
+ break;
+ case ROCKET_WAIT:
+ pc.printf("WAIT\r\n");
+ break;
+ case ROCKET_FLIGHT:
+ pc.printf("FLIGHT\r\n");
+ break;
+ case ROCKET_SEP1:
+ pc.printf("SEPARATE 1\r\n");
+ break;
+ case ROCKET_SEP2:
+ pc.printf("SEPARATE 2\r\n");
+ break;
+ case ROCKET_RECOVERY:
+ pc.printf("RECOVERY\r\n");
+ break;
+ }
+
+ pc.printf("Power : ");
+ if(ex_power){
+ pc.printf("External\r\n");
+ }
+ else{
+ pc.printf("Battery\r\n");
+ }
+
+ pc.printf("Separate : %d, %d\r\n", separate1, separate2);
+ pc.printf("UP DOWN TOP : %d, %d, %d\r\n", ok_up, ok_down, ok_top);
+ pc.printf("SAVE : %d\r\n", save);
+ pc.printf("Flight Pin : %d\r\n", flight_pin);
+
+ 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("INA_in : %.2f[V], %.2f[A]\r\n", ina_in_v, ina_in_i);
+ pc.printf("INA_ex : %.2f[V], %.2f[A]\r\n", ina_ex_v, ina_ex_i);
+
+ pc.printf("LPS33HW : %.4f[hPa], %.2f[m], %.2f[m/s]\r\n", pres33, alt33, speed33);
+ pc.printf("SHT35 : %.2f[degC], %.2f[%%]\r\n", temp35, hum35);
+ pc.printf("Pitot : %.2f[m/s]\r\n", pitot_speed);
+
+ pc.printf("\n\n\n");
+}
+
+
+/*******************************************************************************
+HEADER_STARTを受信したら呼び出される
+*******************************************************************************/
+void readHeaderStart(){
+ char what = es920.data[0];
+ switch(what){
+ case 'W':
+ pc.printf("Phase Change : WAIT\r\n");
+ break;
+
+ case 'S':
+ pc.printf("Phase Change : SAFETY\r\n");
+ break;
+
+ case 'F':
+ pc.printf("Phase Change : FLIGHT\r\n");
+ break;
+
+ case '1':
+ pc.printf("Separate1 Start\r\n");
+ break;
+
+ case '2':
+ pc.printf("Separate2 Start\r\n");
+ break;
+
+ case 'C':
+ pc.printf("File Closed\r\n");
+ break;
+
+ case 'B':
+ pc.printf("Buzzer Change\r\n");
+ break;
+
+ default:
+ pc.printf("????? Start\r\n");
+ break;
+ }
+}
+