PQ_Hybrid_Electrical_Equipment_Team
/
Hybrid_IZU201811_GNDv1
PQ-013 Felix-Luminousの地上局プログラム ES920LR使用
Revision 0:d99384e36f64, committed 2018-11-14
- Comitter:
- Sigma884
- Date:
- Wed Nov 14 09:33:58 2018 +0000
- Commit message:
- PQ-013 Felix-Luminous?????????; ES920LR??
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ES920LR.lib Wed Nov 14 09:33:58 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/teams/PQ_Hybrid_Electrical_Equipment_Team/code/ES920LR/#4e3a413e04d9
--- /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; + } +} +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Nov 14 09:33:58 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187 \ No newline at end of file