2017年伊豆大島共同打ち上げ実験用電装モジュール搭載GPS測位プログラム
Dependents: Hybrid_interruptGPS Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET ... more
GPS_interrupt.cpp@4:758f97bee95a, 2017-01-15 (annotated)
- Committer:
- Gaku0606
- Date:
- Sun Jan 15 20:01:46 2017 +0000
- Revision:
- 4:758f97bee95a
- Parent:
- 3:8e66ec281888
- Child:
- 6:2f91c71d64b1
GPS_interrupt.h ver 1.3.5
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 | 4:758f97bee95a | 3 | |
Gaku0606 | 4:758f97bee95a | 4 | /*double GPS_interrupt::latitude; |
Gaku0606 | 0:74d8e952a3bd | 5 | double GPS_interrupt::longitude; |
Gaku0606 | 0:74d8e952a3bd | 6 | int GPS_interrupt::year; |
Gaku0606 | 0:74d8e952a3bd | 7 | int GPS_interrupt::month; |
Gaku0606 | 0:74d8e952a3bd | 8 | int GPS_interrupt::day; |
Gaku0606 | 0:74d8e952a3bd | 9 | int GPS_interrupt::hour; |
Gaku0606 | 0:74d8e952a3bd | 10 | int GPS_interrupt::minutes; |
Gaku0606 | 0:74d8e952a3bd | 11 | double GPS_interrupt::seconds; |
Gaku0606 | 0:74d8e952a3bd | 12 | double GPS_interrupt::degree = 0; |
Gaku0606 | 0:74d8e952a3bd | 13 | double GPS_interrupt::knot = 0; |
Gaku0606 | 0:74d8e952a3bd | 14 | double GPS_interrupt::height = 0; |
Gaku0606 | 0:74d8e952a3bd | 15 | double GPS_interrupt::geoid = 0; |
Gaku0606 | 0:74d8e952a3bd | 16 | int GPS_interrupt::number = 0; |
Gaku0606 | 0:74d8e952a3bd | 17 | |
Gaku0606 | 0:74d8e952a3bd | 18 | char GPS_interrupt::gps_buffer_A[128]; |
Gaku0606 | 0:74d8e952a3bd | 19 | char GPS_interrupt::gps_buffer_B[128]; |
Gaku0606 | 0:74d8e952a3bd | 20 | char* GPS_interrupt::gps_read_buffer = gps_buffer_B; |
Gaku0606 | 4:758f97bee95a | 21 | bool GPS_interrupt::gps_readable = false;*/ |
Gaku0606 | 0:74d8e952a3bd | 22 | |
Gaku0606 | 4:758f97bee95a | 23 | //GPS_interrupt* GPS_interrupt::gps_irq; |
Gaku0606 | 0:74d8e952a3bd | 24 | |
Gaku0606 | 2:7be89bab6db9 | 25 | //Timeout timeout_clear; |
Gaku0606 | 2:7be89bab6db9 | 26 | |
Gaku0606 | 4:758f97bee95a | 27 | void GPS_interrupt::debug(bool tf){ |
Gaku0606 | 4:758f97bee95a | 28 | if(tf){ |
Gaku0606 | 4:758f97bee95a | 29 | debugFlag = true; |
Gaku0606 | 4:758f97bee95a | 30 | } |
Gaku0606 | 4:758f97bee95a | 31 | else{ |
Gaku0606 | 4:758f97bee95a | 32 | debugFlag = false; |
Gaku0606 | 4:758f97bee95a | 33 | } |
Gaku0606 | 2:7be89bab6db9 | 34 | } |
Gaku0606 | 2:7be89bab6db9 | 35 | |
Gaku0606 | 2:7be89bab6db9 | 36 | unsigned char GPS_interrupt::checkSum(char *str){ |
Gaku0606 | 2:7be89bab6db9 | 37 | int num = strlen(str); |
Gaku0606 | 2:7be89bab6db9 | 38 | unsigned char val = 0; |
Gaku0606 | 2:7be89bab6db9 | 39 | for(int i = 0; i< num; i++){ |
Gaku0606 | 2:7be89bab6db9 | 40 | val = val ^ str[i]; |
Gaku0606 | 2:7be89bab6db9 | 41 | } |
Gaku0606 | 2:7be89bab6db9 | 42 | return val; |
Gaku0606 | 2:7be89bab6db9 | 43 | } |
Gaku0606 | 2:7be89bab6db9 | 44 | |
Gaku0606 | 4:758f97bee95a | 45 | GPS_interrupt::GPS_interrupt(RawSerial *_gps, int _frequency){ |
Gaku0606 | 2:7be89bab6db9 | 46 | |
Gaku0606 | 4:758f97bee95a | 47 | debugFlag = false; |
Gaku0606 | 4:758f97bee95a | 48 | initialize(); |
Gaku0606 | 4:758f97bee95a | 49 | baudrate = 115200;//_baudrate; |
Gaku0606 | 2:7be89bab6db9 | 50 | frequency = _frequency; |
Gaku0606 | 0:74d8e952a3bd | 51 | gps_irq = this; |
Gaku0606 | 0:74d8e952a3bd | 52 | gps = _gps; |
Gaku0606 | 4:758f97bee95a | 53 | gps->baud(9600); |
Gaku0606 | 4:758f97bee95a | 54 | if(baudrate == 9600){ |
Gaku0606 | 4:758f97bee95a | 55 | gps->printf("$PMTK251,9600*17\r\n"); |
Gaku0606 | 4:758f97bee95a | 56 | } |
Gaku0606 | 4:758f97bee95a | 57 | else if(baudrate == 19200){ |
Gaku0606 | 4:758f97bee95a | 58 | gps->printf("$PMTK251,19200*22\r\n"); |
Gaku0606 | 4:758f97bee95a | 59 | } |
Gaku0606 | 4:758f97bee95a | 60 | else if( baudrate == 38400){ |
Gaku0606 | 4:758f97bee95a | 61 | gps->printf("$PMTK251,38400*27\r\n"); |
Gaku0606 | 4:758f97bee95a | 62 | } |
Gaku0606 | 4:758f97bee95a | 63 | else if(baudrate == 56700){ |
Gaku0606 | 4:758f97bee95a | 64 | gps->printf("$PMTK251,57600*2C\r\n"); |
Gaku0606 | 4:758f97bee95a | 65 | } |
Gaku0606 | 4:758f97bee95a | 66 | else if(baudrate == 115200){ |
Gaku0606 | 4:758f97bee95a | 67 | gps->printf("$PMTK251,115200*1F\r\n"); |
Gaku0606 | 4:758f97bee95a | 68 | wait(0.1); |
Gaku0606 | 4:758f97bee95a | 69 | gps->printf("$PMTK251,115200*1F\r\n"); |
Gaku0606 | 4:758f97bee95a | 70 | wait(0.2); |
Gaku0606 | 4:758f97bee95a | 71 | gps->printf("$PMTK251,115200*1F\r\n"); |
Gaku0606 | 4:758f97bee95a | 72 | wait(0.2); |
Gaku0606 | 4:758f97bee95a | 73 | } |
Gaku0606 | 4:758f97bee95a | 74 | gps->baud(115200); |
Gaku0606 | 4:758f97bee95a | 75 | baudrate = 115200; |
Gaku0606 | 2:7be89bab6db9 | 76 | wait(0.1); |
Gaku0606 | 4:758f97bee95a | 77 | |
Gaku0606 | 4:758f97bee95a | 78 | //gps->printf("$PMTK351,1*28\r\n");//$PMTK351,1*28<CR><LF> :Enable QZSS NMEA format |
Gaku0606 | 2:7be89bab6db9 | 79 | unsigned char checksum = 0; |
Gaku0606 | 4:758f97bee95a | 80 | checksum = checkSum("PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0"); |
Gaku0606 | 2:7be89bab6db9 | 81 | //$PMTK314,GLL,RMC,VTG,GGA,GSA,GSV,0,0,0,0,0,0,0,0,0,0,0,ZDA,MCHN,チェックサム |
Gaku0606 | 4:758f97bee95a | 82 | gps->printf("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*%02X\r\n", checksum); |
Gaku0606 | 4:758f97bee95a | 83 | wait(0.2); |
Gaku0606 | 4:758f97bee95a | 84 | gps->printf("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*%02X\r\n", checksum); |
Gaku0606 | 4:758f97bee95a | 85 | wait(0.2); |
Gaku0606 | 4:758f97bee95a | 86 | gps->printf("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*%02X\r\n", checksum); |
Gaku0606 | 4:758f97bee95a | 87 | wait(0.2); |
Gaku0606 | 4:758f97bee95a | 88 | if(_frequency == 1) gps->printf("$PMTK220,1000*1F\r\n"); |
Gaku0606 | 4:758f97bee95a | 89 | else if(_frequency == 2) gps->printf("$PMTK220,500*2B\r\n"); |
Gaku0606 | 4:758f97bee95a | 90 | else if(_frequency == 3) gps->printf("$PMTK220,333*2D\r\n"); |
Gaku0606 | 4:758f97bee95a | 91 | else if(_frequency == 4) gps->printf("$PMTK220,250*29\r\n"); |
Gaku0606 | 4:758f97bee95a | 92 | else if(_frequency == 5) gps->printf("$PMTK220,200*2C\r\n"); |
Gaku0606 | 4:758f97bee95a | 93 | else if(_frequency == 10) gps->printf("$PMTK220,100*2F\r\n"); |
Gaku0606 | 4:758f97bee95a | 94 | wait(0.2); |
Gaku0606 | 0:74d8e952a3bd | 95 | gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq); |
Gaku0606 | 0:74d8e952a3bd | 96 | } |
Gaku0606 | 0:74d8e952a3bd | 97 | |
Gaku0606 | 0:74d8e952a3bd | 98 | void GPS_interrupt::initialize(){ |
Gaku0606 | 0:74d8e952a3bd | 99 | latitude = 0.0f; |
Gaku0606 | 0:74d8e952a3bd | 100 | longitude = 0.0f; |
Gaku0606 | 0:74d8e952a3bd | 101 | year = 0; |
Gaku0606 | 0:74d8e952a3bd | 102 | month = 0; |
Gaku0606 | 0:74d8e952a3bd | 103 | day = 0; |
Gaku0606 | 0:74d8e952a3bd | 104 | hour = 0; |
Gaku0606 | 0:74d8e952a3bd | 105 | minutes = 0; |
Gaku0606 | 0:74d8e952a3bd | 106 | seconds = 0; |
Gaku0606 | 2:7be89bab6db9 | 107 | knot = 0; |
Gaku0606 | 2:7be89bab6db9 | 108 | degree = 0; |
Gaku0606 | 2:7be89bab6db9 | 109 | number = 0; |
Gaku0606 | 2:7be89bab6db9 | 110 | height = 0; |
Gaku0606 | 2:7be89bab6db9 | 111 | geoid = 0; |
Gaku0606 | 0:74d8e952a3bd | 112 | memset(gps_buffer_A, '\0', 128); |
Gaku0606 | 0:74d8e952a3bd | 113 | memset(gps_buffer_B, '\0', 128); |
Gaku0606 | 0:74d8e952a3bd | 114 | gps_readable = false; |
Gaku0606 | 0:74d8e952a3bd | 115 | } |
Gaku0606 | 2:7be89bab6db9 | 116 | void GPS_interrupt::rmc_initialize(){ |
Gaku0606 | 2:7be89bab6db9 | 117 | latitude = 0.0f; |
Gaku0606 | 2:7be89bab6db9 | 118 | longitude = 0.0f; |
Gaku0606 | 2:7be89bab6db9 | 119 | year = 0; |
Gaku0606 | 2:7be89bab6db9 | 120 | month = 0; |
Gaku0606 | 2:7be89bab6db9 | 121 | day = 0; |
Gaku0606 | 2:7be89bab6db9 | 122 | hour = 0; |
Gaku0606 | 2:7be89bab6db9 | 123 | minutes = 0; |
Gaku0606 | 2:7be89bab6db9 | 124 | seconds = 0; |
Gaku0606 | 2:7be89bab6db9 | 125 | knot = 0; |
Gaku0606 | 2:7be89bab6db9 | 126 | degree = 0; |
Gaku0606 | 2:7be89bab6db9 | 127 | } |
Gaku0606 | 2:7be89bab6db9 | 128 | void GPS_interrupt::gga_initialize(){ |
Gaku0606 | 2:7be89bab6db9 | 129 | latitude = 0.0f; |
Gaku0606 | 2:7be89bab6db9 | 130 | longitude = 0.0f; |
Gaku0606 | 2:7be89bab6db9 | 131 | /*year = 0; |
Gaku0606 | 2:7be89bab6db9 | 132 | month = 0; |
Gaku0606 | 2:7be89bab6db9 | 133 | day = 0; |
Gaku0606 | 2:7be89bab6db9 | 134 | hour = 0; |
Gaku0606 | 2:7be89bab6db9 | 135 | minutes = 0; |
Gaku0606 | 2:7be89bab6db9 | 136 | seconds = 0;*/ |
Gaku0606 | 2:7be89bab6db9 | 137 | number = 0; |
Gaku0606 | 2:7be89bab6db9 | 138 | height = 0; |
Gaku0606 | 2:7be89bab6db9 | 139 | geoid = 0; |
Gaku0606 | 2:7be89bab6db9 | 140 | } |
Gaku0606 | 0:74d8e952a3bd | 141 | void GPS_interrupt::gps_auto_receive(){ |
Gaku0606 | 0:74d8e952a3bd | 142 | |
Gaku0606 | 0:74d8e952a3bd | 143 | static char str_temp[128] = {'\0'}; |
Gaku0606 | 0:74d8e952a3bd | 144 | static char temp = 0; |
Gaku0606 | 0:74d8e952a3bd | 145 | static bool start_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 146 | static unsigned char current = 0; |
Gaku0606 | 0:74d8e952a3bd | 147 | static char classify_AB = 0; |
Gaku0606 | 0:74d8e952a3bd | 148 | static char* save_buffer = gps_buffer_A; |
Gaku0606 | 0:74d8e952a3bd | 149 | |
Gaku0606 | 0:74d8e952a3bd | 150 | temp = gps->getc(); |
Gaku0606 | 2:7be89bab6db9 | 151 | |
Gaku0606 | 2:7be89bab6db9 | 152 | //printf("%c", temp); |
Gaku0606 | 2:7be89bab6db9 | 153 | |
Gaku0606 | 0:74d8e952a3bd | 154 | if(temp == '$'){ |
Gaku0606 | 0:74d8e952a3bd | 155 | current = 1; |
Gaku0606 | 0:74d8e952a3bd | 156 | start_flag = true; |
Gaku0606 | 0:74d8e952a3bd | 157 | memset(save_buffer, '\0', 128);//初期化 |
Gaku0606 | 0:74d8e952a3bd | 158 | save_buffer[0] = '$'; |
Gaku0606 | 0:74d8e952a3bd | 159 | return; |
Gaku0606 | 0:74d8e952a3bd | 160 | } |
Gaku0606 | 0:74d8e952a3bd | 161 | if(start_flag){//1行スタート |
Gaku0606 | 0:74d8e952a3bd | 162 | if(temp == '\r'){//1行終了 |
Gaku0606 | 0:74d8e952a3bd | 163 | save_buffer[current] = '\0'; |
Gaku0606 | 0:74d8e952a3bd | 164 | start_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 165 | current = 1; |
Gaku0606 | 0:74d8e952a3bd | 166 | gps_read_buffer = save_buffer; |
Gaku0606 | 2:7be89bab6db9 | 167 | |
Gaku0606 | 2:7be89bab6db9 | 168 | //debug(gps_read_buffer); |
Gaku0606 | 2:7be89bab6db9 | 169 | //printf("%s\r\n", save_buffer); |
Gaku0606 | 2:7be89bab6db9 | 170 | |
Gaku0606 | 0:74d8e952a3bd | 171 | if(classify_AB){ |
Gaku0606 | 0:74d8e952a3bd | 172 | save_buffer = gps_buffer_A;//バッファ切換 |
Gaku0606 | 0:74d8e952a3bd | 173 | classify_AB = ~classify_AB; |
Gaku0606 | 0:74d8e952a3bd | 174 | } |
Gaku0606 | 0:74d8e952a3bd | 175 | else{ |
Gaku0606 | 0:74d8e952a3bd | 176 | save_buffer = gps_buffer_B; |
Gaku0606 | 0:74d8e952a3bd | 177 | classify_AB = ~classify_AB; |
Gaku0606 | 0:74d8e952a3bd | 178 | } |
Gaku0606 | 4:758f97bee95a | 179 | |
Gaku0606 | 3:8e66ec281888 | 180 | if((strncmp(gps_read_buffer, "$GPRMC", 6) == 0) || (strncmp(gps_read_buffer, "$GNRMC", 6) == 0)){ |
Gaku0606 | 2:7be89bab6db9 | 181 | memset(str_temp, '\0', 128); |
Gaku0606 | 0:74d8e952a3bd | 182 | strcpy(str_temp, gps_read_buffer); |
Gaku0606 | 2:7be89bab6db9 | 183 | if(processGPRMC(str_temp)){ |
Gaku0606 | 0:74d8e952a3bd | 184 | gps_readable = true; |
Gaku0606 | 4:758f97bee95a | 185 | return;//データが取得できればここで終了 |
Gaku0606 | 0:74d8e952a3bd | 186 | } |
Gaku0606 | 0:74d8e952a3bd | 187 | } |
Gaku0606 | 2:7be89bab6db9 | 188 | else if(strncmp(gps_read_buffer, "$GPGGA", 6) == 0){ |
Gaku0606 | 2:7be89bab6db9 | 189 | memset(str_temp, '\0', 128); |
Gaku0606 | 2:7be89bab6db9 | 190 | strcpy(str_temp, gps_read_buffer); |
Gaku0606 | 2:7be89bab6db9 | 191 | if(processGPGGA(str_temp)){ |
Gaku0606 | 2:7be89bab6db9 | 192 | gps_readable = true; |
Gaku0606 | 4:758f97bee95a | 193 | return;//データが取得できればここで終了 |
Gaku0606 | 4:758f97bee95a | 194 | } |
Gaku0606 | 2:7be89bab6db9 | 195 | } |
Gaku0606 | 4:758f97bee95a | 196 | |
Gaku0606 | 4:758f97bee95a | 197 | if(debugFlag) printf("debug \"%s\"\r\n", gps_read_buffer); |
Gaku0606 | 4:758f97bee95a | 198 | |
Gaku0606 | 2:7be89bab6db9 | 199 | //else if(strncmp(gps_read_buffer, "$GPGSV", 6) == 0){ |
Gaku0606 | 2:7be89bab6db9 | 200 | //printf("%s\r\n", gps_read_buffer); |
Gaku0606 | 2:7be89bab6db9 | 201 | //} |
Gaku0606 | 0:74d8e952a3bd | 202 | } |
Gaku0606 | 0:74d8e952a3bd | 203 | else{ |
Gaku0606 | 0:74d8e952a3bd | 204 | save_buffer[current] = temp; |
Gaku0606 | 0:74d8e952a3bd | 205 | current++; |
Gaku0606 | 0:74d8e952a3bd | 206 | if(current >= 127){ |
Gaku0606 | 0:74d8e952a3bd | 207 | current = 1; |
Gaku0606 | 0:74d8e952a3bd | 208 | start_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 209 | } |
Gaku0606 | 0:74d8e952a3bd | 210 | } |
Gaku0606 | 0:74d8e952a3bd | 211 | } |
Gaku0606 | 0:74d8e952a3bd | 212 | } |
Gaku0606 | 0:74d8e952a3bd | 213 | |
Gaku0606 | 2:7be89bab6db9 | 214 | bool GPS_interrupt::processGPRMC(char *line){ |
Gaku0606 | 0:74d8e952a3bd | 215 | |
Gaku0606 | 0:74d8e952a3bd | 216 | char *tok; //strtokで帰ってくる文字列のポインター |
Gaku0606 | 0:74d8e952a3bd | 217 | bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue |
Gaku0606 | 0:74d8e952a3bd | 218 | //bool result = false; |
Gaku0606 | 0:74d8e952a3bd | 219 | //char num[4] = "";//衛星数の文字データ |
Gaku0606 | 0:74d8e952a3bd | 220 | //char kaibatsu[8], g_height[8]; |
Gaku0606 | 0:74d8e952a3bd | 221 | //char data_GPS[256] =""; |
Gaku0606 | 0:74d8e952a3bd | 222 | char angle[8], speed[8]; |
Gaku0606 | 0:74d8e952a3bd | 223 | char ido[16] = "", keido[16] = ""; |
Gaku0606 | 0:74d8e952a3bd | 224 | |
Gaku0606 | 2:7be89bab6db9 | 225 | double X = 0, Y = 0; |
Gaku0606 | 0:74d8e952a3bd | 226 | double X_m = 0, Y_m = 0;//GPS座標の"分"の部分 |
Gaku0606 | 0:74d8e952a3bd | 227 | char zikann[11], hizuke[7]; |
Gaku0606 | 0:74d8e952a3bd | 228 | |
Gaku0606 | 0:74d8e952a3bd | 229 | //bool gga_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 230 | bool rmc_flag = false; |
Gaku0606 | 2:7be89bab6db9 | 231 | char tok_count = 0; |
Gaku0606 | 0:74d8e952a3bd | 232 | |
Gaku0606 | 4:758f97bee95a | 233 | //rmc_initialize(); |
Gaku0606 | 0:74d8e952a3bd | 234 | |
Gaku0606 | 0:74d8e952a3bd | 235 | //_____GPRMC___________________________________ |
Gaku0606 | 0:74d8e952a3bd | 236 | //データ文字列の先頭から6文字目までが$GPRMCなら0が返ってくる |
Gaku0606 | 2:7be89bab6db9 | 237 | //if (strncmp(line, "$GPRMC", 6) == 0){ |
Gaku0606 | 0:74d8e952a3bd | 238 | |
Gaku0606 | 0:74d8e952a3bd | 239 | tok = strtok(line, ","); |
Gaku0606 | 2:7be89bab6db9 | 240 | tok_count = 0; |
Gaku0606 | 0:74d8e952a3bd | 241 | |
Gaku0606 | 0:74d8e952a3bd | 242 | //$GPRMC, 002519.799, V, , , , , 0.00, 0.00, 060180, , , N * 4A |
Gaku0606 | 0:74d8e952a3bd | 243 | |
Gaku0606 | 0:74d8e952a3bd | 244 | // 0 , 1 , 2,3,4,5,6, 7 , 8 , 9 ,10,11 ,12 |
Gaku0606 | 0:74d8e952a3bd | 245 | //$GPRMC,233514.000,A,3022.5291,N,13057.6141,E,0.41,335.09,030316,,,A*6C |
Gaku0606 | 0:74d8e952a3bd | 246 | |
Gaku0606 | 0:74d8e952a3bd | 247 | // 0 , 1 ,2, 3 ,4, 5 ,6, 7 , 8 , 9 ,1, 1 ,1 |
Gaku0606 | 0:74d8e952a3bd | 248 | // 0 1 2 |
Gaku0606 | 0:74d8e952a3bd | 249 | while (1){ |
Gaku0606 | 2:7be89bab6db9 | 250 | switch (tok_count){ |
Gaku0606 | 2:7be89bab6db9 | 251 | case 1://時分秒 |
Gaku0606 | 2:7be89bab6db9 | 252 | strcpy(zikann, tok);//zikann = "233514.000\0" |
Gaku0606 | 2:7be89bab6db9 | 253 | break; |
Gaku0606 | 2:7be89bab6db9 | 254 | case 2://有効か無効か判定 |
Gaku0606 | 2:7be89bab6db9 | 255 | if (strncmp(tok, "V", 1) == 0) return false; |
Gaku0606 | 2:7be89bab6db9 | 256 | else if (strcmp(tok, "A") == 0) rmc_flag = true; |
Gaku0606 | 2:7be89bab6db9 | 257 | break; |
Gaku0606 | 2:7be89bab6db9 | 258 | case 3://緯度 |
Gaku0606 | 2:7be89bab6db9 | 259 | strcpy(ido, tok); |
Gaku0606 | 2:7be89bab6db9 | 260 | break; |
Gaku0606 | 2:7be89bab6db9 | 261 | case 4://北緯か南緯か |
Gaku0606 | 2:7be89bab6db9 | 262 | if (strcmp(tok, "S") == 0) latSign = true; |
Gaku0606 | 2:7be89bab6db9 | 263 | break; |
Gaku0606 | 2:7be89bab6db9 | 264 | case 5://経度 |
Gaku0606 | 2:7be89bab6db9 | 265 | strcpy(keido, tok); |
Gaku0606 | 2:7be89bab6db9 | 266 | break; |
Gaku0606 | 2:7be89bab6db9 | 267 | case 6://東経か西経か |
Gaku0606 | 2:7be89bab6db9 | 268 | if (strcmp(tok, "W") == 0) lonSign = true; |
Gaku0606 | 2:7be89bab6db9 | 269 | break; |
Gaku0606 | 2:7be89bab6db9 | 270 | case 7://速度(キロノット) |
Gaku0606 | 2:7be89bab6db9 | 271 | strcpy(speed, tok); |
Gaku0606 | 2:7be89bab6db9 | 272 | break; |
Gaku0606 | 2:7be89bab6db9 | 273 | case 8://角度 |
Gaku0606 | 2:7be89bab6db9 | 274 | strcpy(angle, tok); |
Gaku0606 | 2:7be89bab6db9 | 275 | break; |
Gaku0606 | 2:7be89bab6db9 | 276 | case 9://日付 |
Gaku0606 | 2:7be89bab6db9 | 277 | strcpy(hizuke, tok);//hizuke = "030316\0" |
Gaku0606 | 2:7be89bab6db9 | 278 | break; |
Gaku0606 | 0:74d8e952a3bd | 279 | } |
Gaku0606 | 2:7be89bab6db9 | 280 | |
Gaku0606 | 0:74d8e952a3bd | 281 | tok = strtok(NULL, ",");//comma = ",\0" |
Gaku0606 | 0:74d8e952a3bd | 282 | tok_count++; |
Gaku0606 | 0:74d8e952a3bd | 283 | if (tok == NULL) break; |
Gaku0606 | 0:74d8e952a3bd | 284 | } |
Gaku0606 | 0:74d8e952a3bd | 285 | if (rmc_flag){ |
Gaku0606 | 0:74d8e952a3bd | 286 | rmc_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 287 | //ddmmyy |
Gaku0606 | 0:74d8e952a3bd | 288 | int dmy = 0; |
Gaku0606 | 0:74d8e952a3bd | 289 | dmy = atoi(hizuke); |
Gaku0606 | 0:74d8e952a3bd | 290 | day = dmy / 10000; //030316 / 10000 = 3 |
Gaku0606 | 0:74d8e952a3bd | 291 | month = (dmy - day * 10000) / 100;//30316 - 30000 = 316, 316/100= 3 |
Gaku0606 | 0:74d8e952a3bd | 292 | year = dmy - month * 100 - day * 10000 + 2000; |
Gaku0606 | 0:74d8e952a3bd | 293 | |
Gaku0606 | 0:74d8e952a3bd | 294 | //hhmmss.ss |
Gaku0606 | 0:74d8e952a3bd | 295 | seconds = atof(zikann);//sec = 233514.000 |
Gaku0606 | 0:74d8e952a3bd | 296 | hour = (int)(seconds / 10000.0); //233514/10000=(int)23 |
Gaku0606 | 0:74d8e952a3bd | 297 | minutes = (seconds - hour * 10000) / 100;//233514.0-230000=3514.0,3514.0/100=35 |
Gaku0606 | 0:74d8e952a3bd | 298 | seconds = seconds - ((double)minutes * 100.0 + (double)hour * 10000.0); |
Gaku0606 | 0:74d8e952a3bd | 299 | |
Gaku0606 | 0:74d8e952a3bd | 300 | // getLonLatのとき有効 |
Gaku0606 | 0:74d8e952a3bd | 301 | Y_m = atof(ido);//*Y_m = 3457.5571 |
Gaku0606 | 0:74d8e952a3bd | 302 | Y = (int)(Y_m / 100.0);//Y = 34.0 |
Gaku0606 | 0:74d8e952a3bd | 303 | Y_m = Y_m - Y * 100.0;//Y_m = 3457.5571 - 34*100 = 57.5571 |
Gaku0606 | 0:74d8e952a3bd | 304 | |
Gaku0606 | 0:74d8e952a3bd | 305 | X_m = atof(keido);//*X = 13057.6142 |
Gaku0606 | 0:74d8e952a3bd | 306 | X = (int)(X_m / 100.0);//X = 130.0 |
Gaku0606 | 0:74d8e952a3bd | 307 | X_m = X_m - X * 100.0;//X_m = 13057.6142 - 13000.0 = 57.6142 |
Gaku0606 | 0:74d8e952a3bd | 308 | |
Gaku0606 | 0:74d8e952a3bd | 309 | //GPS calculation |
Gaku0606 | 0:74d8e952a3bd | 310 | longitude = X + X_m / 60.0;// 34.959285 |
Gaku0606 | 0:74d8e952a3bd | 311 | latitude = Y + Y_m / 60.0;//137.090290 |
Gaku0606 | 0:74d8e952a3bd | 312 | |
Gaku0606 | 2:7be89bab6db9 | 313 | if (lonSign) longitude *= (-1);//西経なら経度を負に |
Gaku0606 | 2:7be89bab6db9 | 314 | if (latSign) latitude *= (-1);//南緯なら緯度を負に |
Gaku0606 | 0:74d8e952a3bd | 315 | knot = atof(speed); |
Gaku0606 | 0:74d8e952a3bd | 316 | degree = atof(angle); |
Gaku0606 | 4:758f97bee95a | 317 | |
Gaku0606 | 4:758f97bee95a | 318 | return true; |
Gaku0606 | 4:758f97bee95a | 319 | |
Gaku0606 | 0:74d8e952a3bd | 320 | } |
Gaku0606 | 4:758f97bee95a | 321 | else{ |
Gaku0606 | 4:758f97bee95a | 322 | return false; |
Gaku0606 | 4:758f97bee95a | 323 | } |
Gaku0606 | 2:7be89bab6db9 | 324 | //} |
Gaku0606 | 2:7be89bab6db9 | 325 | //else return false; |
Gaku0606 | 0:74d8e952a3bd | 326 | } |
Gaku0606 | 0:74d8e952a3bd | 327 | |
Gaku0606 | 2:7be89bab6db9 | 328 | bool GPS_interrupt::processGPGGA(char *line){ |
Gaku0606 | 2:7be89bab6db9 | 329 | |
Gaku0606 | 2:7be89bab6db9 | 330 | char *tok; //strtokで帰ってくる文字列のポインター |
Gaku0606 | 2:7be89bab6db9 | 331 | bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue |
Gaku0606 | 2:7be89bab6db9 | 332 | //bool result = false; |
Gaku0606 | 2:7be89bab6db9 | 333 | char num[4] = "";//衛星数の文字データ |
Gaku0606 | 2:7be89bab6db9 | 334 | char kaibatsu[8], g_height[8]; |
Gaku0606 | 2:7be89bab6db9 | 335 | //char data_GPS[256] =""; |
Gaku0606 | 2:7be89bab6db9 | 336 | //char angle[8], speed[8]; |
Gaku0606 | 2:7be89bab6db9 | 337 | char ido[16] = "", keido[16] = ""; |
Gaku0606 | 2:7be89bab6db9 | 338 | |
Gaku0606 | 2:7be89bab6db9 | 339 | double X = 0, Y = 0; |
Gaku0606 | 2:7be89bab6db9 | 340 | double X_m = 0, Y_m = 0;//GPS座標の"分"の部分 |
Gaku0606 | 2:7be89bab6db9 | 341 | //char zikann[11], hizuke[7]; |
Gaku0606 | 2:7be89bab6db9 | 342 | char status[2]; |
Gaku0606 | 2:7be89bab6db9 | 343 | |
Gaku0606 | 2:7be89bab6db9 | 344 | bool gga_flag = false; |
Gaku0606 | 2:7be89bab6db9 | 345 | //bool rmc_flag = false; |
Gaku0606 | 2:7be89bab6db9 | 346 | char tok_count = 0; |
Gaku0606 | 2:7be89bab6db9 | 347 | |
Gaku0606 | 4:758f97bee95a | 348 | //gga_initialize(); |
Gaku0606 | 2:7be89bab6db9 | 349 | |
Gaku0606 | 2:7be89bab6db9 | 350 | //データ文字列の先頭から6文字目までが$GPGLLなら0が返ってくる |
Gaku0606 | 2:7be89bab6db9 | 351 | //if (strncmp(data_GPS, "$GPGGA", 6) == 0){ |
Gaku0606 | 2:7be89bab6db9 | 352 | |
Gaku0606 | 2:7be89bab6db9 | 353 | tok = strtok(line, ","); |
Gaku0606 | 2:7be89bab6db9 | 354 | tok_count = 0; |
Gaku0606 | 2:7be89bab6db9 | 355 | |
Gaku0606 | 2:7be89bab6db9 | 356 | //$GPGGA, 002519.799, , , , , 0, 0, , , M, , M, , *40 |
Gaku0606 | 2:7be89bab6db9 | 357 | |
Gaku0606 | 2:7be89bab6db9 | 358 | // 0 , 1 , , , , , 2, 3, , , 4, , 5, , 6 |
Gaku0606 | 2:7be89bab6db9 | 359 | |
Gaku0606 | 2:7be89bab6db9 | 360 | //$GPGGA,233515.000,3022.5292,N,13057.6142,E,1,9,0.88,11.3,M,29.3,M,,*63 |
Gaku0606 | 2:7be89bab6db9 | 361 | |
Gaku0606 | 2:7be89bab6db9 | 362 | // 0 , 1 , 2 ,3, 4 ,5,6,7, 8 , 9 ,1, 11 ,1 |
Gaku0606 | 2:7be89bab6db9 | 363 | // 0 2 |
Gaku0606 | 2:7be89bab6db9 | 364 | while (1){ |
Gaku0606 | 2:7be89bab6db9 | 365 | switch (tok_count){ |
Gaku0606 | 2:7be89bab6db9 | 366 | case 2://緯度 |
Gaku0606 | 2:7be89bab6db9 | 367 | strcpy(ido, tok);//ido = "30" |
Gaku0606 | 2:7be89bab6db9 | 368 | if (strcmp(ido, "0") == 0) return false;//GPS無効 |
Gaku0606 | 2:7be89bab6db9 | 369 | break; |
Gaku0606 | 2:7be89bab6db9 | 370 | case 3://北緯か南緯か |
Gaku0606 | 2:7be89bab6db9 | 371 | if (strcmp(tok, "S") == 0) latSign = true; |
Gaku0606 | 2:7be89bab6db9 | 372 | break; |
Gaku0606 | 2:7be89bab6db9 | 373 | case 4: |
Gaku0606 | 2:7be89bab6db9 | 374 | strcpy(keido, tok);//keido = "130" |
Gaku0606 | 2:7be89bab6db9 | 375 | break; |
Gaku0606 | 2:7be89bab6db9 | 376 | case 5://東経か西経か |
Gaku0606 | 2:7be89bab6db9 | 377 | if (strcmp(tok, "W") == 0) lonSign = true; |
Gaku0606 | 2:7be89bab6db9 | 378 | break; |
Gaku0606 | 2:7be89bab6db9 | 379 | case 6: |
Gaku0606 | 2:7be89bab6db9 | 380 | strcpy(status, tok); |
Gaku0606 | 2:7be89bab6db9 | 381 | if (strcmp(status, "0") != 0) gga_flag = true;//GPS有効 |
Gaku0606 | 2:7be89bab6db9 | 382 | break; |
Gaku0606 | 2:7be89bab6db9 | 383 | case 7: |
Gaku0606 | 2:7be89bab6db9 | 384 | strcpy(num, tok); |
Gaku0606 | 2:7be89bab6db9 | 385 | break; |
Gaku0606 | 2:7be89bab6db9 | 386 | case 9: |
Gaku0606 | 2:7be89bab6db9 | 387 | strcpy(kaibatsu, tok); |
Gaku0606 | 2:7be89bab6db9 | 388 | break; |
Gaku0606 | 2:7be89bab6db9 | 389 | case 11: |
Gaku0606 | 2:7be89bab6db9 | 390 | strcpy(g_height, tok); |
Gaku0606 | 2:7be89bab6db9 | 391 | break; |
Gaku0606 | 2:7be89bab6db9 | 392 | } |
Gaku0606 | 2:7be89bab6db9 | 393 | tok = strtok(NULL, ",");//comma = ",\0" |
Gaku0606 | 2:7be89bab6db9 | 394 | tok_count++; |
Gaku0606 | 2:7be89bab6db9 | 395 | if (tok == NULL) break; |
Gaku0606 | 2:7be89bab6db9 | 396 | } |
Gaku0606 | 2:7be89bab6db9 | 397 | if (gga_flag){ |
Gaku0606 | 2:7be89bab6db9 | 398 | |
Gaku0606 | 2:7be89bab6db9 | 399 | height = atof(kaibatsu); |
Gaku0606 | 2:7be89bab6db9 | 400 | geoid = atof(g_height); |
Gaku0606 | 2:7be89bab6db9 | 401 | number = atoi(num); |
Gaku0606 | 2:7be89bab6db9 | 402 | /* |
Gaku0606 | 2:7be89bab6db9 | 403 | //ddmmyy |
Gaku0606 | 2:7be89bab6db9 | 404 | long dmy = 0; |
Gaku0606 | 2:7be89bab6db9 | 405 | dmy = atol(hizuke); |
Gaku0606 | 2:7be89bab6db9 | 406 | day = (int)(dmy / 10000); //030316 / 10000 = 3 |
Gaku0606 | 2:7be89bab6db9 | 407 | month = (int)(dmy / 100 - day * 100);//30316 - 30000 = 316, 316/100= 3 |
Gaku0606 | 2:7be89bab6db9 | 408 | year = dmy - month * 100 - (long)day * 10000 + 2000; |
Gaku0606 | 2:7be89bab6db9 | 409 | |
Gaku0606 | 2:7be89bab6db9 | 410 | //hhmmss.ss |
Gaku0606 | 2:7be89bab6db9 | 411 | seconds = atof(zikann);//sec = 233514.000 |
Gaku0606 | 2:7be89bab6db9 | 412 | hour = (int)(seconds / 10000.0); //233514/10000=(int)23 |
Gaku0606 | 2:7be89bab6db9 | 413 | minutes = (int)(seconds / 100.0 - hour * 100.0);//233514.0-230000=3514.0,3514.0/100=35 |
Gaku0606 | 2:7be89bab6db9 | 414 | seconds = seconds - ((double)minutes * 100.0 + (double)hour * 10000.0); |
Gaku0606 | 2:7be89bab6db9 | 415 | */ |
Gaku0606 | 2:7be89bab6db9 | 416 | // getLonLatのとき有効 |
Gaku0606 | 2:7be89bab6db9 | 417 | Y_m = atof(ido);//Y_m = 3457.5571 |
Gaku0606 | 2:7be89bab6db9 | 418 | Y = (int)(Y_m / 100.0);//Y = 34.0 |
Gaku0606 | 2:7be89bab6db9 | 419 | Y_m = Y_m - Y * 100.0;//Y_m = 3457.5571 - 34*100 = 57.5571 |
Gaku0606 | 2:7be89bab6db9 | 420 | |
Gaku0606 | 2:7be89bab6db9 | 421 | X_m = atof(keido);//X = 13057.6142 |
Gaku0606 | 2:7be89bab6db9 | 422 | X = (int)(X_m / 100.0);//X = 130.0 |
Gaku0606 | 2:7be89bab6db9 | 423 | X_m = X_m - X * 100.0;//X_m = 13057.6142 - 13000.0 = 57.6142 |
Gaku0606 | 2:7be89bab6db9 | 424 | |
Gaku0606 | 2:7be89bab6db9 | 425 | //GPS calculation |
Gaku0606 | 2:7be89bab6db9 | 426 | longitude = X + X_m / 60.0;// 34.959285 |
Gaku0606 | 2:7be89bab6db9 | 427 | latitude = Y + Y_m / 60.0;//137.090290 |
Gaku0606 | 2:7be89bab6db9 | 428 | if (lonSign == true) longitude = longitude * (-1);//西経なら経度を負に |
Gaku0606 | 2:7be89bab6db9 | 429 | if (latSign == true) latitude = latitude * (-1);//南緯なら緯度を負に |
Gaku0606 | 2:7be89bab6db9 | 430 | |
Gaku0606 | 2:7be89bab6db9 | 431 | return true; |
Gaku0606 | 2:7be89bab6db9 | 432 | } |
Gaku0606 | 4:758f97bee95a | 433 | else{ |
Gaku0606 | 4:758f97bee95a | 434 | return false; |
Gaku0606 | 4:758f97bee95a | 435 | } |
Gaku0606 | 2:7be89bab6db9 | 436 | //} |
Gaku0606 | 2:7be89bab6db9 | 437 | |
Gaku0606 | 2:7be89bab6db9 | 438 | } |