Hybrid_OB2021
/
Hybrid_OB2021_GROUND
v1
main.cpp@3:8e2d7ebdafb7, 2021-10-19 (annotated)
- Committer:
- imadaemi
- Date:
- Tue Oct 19 08:24:37 2021 +0000
- Revision:
- 3:8e2d7ebdafb7
- Parent:
- 2:41238982b498
v4
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
imadaemi | 0:f4fb5a0f6981 | 1 | #include "mbed.h" |
imadaemi | 0:f4fb5a0f6981 | 2 | #include "IM920.h" |
imadaemi | 0:f4fb5a0f6981 | 3 | |
imadaemi | 2:41238982b498 | 4 | #define RESET_TIME 1800 |
imadaemi | 2:41238982b498 | 5 | |
imadaemi | 2:41238982b498 | 6 | #define TEMP_MULTIPLIER 100 |
imadaemi | 2:41238982b498 | 7 | #define LPF_ALT_MULTIPLIER 100 |
imadaemi | 2:41238982b498 | 8 | #define LPF_VEL_MULTIPLIER 100 |
imadaemi | 2:41238982b498 | 9 | |
imadaemi | 0:f4fb5a0f6981 | 10 | // *************************************************** |
imadaemi | 0:f4fb5a0f6981 | 11 | // コンストラクタ |
imadaemi | 0:f4fb5a0f6981 | 12 | // *************************************************** |
imadaemi | 0:f4fb5a0f6981 | 13 | Serial pc(USBTX, USBRX, 115200); |
imadaemi | 0:f4fb5a0f6981 | 14 | Serial im920_serial(D1, D0, 115200); |
imadaemi | 0:f4fb5a0f6981 | 15 | |
imadaemi | 0:f4fb5a0f6981 | 16 | IM920 im920(im920_serial, pc, 115200); |
imadaemi | 0:f4fb5a0f6981 | 17 | |
imadaemi | 0:f4fb5a0f6981 | 18 | // *************************************************** |
imadaemi | 0:f4fb5a0f6981 | 19 | // 関数の宣言 |
imadaemi | 0:f4fb5a0f6981 | 20 | // *************************************************** |
imadaemi | 0:f4fb5a0f6981 | 21 | void ReadPC(); |
imadaemi | 0:f4fb5a0f6981 | 22 | void ReadSensorSetup(); |
imadaemi | 0:f4fb5a0f6981 | 23 | void ReadData(); |
imadaemi | 2:41238982b498 | 24 | void ReadLaunchTime(); |
imadaemi | 0:f4fb5a0f6981 | 25 | void Read0xA5(); |
imadaemi | 0:f4fb5a0f6981 | 26 | void Help(); |
imadaemi | 0:f4fb5a0f6981 | 27 | |
imadaemi | 0:f4fb5a0f6981 | 28 | // *************************************************** |
imadaemi | 0:f4fb5a0f6981 | 29 | // 変数の宣言 |
imadaemi | 0:f4fb5a0f6981 | 30 | // *************************************************** |
imadaemi | 2:41238982b498 | 31 | int minute; |
imadaemi | 2:41238982b498 | 32 | int second; |
imadaemi | 2:41238982b498 | 33 | int launch_minute; |
imadaemi | 2:41238982b498 | 34 | int launch_second; |
imadaemi | 2:41238982b498 | 35 | int flight_minute = 0; |
imadaemi | 2:41238982b498 | 36 | int flight_second = 0; |
imadaemi | 2:41238982b498 | 37 | float flight_time = 0; |
imadaemi | 2:41238982b498 | 38 | float duration_flight_time = 0; |
imadaemi | 2:41238982b498 | 39 | float main_time; |
imadaemi | 2:41238982b498 | 40 | float duration_main_time = 0; |
imadaemi | 2:41238982b498 | 41 | float launch_time = 0; |
imadaemi | 2:41238982b498 | 42 | int time_reset_counter = 0; |
imadaemi | 2:41238982b498 | 43 | int time_reset_counter_launch = 0; |
imadaemi | 2:41238982b498 | 44 | |
imadaemi | 2:41238982b498 | 45 | char mode; |
imadaemi | 2:41238982b498 | 46 | |
imadaemi | 2:41238982b498 | 47 | bool top_detect; |
imadaemi | 2:41238982b498 | 48 | bool flightpin_status; |
imadaemi | 2:41238982b498 | 49 | bool nich_status; |
imadaemi | 2:41238982b498 | 50 | bool save_data_status; |
imadaemi | 2:41238982b498 | 51 | char bitshift_sum; |
imadaemi | 1:69472741bd36 | 52 | |
imadaemi | 0:f4fb5a0f6981 | 53 | bool header_set = false; |
imadaemi | 0:f4fb5a0f6981 | 54 | char im_buf[16];//16なのって意味ある? |
imadaemi | 0:f4fb5a0f6981 | 55 | int im_buf_n = 0; |
imadaemi | 0:f4fb5a0f6981 | 56 | |
imadaemi | 2:41238982b498 | 57 | char eeprom_number; |
imadaemi | 2:41238982b498 | 58 | |
imadaemi | 0:f4fb5a0f6981 | 59 | float lat, lon, height; |
imadaemi | 2:41238982b498 | 60 | float press, temp, lpf_altitude; |
imadaemi | 2:41238982b498 | 61 | float lpf_velocity; |
imadaemi | 0:f4fb5a0f6981 | 62 | |
imadaemi | 0:f4fb5a0f6981 | 63 | float imu[6], mag[3]; |
imadaemi | 0:f4fb5a0f6981 | 64 | float mag_geo[3]; |
imadaemi | 0:f4fb5a0f6981 | 65 | |
imadaemi | 0:f4fb5a0f6981 | 66 | float voltage_main, voltage_sep; |
imadaemi | 0:f4fb5a0f6981 | 67 | float current_main, current_sep; |
imadaemi | 0:f4fb5a0f6981 | 68 | |
imadaemi | 0:f4fb5a0f6981 | 69 | // *************************************************** |
imadaemi | 0:f4fb5a0f6981 | 70 | // 無線のヘッダまとめ |
imadaemi | 0:f4fb5a0f6981 | 71 | // *************************************************** |
imadaemi | 0:f4fb5a0f6981 | 72 | const char HEADER_SETUP = 0x01; |
imadaemi | 2:41238982b498 | 73 | // 0x01, GPS, LPS22HB, MPU9250, MPU9250_MAG, INA226_MAIN, INA226_SEP |
imadaemi | 2:41238982b498 | 74 | // 1 1 1 1 1 1 1 |
imadaemi | 0:f4fb5a0f6981 | 75 | |
imadaemi | 2:41238982b498 | 76 | const char HEADER_DATA = 0x02; |
imadaemi | 2:41238982b498 | 77 | // 0xA2, duration_main_time, duration_flight_time, mode, nich_status, flightpin_status, lat, lon, height, press, temp, altitude, voltage_main, voltage_sep, current_main, current_sep |
imadaemi | 2:41238982b498 | 78 | // 1 4 4 1 0.125 0.125 4 4 4 4 4 4 4 4 4 4 |
imadaemi | 0:f4fb5a0f6981 | 79 | |
imadaemi | 2:41238982b498 | 80 | const char HEADER_LAUNCH_TIME = 0x03; |
imadaemi | 2:41238982b498 | 81 | |
imadaemi | 2:41238982b498 | 82 | const char HEADER_ECHO = 0x05; |
imadaemi | 0:f4fb5a0f6981 | 83 | // 0xA5,コマンド |
imadaemi | 0:f4fb5a0f6981 | 84 | // 1 1 |
imadaemi | 0:f4fb5a0f6981 | 85 | |
imadaemi | 0:f4fb5a0f6981 | 86 | // *************************************************** |
imadaemi | 0:f4fb5a0f6981 | 87 | // メイン関数 |
imadaemi | 0:f4fb5a0f6981 | 88 | // *************************************************** |
imadaemi | 0:f4fb5a0f6981 | 89 | int main() { |
imadaemi | 2:41238982b498 | 90 | //wait(0.5f); |
imadaemi | 0:f4fb5a0f6981 | 91 | pc.printf("Hello Ground!\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 92 | pc.attach(&ReadPC, Serial::RxIrq); |
imadaemi | 2:41238982b498 | 93 | im920.attach(&ReadLaunchTime, HEADER_LAUNCH_TIME); |
imadaemi | 0:f4fb5a0f6981 | 94 | im920.attach(&Read0xA5, HEADER_ECHO); |
imadaemi | 0:f4fb5a0f6981 | 95 | im920.attach(&ReadSensorSetup, HEADER_SETUP); |
imadaemi | 0:f4fb5a0f6981 | 96 | im920.attach(&ReadData, HEADER_DATA); |
imadaemi | 0:f4fb5a0f6981 | 97 | } |
imadaemi | 0:f4fb5a0f6981 | 98 | |
imadaemi | 0:f4fb5a0f6981 | 99 | void ReadPC(){ |
imadaemi | 0:f4fb5a0f6981 | 100 | char c = pc.getc(); |
imadaemi | 0:f4fb5a0f6981 | 101 | if(!header_set){ |
imadaemi | 0:f4fb5a0f6981 | 102 | im920.header((char)0xF0); |
imadaemi | 0:f4fb5a0f6981 | 103 | header_set = true; |
imadaemi | 0:f4fb5a0f6981 | 104 | } |
imadaemi | 0:f4fb5a0f6981 | 105 | |
imadaemi | 0:f4fb5a0f6981 | 106 | switch(c){ |
imadaemi | 2:41238982b498 | 107 | case 'U': |
imadaemi | 2:41238982b498 | 108 | im920.write((char)c); |
imadaemi | 2:41238982b498 | 109 | im_buf[im_buf_n] = 'U'; |
imadaemi | 2:41238982b498 | 110 | im_buf_n ++; |
imadaemi | 2:41238982b498 | 111 | pc.printf("INPUT : U\r\n"); |
imadaemi | 2:41238982b498 | 112 | if(header_set){ |
imadaemi | 2:41238982b498 | 113 | im920.send(); |
imadaemi | 2:41238982b498 | 114 | pc.printf("Send : %s\r\n", im_buf); |
imadaemi | 2:41238982b498 | 115 | header_set = false; |
imadaemi | 2:41238982b498 | 116 | for(int i = 0; i < im_buf_n; i ++){ |
imadaemi | 2:41238982b498 | 117 | im_buf[i] = '\0'; |
imadaemi | 2:41238982b498 | 118 | } |
imadaemi | 2:41238982b498 | 119 | im_buf_n = 0; |
imadaemi | 2:41238982b498 | 120 | } |
imadaemi | 2:41238982b498 | 121 | break; |
imadaemi | 2:41238982b498 | 122 | |
imadaemi | 2:41238982b498 | 123 | case 'O': |
imadaemi | 2:41238982b498 | 124 | im920.write((char)c); |
imadaemi | 2:41238982b498 | 125 | im_buf[im_buf_n] = 'O'; |
imadaemi | 2:41238982b498 | 126 | im_buf_n ++; |
imadaemi | 2:41238982b498 | 127 | pc.printf("INPUT : O\r\n"); |
imadaemi | 2:41238982b498 | 128 | break; |
imadaemi | 2:41238982b498 | 129 | |
imadaemi | 2:41238982b498 | 130 | case 'F': |
imadaemi | 2:41238982b498 | 131 | im920.write((char)c); |
imadaemi | 2:41238982b498 | 132 | im_buf[im_buf_n] = 'F'; |
imadaemi | 2:41238982b498 | 133 | im_buf_n ++; |
imadaemi | 2:41238982b498 | 134 | pc.printf("INPUT : F\r\n"); |
imadaemi | 2:41238982b498 | 135 | |
imadaemi | 2:41238982b498 | 136 | if(header_set){ |
imadaemi | 2:41238982b498 | 137 | im920.send(); |
imadaemi | 2:41238982b498 | 138 | pc.printf("Send : %s\r\n", im_buf); |
imadaemi | 2:41238982b498 | 139 | header_set = false; |
imadaemi | 2:41238982b498 | 140 | for(int i = 0; i < im_buf_n; i ++){ |
imadaemi | 2:41238982b498 | 141 | im_buf[i] = '\0'; |
imadaemi | 2:41238982b498 | 142 | } |
imadaemi | 2:41238982b498 | 143 | im_buf_n = 0; |
imadaemi | 2:41238982b498 | 144 | } |
imadaemi | 2:41238982b498 | 145 | break; |
imadaemi | 2:41238982b498 | 146 | |
imadaemi | 2:41238982b498 | 147 | case 'S': |
imadaemi | 2:41238982b498 | 148 | im920.write((char)c); |
imadaemi | 2:41238982b498 | 149 | im_buf[im_buf_n] = 'S'; |
imadaemi | 2:41238982b498 | 150 | im_buf_n ++; |
imadaemi | 2:41238982b498 | 151 | pc.printf("INPUT : S\r\n"); |
imadaemi | 2:41238982b498 | 152 | |
imadaemi | 2:41238982b498 | 153 | if(header_set){ |
imadaemi | 2:41238982b498 | 154 | im920.send(); |
imadaemi | 2:41238982b498 | 155 | pc.printf("Send : %s\r\n", im_buf); |
imadaemi | 2:41238982b498 | 156 | header_set = false; |
imadaemi | 2:41238982b498 | 157 | for(int i = 0; i < im_buf_n; i ++){ |
imadaemi | 2:41238982b498 | 158 | im_buf[i] = '\0'; |
imadaemi | 2:41238982b498 | 159 | } |
imadaemi | 2:41238982b498 | 160 | im_buf_n = 0; |
imadaemi | 2:41238982b498 | 161 | } |
imadaemi | 2:41238982b498 | 162 | break; |
imadaemi | 2:41238982b498 | 163 | |
imadaemi | 0:f4fb5a0f6981 | 164 | case '0': |
imadaemi | 0:f4fb5a0f6981 | 165 | im920.write((char)0x00); |
imadaemi | 0:f4fb5a0f6981 | 166 | im_buf[im_buf_n] = '0'; |
imadaemi | 0:f4fb5a0f6981 | 167 | im_buf_n ++; |
imadaemi | 0:f4fb5a0f6981 | 168 | pc.printf("INPUT : 0\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 169 | break; |
imadaemi | 0:f4fb5a0f6981 | 170 | |
imadaemi | 0:f4fb5a0f6981 | 171 | case '1': |
imadaemi | 0:f4fb5a0f6981 | 172 | im920.write((char)0x01); |
imadaemi | 0:f4fb5a0f6981 | 173 | im_buf[im_buf_n] = '1'; |
imadaemi | 0:f4fb5a0f6981 | 174 | im_buf_n ++; |
imadaemi | 0:f4fb5a0f6981 | 175 | pc.printf("INPUT : 1\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 176 | break; |
imadaemi | 0:f4fb5a0f6981 | 177 | |
imadaemi | 0:f4fb5a0f6981 | 178 | case '?': |
imadaemi | 0:f4fb5a0f6981 | 179 | Help(); |
imadaemi | 0:f4fb5a0f6981 | 180 | break; |
imadaemi | 0:f4fb5a0f6981 | 181 | |
imadaemi | 0:f4fb5a0f6981 | 182 | case '\n': |
imadaemi | 0:f4fb5a0f6981 | 183 | if(header_set){ |
imadaemi | 0:f4fb5a0f6981 | 184 | im920.send(); |
imadaemi | 0:f4fb5a0f6981 | 185 | pc.printf("Send : %s\r\n", im_buf); |
imadaemi | 0:f4fb5a0f6981 | 186 | header_set = false; |
imadaemi | 0:f4fb5a0f6981 | 187 | for(int i = 0; i < im_buf_n; i ++){ |
imadaemi | 0:f4fb5a0f6981 | 188 | im_buf[i] = '\0'; |
imadaemi | 0:f4fb5a0f6981 | 189 | } |
imadaemi | 0:f4fb5a0f6981 | 190 | im_buf_n = 0; |
imadaemi | 0:f4fb5a0f6981 | 191 | } |
imadaemi | 0:f4fb5a0f6981 | 192 | break; |
imadaemi | 0:f4fb5a0f6981 | 193 | } |
imadaemi | 0:f4fb5a0f6981 | 194 | } |
imadaemi | 0:f4fb5a0f6981 | 195 | |
imadaemi | 0:f4fb5a0f6981 | 196 | void ReadSensorSetup(){ |
imadaemi | 0:f4fb5a0f6981 | 197 | pc.printf("\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 198 | pc.printf("****Sensor Setup****\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 199 | |
imadaemi | 0:f4fb5a0f6981 | 200 | if(im920.data[1] == 1){ |
imadaemi | 0:f4fb5a0f6981 | 201 | pc.printf("GPS : OK\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 202 | }else{ |
imadaemi | 0:f4fb5a0f6981 | 203 | pc.printf("GPS : NG\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 204 | } |
imadaemi | 0:f4fb5a0f6981 | 205 | if(im920.data[2] == 1){ |
imadaemi | 0:f4fb5a0f6981 | 206 | pc.printf("LPS22HB : OK\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 207 | }else{ |
imadaemi | 0:f4fb5a0f6981 | 208 | pc.printf("LPS22HB : NG\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 209 | } |
imadaemi | 0:f4fb5a0f6981 | 210 | if(im920.data[3] == 1){ |
imadaemi | 0:f4fb5a0f6981 | 211 | pc.printf("MPU9250 : OK\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 212 | }else{ |
imadaemi | 0:f4fb5a0f6981 | 213 | pc.printf("MPU9250 : NG\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 214 | } |
imadaemi | 0:f4fb5a0f6981 | 215 | if(im920.data[4] == 1){ |
imadaemi | 0:f4fb5a0f6981 | 216 | pc.printf("MPU9250_MAG : OK\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 217 | }else{ |
imadaemi | 0:f4fb5a0f6981 | 218 | pc.printf("MPU9250_MAG : NG\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 219 | } |
imadaemi | 0:f4fb5a0f6981 | 220 | if(im920.data[5] == 1){ |
imadaemi | 0:f4fb5a0f6981 | 221 | pc.printf("INA226_MAIN : OK\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 222 | }else{ |
imadaemi | 0:f4fb5a0f6981 | 223 | pc.printf("INA226_MAIN : NG\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 224 | } |
imadaemi | 0:f4fb5a0f6981 | 225 | if(im920.data[6] == 1){ |
imadaemi | 0:f4fb5a0f6981 | 226 | pc.printf("INA226_SEP : OK\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 227 | }else{ |
imadaemi | 0:f4fb5a0f6981 | 228 | pc.printf("INA226_SEP : NG\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 229 | } |
imadaemi | 0:f4fb5a0f6981 | 230 | |
imadaemi | 0:f4fb5a0f6981 | 231 | for(int i = 0; i < 20; i++){ |
imadaemi | 0:f4fb5a0f6981 | 232 | pc.printf("*"); |
imadaemi | 0:f4fb5a0f6981 | 233 | } |
imadaemi | 0:f4fb5a0f6981 | 234 | pc.printf("\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 235 | } |
imadaemi | 0:f4fb5a0f6981 | 236 | |
imadaemi | 0:f4fb5a0f6981 | 237 | void ReadData(){ |
imadaemi | 1:69472741bd36 | 238 | //pc.printf("***Acquired Data****\r\n"); |
imadaemi | 1:69472741bd36 | 239 | pc.printf("***Recieved Data****\r\n"); |
imadaemi | 2:41238982b498 | 240 | |
imadaemi | 0:f4fb5a0f6981 | 241 | int bit = 1; |
imadaemi | 2:41238982b498 | 242 | |
imadaemi | 2:41238982b498 | 243 | time_reset_counter = (short)im920.toShort(bit); |
imadaemi | 2:41238982b498 | 244 | bit += 2; |
imadaemi | 2:41238982b498 | 245 | |
imadaemi | 2:41238982b498 | 246 | main_time = (float)im920.toFloat(bit); |
imadaemi | 2:41238982b498 | 247 | main_time /= 1000; |
imadaemi | 2:41238982b498 | 248 | bit += 4; |
imadaemi | 2:41238982b498 | 249 | |
imadaemi | 2:41238982b498 | 250 | minute = main_time / 60 + time_reset_counter*30; |
imadaemi | 2:41238982b498 | 251 | //pc.printf("%d\t%f\r\n",minute,minute); |
imadaemi | 2:41238982b498 | 252 | second = (int)main_time % 60; |
imadaemi | 2:41238982b498 | 253 | |
imadaemi | 2:41238982b498 | 254 | //duration_main_time = main_time + RESET_TIME*time_reset_counter; |
imadaemi | 2:41238982b498 | 255 | pc.printf("Main Time : %d : %02d\r\n",minute,second); |
imadaemi | 2:41238982b498 | 256 | //pc.printf("Main Time : %f\r\n",duration_main_time); |
imadaemi | 2:41238982b498 | 257 | |
imadaemi | 2:41238982b498 | 258 | pc.printf("Launch Time : %d : %02d\r\n",launch_minute,launch_second); |
imadaemi | 2:41238982b498 | 259 | |
imadaemi | 2:41238982b498 | 260 | if(mode >= 0x04){ |
imadaemi | 2:41238982b498 | 261 | if(second >= launch_second){ |
imadaemi | 2:41238982b498 | 262 | flight_minute = minute - launch_minute; |
imadaemi | 2:41238982b498 | 263 | flight_second = second - launch_second; |
imadaemi | 2:41238982b498 | 264 | }else{ |
imadaemi | 2:41238982b498 | 265 | flight_minute = minute - launch_minute - 1; |
imadaemi | 2:41238982b498 | 266 | flight_second = 60 + second - launch_second; |
imadaemi | 2:41238982b498 | 267 | } |
imadaemi | 2:41238982b498 | 268 | pc.printf("Flight Time : %d : %02d\r\n",flight_minute,flight_second); |
imadaemi | 2:41238982b498 | 269 | }else{ |
imadaemi | 2:41238982b498 | 270 | pc.printf("Flight Time : %d : %02d\r\n",flight_minute,flight_second); |
imadaemi | 2:41238982b498 | 271 | } |
imadaemi | 2:41238982b498 | 272 | |
imadaemi | 2:41238982b498 | 273 | mode = im920.data[bit]; |
imadaemi | 2:41238982b498 | 274 | bit += 1; |
imadaemi | 2:41238982b498 | 275 | switch(mode){ |
imadaemi | 2:41238982b498 | 276 | case 0x00: |
imadaemi | 2:41238982b498 | 277 | pc.printf("Mode : SETUP\r\n"); |
imadaemi | 2:41238982b498 | 278 | break; |
imadaemi | 2:41238982b498 | 279 | |
imadaemi | 2:41238982b498 | 280 | case 0x01: |
imadaemi | 2:41238982b498 | 281 | pc.printf("Mode : SAFETY\r\n"); |
imadaemi | 2:41238982b498 | 282 | break; |
imadaemi | 2:41238982b498 | 283 | |
imadaemi | 2:41238982b498 | 284 | case 0x02: |
imadaemi | 2:41238982b498 | 285 | pc.printf("Mode : TO_ONCALL\r\n"); |
imadaemi | 2:41238982b498 | 286 | break; |
imadaemi | 2:41238982b498 | 287 | |
imadaemi | 2:41238982b498 | 288 | case 0x03: |
imadaemi | 2:41238982b498 | 289 | pc.printf("Mode : ONCALL\r\n"); |
imadaemi | 2:41238982b498 | 290 | break; |
imadaemi | 2:41238982b498 | 291 | |
imadaemi | 2:41238982b498 | 292 | case 0x04: |
imadaemi | 2:41238982b498 | 293 | pc.printf("Mode : TO_FLIGHT\r\n"); |
imadaemi | 2:41238982b498 | 294 | break; |
imadaemi | 2:41238982b498 | 295 | |
imadaemi | 2:41238982b498 | 296 | case 0x05: |
imadaemi | 2:41238982b498 | 297 | pc.printf("Mode : FLIGHT\r\n"); |
imadaemi | 2:41238982b498 | 298 | break; |
imadaemi | 2:41238982b498 | 299 | |
imadaemi | 2:41238982b498 | 300 | case 0x06: |
imadaemi | 2:41238982b498 | 301 | pc.printf("Mode : TO_SEPARATE\r\n"); |
imadaemi | 2:41238982b498 | 302 | break; |
imadaemi | 2:41238982b498 | 303 | |
imadaemi | 2:41238982b498 | 304 | case 0x07: |
imadaemi | 2:41238982b498 | 305 | pc.printf("Mode : SEPARATE\r\n"); |
imadaemi | 2:41238982b498 | 306 | break; |
imadaemi | 2:41238982b498 | 307 | |
imadaemi | 2:41238982b498 | 308 | case 0x08: |
imadaemi | 2:41238982b498 | 309 | pc.printf("Mode : TO_DESCEND\r\n"); |
imadaemi | 2:41238982b498 | 310 | break; |
imadaemi | 2:41238982b498 | 311 | |
imadaemi | 2:41238982b498 | 312 | case 0x09: |
imadaemi | 2:41238982b498 | 313 | pc.printf("Mode : DESCEND\r\n"); |
imadaemi | 2:41238982b498 | 314 | break; |
imadaemi | 2:41238982b498 | 315 | |
imadaemi | 2:41238982b498 | 316 | case 0x0a: |
imadaemi | 2:41238982b498 | 317 | pc.printf("Mode : TO_COLLECTION\r\n"); |
imadaemi | 2:41238982b498 | 318 | break; |
imadaemi | 2:41238982b498 | 319 | |
imadaemi | 2:41238982b498 | 320 | case 0x0b: |
imadaemi | 2:41238982b498 | 321 | pc.printf("Mode : COLLECTION\r\n"); |
imadaemi | 2:41238982b498 | 322 | break; |
imadaemi | 2:41238982b498 | 323 | } |
imadaemi | 2:41238982b498 | 324 | |
imadaemi | 2:41238982b498 | 325 | /*bool1個を1バイトで送る |
imadaemi | 2:41238982b498 | 326 | flightpin_status = im920.data[bit]; |
imadaemi | 2:41238982b498 | 327 | bit += 1; |
imadaemi | 2:41238982b498 | 328 | pc.printf("FlightPin Status: %d\r\n",flightpin_status); |
imadaemi | 2:41238982b498 | 329 | |
imadaemi | 1:69472741bd36 | 330 | nich_status = im920.data[bit]; |
imadaemi | 1:69472741bd36 | 331 | bit += 1; |
imadaemi | 2:41238982b498 | 332 | pc.printf("Nichrome Status : %d\r\n",nich_status); |
imadaemi | 2:41238982b498 | 333 | */ |
imadaemi | 2:41238982b498 | 334 | |
imadaemi | 2:41238982b498 | 335 | //bool8個を1バイトで送る |
imadaemi | 2:41238982b498 | 336 | bitshift_sum = im920.data[bit]; |
imadaemi | 2:41238982b498 | 337 | bit += 1; |
imadaemi | 2:41238982b498 | 338 | //pc.printf("Bitshift Sum : %d\r\n",bitshift_sum); |
imadaemi | 2:41238982b498 | 339 | |
imadaemi | 2:41238982b498 | 340 | flightpin_status = 0x01 & (bitshift_sum); |
imadaemi | 2:41238982b498 | 341 | pc.printf("FlightPin Status: %d\r\n",flightpin_status); |
imadaemi | 2:41238982b498 | 342 | |
imadaemi | 2:41238982b498 | 343 | //nich_status = bitshift_sum >> 1; |
imadaemi | 2:41238982b498 | 344 | nich_status = 0x02 & (bitshift_sum); |
imadaemi | 2:41238982b498 | 345 | pc.printf("Nichrome Status : %d\r\n",nich_status); |
imadaemi | 2:41238982b498 | 346 | |
imadaemi | 2:41238982b498 | 347 | top_detect = 0x04 & (bitshift_sum); |
imadaemi | 2:41238982b498 | 348 | pc.printf("Top Detect : %d\r\n",top_detect); |
imadaemi | 2:41238982b498 | 349 | |
imadaemi | 2:41238982b498 | 350 | save_data_status = 0x08 & (bitshift_sum); |
imadaemi | 2:41238982b498 | 351 | pc.printf("Save Data Status: %d\r\n",save_data_status); |
imadaemi | 2:41238982b498 | 352 | |
imadaemi | 2:41238982b498 | 353 | eeprom_number = im920.data[bit]; |
imadaemi | 2:41238982b498 | 354 | bit += 1; |
imadaemi | 2:41238982b498 | 355 | pc.printf("eeprom_number : %d\r\n",eeprom_number); |
imadaemi | 2:41238982b498 | 356 | |
imadaemi | 0:f4fb5a0f6981 | 357 | lat = (float)im920.toFloat(bit); |
imadaemi | 0:f4fb5a0f6981 | 358 | bit += 4; |
imadaemi | 1:69472741bd36 | 359 | pc.printf("Latitude : %.7f\r\n",lat); |
imadaemi | 0:f4fb5a0f6981 | 360 | lon = (float)im920.toFloat(bit); |
imadaemi | 0:f4fb5a0f6981 | 361 | bit += 4; |
imadaemi | 1:69472741bd36 | 362 | pc.printf("Longitude : %.7f\r\n",lon); |
imadaemi | 2:41238982b498 | 363 | /* |
imadaemi | 1:69472741bd36 | 364 | height = (float)im920.toFloat(bit); |
imadaemi | 0:f4fb5a0f6981 | 365 | bit += 4; |
imadaemi | 1:69472741bd36 | 366 | pc.printf("Height : %.7f\r\n",height); |
imadaemi | 2:41238982b498 | 367 | */ |
imadaemi | 0:f4fb5a0f6981 | 368 | press = (float)im920.toFloat(bit); |
imadaemi | 0:f4fb5a0f6981 | 369 | bit += 4; |
imadaemi | 2:41238982b498 | 370 | pc.printf("Pressure : %.2f [hPa]\r\n", press); |
imadaemi | 2:41238982b498 | 371 | temp = (short)im920.toShort(bit); |
imadaemi | 2:41238982b498 | 372 | temp /= TEMP_MULTIPLIER; |
imadaemi | 2:41238982b498 | 373 | bit += 2; |
imadaemi | 2:41238982b498 | 374 | pc.printf("Temperarure : %.2f [degC]\r\n", temp); |
imadaemi | 2:41238982b498 | 375 | lpf_altitude = (short)im920.toShort(bit); |
imadaemi | 2:41238982b498 | 376 | lpf_altitude /= LPF_ALT_MULTIPLIER; |
imadaemi | 2:41238982b498 | 377 | bit += 2; |
imadaemi | 2:41238982b498 | 378 | pc.printf("LPF Altitude : %.2f [m]\r\n", lpf_altitude); |
imadaemi | 2:41238982b498 | 379 | |
imadaemi | 2:41238982b498 | 380 | lpf_velocity = (short)im920.toShort(bit); |
imadaemi | 2:41238982b498 | 381 | lpf_velocity /= LPF_VEL_MULTIPLIER; |
imadaemi | 2:41238982b498 | 382 | bit += 2; |
imadaemi | 2:41238982b498 | 383 | pc.printf("Lpf Velocity : %f [m/s]\r\n", lpf_velocity); |
imadaemi | 0:f4fb5a0f6981 | 384 | |
imadaemi | 2:41238982b498 | 385 | voltage_main = (short)im920.toShort(bit); |
imadaemi | 2:41238982b498 | 386 | voltage_main /= 1000; |
imadaemi | 2:41238982b498 | 387 | bit += 2; |
imadaemi | 2:41238982b498 | 388 | pc.printf("Voltage Main : %.2f [V]\r\n",voltage_main); |
imadaemi | 2:41238982b498 | 389 | current_main = (short)im920.toShort(bit); |
imadaemi | 2:41238982b498 | 390 | current_main /= 1000; |
imadaemi | 2:41238982b498 | 391 | bit += 2; |
imadaemi | 2:41238982b498 | 392 | pc.printf("Crrent Main : %.2f [A]\r\n",current_main); |
imadaemi | 2:41238982b498 | 393 | voltage_sep = (short)im920.toShort(bit); |
imadaemi | 2:41238982b498 | 394 | voltage_sep /= 1000; |
imadaemi | 2:41238982b498 | 395 | bit += 2; |
imadaemi | 2:41238982b498 | 396 | pc.printf("Voltage Sep : %.2f [V]\r\n",voltage_sep); |
imadaemi | 2:41238982b498 | 397 | current_sep = (short)im920.toShort(bit); |
imadaemi | 2:41238982b498 | 398 | current_sep /= 1000; |
imadaemi | 0:f4fb5a0f6981 | 399 | bit += 4; |
imadaemi | 2:41238982b498 | 400 | pc.printf("Current Sep : %.2f [A]\r\n",current_sep); |
imadaemi | 0:f4fb5a0f6981 | 401 | |
imadaemi | 0:f4fb5a0f6981 | 402 | for(int i = 0; i < 20; i++){ |
imadaemi | 0:f4fb5a0f6981 | 403 | pc.printf("*"); |
imadaemi | 0:f4fb5a0f6981 | 404 | } |
imadaemi | 0:f4fb5a0f6981 | 405 | pc.printf("\r\n\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 406 | } |
imadaemi | 0:f4fb5a0f6981 | 407 | |
imadaemi | 2:41238982b498 | 408 | void ReadLaunchTime(){ |
imadaemi | 2:41238982b498 | 409 | int bit = 1; |
imadaemi | 2:41238982b498 | 410 | launch_time = (float)im920.toFloat(bit); |
imadaemi | 2:41238982b498 | 411 | bit += 4; |
imadaemi | 2:41238982b498 | 412 | launch_time /= 1000; |
imadaemi | 2:41238982b498 | 413 | time_reset_counter_launch = (short)im920.toShort(bit); |
imadaemi | 2:41238982b498 | 414 | bit += 2; |
imadaemi | 3:8e2d7ebdafb7 | 415 | launch_minute = launch_time / 60 + time_reset_counter_launch*30; |
imadaemi | 2:41238982b498 | 416 | launch_second = (int)launch_time % 60; |
imadaemi | 2:41238982b498 | 417 | pc.printf("Get launch time\r\n"); |
imadaemi | 2:41238982b498 | 418 | pc.printf("launch time : %f\r\n",launch_time); |
imadaemi | 2:41238982b498 | 419 | } |
imadaemi | 2:41238982b498 | 420 | |
imadaemi | 0:f4fb5a0f6981 | 421 | void Read0xA5(){ |
imadaemi | 0:f4fb5a0f6981 | 422 | for(int i = 0; i < 20; i++){ |
imadaemi | 0:f4fb5a0f6981 | 423 | pc.printf("*"); |
imadaemi | 0:f4fb5a0f6981 | 424 | } |
imadaemi | 0:f4fb5a0f6981 | 425 | pc.printf("\r\nIM920 ECHO : "); |
imadaemi | 0:f4fb5a0f6981 | 426 | |
imadaemi | 0:f4fb5a0f6981 | 427 | switch(im920.data[1]){ |
imadaemi | 2:41238982b498 | 428 | case 'U': |
imadaemi | 2:41238982b498 | 429 | pc.printf("MODE SETUP\r\n"); |
imadaemi | 2:41238982b498 | 430 | break; |
imadaemi | 2:41238982b498 | 431 | |
imadaemi | 2:41238982b498 | 432 | case 'O': |
imadaemi | 2:41238982b498 | 433 | pc.printf("MODE ONCALL\r\n"); |
imadaemi | 2:41238982b498 | 434 | break; |
imadaemi | 2:41238982b498 | 435 | |
imadaemi | 2:41238982b498 | 436 | case 'F': |
imadaemi | 2:41238982b498 | 437 | pc.printf("MODE FLIGHT\r\n"); |
imadaemi | 2:41238982b498 | 438 | break; |
imadaemi | 2:41238982b498 | 439 | |
imadaemi | 2:41238982b498 | 440 | case 'S': |
imadaemi | 2:41238982b498 | 441 | pc.printf("MODE SEPARATE\r\n"); |
imadaemi | 2:41238982b498 | 442 | break; |
imadaemi | 2:41238982b498 | 443 | |
imadaemi | 2:41238982b498 | 444 | case 'D': |
imadaemi | 2:41238982b498 | 445 | pc.printf("MODE DESCEND\r\n"); |
imadaemi | 2:41238982b498 | 446 | break; |
imadaemi | 2:41238982b498 | 447 | |
imadaemi | 2:41238982b498 | 448 | case 'C': |
imadaemi | 2:41238982b498 | 449 | pc.printf("MODE COLLECTION\r\n"); |
imadaemi | 2:41238982b498 | 450 | break; |
imadaemi | 2:41238982b498 | 451 | |
imadaemi | 0:f4fb5a0f6981 | 452 | case 0x01: |
imadaemi | 0:f4fb5a0f6981 | 453 | pc.printf("SEPARATE\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 454 | break; |
imadaemi | 0:f4fb5a0f6981 | 455 | |
imadaemi | 0:f4fb5a0f6981 | 456 | case 0x00: |
imadaemi | 0:f4fb5a0f6981 | 457 | pc.printf("STOP SEPARATE\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 458 | break; |
imadaemi | 0:f4fb5a0f6981 | 459 | } |
imadaemi | 0:f4fb5a0f6981 | 460 | |
imadaemi | 0:f4fb5a0f6981 | 461 | for(int i = 0; i < 20; i++){ |
imadaemi | 0:f4fb5a0f6981 | 462 | pc.printf("*"); |
imadaemi | 0:f4fb5a0f6981 | 463 | } |
imadaemi | 0:f4fb5a0f6981 | 464 | pc.printf("\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 465 | } |
imadaemi | 0:f4fb5a0f6981 | 466 | |
imadaemi | 0:f4fb5a0f6981 | 467 | void Help(){ |
imadaemi | 0:f4fb5a0f6981 | 468 | pc.printf("\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 469 | pc.printf("**I can help you!***\r\n"); |
imadaemi | 2:41238982b498 | 470 | pc.printf("U : Mode Setup\r\n"); |
imadaemi | 2:41238982b498 | 471 | pc.printf("O : Mode Oncall\r\n"); |
imadaemi | 2:41238982b498 | 472 | pc.printf("F : Mode Flight\r\n"); |
imadaemi | 2:41238982b498 | 473 | pc.printf("S : Mode Separate\r\n"); |
imadaemi | 3:8e2d7ebdafb7 | 474 | // pc.printf("D : Mode Descend\r\n"); |
imadaemi | 3:8e2d7ebdafb7 | 475 | // pc.printf("C : Mode Collection\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 476 | pc.printf("0 : Stop Separate\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 477 | pc.printf("1 : Separate\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 478 | pc.printf("? : Help\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 479 | pc.printf("********************\r\n\r\n"); |
imadaemi | 0:f4fb5a0f6981 | 480 | } |