2017年伊豆大島共同打ち上げ実験用電装モジュール搭載GPS測位プログラム
Dependents: Hybrid_interruptGPS Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET ... more
GPS_interrupt.cpp@0:74d8e952a3bd, 2017-01-02 (annotated)
- Committer:
- Gaku0606
- Date:
- Mon Jan 02 00:40:46 2017 +0000
- Revision:
- 0:74d8e952a3bd
- Child:
- 2:7be89bab6db9
GPS_interrupt.lib ver1.0.3
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Gaku0606 | 0:74d8e952a3bd | 1 | #include "mbed.h" |
Gaku0606 | 0:74d8e952a3bd | 2 | #include "GPS_interrupt.h" |
Gaku0606 | 0:74d8e952a3bd | 3 | double GPS_interrupt::latitude; |
Gaku0606 | 0:74d8e952a3bd | 4 | double GPS_interrupt::longitude; |
Gaku0606 | 0:74d8e952a3bd | 5 | int GPS_interrupt::year; |
Gaku0606 | 0:74d8e952a3bd | 6 | int GPS_interrupt::month; |
Gaku0606 | 0:74d8e952a3bd | 7 | int GPS_interrupt::day; |
Gaku0606 | 0:74d8e952a3bd | 8 | int GPS_interrupt::hour; |
Gaku0606 | 0:74d8e952a3bd | 9 | int GPS_interrupt::minutes; |
Gaku0606 | 0:74d8e952a3bd | 10 | double GPS_interrupt::seconds; |
Gaku0606 | 0:74d8e952a3bd | 11 | double GPS_interrupt::degree = 0; |
Gaku0606 | 0:74d8e952a3bd | 12 | double GPS_interrupt::knot = 0; |
Gaku0606 | 0:74d8e952a3bd | 13 | double GPS_interrupt::height = 0; |
Gaku0606 | 0:74d8e952a3bd | 14 | double GPS_interrupt::geoid = 0; |
Gaku0606 | 0:74d8e952a3bd | 15 | int GPS_interrupt::number = 0; |
Gaku0606 | 0:74d8e952a3bd | 16 | |
Gaku0606 | 0:74d8e952a3bd | 17 | char GPS_interrupt::gps_buffer_A[128]; |
Gaku0606 | 0:74d8e952a3bd | 18 | char GPS_interrupt::gps_buffer_B[128]; |
Gaku0606 | 0:74d8e952a3bd | 19 | char* GPS_interrupt::gps_read_buffer = gps_buffer_B; |
Gaku0606 | 0:74d8e952a3bd | 20 | bool GPS_interrupt::gps_readable = false; |
Gaku0606 | 0:74d8e952a3bd | 21 | |
Gaku0606 | 0:74d8e952a3bd | 22 | GPS_interrupt* GPS_interrupt::gps_irq; |
Gaku0606 | 0:74d8e952a3bd | 23 | |
Gaku0606 | 0:74d8e952a3bd | 24 | GPS_interrupt::GPS_interrupt(RawSerial *_gps, int baudrate){ |
Gaku0606 | 0:74d8e952a3bd | 25 | gps_irq = this; |
Gaku0606 | 0:74d8e952a3bd | 26 | gps = _gps; |
Gaku0606 | 0:74d8e952a3bd | 27 | gps->baud(baudrate); |
Gaku0606 | 0:74d8e952a3bd | 28 | gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq); |
Gaku0606 | 0:74d8e952a3bd | 29 | } |
Gaku0606 | 0:74d8e952a3bd | 30 | |
Gaku0606 | 0:74d8e952a3bd | 31 | void GPS_interrupt::initialize(){ |
Gaku0606 | 0:74d8e952a3bd | 32 | latitude = 0.0f; |
Gaku0606 | 0:74d8e952a3bd | 33 | longitude = 0.0f; |
Gaku0606 | 0:74d8e952a3bd | 34 | year = 0; |
Gaku0606 | 0:74d8e952a3bd | 35 | month = 0; |
Gaku0606 | 0:74d8e952a3bd | 36 | day = 0; |
Gaku0606 | 0:74d8e952a3bd | 37 | hour = 0; |
Gaku0606 | 0:74d8e952a3bd | 38 | minutes = 0; |
Gaku0606 | 0:74d8e952a3bd | 39 | seconds = 0; |
Gaku0606 | 0:74d8e952a3bd | 40 | memset(gps_buffer_A, '\0', 128); |
Gaku0606 | 0:74d8e952a3bd | 41 | memset(gps_buffer_B, '\0', 128); |
Gaku0606 | 0:74d8e952a3bd | 42 | gps_readable = false; |
Gaku0606 | 0:74d8e952a3bd | 43 | } |
Gaku0606 | 0:74d8e952a3bd | 44 | void GPS_interrupt::gps_auto_receive(){ |
Gaku0606 | 0:74d8e952a3bd | 45 | |
Gaku0606 | 0:74d8e952a3bd | 46 | static char str_temp[128] = {'\0'}; |
Gaku0606 | 0:74d8e952a3bd | 47 | static char temp = 0; |
Gaku0606 | 0:74d8e952a3bd | 48 | static bool start_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 49 | static unsigned char current = 0; |
Gaku0606 | 0:74d8e952a3bd | 50 | static char classify_AB = 0; |
Gaku0606 | 0:74d8e952a3bd | 51 | static char* save_buffer = gps_buffer_A; |
Gaku0606 | 0:74d8e952a3bd | 52 | |
Gaku0606 | 0:74d8e952a3bd | 53 | temp = gps->getc(); |
Gaku0606 | 0:74d8e952a3bd | 54 | if(temp == '$'){ |
Gaku0606 | 0:74d8e952a3bd | 55 | current = 1; |
Gaku0606 | 0:74d8e952a3bd | 56 | start_flag = true; |
Gaku0606 | 0:74d8e952a3bd | 57 | memset(save_buffer, '\0', 128);//初期化 |
Gaku0606 | 0:74d8e952a3bd | 58 | save_buffer[0] = '$'; |
Gaku0606 | 0:74d8e952a3bd | 59 | return; |
Gaku0606 | 0:74d8e952a3bd | 60 | } |
Gaku0606 | 0:74d8e952a3bd | 61 | if(start_flag){//1行スタート |
Gaku0606 | 0:74d8e952a3bd | 62 | if(temp == '\r'){//1行終了 |
Gaku0606 | 0:74d8e952a3bd | 63 | save_buffer[current] = '\0'; |
Gaku0606 | 0:74d8e952a3bd | 64 | start_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 65 | current = 1; |
Gaku0606 | 0:74d8e952a3bd | 66 | gps_read_buffer = save_buffer; |
Gaku0606 | 0:74d8e952a3bd | 67 | if(classify_AB){ |
Gaku0606 | 0:74d8e952a3bd | 68 | save_buffer = gps_buffer_A;//バッファ切換 |
Gaku0606 | 0:74d8e952a3bd | 69 | classify_AB = ~classify_AB; |
Gaku0606 | 0:74d8e952a3bd | 70 | } |
Gaku0606 | 0:74d8e952a3bd | 71 | else{ |
Gaku0606 | 0:74d8e952a3bd | 72 | save_buffer = gps_buffer_B; |
Gaku0606 | 0:74d8e952a3bd | 73 | classify_AB = ~classify_AB; |
Gaku0606 | 0:74d8e952a3bd | 74 | } |
Gaku0606 | 0:74d8e952a3bd | 75 | if(strncmp(gps_read_buffer, "$GPRMC", 6) == 0){ |
Gaku0606 | 0:74d8e952a3bd | 76 | strcpy(str_temp, gps_read_buffer); |
Gaku0606 | 0:74d8e952a3bd | 77 | if(processGPS(str_temp)){ |
Gaku0606 | 0:74d8e952a3bd | 78 | gps_readable = true; |
Gaku0606 | 0:74d8e952a3bd | 79 | } |
Gaku0606 | 0:74d8e952a3bd | 80 | } |
Gaku0606 | 0:74d8e952a3bd | 81 | } |
Gaku0606 | 0:74d8e952a3bd | 82 | else{ |
Gaku0606 | 0:74d8e952a3bd | 83 | save_buffer[current] = temp; |
Gaku0606 | 0:74d8e952a3bd | 84 | current++; |
Gaku0606 | 0:74d8e952a3bd | 85 | if(current >= 127){ |
Gaku0606 | 0:74d8e952a3bd | 86 | current = 1; |
Gaku0606 | 0:74d8e952a3bd | 87 | start_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 88 | } |
Gaku0606 | 0:74d8e952a3bd | 89 | } |
Gaku0606 | 0:74d8e952a3bd | 90 | } |
Gaku0606 | 0:74d8e952a3bd | 91 | |
Gaku0606 | 0:74d8e952a3bd | 92 | } |
Gaku0606 | 0:74d8e952a3bd | 93 | |
Gaku0606 | 0:74d8e952a3bd | 94 | bool GPS_interrupt::processGPS(char *line){ |
Gaku0606 | 0:74d8e952a3bd | 95 | |
Gaku0606 | 0:74d8e952a3bd | 96 | char *tok; //strtokで帰ってくる文字列のポインター |
Gaku0606 | 0:74d8e952a3bd | 97 | bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue |
Gaku0606 | 0:74d8e952a3bd | 98 | //bool result = false; |
Gaku0606 | 0:74d8e952a3bd | 99 | //char num[4] = "";//衛星数の文字データ |
Gaku0606 | 0:74d8e952a3bd | 100 | //char kaibatsu[8], g_height[8]; |
Gaku0606 | 0:74d8e952a3bd | 101 | //char data_GPS[256] =""; |
Gaku0606 | 0:74d8e952a3bd | 102 | char angle[8], speed[8]; |
Gaku0606 | 0:74d8e952a3bd | 103 | char ido[16] = "", keido[16] = ""; |
Gaku0606 | 0:74d8e952a3bd | 104 | |
Gaku0606 | 0:74d8e952a3bd | 105 | double X = 0, Y = 0; |
Gaku0606 | 0:74d8e952a3bd | 106 | double X_m = 0, Y_m = 0;//GPS座標の"分"の部分 |
Gaku0606 | 0:74d8e952a3bd | 107 | char zikann[11], hizuke[7]; |
Gaku0606 | 0:74d8e952a3bd | 108 | |
Gaku0606 | 0:74d8e952a3bd | 109 | //bool gga_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 110 | bool rmc_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 111 | |
Gaku0606 | 0:74d8e952a3bd | 112 | initialize(); |
Gaku0606 | 0:74d8e952a3bd | 113 | |
Gaku0606 | 0:74d8e952a3bd | 114 | //_____GPRMC___________________________________ |
Gaku0606 | 0:74d8e952a3bd | 115 | |
Gaku0606 | 0:74d8e952a3bd | 116 | //データ文字列の先頭から6文字目までが$GPRMCなら0が返ってくる |
Gaku0606 | 0:74d8e952a3bd | 117 | if (strncmp(line, "$GPRMC", 6) == 0){ |
Gaku0606 | 0:74d8e952a3bd | 118 | |
Gaku0606 | 0:74d8e952a3bd | 119 | tok = strtok(line, ","); |
Gaku0606 | 0:74d8e952a3bd | 120 | char tok_count = 0; |
Gaku0606 | 0:74d8e952a3bd | 121 | |
Gaku0606 | 0:74d8e952a3bd | 122 | //$GPRMC, 002519.799, V, , , , , 0.00, 0.00, 060180, , , N * 4A |
Gaku0606 | 0:74d8e952a3bd | 123 | |
Gaku0606 | 0:74d8e952a3bd | 124 | // 0 , 1 , 2,3,4,5,6, 7 , 8 , 9 ,10,11 ,12 |
Gaku0606 | 0:74d8e952a3bd | 125 | //$GPRMC,233514.000,A,3022.5291,N,13057.6141,E,0.41,335.09,030316,,,A*6C |
Gaku0606 | 0:74d8e952a3bd | 126 | |
Gaku0606 | 0:74d8e952a3bd | 127 | // 0 , 1 ,2, 3 ,4, 5 ,6, 7 , 8 , 9 ,1, 1 ,1 |
Gaku0606 | 0:74d8e952a3bd | 128 | // 0 1 2 |
Gaku0606 | 0:74d8e952a3bd | 129 | while (1){ |
Gaku0606 | 0:74d8e952a3bd | 130 | |
Gaku0606 | 0:74d8e952a3bd | 131 | //___UTC時間_____________ |
Gaku0606 | 0:74d8e952a3bd | 132 | if (tok_count == 1){ |
Gaku0606 | 0:74d8e952a3bd | 133 | strcpy(zikann, tok);//zikann = "233514.000\0" |
Gaku0606 | 0:74d8e952a3bd | 134 | } |
Gaku0606 | 0:74d8e952a3bd | 135 | //___確認,経度___________ |
Gaku0606 | 0:74d8e952a3bd | 136 | else if (tok_count == 2){ |
Gaku0606 | 0:74d8e952a3bd | 137 | if (strncmp(tok, "V", 1) == 0) return false; |
Gaku0606 | 0:74d8e952a3bd | 138 | else if (strcmp(tok, "A") == 0) rmc_flag = true; |
Gaku0606 | 0:74d8e952a3bd | 139 | } |
Gaku0606 | 0:74d8e952a3bd | 140 | else if (tok_count == 3){//緯度 |
Gaku0606 | 0:74d8e952a3bd | 141 | strcpy(ido, tok); |
Gaku0606 | 0:74d8e952a3bd | 142 | } |
Gaku0606 | 0:74d8e952a3bd | 143 | else if (tok_count == 4){//北緯か南緯か |
Gaku0606 | 0:74d8e952a3bd | 144 | if (strcmp(tok, "S") == 0) latSign = true; |
Gaku0606 | 0:74d8e952a3bd | 145 | } |
Gaku0606 | 0:74d8e952a3bd | 146 | else if (tok_count == 5){//経度 |
Gaku0606 | 0:74d8e952a3bd | 147 | strcpy(keido, tok); |
Gaku0606 | 0:74d8e952a3bd | 148 | } |
Gaku0606 | 0:74d8e952a3bd | 149 | else if (tok_count == 6){//東経か西経か |
Gaku0606 | 0:74d8e952a3bd | 150 | if (strcmp(tok, "W") == 0) lonSign = true; |
Gaku0606 | 0:74d8e952a3bd | 151 | } |
Gaku0606 | 0:74d8e952a3bd | 152 | else if (tok_count == 7){//速度(キロノット) |
Gaku0606 | 0:74d8e952a3bd | 153 | strcpy(speed, tok); |
Gaku0606 | 0:74d8e952a3bd | 154 | } |
Gaku0606 | 0:74d8e952a3bd | 155 | else if (tok_count == 8){//角度 |
Gaku0606 | 0:74d8e952a3bd | 156 | strcpy(angle, tok); |
Gaku0606 | 0:74d8e952a3bd | 157 | } |
Gaku0606 | 0:74d8e952a3bd | 158 | //___日付________________ |
Gaku0606 | 0:74d8e952a3bd | 159 | else if (tok_count == 9){ |
Gaku0606 | 0:74d8e952a3bd | 160 | strcpy(hizuke, tok);//hizuke = "030316\0" |
Gaku0606 | 0:74d8e952a3bd | 161 | } |
Gaku0606 | 0:74d8e952a3bd | 162 | tok = strtok(NULL, ",");//comma = ",\0" |
Gaku0606 | 0:74d8e952a3bd | 163 | tok_count++; |
Gaku0606 | 0:74d8e952a3bd | 164 | if (tok == NULL) break; |
Gaku0606 | 0:74d8e952a3bd | 165 | } |
Gaku0606 | 0:74d8e952a3bd | 166 | if (rmc_flag){ |
Gaku0606 | 0:74d8e952a3bd | 167 | rmc_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 168 | //ddmmyy |
Gaku0606 | 0:74d8e952a3bd | 169 | int dmy = 0; |
Gaku0606 | 0:74d8e952a3bd | 170 | dmy = atoi(hizuke); |
Gaku0606 | 0:74d8e952a3bd | 171 | day = dmy / 10000; //030316 / 10000 = 3 |
Gaku0606 | 0:74d8e952a3bd | 172 | month = (dmy - day * 10000) / 100;//30316 - 30000 = 316, 316/100= 3 |
Gaku0606 | 0:74d8e952a3bd | 173 | year = dmy - month * 100 - day * 10000 + 2000; |
Gaku0606 | 0:74d8e952a3bd | 174 | |
Gaku0606 | 0:74d8e952a3bd | 175 | //hhmmss.ss |
Gaku0606 | 0:74d8e952a3bd | 176 | seconds = atof(zikann);//sec = 233514.000 |
Gaku0606 | 0:74d8e952a3bd | 177 | hour = (int)(seconds / 10000.0); //233514/10000=(int)23 |
Gaku0606 | 0:74d8e952a3bd | 178 | minutes = (seconds - hour * 10000) / 100;//233514.0-230000=3514.0,3514.0/100=35 |
Gaku0606 | 0:74d8e952a3bd | 179 | seconds = seconds - ((double)minutes * 100.0 + (double)hour * 10000.0); |
Gaku0606 | 0:74d8e952a3bd | 180 | |
Gaku0606 | 0:74d8e952a3bd | 181 | // getLonLatのとき有効 |
Gaku0606 | 0:74d8e952a3bd | 182 | Y_m = atof(ido);//*Y_m = 3457.5571 |
Gaku0606 | 0:74d8e952a3bd | 183 | Y = (int)(Y_m / 100.0);//Y = 34.0 |
Gaku0606 | 0:74d8e952a3bd | 184 | Y_m = Y_m - Y * 100.0;//Y_m = 3457.5571 - 34*100 = 57.5571 |
Gaku0606 | 0:74d8e952a3bd | 185 | |
Gaku0606 | 0:74d8e952a3bd | 186 | X_m = atof(keido);//*X = 13057.6142 |
Gaku0606 | 0:74d8e952a3bd | 187 | X = (int)(X_m / 100.0);//X = 130.0 |
Gaku0606 | 0:74d8e952a3bd | 188 | X_m = X_m - X * 100.0;//X_m = 13057.6142 - 13000.0 = 57.6142 |
Gaku0606 | 0:74d8e952a3bd | 189 | |
Gaku0606 | 0:74d8e952a3bd | 190 | //GPS calculation |
Gaku0606 | 0:74d8e952a3bd | 191 | longitude = X + X_m / 60.0;// 34.959285 |
Gaku0606 | 0:74d8e952a3bd | 192 | latitude = Y + Y_m / 60.0;//137.090290 |
Gaku0606 | 0:74d8e952a3bd | 193 | |
Gaku0606 | 0:74d8e952a3bd | 194 | if (lonSign == true) longitude *= (-1);//西経なら経度を負に |
Gaku0606 | 0:74d8e952a3bd | 195 | if (latSign == true) latitude *= (-1);//南緯なら緯度を負に |
Gaku0606 | 0:74d8e952a3bd | 196 | knot = atof(speed); |
Gaku0606 | 0:74d8e952a3bd | 197 | degree = atof(angle); |
Gaku0606 | 0:74d8e952a3bd | 198 | if(longitude > 129.0 && latitude > 29.0){ |
Gaku0606 | 0:74d8e952a3bd | 199 | return true; |
Gaku0606 | 0:74d8e952a3bd | 200 | } |
Gaku0606 | 0:74d8e952a3bd | 201 | else return false; |
Gaku0606 | 0:74d8e952a3bd | 202 | } |
Gaku0606 | 0:74d8e952a3bd | 203 | else return false; |
Gaku0606 | 0:74d8e952a3bd | 204 | } |
Gaku0606 | 0:74d8e952a3bd | 205 | else return false; |
Gaku0606 | 0:74d8e952a3bd | 206 | } |
Gaku0606 | 0:74d8e952a3bd | 207 |