PQ-013 Felix-Luminousの地上局プログラム ES920LR使用
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Tue Jul 12 2022 21:13:52 by
1.7.2