PQ_Hybrid_Electrical_Equipment_Team / Mbed 2 deprecated Hybrid_IZU201811_GNDv1

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

Dependencies:   ES920LR mbed

main.cpp@0:d99384e36f64, 2018-11-14 (annotated)

Committer:
Sigma884
Date:
Wed Nov 14 09:33:58 2018 +0000
Revision:
0:d99384e36f64
PQ-013 Felix-Luminous?????????; ES920LR??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Sigma884 0:d99384e36f64 1 #include "mbed.h"
Sigma884 0:d99384e36f64 2 #include "ES920LR.hpp"
Sigma884 0:d99384e36f64 3
Sigma884 0:d99384e36f64 4 /*******************************************************************************
Sigma884 0:d99384e36f64 5 コンストラクタ
Sigma884 0:d99384e36f64 6 *******************************************************************************/
Sigma884 0:d99384e36f64 7 RawSerial pc(USBTX, USBRX, 115200);
Sigma884 0:d99384e36f64 8 RawSerial serial_es920(D1, D0);
Sigma884 0:d99384e36f64 9
Sigma884 0:d99384e36f64 10 ES920LR es920(serial_es920, pc, 115200);
Sigma884 0:d99384e36f64 11
Sigma884 0:d99384e36f64 12 /*******************************************************************************
Sigma884 0:d99384e36f64 13 関数のプロトタイプ宣言
Sigma884 0:d99384e36f64 14 *******************************************************************************/
Sigma884 0:d99384e36f64 15 void readPC();
Sigma884 0:d99384e36f64 16 void printHelp();
Sigma884 0:d99384e36f64 17
Sigma884 0:d99384e36f64 18 void sendUpLink(char command);
Sigma884 0:d99384e36f64 19 void readHeaderSensorSetup();
Sigma884 0:d99384e36f64 20 void readHeaderGPSSetup();
Sigma884 0:d99384e36f64 21 void readHeaderData();
Sigma884 0:d99384e36f64 22 void readHeaderStart();
Sigma884 0:d99384e36f64 23
Sigma884 0:d99384e36f64 24 /*******************************************************************************
Sigma884 0:d99384e36f64 25 変数の宣言
Sigma884 0:d99384e36f64 26 *******************************************************************************/
Sigma884 0:d99384e36f64 27 int time_read, time_reset, time_min, time_sec;
Sigma884 0:d99384e36f64 28 int flight_time_read, flight_time_reset, flight_time_min, flight_time_sec;
Sigma884 0:d99384e36f64 29
Sigma884 0:d99384e36f64 30 char rocket_phase;
Sigma884 0:d99384e36f64 31
Sigma884 0:d99384e36f64 32 bool ex_power;
Sigma884 0:d99384e36f64 33 bool separate1, separate2;
Sigma884 0:d99384e36f64 34 bool ok_up, ok_down, ok_top;
Sigma884 0:d99384e36f64 35 bool save;
Sigma884 0:d99384e36f64 36 bool flight_pin;
Sigma884 0:d99384e36f64 37
Sigma884 0:d99384e36f64 38 float gps_lat, gps_lon, gps_alt, gps_knot, gps_deg;
Sigma884 0:d99384e36f64 39 int gps_sat;
Sigma884 0:d99384e36f64 40
Sigma884 0:d99384e36f64 41 float ina_in_v, ina_in_i, ina_ex_v, ina_ex_i;
Sigma884 0:d99384e36f64 42
Sigma884 0:d99384e36f64 43 float pres33, alt33, speed33;
Sigma884 0:d99384e36f64 44
Sigma884 0:d99384e36f64 45 float temp35, hum35;
Sigma884 0:d99384e36f64 46
Sigma884 0:d99384e36f64 47 float pitot_speed;
Sigma884 0:d99384e36f64 48
Sigma884 0:d99384e36f64 49 int gps_wait_count;
Sigma884 0:d99384e36f64 50
Sigma884 0:d99384e36f64 51
Sigma884 0:d99384e36f64 52 /*******************************************************************************
Sigma884 0:d99384e36f64 53 定数
Sigma884 0:d99384e36f64 54 *******************************************************************************/
Sigma884 0:d99384e36f64 55 const float ES920_MAX_20 = 0.000305176;
Sigma884 0:d99384e36f64 56 const float ES920_MAX_100 = 0.001525879;
Sigma884 0:d99384e36f64 57 const float ES920_MAX_200 = 0.003051758;
Sigma884 0:d99384e36f64 58 const float ES920_MAX_500 = 0.007629395;
Sigma884 0:d99384e36f64 59 const float ES920_MAX_1500 = 0.022888184;
Sigma884 0:d99384e36f64 60 const float ES920_MAX_3000 = 0.045776367;
Sigma884 0:d99384e36f64 61
Sigma884 0:d99384e36f64 62
Sigma884 0:d99384e36f64 63 /*******************************************************************************
Sigma884 0:d99384e36f64 64 無線のヘッダまとめ(ROCKET -> GND)
Sigma884 0:d99384e36f64 65 *******************************************************************************/
Sigma884 0:d99384e36f64 66 const char HEADER_SENSOR_SETUP = 0x01;
Sigma884 0:d99384e36f64 67 // 0x01, ADXL, INA_in, INA_ex, MPU, MPU_m, 22HB, 33HW, SHT, TSL, SD
Sigma884 0:d99384e36f64 68 // 0 1 1 1 1 1 1 1 1 1 1
Sigma884 0:d99384e36f64 69 // 0 1 2 3 4 5 6 7 8 9
Sigma884 0:d99384e36f64 70
Sigma884 0:d99384e36f64 71 const char HEADER_GPS_SETUP = 0x02;
Sigma884 0:d99384e36f64 72 // 0x02, readable, wait_count
Sigma884 0:d99384e36f64 73 // 0 1 4
Sigma884 0:d99384e36f64 74 // 0 1
Sigma884 0:d99384e36f64 75 // 0x00 : NG
Sigma884 0:d99384e36f64 76 // 0x01 : OK
Sigma884 0:d99384e36f64 77 // 0xAA : Finish
Sigma884 0:d99384e36f64 78 // 0xFF : Ignore
Sigma884 0:d99384e36f64 79
Sigma884 0:d99384e36f64 80 const char HEADER_DATA = 0x10;
Sigma884 0:d99384e36f64 81 //起動時間, フライト時間, フェーズ, 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
Sigma884 0:d99384e36f64 82 //4(2,2) 4(2,2) 1 1 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2
Sigma884 0:d99384e36f64 83 //0 2 4 6 8 9 10 12 14 16 18 20 22 24 26 28 30 32 34 36 38 40
Sigma884 0:d99384e36f64 84 // 0x01 : ex_pow
Sigma884 0:d99384e36f64 85 // 0x02 : Sep1
Sigma884 0:d99384e36f64 86 // 0x04 : Sep2
Sigma884 0:d99384e36f64 87 // 0x08 : ok_up
Sigma884 0:d99384e36f64 88 // 0x10 : ok_down
Sigma884 0:d99384e36f64 89 // 0x20 : ok_top
Sigma884 0:d99384e36f64 90 // 0x40 : save
Sigma884 0:d99384e36f64 91 // 0x80 : gps_yn
Sigma884 0:d99384e36f64 92
Sigma884 0:d99384e36f64 93 const char HEADER_START = 0x11;
Sigma884 0:d99384e36f64 94 // 0x11, What
Sigma884 0:d99384e36f64 95 // 0 1
Sigma884 0:d99384e36f64 96 // 0
Sigma884 0:d99384e36f64 97 // 'W' : 安全モード->離床検知モード
Sigma884 0:d99384e36f64 98 // 'S' : 離床検知モード->安全モード
Sigma884 0:d99384e36f64 99 // 'F' : 離床検知モード->フライトモード
Sigma884 0:d99384e36f64 100 // '1' : 1段目分離
Sigma884 0:d99384e36f64 101 // '2' : 2段目分離
Sigma884 0:d99384e36f64 102
Sigma884 0:d99384e36f64 103
Sigma884 0:d99384e36f64 104 /*******************************************************************************
Sigma884 0:d99384e36f64 105 無線のヘッダまとめ(GND -> ROCKET)
Sigma884 0:d99384e36f64 106 *******************************************************************************/
Sigma884 0:d99384e36f64 107 const char HEADER_COMMAND = 0xcd;
Sigma884 0:d99384e36f64 108 // 0xcd,コマンド
Sigma884 0:d99384e36f64 109 // 0 1
Sigma884 0:d99384e36f64 110 // 0 1
Sigma884 0:d99384e36f64 111
Sigma884 0:d99384e36f64 112
Sigma884 0:d99384e36f64 113 /*******************************************************************************
Sigma884 0:d99384e36f64 114 ロケットのフェーズ
Sigma884 0:d99384e36f64 115 *******************************************************************************/
Sigma884 0:d99384e36f64 116 const char ROCKET_SETUP = 0x00; //セットアップ中
Sigma884 0:d99384e36f64 117 const char ROCKET_SAFETY = 0x01; //安全モード
Sigma884 0:d99384e36f64 118 const char ROCKET_WAIT = 0x02; //待機モード
Sigma884 0:d99384e36f64 119 const char ROCKET_FLIGHT = 0x03; //フライトモード
Sigma884 0:d99384e36f64 120 const char ROCKET_SEP1 = 0x04; //分離1モード
Sigma884 0:d99384e36f64 121 const char ROCKET_SEP2 = 0x05; //分離2モード
Sigma884 0:d99384e36f64 122 const char ROCKET_RECOVERY = 0x06; //回収モード
Sigma884 0:d99384e36f64 123
Sigma884 0:d99384e36f64 124
Sigma884 0:d99384e36f64 125 /*******************************************************************************
Sigma884 0:d99384e36f64 126 メイン関数
Sigma884 0:d99384e36f64 127 *******************************************************************************/
Sigma884 0:d99384e36f64 128 int main(){
Sigma884 0:d99384e36f64 129 printHelp();
Sigma884 0:d99384e36f64 130
Sigma884 0:d99384e36f64 131 pc.attach(&readPC, Serial::RxIrq);
Sigma884 0:d99384e36f64 132
Sigma884 0:d99384e36f64 133 es920.attach(&readHeaderSensorSetup, HEADER_SENSOR_SETUP);
Sigma884 0:d99384e36f64 134 es920.attach(&readHeaderGPSSetup, HEADER_GPS_SETUP);
Sigma884 0:d99384e36f64 135 es920.attach(&readHeaderData, HEADER_DATA);
Sigma884 0:d99384e36f64 136 es920.attach(&readHeaderStart, HEADER_START);
Sigma884 0:d99384e36f64 137
Sigma884 0:d99384e36f64 138 while(1){
Sigma884 0:d99384e36f64 139 }
Sigma884 0:d99384e36f64 140 }
Sigma884 0:d99384e36f64 141
Sigma884 0:d99384e36f64 142
Sigma884 0:d99384e36f64 143 /*******************************************************************************
Sigma884 0:d99384e36f64 144 PCからの読み取り
Sigma884 0:d99384e36f64 145 *******************************************************************************/
Sigma884 0:d99384e36f64 146 void readPC(){
Sigma884 0:d99384e36f64 147 char ch = pc.getc();
Sigma884 0:d99384e36f64 148 pc.printf("Input : %c\r\n", ch);
Sigma884 0:d99384e36f64 149 switch(ch){
Sigma884 0:d99384e36f64 150 case 'W':
Sigma884 0:d99384e36f64 151 case 'S':
Sigma884 0:d99384e36f64 152 case 'F':
Sigma884 0:d99384e36f64 153 case '1':
Sigma884 0:d99384e36f64 154 case '2':
Sigma884 0:d99384e36f64 155 case 'C':
Sigma884 0:d99384e36f64 156 case 'B':
Sigma884 0:d99384e36f64 157 case 'O':
Sigma884 0:d99384e36f64 158 sendUpLink(ch);
Sigma884 0:d99384e36f64 159 break;
Sigma884 0:d99384e36f64 160
Sigma884 0:d99384e36f64 161 case '?':
Sigma884 0:d99384e36f64 162 printHelp();
Sigma884 0:d99384e36f64 163 break;
Sigma884 0:d99384e36f64 164 }
Sigma884 0:d99384e36f64 165 }
Sigma884 0:d99384e36f64 166
Sigma884 0:d99384e36f64 167
Sigma884 0:d99384e36f64 168 /*******************************************************************************
Sigma884 0:d99384e36f64 169 ヘルプを表示
Sigma884 0:d99384e36f64 170 *******************************************************************************/
Sigma884 0:d99384e36f64 171 void printHelp(){
Sigma884 0:d99384e36f64 172 for(int i = 0; i < 20; i ++){
Sigma884 0:d99384e36f64 173 pc.printf("*");
Sigma884 0:d99384e36f64 174 }
Sigma884 0:d99384e36f64 175
Sigma884 0:d99384e36f64 176 pc.printf("\r\nCommands\r\n");
Sigma884 0:d99384e36f64 177 pc.printf(" W : Safety -> Wait\r\n");
Sigma884 0:d99384e36f64 178 pc.printf(" S : Wait -> Safety\r\n");
Sigma884 0:d99384e36f64 179 pc.printf(" F : Wait -> Flight\r\n");
Sigma884 0:d99384e36f64 180 pc.printf(" 1(one) : 1st Separate 5[s]\r\n");
Sigma884 0:d99384e36f64 181 pc.printf(" 2(two) : 2nd Separate 5[s]\r\n");
Sigma884 0:d99384e36f64 182 pc.printf(" C : Stop Recording\r\n");
Sigma884 0:d99384e36f64 183 pc.printf(" O : Start Recording\r\n");
Sigma884 0:d99384e36f64 184 pc.printf(" B : Buzzer On/Off\r\n");
Sigma884 0:d99384e36f64 185 pc.printf(" ? : HELP\r\n");
Sigma884 0:d99384e36f64 186
Sigma884 0:d99384e36f64 187 for(int i = 0; i < 20; i ++){
Sigma884 0:d99384e36f64 188 pc.printf("*");
Sigma884 0:d99384e36f64 189 }
Sigma884 0:d99384e36f64 190 pc.printf("\r\n");
Sigma884 0:d99384e36f64 191 wait(1.0f);
Sigma884 0:d99384e36f64 192 }
Sigma884 0:d99384e36f64 193
Sigma884 0:d99384e36f64 194
Sigma884 0:d99384e36f64 195 /*******************************************************************************
Sigma884 0:d99384e36f64 196 アップリンクを送信する
Sigma884 0:d99384e36f64 197 *******************************************************************************/
Sigma884 0:d99384e36f64 198 void sendUpLink(char command){
Sigma884 0:d99384e36f64 199 es920 << HEADER_COMMAND;
Sigma884 0:d99384e36f64 200 es920 << command;
Sigma884 0:d99384e36f64 201 es920.send();
Sigma884 0:d99384e36f64 202 }
Sigma884 0:d99384e36f64 203
Sigma884 0:d99384e36f64 204
Sigma884 0:d99384e36f64 205 /*******************************************************************************
Sigma884 0:d99384e36f64 206 HEADER_SENSOR_SETUPを受信したら呼び出される
Sigma884 0:d99384e36f64 207 *******************************************************************************/
Sigma884 0:d99384e36f64 208 void readHeaderSensorSetup(){
Sigma884 0:d99384e36f64 209 pc.printf("\r\n**************************************************\r\n");
Sigma884 0:d99384e36f64 210 pc.printf("Sensor Setting Start!!\r\n");
Sigma884 0:d99384e36f64 211
Sigma884 0:d99384e36f64 212 ///////////////////////////////////////////ADXL375
Sigma884 0:d99384e36f64 213 if(es920.data[0] == 0x01){
Sigma884 0:d99384e36f64 214 pc.printf("ADXL375 : OK\r\n");
Sigma884 0:d99384e36f64 215 }
Sigma884 0:d99384e36f64 216 else{
Sigma884 0:d99384e36f64 217 pc.printf("ADXL375 : NG...\r\n");
Sigma884 0:d99384e36f64 218 }
Sigma884 0:d99384e36f64 219
Sigma884 0:d99384e36f64 220 ///////////////////////////////////////////INA226_in
Sigma884 0:d99384e36f64 221 if(es920.data[1] == 0x01){
Sigma884 0:d99384e36f64 222 pc.printf("INA226_in : OK!!\r\n");
Sigma884 0:d99384e36f64 223 }
Sigma884 0:d99384e36f64 224 else{
Sigma884 0:d99384e36f64 225 pc.printf("INA226_in : NG...\r\n");
Sigma884 0:d99384e36f64 226 }
Sigma884 0:d99384e36f64 227
Sigma884 0:d99384e36f64 228 ///////////////////////////////////////////INA226_ex
Sigma884 0:d99384e36f64 229 if(es920.data[2] == 0x01){
Sigma884 0:d99384e36f64 230 pc.printf("INA226_ex OK!!\r\n");
Sigma884 0:d99384e36f64 231 }
Sigma884 0:d99384e36f64 232 else{
Sigma884 0:d99384e36f64 233 pc.printf("INA226_ex NG...\r\n");
Sigma884 0:d99384e36f64 234 }
Sigma884 0:d99384e36f64 235
Sigma884 0:d99384e36f64 236 ///////////////////////////////////////////MPU9250
Sigma884 0:d99384e36f64 237 if(es920.data[3] == 0x01){
Sigma884 0:d99384e36f64 238 pc.printf("MPU9250 : OK!!\r\n");
Sigma884 0:d99384e36f64 239 }
Sigma884 0:d99384e36f64 240 else{
Sigma884 0:d99384e36f64 241 pc.printf("MPU9250 : NG...\r\n");
Sigma884 0:d99384e36f64 242 }
Sigma884 0:d99384e36f64 243 if(es920.data[4] == 0x01){
Sigma884 0:d99384e36f64 244 pc.printf("MPU9250 MAG : OK!!\r\n");
Sigma884 0:d99384e36f64 245 }
Sigma884 0:d99384e36f64 246 else{
Sigma884 0:d99384e36f64 247 pc.printf("MPU9250 MAG : NG...\r\n");
Sigma884 0:d99384e36f64 248 }
Sigma884 0:d99384e36f64 249
Sigma884 0:d99384e36f64 250 ///////////////////////////////////////////LPS22HB
Sigma884 0:d99384e36f64 251 if(es920.data[5] == 0x01){
Sigma884 0:d99384e36f64 252 pc.printf("LPS22HB : OK!!\r\n");
Sigma884 0:d99384e36f64 253 }
Sigma884 0:d99384e36f64 254 else{
Sigma884 0:d99384e36f64 255 pc.printf("LPS22HB : NG...\r\n");
Sigma884 0:d99384e36f64 256 }
Sigma884 0:d99384e36f64 257
Sigma884 0:d99384e36f64 258 ///////////////////////////////////////////LPS33HW
Sigma884 0:d99384e36f64 259 if(es920.data[6] == 0x01){
Sigma884 0:d99384e36f64 260 pc.printf("LPS33HW : OK!!\r\n");
Sigma884 0:d99384e36f64 261 }
Sigma884 0:d99384e36f64 262 else{
Sigma884 0:d99384e36f64 263 pc.printf("LPS33HW : NG...\r\n");
Sigma884 0:d99384e36f64 264 }
Sigma884 0:d99384e36f64 265
Sigma884 0:d99384e36f64 266 ///////////////////////////////////////////SHT35
Sigma884 0:d99384e36f64 267 if(es920.data[7] == 0x01){
Sigma884 0:d99384e36f64 268 pc.printf("SHT35 : OK!!\r\n");
Sigma884 0:d99384e36f64 269 }
Sigma884 0:d99384e36f64 270 else{
Sigma884 0:d99384e36f64 271 pc.printf("SHT35 : NG...\r\n");
Sigma884 0:d99384e36f64 272 }
Sigma884 0:d99384e36f64 273
Sigma884 0:d99384e36f64 274 ///////////////////////////////////////////TSL2561
Sigma884 0:d99384e36f64 275 if(es920.data[8] == 0x01){
Sigma884 0:d99384e36f64 276 pc.printf("TSL2561 : OK!!\r\n");
Sigma884 0:d99384e36f64 277 }
Sigma884 0:d99384e36f64 278 else{
Sigma884 0:d99384e36f64 279 pc.printf("SL2561 : NG...\r\n");
Sigma884 0:d99384e36f64 280 }
Sigma884 0:d99384e36f64 281
Sigma884 0:d99384e36f64 282 ///////////////////////////////////////////SD
Sigma884 0:d99384e36f64 283 if(es920.data[9] == 0x01){
Sigma884 0:d99384e36f64 284 pc.printf("SD : OK!!\r\n");
Sigma884 0:d99384e36f64 285 }
Sigma884 0:d99384e36f64 286 else{
Sigma884 0:d99384e36f64 287 pc.printf("SD : NG...\r\n");
Sigma884 0:d99384e36f64 288 }
Sigma884 0:d99384e36f64 289
Sigma884 0:d99384e36f64 290 pc.printf("Sensor Setting Finished!!\r\n");
Sigma884 0:d99384e36f64 291 pc.printf("**************************************************\r\n");
Sigma884 0:d99384e36f64 292 }
Sigma884 0:d99384e36f64 293
Sigma884 0:d99384e36f64 294
Sigma884 0:d99384e36f64 295 /*******************************************************************************
Sigma884 0:d99384e36f64 296 HEADER_GPS_SETUPを受信したら呼び出される
Sigma884 0:d99384e36f64 297 *******************************************************************************/
Sigma884 0:d99384e36f64 298 void readHeaderGPSSetup(){
Sigma884 0:d99384e36f64 299 switch(es920.data[0]){
Sigma884 0:d99384e36f64 300 case 0x00:
Sigma884 0:d99384e36f64 301 gps_wait_count = es920.toInt(1);
Sigma884 0:d99384e36f64 302 pc.printf("GPS Wait : %d\r\n", gps_wait_count);
Sigma884 0:d99384e36f64 303 break;
Sigma884 0:d99384e36f64 304
Sigma884 0:d99384e36f64 305 case 0x01:
Sigma884 0:d99384e36f64 306 pc.printf("GPS Wait Complete!!\r\n");
Sigma884 0:d99384e36f64 307 break;
Sigma884 0:d99384e36f64 308
Sigma884 0:d99384e36f64 309 case 0xAA:
Sigma884 0:d99384e36f64 310 pc.printf("GPS Setting Finished!!\r\n");
Sigma884 0:d99384e36f64 311 pc.printf("\r\n**************************************************\r\n");
Sigma884 0:d99384e36f64 312 break;
Sigma884 0:d99384e36f64 313
Sigma884 0:d99384e36f64 314 case 0xFF:
Sigma884 0:d99384e36f64 315 pc.printf("GPS Setting Ignore...\r\n");
Sigma884 0:d99384e36f64 316 pc.printf("\r\n**************************************************\r\n");
Sigma884 0:d99384e36f64 317 break;
Sigma884 0:d99384e36f64 318 }
Sigma884 0:d99384e36f64 319 }
Sigma884 0:d99384e36f64 320
Sigma884 0:d99384e36f64 321
Sigma884 0:d99384e36f64 322 /*******************************************************************************
Sigma884 0:d99384e36f64 323 HEADER_DATAを受信したら呼び出される
Sigma884 0:d99384e36f64 324 *******************************************************************************/
Sigma884 0:d99384e36f64 325 void readHeaderData(){
Sigma884 0:d99384e36f64 326 int i = 0; //0
Sigma884 0:d99384e36f64 327 time_reset = (int)es920.toShort(i);
Sigma884 0:d99384e36f64 328 i += 2; //2
Sigma884 0:d99384e36f64 329 time_read = (int)es920.toShort(i);
Sigma884 0:d99384e36f64 330 i += 2; //4
Sigma884 0:d99384e36f64 331 flight_time_reset = (int)es920.toShort(i);
Sigma884 0:d99384e36f64 332 i += 2; //6
Sigma884 0:d99384e36f64 333 flight_time_read = (int)es920.toShort(i);
Sigma884 0:d99384e36f64 334 i += 2; //8
Sigma884 0:d99384e36f64 335
Sigma884 0:d99384e36f64 336 rocket_phase = es920.data[i];
Sigma884 0:d99384e36f64 337 i += 1; //9
Sigma884 0:d99384e36f64 338
Sigma884 0:d99384e36f64 339 char rocket_status = es920.data[i];
Sigma884 0:d99384e36f64 340 i += 1; //10
Sigma884 0:d99384e36f64 341 ex_power = false;
Sigma884 0:d99384e36f64 342 separate1 = false;
Sigma884 0:d99384e36f64 343 separate2 = false;
Sigma884 0:d99384e36f64 344 ok_up = false;
Sigma884 0:d99384e36f64 345 ok_down = false;
Sigma884 0:d99384e36f64 346 ok_top = false;
Sigma884 0:d99384e36f64 347 save = false;
Sigma884 0:d99384e36f64 348 flight_pin = false;
Sigma884 0:d99384e36f64 349 if(rocket_status & 0x01)ex_power = true;
Sigma884 0:d99384e36f64 350 if(rocket_status & 0x02)separate1 = true;
Sigma884 0:d99384e36f64 351 if(rocket_status & 0x04)separate2 = true;
Sigma884 0:d99384e36f64 352 if(rocket_status & 0x08)ok_up = true;
Sigma884 0:d99384e36f64 353 if(rocket_status & 0x10)ok_down = true;
Sigma884 0:d99384e36f64 354 if(rocket_status & 0x20)ok_top = true;
Sigma884 0:d99384e36f64 355 if(rocket_status & 0x40)save = true;
Sigma884 0:d99384e36f64 356 if(rocket_status & 0x80)flight_pin = true;
Sigma884 0:d99384e36f64 357
Sigma884 0:d99384e36f64 358 gps_lat = (float)es920.toShort(i) * ES920_MAX_100;
Sigma884 0:d99384e36f64 359 i += 2; //12
Sigma884 0:d99384e36f64 360 gps_lon = (float)es920.toShort(i) * ES920_MAX_500;
Sigma884 0:d99384e36f64 361 i += 2; //14
Sigma884 0:d99384e36f64 362 gps_alt = (float)es920.toShort(i) * ES920_MAX_500;
Sigma884 0:d99384e36f64 363 i += 2; //16
Sigma884 0:d99384e36f64 364 gps_knot = (float)es920.toShort(i) * ES920_MAX_200;
Sigma884 0:d99384e36f64 365 i += 2; //18
Sigma884 0:d99384e36f64 366 gps_deg = (float)es920.toShort(i) * ES920_MAX_1500;
Sigma884 0:d99384e36f64 367 i += 2; //20
Sigma884 0:d99384e36f64 368 gps_sat = (int)es920.toShort(i);
Sigma884 0:d99384e36f64 369 i += 2; //22
Sigma884 0:d99384e36f64 370 ina_in_v = (float)es920.toShort(i) * ES920_MAX_20;
Sigma884 0:d99384e36f64 371 i += 2; //24
Sigma884 0:d99384e36f64 372 ina_in_i = (float)es920.toShort(i) * ES920_MAX_20;
Sigma884 0:d99384e36f64 373 i += 2; //26
Sigma884 0:d99384e36f64 374 ina_ex_v = (float)es920.toShort(i) * ES920_MAX_20;
Sigma884 0:d99384e36f64 375 i += 2; //28
Sigma884 0:d99384e36f64 376 ina_ex_i = (float)es920.toShort(i) * ES920_MAX_20;
Sigma884 0:d99384e36f64 377 i += 2; //30
Sigma884 0:d99384e36f64 378 pres33 = (float)es920.toShort(i) * ES920_MAX_3000;
Sigma884 0:d99384e36f64 379 i += 2; //32
Sigma884 0:d99384e36f64 380 alt33 = (float)es920.toShort(i) * ES920_MAX_500;
Sigma884 0:d99384e36f64 381 i += 2; //34
Sigma884 0:d99384e36f64 382 speed33 = (float)es920.toShort(i) * ES920_MAX_100;
Sigma884 0:d99384e36f64 383 i += 2; //36
Sigma884 0:d99384e36f64 384 temp35 = (float)es920.toShort(i) * ES920_MAX_100;
Sigma884 0:d99384e36f64 385 i += 2; //38
Sigma884 0:d99384e36f64 386 hum35 = (float)es920.toShort(i) * ES920_MAX_200;
Sigma884 0:d99384e36f64 387 i += 2; //40
Sigma884 0:d99384e36f64 388 pitot_speed = (float)es920.toShort(i) * ES920_MAX_200;
Sigma884 0:d99384e36f64 389
Sigma884 0:d99384e36f64 390
Sigma884 0:d99384e36f64 391 time_min = time_reset * 30 + (float)floor((double)time_read / 60);
Sigma884 0:d99384e36f64 392 time_sec = time_read % 60;
Sigma884 0:d99384e36f64 393 flight_time_min = flight_time_reset * 30 + floor((double)flight_time_read / 60);
Sigma884 0:d99384e36f64 394 flight_time_sec = flight_time_read % 60;
Sigma884 0:d99384e36f64 395
Sigma884 0:d99384e36f64 396 pc.printf("Time : %d:%02d\r\n", time_min, time_sec);
Sigma884 0:d99384e36f64 397 pc.printf("FlightTime : %d:%02d\r\n", flight_time_min, flight_time_sec);
Sigma884 0:d99384e36f64 398
Sigma884 0:d99384e36f64 399 pc.printf("Phase : ");
Sigma884 0:d99384e36f64 400 switch(rocket_phase){
Sigma884 0:d99384e36f64 401 case ROCKET_SETUP:
Sigma884 0:d99384e36f64 402 pc.printf("SETUP\r\n");
Sigma884 0:d99384e36f64 403 break;
Sigma884 0:d99384e36f64 404 case ROCKET_SAFETY:
Sigma884 0:d99384e36f64 405 pc.printf("SAFETY\r\n");
Sigma884 0:d99384e36f64 406 break;
Sigma884 0:d99384e36f64 407 case ROCKET_WAIT:
Sigma884 0:d99384e36f64 408 pc.printf("WAIT\r\n");
Sigma884 0:d99384e36f64 409 break;
Sigma884 0:d99384e36f64 410 case ROCKET_FLIGHT:
Sigma884 0:d99384e36f64 411 pc.printf("FLIGHT\r\n");
Sigma884 0:d99384e36f64 412 break;
Sigma884 0:d99384e36f64 413 case ROCKET_SEP1:
Sigma884 0:d99384e36f64 414 pc.printf("SEPARATE 1\r\n");
Sigma884 0:d99384e36f64 415 break;
Sigma884 0:d99384e36f64 416 case ROCKET_SEP2:
Sigma884 0:d99384e36f64 417 pc.printf("SEPARATE 2\r\n");
Sigma884 0:d99384e36f64 418 break;
Sigma884 0:d99384e36f64 419 case ROCKET_RECOVERY:
Sigma884 0:d99384e36f64 420 pc.printf("RECOVERY\r\n");
Sigma884 0:d99384e36f64 421 break;
Sigma884 0:d99384e36f64 422 }
Sigma884 0:d99384e36f64 423
Sigma884 0:d99384e36f64 424 pc.printf("Power : ");
Sigma884 0:d99384e36f64 425 if(ex_power){
Sigma884 0:d99384e36f64 426 pc.printf("External\r\n");
Sigma884 0:d99384e36f64 427 }
Sigma884 0:d99384e36f64 428 else{
Sigma884 0:d99384e36f64 429 pc.printf("Battery\r\n");
Sigma884 0:d99384e36f64 430 }
Sigma884 0:d99384e36f64 431
Sigma884 0:d99384e36f64 432 pc.printf("Separate : %d, %d\r\n", separate1, separate2);
Sigma884 0:d99384e36f64 433 pc.printf("UP DOWN TOP : %d, %d, %d\r\n", ok_up, ok_down, ok_top);
Sigma884 0:d99384e36f64 434 pc.printf("SAVE : %d\r\n", save);
Sigma884 0:d99384e36f64 435 pc.printf("Flight Pin : %d\r\n", flight_pin);
Sigma884 0:d99384e36f64 436
Sigma884 0:d99384e36f64 437 pc.printf("GPS : %3.7f, %3.7f, %.2f\r\n", gps_lat, gps_lon, gps_alt);
Sigma884 0:d99384e36f64 438 pc.printf("Move : %.2f[knot], %.2f[deg]\r\n", gps_knot, gps_deg);
Sigma884 0:d99384e36f64 439 pc.printf("Sats : %d\r\n", gps_sat);
Sigma884 0:d99384e36f64 440
Sigma884 0:d99384e36f64 441 pc.printf("INA_in : %.2f[V], %.2f[A]\r\n", ina_in_v, ina_in_i);
Sigma884 0:d99384e36f64 442 pc.printf("INA_ex : %.2f[V], %.2f[A]\r\n", ina_ex_v, ina_ex_i);
Sigma884 0:d99384e36f64 443
Sigma884 0:d99384e36f64 444 pc.printf("LPS33HW : %.4f[hPa], %.2f[m], %.2f[m/s]\r\n", pres33, alt33, speed33);
Sigma884 0:d99384e36f64 445 pc.printf("SHT35 : %.2f[degC], %.2f[%%]\r\n", temp35, hum35);
Sigma884 0:d99384e36f64 446 pc.printf("Pitot : %.2f[m/s]\r\n", pitot_speed);
Sigma884 0:d99384e36f64 447
Sigma884 0:d99384e36f64 448 pc.printf("\n\n\n");
Sigma884 0:d99384e36f64 449 }
Sigma884 0:d99384e36f64 450
Sigma884 0:d99384e36f64 451
Sigma884 0:d99384e36f64 452 /*******************************************************************************
Sigma884 0:d99384e36f64 453 HEADER_STARTを受信したら呼び出される
Sigma884 0:d99384e36f64 454 *******************************************************************************/
Sigma884 0:d99384e36f64 455 void readHeaderStart(){
Sigma884 0:d99384e36f64 456 char what = es920.data[0];
Sigma884 0:d99384e36f64 457 switch(what){
Sigma884 0:d99384e36f64 458 case 'W':
Sigma884 0:d99384e36f64 459 pc.printf("Phase Change : WAIT\r\n");
Sigma884 0:d99384e36f64 460 break;
Sigma884 0:d99384e36f64 461
Sigma884 0:d99384e36f64 462 case 'S':
Sigma884 0:d99384e36f64 463 pc.printf("Phase Change : SAFETY\r\n");
Sigma884 0:d99384e36f64 464 break;
Sigma884 0:d99384e36f64 465
Sigma884 0:d99384e36f64 466 case 'F':
Sigma884 0:d99384e36f64 467 pc.printf("Phase Change : FLIGHT\r\n");
Sigma884 0:d99384e36f64 468 break;
Sigma884 0:d99384e36f64 469
Sigma884 0:d99384e36f64 470 case '1':
Sigma884 0:d99384e36f64 471 pc.printf("Separate1 Start\r\n");
Sigma884 0:d99384e36f64 472 break;
Sigma884 0:d99384e36f64 473
Sigma884 0:d99384e36f64 474 case '2':
Sigma884 0:d99384e36f64 475 pc.printf("Separate2 Start\r\n");
Sigma884 0:d99384e36f64 476 break;
Sigma884 0:d99384e36f64 477
Sigma884 0:d99384e36f64 478 case 'C':
Sigma884 0:d99384e36f64 479 pc.printf("File Closed\r\n");
Sigma884 0:d99384e36f64 480 break;
Sigma884 0:d99384e36f64 481
Sigma884 0:d99384e36f64 482 case 'B':
Sigma884 0:d99384e36f64 483 pc.printf("Buzzer Change\r\n");
Sigma884 0:d99384e36f64 484 break;
Sigma884 0:d99384e36f64 485
Sigma884 0:d99384e36f64 486 default:
Sigma884 0:d99384e36f64 487 pc.printf("????? Start\r\n");
Sigma884 0:d99384e36f64 488 break;
Sigma884 0:d99384e36f64 489 }
Sigma884 0:d99384e36f64 490 }
Sigma884 0:d99384e36f64 491