PQ_Hybrid_Electrical_Equipment_Team
/
Hybrid_IZU201811_GNDv1
PQ-013 Felix-Luminousの地上局プログラム ES920LR使用
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?
User | Revision | Line number | New 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 |