PQ-013 Felix-Luminousの地上局プログラム ES920LR使用

Dependencies:   ES920LR mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "ES920LR.hpp"
00003 
00004 /*******************************************************************************
00005 コンストラクタ
00006 *******************************************************************************/
00007 RawSerial pc(USBTX, USBRX, 115200);
00008 RawSerial serial_es920(D1, D0);
00009 
00010 ES920LR es920(serial_es920, pc, 115200);
00011 
00012 /*******************************************************************************
00013 関数のプロトタイプ宣言
00014 *******************************************************************************/
00015 void readPC();
00016 void printHelp();
00017 
00018 void sendUpLink(char command);
00019 void readHeaderSensorSetup();
00020 void readHeaderGPSSetup();
00021 void readHeaderData();
00022 void readHeaderStart();
00023 
00024 /*******************************************************************************
00025 変数の宣言
00026 *******************************************************************************/
00027 int time_read, time_reset, time_min, time_sec;
00028 int flight_time_read, flight_time_reset, flight_time_min, flight_time_sec;
00029 
00030 char rocket_phase;
00031 
00032 bool ex_power;
00033 bool separate1, separate2;
00034 bool ok_up, ok_down, ok_top;
00035 bool save;
00036 bool flight_pin;
00037 
00038 float gps_lat, gps_lon, gps_alt, gps_knot, gps_deg;
00039 int gps_sat;
00040 
00041 float ina_in_v, ina_in_i, ina_ex_v, ina_ex_i;
00042 
00043 float pres33, alt33, speed33;
00044 
00045 float temp35, hum35;
00046 
00047 float pitot_speed;
00048 
00049 int gps_wait_count;
00050 
00051 
00052 /*******************************************************************************
00053 定数
00054 *******************************************************************************/
00055 const float ES920_MAX_20 = 0.000305176;
00056 const float ES920_MAX_100 = 0.001525879;
00057 const float ES920_MAX_200 = 0.003051758;
00058 const float ES920_MAX_500 = 0.007629395;
00059 const float ES920_MAX_1500 = 0.022888184;
00060 const float ES920_MAX_3000 = 0.045776367;
00061 
00062 
00063 /*******************************************************************************
00064 無線のヘッダまとめ(ROCKET -> GND)
00065 *******************************************************************************/
00066 const char HEADER_SENSOR_SETUP = 0x01;
00067 // 0x01, ADXL, INA_in, INA_ex, MPU, MPU_m, 22HB, 33HW, SHT, TSL, SD
00068 //   0     1      1       1     1     1      1     1    1    1    1
00069 //         0      1       2     3     4      5     6    7    8    9
00070 
00071 const char HEADER_GPS_SETUP = 0x02;
00072 // 0x02, readable, wait_count
00073 //   0      1          4
00074 //          0          1
00075 //        0x00 : NG
00076 //        0x01 : OK
00077 //        0xAA : Finish
00078 //        0xFF : Ignore
00079 
00080 const char HEADER_DATA = 0x10;
00081 //起動時間, フライト時間, フェーズ, 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 
00082 //4(2,2)   4(2,2)      1         1                                                            2     2    2    2    2    2    2     2     2     2      2       2      2        2       2      2 
00083 //0 2      4 6         8         9                                                            10    12   14   16   18   20   22    24    26    28     30      32     34       36      38     40 
00084 //                                  0x01 : ex_pow
00085 //                                  0x02 : Sep1
00086 //                                  0x04 : Sep2
00087 //                                  0x08 : ok_up
00088 //                                  0x10 : ok_down
00089 //                                  0x20 : ok_top
00090 //                                  0x40 : save
00091 //                                  0x80 : gps_yn
00092 
00093 const char HEADER_START = 0x11;
00094 // 0x11, What
00095 //   0     1
00096 //         0
00097 //      'W' : 安全モード->離床検知モード
00098 //      'S' : 離床検知モード->安全モード
00099 //      'F' : 離床検知モード->フライトモード
00100 //      '1' : 1段目分離
00101 //      '2' : 2段目分離
00102 
00103 
00104 /*******************************************************************************
00105 無線のヘッダまとめ(GND -> ROCKET)
00106 *******************************************************************************/
00107 const char HEADER_COMMAND = 0xcd;
00108 // 0xcd,コマンド
00109 //   0     1
00110 //   0     1
00111 
00112 
00113 /*******************************************************************************
00114 ロケットのフェーズ
00115 *******************************************************************************/
00116 const char ROCKET_SETUP = 0x00;     //セットアップ中
00117 const char ROCKET_SAFETY = 0x01;    //安全モード
00118 const char ROCKET_WAIT = 0x02;      //待機モード
00119 const char ROCKET_FLIGHT = 0x03;    //フライトモード
00120 const char ROCKET_SEP1 = 0x04;      //分離1モード
00121 const char ROCKET_SEP2 = 0x05;      //分離2モード
00122 const char ROCKET_RECOVERY = 0x06;  //回収モード
00123 
00124 
00125 /*******************************************************************************
00126 メイン関数
00127 *******************************************************************************/
00128 int main(){
00129     printHelp();
00130     
00131     pc.attach(&readPC, Serial::RxIrq);
00132     
00133     es920.attach(&readHeaderSensorSetup, HEADER_SENSOR_SETUP);
00134     es920.attach(&readHeaderGPSSetup, HEADER_GPS_SETUP);
00135     es920.attach(&readHeaderData, HEADER_DATA);
00136     es920.attach(&readHeaderStart, HEADER_START);
00137     
00138     while(1){
00139     }
00140 }
00141 
00142 
00143 /*******************************************************************************
00144 PCからの読み取り
00145 *******************************************************************************/
00146 void readPC(){
00147     char ch = pc.getc();
00148     pc.printf("Input : %c\r\n", ch);
00149     switch(ch){
00150         case 'W':
00151         case 'S':
00152         case 'F':
00153         case '1':
00154         case '2':
00155         case 'C':
00156         case 'B':
00157         case 'O':
00158         sendUpLink(ch);
00159         break;
00160         
00161         case '?':
00162         printHelp();
00163         break;
00164     }
00165 }
00166 
00167 
00168 /*******************************************************************************
00169 ヘルプを表示
00170 *******************************************************************************/
00171 void printHelp(){
00172     for(int i = 0; i < 20; i ++){
00173         pc.printf("*");
00174     }
00175     
00176     pc.printf("\r\nCommands\r\n");
00177     pc.printf("  W      : Safety -> Wait\r\n");
00178     pc.printf("  S      : Wait -> Safety\r\n");
00179     pc.printf("  F      : Wait -> Flight\r\n");
00180     pc.printf("  1(one) : 1st Separate 5[s]\r\n");
00181     pc.printf("  2(two) : 2nd Separate 5[s]\r\n");
00182     pc.printf("  C      : Stop Recording\r\n");
00183     pc.printf("  O      : Start Recording\r\n");
00184     pc.printf("  B      : Buzzer On/Off\r\n");
00185     pc.printf("  ?      : HELP\r\n");
00186     
00187     for(int i = 0; i < 20; i ++){
00188         pc.printf("*");
00189     }
00190     pc.printf("\r\n");
00191     wait(1.0f);
00192 }
00193 
00194 
00195 /*******************************************************************************
00196 アップリンクを送信する
00197 *******************************************************************************/
00198 void sendUpLink(char command){
00199     es920 << HEADER_COMMAND;
00200     es920 << command;
00201     es920.send();
00202 }
00203 
00204 
00205 /*******************************************************************************
00206 HEADER_SENSOR_SETUPを受信したら呼び出される
00207 *******************************************************************************/
00208 void readHeaderSensorSetup(){
00209     pc.printf("\r\n**************************************************\r\n");
00210     pc.printf("Sensor Setting Start!!\r\n");
00211     
00212     ///////////////////////////////////////////ADXL375
00213     if(es920.data[0] == 0x01){
00214         pc.printf("ADXL375 : OK\r\n");
00215     }
00216     else{
00217         pc.printf("ADXL375 : NG...\r\n");
00218     }
00219     
00220     ///////////////////////////////////////////INA226_in
00221     if(es920.data[1] == 0x01){
00222         pc.printf("INA226_in : OK!!\r\n");
00223     }
00224     else{
00225         pc.printf("INA226_in : NG...\r\n");
00226     }
00227     
00228     ///////////////////////////////////////////INA226_ex
00229     if(es920.data[2] == 0x01){
00230         pc.printf("INA226_ex OK!!\r\n");
00231     }
00232     else{
00233         pc.printf("INA226_ex NG...\r\n");
00234     }
00235 
00236     ///////////////////////////////////////////MPU9250
00237     if(es920.data[3] == 0x01){
00238         pc.printf("MPU9250 : OK!!\r\n");
00239     }
00240     else{
00241         pc.printf("MPU9250 : NG...\r\n");
00242     }
00243     if(es920.data[4] == 0x01){
00244         pc.printf("MPU9250 MAG : OK!!\r\n");
00245     }
00246     else{
00247         pc.printf("MPU9250 MAG : NG...\r\n");
00248     }
00249     
00250     ///////////////////////////////////////////LPS22HB
00251     if(es920.data[5] == 0x01){
00252         pc.printf("LPS22HB : OK!!\r\n");
00253     }
00254     else{
00255         pc.printf("LPS22HB : NG...\r\n");
00256     }
00257     
00258     ///////////////////////////////////////////LPS33HW
00259     if(es920.data[6] == 0x01){
00260         pc.printf("LPS33HW : OK!!\r\n");
00261     }
00262     else{
00263         pc.printf("LPS33HW : NG...\r\n");
00264     }
00265     
00266     ///////////////////////////////////////////SHT35
00267     if(es920.data[7] == 0x01){
00268         pc.printf("SHT35 : OK!!\r\n");
00269     }
00270     else{
00271         pc.printf("SHT35 : NG...\r\n");
00272     }
00273     
00274     ///////////////////////////////////////////TSL2561
00275     if(es920.data[8] == 0x01){
00276         pc.printf("TSL2561 : OK!!\r\n");
00277     }
00278     else{
00279         pc.printf("SL2561 : NG...\r\n");
00280     }
00281     
00282     ///////////////////////////////////////////SD
00283     if(es920.data[9] == 0x01){
00284         pc.printf("SD : OK!!\r\n");
00285     }
00286     else{
00287         pc.printf("SD : NG...\r\n");
00288     }
00289     
00290     pc.printf("Sensor Setting Finished!!\r\n");
00291     pc.printf("**************************************************\r\n");
00292 }
00293 
00294 
00295 /*******************************************************************************
00296 HEADER_GPS_SETUPを受信したら呼び出される
00297 *******************************************************************************/
00298 void readHeaderGPSSetup(){
00299     switch(es920.data[0]){
00300         case 0x00:
00301         gps_wait_count = es920.toInt(1);
00302         pc.printf("GPS Wait : %d\r\n", gps_wait_count);
00303         break;
00304         
00305         case 0x01:
00306         pc.printf("GPS Wait Complete!!\r\n");
00307         break;
00308         
00309         case 0xAA:
00310         pc.printf("GPS Setting Finished!!\r\n");
00311         pc.printf("\r\n**************************************************\r\n");
00312         break;
00313         
00314         case 0xFF:
00315         pc.printf("GPS Setting Ignore...\r\n");
00316         pc.printf("\r\n**************************************************\r\n");
00317         break;
00318     }
00319 }
00320 
00321 
00322 /*******************************************************************************
00323 HEADER_DATAを受信したら呼び出される
00324 *******************************************************************************/
00325 void readHeaderData(){
00326     int i = 0;                              //0
00327     time_reset = (int)es920.toShort(i);
00328     i += 2;                                 //2
00329     time_read = (int)es920.toShort(i);
00330     i += 2;                                 //4
00331     flight_time_reset = (int)es920.toShort(i);
00332     i += 2;                                 //6
00333     flight_time_read = (int)es920.toShort(i);
00334     i += 2;                                 //8
00335     
00336     rocket_phase = es920.data[i];
00337     i += 1;                                 //9
00338     
00339     char rocket_status = es920.data[i];
00340     i += 1;                                 //10
00341     ex_power = false;
00342     separate1 = false;
00343     separate2 = false;
00344     ok_up = false;
00345     ok_down = false;
00346     ok_top = false;
00347     save = false;
00348     flight_pin = false;
00349     if(rocket_status & 0x01)ex_power = true;
00350     if(rocket_status & 0x02)separate1 = true;
00351     if(rocket_status & 0x04)separate2 = true;
00352     if(rocket_status & 0x08)ok_up = true;
00353     if(rocket_status & 0x10)ok_down = true;
00354     if(rocket_status & 0x20)ok_top = true;
00355     if(rocket_status & 0x40)save = true;
00356     if(rocket_status & 0x80)flight_pin = true;
00357     
00358     gps_lat = (float)es920.toShort(i) * ES920_MAX_100;
00359     i += 2;                                         //12
00360     gps_lon = (float)es920.toShort(i) * ES920_MAX_500;
00361     i += 2;                                         //14
00362     gps_alt = (float)es920.toShort(i) * ES920_MAX_500;
00363     i += 2;                                         //16
00364     gps_knot = (float)es920.toShort(i) * ES920_MAX_200;
00365     i += 2;                                         //18
00366     gps_deg = (float)es920.toShort(i) * ES920_MAX_1500;
00367     i += 2;                                         //20
00368     gps_sat = (int)es920.toShort(i);
00369     i += 2;                                         //22
00370     ina_in_v = (float)es920.toShort(i) * ES920_MAX_20;
00371     i += 2;                                         //24
00372     ina_in_i = (float)es920.toShort(i) * ES920_MAX_20;
00373     i += 2;                                         //26
00374     ina_ex_v = (float)es920.toShort(i) * ES920_MAX_20;
00375     i += 2;                                         //28
00376     ina_ex_i = (float)es920.toShort(i) * ES920_MAX_20;
00377     i += 2;                                         //30
00378     pres33 = (float)es920.toShort(i) * ES920_MAX_3000;
00379     i += 2;                                         //32
00380     alt33 = (float)es920.toShort(i) * ES920_MAX_500;
00381     i += 2;                                         //34
00382     speed33 = (float)es920.toShort(i) * ES920_MAX_100;
00383     i += 2;                                         //36
00384     temp35 = (float)es920.toShort(i) * ES920_MAX_100;
00385     i += 2;                                         //38
00386     hum35 = (float)es920.toShort(i) * ES920_MAX_200;
00387     i += 2;                                         //40
00388     pitot_speed = (float)es920.toShort(i) * ES920_MAX_200;
00389     
00390     
00391     time_min = time_reset * 30 + (float)floor((double)time_read / 60);
00392     time_sec = time_read % 60;
00393     flight_time_min = flight_time_reset * 30 + floor((double)flight_time_read / 60);
00394     flight_time_sec = flight_time_read % 60;
00395     
00396     pc.printf("Time : %d:%02d\r\n", time_min, time_sec);
00397     pc.printf("FlightTime : %d:%02d\r\n", flight_time_min, flight_time_sec);
00398     
00399     pc.printf("Phase : ");
00400     switch(rocket_phase){
00401         case ROCKET_SETUP:
00402         pc.printf("SETUP\r\n");
00403         break;
00404         case ROCKET_SAFETY:
00405         pc.printf("SAFETY\r\n");
00406         break;
00407         case ROCKET_WAIT:
00408         pc.printf("WAIT\r\n");
00409         break;
00410         case ROCKET_FLIGHT:
00411         pc.printf("FLIGHT\r\n");
00412         break;
00413         case ROCKET_SEP1:
00414         pc.printf("SEPARATE 1\r\n");
00415         break;
00416         case ROCKET_SEP2:
00417         pc.printf("SEPARATE 2\r\n");
00418         break;
00419         case ROCKET_RECOVERY:
00420         pc.printf("RECOVERY\r\n");
00421         break;
00422     }
00423     
00424     pc.printf("Power : ");
00425     if(ex_power){
00426         pc.printf("External\r\n");
00427     }
00428     else{
00429         pc.printf("Battery\r\n");
00430     }
00431     
00432     pc.printf("Separate : %d, %d\r\n", separate1, separate2);
00433     pc.printf("UP DOWN TOP : %d, %d, %d\r\n", ok_up, ok_down, ok_top);
00434     pc.printf("SAVE : %d\r\n", save);
00435     pc.printf("Flight Pin : %d\r\n", flight_pin);
00436     
00437     pc.printf("GPS : %3.7f, %3.7f, %.2f\r\n", gps_lat, gps_lon, gps_alt);
00438     pc.printf("Move : %.2f[knot], %.2f[deg]\r\n", gps_knot, gps_deg);
00439     pc.printf("Sats : %d\r\n", gps_sat);
00440     
00441     pc.printf("INA_in : %.2f[V], %.2f[A]\r\n", ina_in_v, ina_in_i);
00442     pc.printf("INA_ex : %.2f[V], %.2f[A]\r\n", ina_ex_v, ina_ex_i);
00443     
00444     pc.printf("LPS33HW : %.4f[hPa], %.2f[m], %.2f[m/s]\r\n", pres33, alt33, speed33);
00445     pc.printf("SHT35 : %.2f[degC], %.2f[%%]\r\n", temp35, hum35);
00446     pc.printf("Pitot : %.2f[m/s]\r\n", pitot_speed);
00447     
00448     pc.printf("\n\n\n");
00449 }
00450 
00451 
00452 /*******************************************************************************
00453 HEADER_STARTを受信したら呼び出される
00454 *******************************************************************************/
00455 void readHeaderStart(){
00456     char what = es920.data[0];
00457     switch(what){
00458         case 'W':
00459         pc.printf("Phase Change : WAIT\r\n");
00460         break;
00461         
00462         case 'S':
00463         pc.printf("Phase Change : SAFETY\r\n");
00464         break;
00465         
00466         case 'F':
00467         pc.printf("Phase Change : FLIGHT\r\n");
00468         break;
00469         
00470         case '1':
00471         pc.printf("Separate1 Start\r\n");
00472         break;
00473         
00474         case '2':
00475         pc.printf("Separate2 Start\r\n");
00476         break;
00477         
00478         case 'C':
00479         pc.printf("File Closed\r\n");
00480         break;
00481         
00482         case 'B':
00483         pc.printf("Buzzer Change\r\n");
00484         break;
00485         
00486         default:
00487         pc.printf("????? Start\r\n");
00488         break;
00489     }
00490 }
00491