2017年伊豆大島共同打ち上げ実験用電装モジュール搭載GPS測位プログラム
Dependents: Hybrid_interruptGPS Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET ... more
GPS_interrupt.cpp@15:6b1ed321c1be, 2018-05-25 (annotated)
- Committer:
- Sigma884
- Date:
- Fri May 25 21:29:17 2018 +0000
- Revision:
- 15:6b1ed321c1be
- Parent:
- 14:23611bb30bc8
- Child:
- 17:4b09630703c0
???????????????????????????
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 | 8:3f32df2b66c0 | 17 | */ |
Gaku0606 | 8:3f32df2b66c0 | 18 | //char GPS_interrupt::gps_buffer_A[128] = {'\0'}; |
Gaku0606 | 8:3f32df2b66c0 | 19 | //char GPS_interrupt::gps_buffer_B[128] = {'\0'}; |
Gaku0606 | 8:3f32df2b66c0 | 20 | //char GPS_interrupt::gps_buffer_C[128] = {'\0'}; |
Gaku0606 | 8:3f32df2b66c0 | 21 | /* |
Gaku0606 | 0:74d8e952a3bd | 22 | char* GPS_interrupt::gps_read_buffer = gps_buffer_B; |
Gaku0606 | 4:758f97bee95a | 23 | bool GPS_interrupt::gps_readable = false;*/ |
Gaku0606 | 0:74d8e952a3bd | 24 | |
Gaku0606 | 4:758f97bee95a | 25 | //GPS_interrupt* GPS_interrupt::gps_irq; |
Gaku0606 | 0:74d8e952a3bd | 26 | |
Gaku0606 | 2:7be89bab6db9 | 27 | //Timeout timeout_clear; |
Gaku0606 | 2:7be89bab6db9 | 28 | |
Gaku0606 | 4:758f97bee95a | 29 | void GPS_interrupt::debug(bool tf){ |
Gaku0606 | 4:758f97bee95a | 30 | if(tf){ |
Gaku0606 | 4:758f97bee95a | 31 | debugFlag = true; |
Gaku0606 | 4:758f97bee95a | 32 | } |
Gaku0606 | 4:758f97bee95a | 33 | else{ |
Gaku0606 | 4:758f97bee95a | 34 | debugFlag = false; |
Gaku0606 | 4:758f97bee95a | 35 | } |
Gaku0606 | 2:7be89bab6db9 | 36 | } |
Gaku0606 | 2:7be89bab6db9 | 37 | |
Gaku0606 | 2:7be89bab6db9 | 38 | unsigned char GPS_interrupt::checkSum(char *str){ |
Gaku0606 | 2:7be89bab6db9 | 39 | int num = strlen(str); |
Gaku0606 | 2:7be89bab6db9 | 40 | unsigned char val = 0; |
Gaku0606 | 2:7be89bab6db9 | 41 | for(int i = 0; i< num; i++){ |
Gaku0606 | 2:7be89bab6db9 | 42 | val = val ^ str[i]; |
Gaku0606 | 2:7be89bab6db9 | 43 | } |
Gaku0606 | 2:7be89bab6db9 | 44 | return val; |
Gaku0606 | 2:7be89bab6db9 | 45 | } |
Sigma884 | 15:6b1ed321c1be | 46 | |
Gaku0606 | 11:1897b52fa8a1 | 47 | GPS_interrupt::GPS_interrupt(Serial *_gps){ |
Gaku0606 | 2:7be89bab6db9 | 48 | |
Gaku0606 | 11:1897b52fa8a1 | 49 | if(_gps == NULL){ |
Gaku0606 | 11:1897b52fa8a1 | 50 | error("GPS UART BUS ERROR!!\r\n"); |
Gaku0606 | 11:1897b52fa8a1 | 51 | } |
Gaku0606 | 6:2f91c71d64b1 | 52 | debugFlag = false; |
Gaku0606 | 6:2f91c71d64b1 | 53 | initialize(); |
Sigma884 | 15:6b1ed321c1be | 54 | //baudrate = 9600;//_baudrate; |
Sigma884 | 15:6b1ed321c1be | 55 | //frequency = 1; |
Gaku0606 | 6:2f91c71d64b1 | 56 | gps_irq = this; |
Gaku0606 | 11:1897b52fa8a1 | 57 | gps = _gps; |
Sigma884 | 15:6b1ed321c1be | 58 | //gps->baud(baudrate); |
Gaku0606 | 6:2f91c71d64b1 | 59 | gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq); |
Gaku0606 | 6:2f91c71d64b1 | 60 | } |
Gaku0606 | 6:2f91c71d64b1 | 61 | |
Sigma884 | 15:6b1ed321c1be | 62 | /* |
Gaku0606 | 11:1897b52fa8a1 | 63 | GPS_interrupt::GPS_interrupt(Serial *_gps, int _baudrate){ |
Gaku0606 | 2:7be89bab6db9 | 64 | |
Gaku0606 | 11:1897b52fa8a1 | 65 | if(_gps == NULL){ |
Gaku0606 | 11:1897b52fa8a1 | 66 | error("GPS UART BUS ERROR!!\r\n"); |
Gaku0606 | 11:1897b52fa8a1 | 67 | } |
Gaku0606 | 4:758f97bee95a | 68 | debugFlag = false; |
Gaku0606 | 4:758f97bee95a | 69 | initialize(); |
Gaku0606 | 11:1897b52fa8a1 | 70 | baudrate = _baudrate; |
Gaku0606 | 11:1897b52fa8a1 | 71 | //frequency = _frequency; |
Gaku0606 | 0:74d8e952a3bd | 72 | gps_irq = this; |
Gaku0606 | 0:74d8e952a3bd | 73 | gps = _gps; |
Gaku0606 | 11:1897b52fa8a1 | 74 | gps->baud(baudrate); |
Sigma884 | 15:6b1ed321c1be | 75 | |
Sigma884 | 15:6b1ed321c1be | 76 | if(baudrate == 9600){ |
Gaku0606 | 4:758f97bee95a | 77 | gps->printf("$PMTK251,9600*17\r\n"); |
Gaku0606 | 4:758f97bee95a | 78 | } |
Gaku0606 | 4:758f97bee95a | 79 | else if(baudrate == 19200){ |
Gaku0606 | 4:758f97bee95a | 80 | gps->printf("$PMTK251,19200*22\r\n"); |
Gaku0606 | 4:758f97bee95a | 81 | } |
Gaku0606 | 4:758f97bee95a | 82 | else if( baudrate == 38400){ |
Gaku0606 | 4:758f97bee95a | 83 | gps->printf("$PMTK251,38400*27\r\n"); |
Gaku0606 | 4:758f97bee95a | 84 | } |
Gaku0606 | 4:758f97bee95a | 85 | else if(baudrate == 56700){ |
Gaku0606 | 4:758f97bee95a | 86 | gps->printf("$PMTK251,57600*2C\r\n"); |
Gaku0606 | 4:758f97bee95a | 87 | } |
Gaku0606 | 4:758f97bee95a | 88 | else if(baudrate == 115200){ |
Gaku0606 | 4:758f97bee95a | 89 | gps->printf("$PMTK251,115200*1F\r\n"); |
Gaku0606 | 4:758f97bee95a | 90 | wait(0.1); |
Gaku0606 | 4:758f97bee95a | 91 | gps->printf("$PMTK251,115200*1F\r\n"); |
Gaku0606 | 4:758f97bee95a | 92 | wait(0.2); |
Gaku0606 | 4:758f97bee95a | 93 | gps->printf("$PMTK251,115200*1F\r\n"); |
Gaku0606 | 4:758f97bee95a | 94 | wait(0.2); |
Gaku0606 | 4:758f97bee95a | 95 | } |
Gaku0606 | 8:3f32df2b66c0 | 96 | gps->baud(9600); |
Gaku0606 | 11:1897b52fa8a1 | 97 | */ |
Gaku0606 | 11:1897b52fa8a1 | 98 | //baudrate = 9600; |
Gaku0606 | 11:1897b52fa8a1 | 99 | /*wait(0.1); |
Gaku0606 | 4:758f97bee95a | 100 | |
Gaku0606 | 4:758f97bee95a | 101 | //gps->printf("$PMTK351,1*28\r\n");//$PMTK351,1*28<CR><LF> :Enable QZSS NMEA format |
Gaku0606 | 2:7be89bab6db9 | 102 | unsigned char checksum = 0; |
Gaku0606 | 4:758f97bee95a | 103 | 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 | 104 | //$PMTK314,GLL,RMC,VTG,GGA,GSA,GSV,0,0,0,0,0,0,0,0,0,0,0,ZDA,MCHN,チェックサム |
Gaku0606 | 4:758f97bee95a | 105 | 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 | 106 | wait(0.2); |
Gaku0606 | 4:758f97bee95a | 107 | 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 | 108 | wait(0.2); |
Gaku0606 | 4:758f97bee95a | 109 | 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 | 110 | wait(0.2); |
Gaku0606 | 4:758f97bee95a | 111 | if(_frequency == 1) gps->printf("$PMTK220,1000*1F\r\n"); |
Gaku0606 | 4:758f97bee95a | 112 | else if(_frequency == 2) gps->printf("$PMTK220,500*2B\r\n"); |
Gaku0606 | 4:758f97bee95a | 113 | else if(_frequency == 3) gps->printf("$PMTK220,333*2D\r\n"); |
Gaku0606 | 4:758f97bee95a | 114 | else if(_frequency == 4) gps->printf("$PMTK220,250*29\r\n"); |
Gaku0606 | 4:758f97bee95a | 115 | else if(_frequency == 5) gps->printf("$PMTK220,200*2C\r\n"); |
Gaku0606 | 4:758f97bee95a | 116 | else if(_frequency == 10) gps->printf("$PMTK220,100*2F\r\n"); |
Gaku0606 | 4:758f97bee95a | 117 | wait(0.2); |
Gaku0606 | 11:1897b52fa8a1 | 118 | */ |
Sigma884 | 15:6b1ed321c1be | 119 | /* |
Gaku0606 | 0:74d8e952a3bd | 120 | gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq); |
Gaku0606 | 0:74d8e952a3bd | 121 | } |
Sigma884 | 15:6b1ed321c1be | 122 | */ |
Sigma884 | 15:6b1ed321c1be | 123 | |
Sigma884 | 15:6b1ed321c1be | 124 | void GPS_interrupt::changeGPSBaud(int _baudrate){ |
Sigma884 | 15:6b1ed321c1be | 125 | baudrate = _baudrate; |
Sigma884 | 15:6b1ed321c1be | 126 | switch(baudrate){ |
Sigma884 | 15:6b1ed321c1be | 127 | case 4800: |
Sigma884 | 15:6b1ed321c1be | 128 | gps -> printf("$PMTK251,4800*14\r\n"); |
Sigma884 | 15:6b1ed321c1be | 129 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 130 | gps->printf("$PMTK251,4800*14\r\n"); |
Sigma884 | 15:6b1ed321c1be | 131 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 132 | gps->printf("$PMTK251,4800*14\r\n"); |
Sigma884 | 15:6b1ed321c1be | 133 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 134 | gps->printf("$PMTK251,4800*14\r\n"); |
Sigma884 | 15:6b1ed321c1be | 135 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 136 | gps->printf("$PMTK251,4800*14\r\n"); |
Sigma884 | 15:6b1ed321c1be | 137 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 138 | gps->baud(4800); |
Sigma884 | 15:6b1ed321c1be | 139 | break; |
Sigma884 | 15:6b1ed321c1be | 140 | |
Sigma884 | 15:6b1ed321c1be | 141 | case 9600: |
Sigma884 | 15:6b1ed321c1be | 142 | gps->printf("$PMTK251,9600*17\r\n"); |
Sigma884 | 15:6b1ed321c1be | 143 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 144 | gps->printf("$PMTK251,9600*17\r\n"); |
Sigma884 | 15:6b1ed321c1be | 145 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 146 | gps->printf("$PMTK251,9600*17\r\n"); |
Sigma884 | 15:6b1ed321c1be | 147 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 148 | gps->printf("$PMTK251,9600*17\r\n"); |
Sigma884 | 15:6b1ed321c1be | 149 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 150 | gps->printf("$PMTK251,9600*17\r\n"); |
Sigma884 | 15:6b1ed321c1be | 151 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 152 | gps->baud(9600); |
Sigma884 | 15:6b1ed321c1be | 153 | break; |
Sigma884 | 15:6b1ed321c1be | 154 | |
Sigma884 | 15:6b1ed321c1be | 155 | case 14400: |
Sigma884 | 15:6b1ed321c1be | 156 | gps->printf("$PMTK251,14400*29\r\n"); |
Sigma884 | 15:6b1ed321c1be | 157 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 158 | gps->printf("$PMTK251,14400*29\r\n"); |
Sigma884 | 15:6b1ed321c1be | 159 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 160 | gps->printf("$PMTK251,14400*29\r\n"); |
Sigma884 | 15:6b1ed321c1be | 161 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 162 | gps->printf("$PMTK251,14400*29\r\n"); |
Sigma884 | 15:6b1ed321c1be | 163 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 164 | gps->printf("$PMTK251,14400*29\r\n"); |
Sigma884 | 15:6b1ed321c1be | 165 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 166 | gps->baud(14400); |
Sigma884 | 15:6b1ed321c1be | 167 | break; |
Sigma884 | 15:6b1ed321c1be | 168 | |
Sigma884 | 15:6b1ed321c1be | 169 | case 19200: |
Sigma884 | 15:6b1ed321c1be | 170 | gps->printf("$PMTK251,19200*22\r\n"); |
Sigma884 | 15:6b1ed321c1be | 171 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 172 | gps->printf("$PMTK251,19200*22\r\n"); |
Sigma884 | 15:6b1ed321c1be | 173 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 174 | gps->printf("$PMTK251,19200*22\r\n"); |
Sigma884 | 15:6b1ed321c1be | 175 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 176 | gps->printf("$PMTK251,19200*22\r\n"); |
Sigma884 | 15:6b1ed321c1be | 177 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 178 | gps->printf("$PMTK251,19200*22\r\n"); |
Sigma884 | 15:6b1ed321c1be | 179 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 180 | gps->baud(19200); |
Sigma884 | 15:6b1ed321c1be | 181 | break; |
Sigma884 | 15:6b1ed321c1be | 182 | |
Sigma884 | 15:6b1ed321c1be | 183 | case 38400: |
Sigma884 | 15:6b1ed321c1be | 184 | gps->printf("$PMTK251,38400*27\r\n"); |
Sigma884 | 15:6b1ed321c1be | 185 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 186 | gps->printf("$PMTK251,38400*27\r\n"); |
Sigma884 | 15:6b1ed321c1be | 187 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 188 | gps->printf("$PMTK251,38400*27\r\n"); |
Sigma884 | 15:6b1ed321c1be | 189 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 190 | gps->printf("$PMTK251,38400*27\r\n"); |
Sigma884 | 15:6b1ed321c1be | 191 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 192 | gps->printf("$PMTK251,38400*27\r\n"); |
Sigma884 | 15:6b1ed321c1be | 193 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 194 | gps->baud(38400); |
Sigma884 | 15:6b1ed321c1be | 195 | break; |
Sigma884 | 15:6b1ed321c1be | 196 | |
Sigma884 | 15:6b1ed321c1be | 197 | case 57600: |
Sigma884 | 15:6b1ed321c1be | 198 | gps->printf("$PMTK251,57600*2C\r\n"); |
Sigma884 | 15:6b1ed321c1be | 199 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 200 | gps->printf("$PMTK251,57600*2C\r\n"); |
Sigma884 | 15:6b1ed321c1be | 201 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 202 | gps->printf("$PMTK251,57600*2C\r\n"); |
Sigma884 | 15:6b1ed321c1be | 203 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 204 | gps->printf("$PMTK251,57600*2C\r\n"); |
Sigma884 | 15:6b1ed321c1be | 205 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 206 | gps->printf("$PMTK251,57600*2C\r\n"); |
Sigma884 | 15:6b1ed321c1be | 207 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 208 | gps->baud(57600); |
Sigma884 | 15:6b1ed321c1be | 209 | break; |
Sigma884 | 15:6b1ed321c1be | 210 | |
Sigma884 | 15:6b1ed321c1be | 211 | case 115200: |
Sigma884 | 15:6b1ed321c1be | 212 | gps->printf("$PMTK251,115200*1F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 213 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 214 | gps->printf("$PMTK251,115200*1F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 215 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 216 | gps->printf("$PMTK251,115200*1F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 217 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 218 | gps->printf("$PMTK251,115200*1F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 219 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 220 | gps->printf("$PMTK251,115200*1F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 221 | wait(0.2f); |
Sigma884 | 15:6b1ed321c1be | 222 | gps->baud(115200); |
Sigma884 | 15:6b1ed321c1be | 223 | break; |
Sigma884 | 15:6b1ed321c1be | 224 | } |
Sigma884 | 15:6b1ed321c1be | 225 | } |
Sigma884 | 15:6b1ed321c1be | 226 | |
Sigma884 | 15:6b1ed321c1be | 227 | bool GPS_interrupt::changeGPSFreq(int _frequency){ |
Sigma884 | 15:6b1ed321c1be | 228 | frequency = _frequency; |
Sigma884 | 15:6b1ed321c1be | 229 | flag_change_freq = 0; |
Sigma884 | 15:6b1ed321c1be | 230 | switch(frequency){ |
Sigma884 | 15:6b1ed321c1be | 231 | case 1: |
Sigma884 | 15:6b1ed321c1be | 232 | gps->printf("$PMTK220,1000*1F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 233 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 234 | gps->printf("$PMTK220,1000*1F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 235 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 236 | gps->printf("$PMTK220,1000*1F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 237 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 238 | break; |
Sigma884 | 15:6b1ed321c1be | 239 | |
Sigma884 | 15:6b1ed321c1be | 240 | case 2: |
Sigma884 | 15:6b1ed321c1be | 241 | gps->printf("$PMTK220,500*2B\r\n"); |
Sigma884 | 15:6b1ed321c1be | 242 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 243 | gps->printf("$PMTK220,500*2B\r\n"); |
Sigma884 | 15:6b1ed321c1be | 244 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 245 | gps->printf("$PMTK220,500*2B\r\n"); |
Sigma884 | 15:6b1ed321c1be | 246 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 247 | break; |
Sigma884 | 15:6b1ed321c1be | 248 | |
Sigma884 | 15:6b1ed321c1be | 249 | case 3: |
Sigma884 | 15:6b1ed321c1be | 250 | gps->printf("$PMTK220,333*2D\r\n"); |
Sigma884 | 15:6b1ed321c1be | 251 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 252 | gps->printf("$PMTK220,333*2D\r\n"); |
Sigma884 | 15:6b1ed321c1be | 253 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 254 | gps->printf("$PMTK220,333*2D\r\n"); |
Sigma884 | 15:6b1ed321c1be | 255 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 256 | break; |
Sigma884 | 15:6b1ed321c1be | 257 | |
Sigma884 | 15:6b1ed321c1be | 258 | case 4: |
Sigma884 | 15:6b1ed321c1be | 259 | gps->printf("$PMTK220,250*29\r\n"); |
Sigma884 | 15:6b1ed321c1be | 260 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 261 | gps->printf("$PMTK220,250*29\r\n"); |
Sigma884 | 15:6b1ed321c1be | 262 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 263 | gps->printf("$PMTK220,250*29\r\n"); |
Sigma884 | 15:6b1ed321c1be | 264 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 265 | break; |
Sigma884 | 15:6b1ed321c1be | 266 | |
Sigma884 | 15:6b1ed321c1be | 267 | case 5: |
Sigma884 | 15:6b1ed321c1be | 268 | gps->printf("$PMTK220,200*2C\r\n"); |
Sigma884 | 15:6b1ed321c1be | 269 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 270 | gps->printf("$PMTK220,200*2C\r\n"); |
Sigma884 | 15:6b1ed321c1be | 271 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 272 | gps->printf("$PMTK220,200*2C\r\n"); |
Sigma884 | 15:6b1ed321c1be | 273 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 274 | break; |
Sigma884 | 15:6b1ed321c1be | 275 | |
Sigma884 | 15:6b1ed321c1be | 276 | case 6: |
Sigma884 | 15:6b1ed321c1be | 277 | gps->printf("$PMTK220,167*2E\r\n"); |
Sigma884 | 15:6b1ed321c1be | 278 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 279 | gps->printf("$PMTK220,167*2E\r\n"); |
Sigma884 | 15:6b1ed321c1be | 280 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 281 | gps->printf("$PMTK220,167*2E\r\n"); |
Sigma884 | 15:6b1ed321c1be | 282 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 283 | break; |
Sigma884 | 15:6b1ed321c1be | 284 | |
Sigma884 | 15:6b1ed321c1be | 285 | case 7: |
Sigma884 | 15:6b1ed321c1be | 286 | gps->printf("$PMTK220,143*28\r\n"); |
Sigma884 | 15:6b1ed321c1be | 287 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 288 | gps->printf("$PMTK220,143*28\r\n"); |
Sigma884 | 15:6b1ed321c1be | 289 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 290 | gps->printf("$PMTK220,143*28\r\n"); |
Sigma884 | 15:6b1ed321c1be | 291 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 292 | break; |
Sigma884 | 15:6b1ed321c1be | 293 | |
Sigma884 | 15:6b1ed321c1be | 294 | case 8: |
Sigma884 | 15:6b1ed321c1be | 295 | gps->printf("$PMTK220,125*28\r\n"); |
Sigma884 | 15:6b1ed321c1be | 296 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 297 | gps->printf("$PMTK220,125*28\r\n"); |
Sigma884 | 15:6b1ed321c1be | 298 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 299 | gps->printf("$PMTK220,125*28\r\n"); |
Sigma884 | 15:6b1ed321c1be | 300 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 301 | break; |
Sigma884 | 15:6b1ed321c1be | 302 | |
Sigma884 | 15:6b1ed321c1be | 303 | case 9: |
Sigma884 | 15:6b1ed321c1be | 304 | gps->printf("$PMTK220,111*2F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 305 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 306 | gps->printf("$PMTK220,111*2F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 307 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 308 | gps->printf("$PMTK220,111*2F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 309 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 310 | break; |
Sigma884 | 15:6b1ed321c1be | 311 | |
Sigma884 | 15:6b1ed321c1be | 312 | case 10: |
Sigma884 | 15:6b1ed321c1be | 313 | gps->printf("$PMTK220,100*2F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 314 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 315 | gps->printf("$PMTK220,100*2F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 316 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 317 | gps->printf("$PMTK220,100*2F\r\n"); |
Sigma884 | 15:6b1ed321c1be | 318 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 319 | break; |
Sigma884 | 15:6b1ed321c1be | 320 | } |
Sigma884 | 15:6b1ed321c1be | 321 | |
Sigma884 | 15:6b1ed321c1be | 322 | for(wait_change = 0; wait_change < 10; wait_change ++){ |
Sigma884 | 15:6b1ed321c1be | 323 | if(flag_change_freq == 1){ |
Sigma884 | 15:6b1ed321c1be | 324 | return true; |
Sigma884 | 15:6b1ed321c1be | 325 | } |
Sigma884 | 15:6b1ed321c1be | 326 | else if(flag_change_freq == -1){ |
Sigma884 | 15:6b1ed321c1be | 327 | return false; |
Sigma884 | 15:6b1ed321c1be | 328 | } |
Sigma884 | 15:6b1ed321c1be | 329 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 330 | } |
Sigma884 | 15:6b1ed321c1be | 331 | return false; |
Sigma884 | 15:6b1ed321c1be | 332 | } |
Sigma884 | 15:6b1ed321c1be | 333 | |
Sigma884 | 15:6b1ed321c1be | 334 | bool GPS_interrupt::changeGPSData(int _GLL, int _RMC, int _VTG, int _GGA, int _GSA, int _GSV){ |
Sigma884 | 15:6b1ed321c1be | 335 | data_sum = _GLL + _RMC + _VTG + _GGA + _GSA + _GSV; |
Sigma884 | 15:6b1ed321c1be | 336 | data_sum = data_sum % 2; |
Sigma884 | 15:6b1ed321c1be | 337 | data_sum += 28; |
Sigma884 | 15:6b1ed321c1be | 338 | gps->printf("$PMTK314,%d,%d,%d,%d,%d,%d,0,0,0,0,0,0,0,0,0,0,0,0,0*%d\r\n", _GLL, _RMC, _VTG, _GGA, _GSA, _GSV, data_sum); |
Sigma884 | 15:6b1ed321c1be | 339 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 340 | gps->printf("$PMTK314,%d,%d,%d,%d,%d,%d,0,0,0,0,0,0,0,0,0,0,0,0,0*%d\r\n", _GLL, _RMC, _VTG, _GGA, _GSA, _GSV, data_sum); |
Sigma884 | 15:6b1ed321c1be | 341 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 342 | gps->printf("$PMTK314,%d,%d,%d,%d,%d,%d,0,0,0,0,0,0,0,0,0,0,0,0,0*%d\r\n", _GLL, _RMC, _VTG, _GGA, _GSA, _GSV, data_sum); |
Sigma884 | 15:6b1ed321c1be | 343 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 344 | |
Sigma884 | 15:6b1ed321c1be | 345 | for(wait_change = 0; wait_change < 10; wait_change ++){ |
Sigma884 | 15:6b1ed321c1be | 346 | if(flag_change_data == 1){ |
Sigma884 | 15:6b1ed321c1be | 347 | return true; |
Sigma884 | 15:6b1ed321c1be | 348 | } |
Sigma884 | 15:6b1ed321c1be | 349 | else if(flag_change_data == -1){ |
Sigma884 | 15:6b1ed321c1be | 350 | return false; |
Sigma884 | 15:6b1ed321c1be | 351 | } |
Sigma884 | 15:6b1ed321c1be | 352 | wait(0.1f); |
Sigma884 | 15:6b1ed321c1be | 353 | } |
Sigma884 | 15:6b1ed321c1be | 354 | return false; |
Sigma884 | 15:6b1ed321c1be | 355 | } |
Gaku0606 | 0:74d8e952a3bd | 356 | |
Gaku0606 | 0:74d8e952a3bd | 357 | void GPS_interrupt::initialize(){ |
Gaku0606 | 0:74d8e952a3bd | 358 | latitude = 0.0f; |
Gaku0606 | 0:74d8e952a3bd | 359 | longitude = 0.0f; |
Gaku0606 | 0:74d8e952a3bd | 360 | year = 0; |
Gaku0606 | 0:74d8e952a3bd | 361 | month = 0; |
Gaku0606 | 0:74d8e952a3bd | 362 | day = 0; |
Gaku0606 | 0:74d8e952a3bd | 363 | hour = 0; |
Gaku0606 | 0:74d8e952a3bd | 364 | minutes = 0; |
Gaku0606 | 0:74d8e952a3bd | 365 | seconds = 0; |
Gaku0606 | 2:7be89bab6db9 | 366 | knot = 0; |
Gaku0606 | 2:7be89bab6db9 | 367 | degree = 0; |
Gaku0606 | 2:7be89bab6db9 | 368 | number = 0; |
Gaku0606 | 2:7be89bab6db9 | 369 | height = 0; |
Gaku0606 | 2:7be89bab6db9 | 370 | geoid = 0; |
Gaku0606 | 0:74d8e952a3bd | 371 | memset(gps_buffer_A, '\0', 128); |
Gaku0606 | 0:74d8e952a3bd | 372 | memset(gps_buffer_B, '\0', 128); |
Gaku0606 | 8:3f32df2b66c0 | 373 | // memset(gps_buffer_C, '\0', 128); |
Gaku0606 | 0:74d8e952a3bd | 374 | gps_readable = false; |
Gaku0606 | 0:74d8e952a3bd | 375 | } |
Gaku0606 | 2:7be89bab6db9 | 376 | void GPS_interrupt::rmc_initialize(){ |
Gaku0606 | 2:7be89bab6db9 | 377 | latitude = 0.0f; |
Gaku0606 | 2:7be89bab6db9 | 378 | longitude = 0.0f; |
Gaku0606 | 2:7be89bab6db9 | 379 | year = 0; |
Gaku0606 | 2:7be89bab6db9 | 380 | month = 0; |
Gaku0606 | 2:7be89bab6db9 | 381 | day = 0; |
Gaku0606 | 2:7be89bab6db9 | 382 | hour = 0; |
Gaku0606 | 2:7be89bab6db9 | 383 | minutes = 0; |
Gaku0606 | 2:7be89bab6db9 | 384 | seconds = 0; |
Gaku0606 | 2:7be89bab6db9 | 385 | knot = 0; |
Gaku0606 | 2:7be89bab6db9 | 386 | degree = 0; |
Gaku0606 | 2:7be89bab6db9 | 387 | } |
Gaku0606 | 2:7be89bab6db9 | 388 | void GPS_interrupt::gga_initialize(){ |
Gaku0606 | 2:7be89bab6db9 | 389 | latitude = 0.0f; |
Gaku0606 | 2:7be89bab6db9 | 390 | longitude = 0.0f; |
Gaku0606 | 2:7be89bab6db9 | 391 | /*year = 0; |
Gaku0606 | 2:7be89bab6db9 | 392 | month = 0; |
Gaku0606 | 2:7be89bab6db9 | 393 | day = 0; |
Gaku0606 | 2:7be89bab6db9 | 394 | hour = 0; |
Gaku0606 | 2:7be89bab6db9 | 395 | minutes = 0; |
Gaku0606 | 2:7be89bab6db9 | 396 | seconds = 0;*/ |
Gaku0606 | 2:7be89bab6db9 | 397 | number = 0; |
Gaku0606 | 2:7be89bab6db9 | 398 | height = 0; |
Gaku0606 | 2:7be89bab6db9 | 399 | geoid = 0; |
Gaku0606 | 2:7be89bab6db9 | 400 | } |
Gaku0606 | 9:dab13bd20f43 | 401 | |
Gaku0606 | 0:74d8e952a3bd | 402 | void GPS_interrupt::gps_auto_receive(){ |
Gaku0606 | 0:74d8e952a3bd | 403 | |
Gaku0606 | 0:74d8e952a3bd | 404 | static char str_temp[128] = {'\0'}; |
Gaku0606 | 8:3f32df2b66c0 | 405 | static unsigned char temp = 0; |
Gaku0606 | 0:74d8e952a3bd | 406 | static bool start_flag = false; |
Gaku0606 | 8:3f32df2b66c0 | 407 | static int current = 0; |
Gaku0606 | 8:3f32df2b66c0 | 408 | static char classify_ABC = 0; |
Gaku0606 | 0:74d8e952a3bd | 409 | static char* save_buffer = gps_buffer_A; |
Gaku0606 | 0:74d8e952a3bd | 410 | |
Gaku0606 | 0:74d8e952a3bd | 411 | temp = gps->getc(); |
Gaku0606 | 2:7be89bab6db9 | 412 | |
Gaku0606 | 11:1897b52fa8a1 | 413 | if(debugFlag) printf("%c", temp); |
Gaku0606 | 2:7be89bab6db9 | 414 | |
Gaku0606 | 0:74d8e952a3bd | 415 | if(temp == '$'){ |
Gaku0606 | 0:74d8e952a3bd | 416 | current = 1; |
Gaku0606 | 0:74d8e952a3bd | 417 | start_flag = true; |
Gaku0606 | 0:74d8e952a3bd | 418 | memset(save_buffer, '\0', 128);//初期化 |
Gaku0606 | 0:74d8e952a3bd | 419 | save_buffer[0] = '$'; |
Gaku0606 | 0:74d8e952a3bd | 420 | return; |
Gaku0606 | 0:74d8e952a3bd | 421 | } |
Gaku0606 | 0:74d8e952a3bd | 422 | if(start_flag){//1行スタート |
Gaku0606 | 0:74d8e952a3bd | 423 | if(temp == '\r'){//1行終了 |
Gaku0606 | 0:74d8e952a3bd | 424 | save_buffer[current] = '\0'; |
Gaku0606 | 0:74d8e952a3bd | 425 | start_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 426 | current = 1; |
Gaku0606 | 0:74d8e952a3bd | 427 | gps_read_buffer = save_buffer; |
Gaku0606 | 2:7be89bab6db9 | 428 | |
Gaku0606 | 2:7be89bab6db9 | 429 | //debug(gps_read_buffer); |
Gaku0606 | 8:3f32df2b66c0 | 430 | //printf("%s\r\n", save_buffer); |
Gaku0606 | 8:3f32df2b66c0 | 431 | //printf("debug \"%s\"\r\n", gps_read_buffer); |
Gaku0606 | 2:7be89bab6db9 | 432 | |
Gaku0606 | 8:3f32df2b66c0 | 433 | /*switch(classify_ABC){ |
Gaku0606 | 8:3f32df2b66c0 | 434 | case 0: |
Gaku0606 | 8:3f32df2b66c0 | 435 | save_buffer = gps_buffer_B; |
Gaku0606 | 8:3f32df2b66c0 | 436 | classify_ABC = 1; |
Gaku0606 | 8:3f32df2b66c0 | 437 | break; |
Gaku0606 | 8:3f32df2b66c0 | 438 | |
Gaku0606 | 8:3f32df2b66c0 | 439 | case 1: |
Gaku0606 | 8:3f32df2b66c0 | 440 | save_buffer = gps_buffer_C;//バッファ切換 |
Gaku0606 | 8:3f32df2b66c0 | 441 | classify_ABC = 2; |
Gaku0606 | 8:3f32df2b66c0 | 442 | break; |
Gaku0606 | 8:3f32df2b66c0 | 443 | |
Gaku0606 | 8:3f32df2b66c0 | 444 | case 2: |
Gaku0606 | 8:3f32df2b66c0 | 445 | save_buffer = gps_buffer_A; |
Gaku0606 | 8:3f32df2b66c0 | 446 | classify_ABC = 0; |
Gaku0606 | 8:3f32df2b66c0 | 447 | }*/ |
Gaku0606 | 8:3f32df2b66c0 | 448 | if(classify_ABC == 0){ |
Gaku0606 | 8:3f32df2b66c0 | 449 | save_buffer = gps_buffer_B; |
Gaku0606 | 8:3f32df2b66c0 | 450 | classify_ABC = 1; |
Gaku0606 | 0:74d8e952a3bd | 451 | } |
Gaku0606 | 8:3f32df2b66c0 | 452 | else if(classify_ABC == 1){ |
Gaku0606 | 8:3f32df2b66c0 | 453 | save_buffer = gps_buffer_A;//バッファ切換 |
Gaku0606 | 8:3f32df2b66c0 | 454 | classify_ABC = 0; |
Gaku0606 | 0:74d8e952a3bd | 455 | } |
Gaku0606 | 8:3f32df2b66c0 | 456 | memset(save_buffer, '\0', 128); |
Gaku0606 | 7:4b893ac95ae1 | 457 | //if(debugFlag) printf("debug \"%s\"\r\n", gps_read_buffer); |
Gaku0606 | 6:2f91c71d64b1 | 458 | |
Gaku0606 | 3:8e66ec281888 | 459 | if((strncmp(gps_read_buffer, "$GPRMC", 6) == 0) || (strncmp(gps_read_buffer, "$GNRMC", 6) == 0)){ |
Gaku0606 | 2:7be89bab6db9 | 460 | memset(str_temp, '\0', 128); |
Gaku0606 | 0:74d8e952a3bd | 461 | strcpy(str_temp, gps_read_buffer); |
Gaku0606 | 2:7be89bab6db9 | 462 | if(processGPRMC(str_temp)){ |
Gaku0606 | 0:74d8e952a3bd | 463 | gps_readable = true; |
Gaku0606 | 4:758f97bee95a | 464 | return;//データが取得できればここで終了 |
Gaku0606 | 0:74d8e952a3bd | 465 | } |
Gaku0606 | 8:3f32df2b66c0 | 466 | else{ |
Gaku0606 | 8:3f32df2b66c0 | 467 | gps_readable = false; |
Gaku0606 | 8:3f32df2b66c0 | 468 | } |
Gaku0606 | 0:74d8e952a3bd | 469 | } |
Gaku0606 | 14:23611bb30bc8 | 470 | else if((strncmp(gps_read_buffer, "$GPGGA", 6) == 0) || (strncmp(gps_read_buffer, "$GNGGA", 6) == 0)){ |
Gaku0606 | 2:7be89bab6db9 | 471 | memset(str_temp, '\0', 128); |
Gaku0606 | 2:7be89bab6db9 | 472 | strcpy(str_temp, gps_read_buffer); |
Gaku0606 | 2:7be89bab6db9 | 473 | if(processGPGGA(str_temp)){ |
Gaku0606 | 2:7be89bab6db9 | 474 | gps_readable = true; |
Gaku0606 | 4:758f97bee95a | 475 | return;//データが取得できればここで終了 |
Gaku0606 | 4:758f97bee95a | 476 | } |
Gaku0606 | 8:3f32df2b66c0 | 477 | else{ |
Gaku0606 | 8:3f32df2b66c0 | 478 | gps_readable = false; |
Gaku0606 | 8:3f32df2b66c0 | 479 | return; |
Gaku0606 | 8:3f32df2b66c0 | 480 | } |
Gaku0606 | 2:7be89bab6db9 | 481 | } |
Gaku0606 | 2:7be89bab6db9 | 482 | //else if(strncmp(gps_read_buffer, "$GPGSV", 6) == 0){ |
Gaku0606 | 2:7be89bab6db9 | 483 | //printf("%s\r\n", gps_read_buffer); |
Gaku0606 | 2:7be89bab6db9 | 484 | //} |
Sigma884 | 15:6b1ed321c1be | 485 | else if(strncmp(gps_read_buffer, "$PMTK001", 8) == 0){ |
Sigma884 | 15:6b1ed321c1be | 486 | memset(str_temp, '\0', 128); |
Sigma884 | 15:6b1ed321c1be | 487 | strcpy(str_temp, gps_read_buffer); |
Sigma884 | 15:6b1ed321c1be | 488 | processPMTK(str_temp); |
Sigma884 | 15:6b1ed321c1be | 489 | } |
Gaku0606 | 0:74d8e952a3bd | 490 | } |
Gaku0606 | 0:74d8e952a3bd | 491 | else{ |
Gaku0606 | 0:74d8e952a3bd | 492 | save_buffer[current] = temp; |
Gaku0606 | 0:74d8e952a3bd | 493 | current++; |
Gaku0606 | 0:74d8e952a3bd | 494 | if(current >= 127){ |
Gaku0606 | 0:74d8e952a3bd | 495 | current = 1; |
Gaku0606 | 0:74d8e952a3bd | 496 | start_flag = false; |
Gaku0606 | 8:3f32df2b66c0 | 497 | memset(save_buffer, '\0', 128); |
Gaku0606 | 0:74d8e952a3bd | 498 | } |
Gaku0606 | 0:74d8e952a3bd | 499 | } |
Gaku0606 | 0:74d8e952a3bd | 500 | } |
Gaku0606 | 0:74d8e952a3bd | 501 | } |
Gaku0606 | 0:74d8e952a3bd | 502 | |
Gaku0606 | 2:7be89bab6db9 | 503 | bool GPS_interrupt::processGPRMC(char *line){ |
Gaku0606 | 0:74d8e952a3bd | 504 | |
Gaku0606 | 0:74d8e952a3bd | 505 | char *tok; //strtokで帰ってくる文字列のポインター |
Gaku0606 | 0:74d8e952a3bd | 506 | bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue |
Gaku0606 | 0:74d8e952a3bd | 507 | //bool result = false; |
Gaku0606 | 0:74d8e952a3bd | 508 | //char num[4] = "";//衛星数の文字データ |
Gaku0606 | 0:74d8e952a3bd | 509 | //char kaibatsu[8], g_height[8]; |
Gaku0606 | 0:74d8e952a3bd | 510 | //char data_GPS[256] =""; |
Gaku0606 | 0:74d8e952a3bd | 511 | char angle[8], speed[8]; |
Gaku0606 | 0:74d8e952a3bd | 512 | char ido[16] = "", keido[16] = ""; |
Gaku0606 | 0:74d8e952a3bd | 513 | |
Gaku0606 | 2:7be89bab6db9 | 514 | double X = 0, Y = 0; |
Gaku0606 | 0:74d8e952a3bd | 515 | double X_m = 0, Y_m = 0;//GPS座標の"分"の部分 |
Gaku0606 | 0:74d8e952a3bd | 516 | char zikann[11], hizuke[7]; |
Gaku0606 | 0:74d8e952a3bd | 517 | |
Gaku0606 | 0:74d8e952a3bd | 518 | //bool gga_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 519 | bool rmc_flag = false; |
Gaku0606 | 2:7be89bab6db9 | 520 | char tok_count = 0; |
Gaku0606 | 0:74d8e952a3bd | 521 | |
Gaku0606 | 4:758f97bee95a | 522 | //rmc_initialize(); |
Gaku0606 | 0:74d8e952a3bd | 523 | |
Gaku0606 | 0:74d8e952a3bd | 524 | //_____GPRMC___________________________________ |
Gaku0606 | 0:74d8e952a3bd | 525 | //データ文字列の先頭から6文字目までが$GPRMCなら0が返ってくる |
Gaku0606 | 2:7be89bab6db9 | 526 | //if (strncmp(line, "$GPRMC", 6) == 0){ |
Gaku0606 | 0:74d8e952a3bd | 527 | |
Gaku0606 | 0:74d8e952a3bd | 528 | tok = strtok(line, ","); |
Gaku0606 | 2:7be89bab6db9 | 529 | tok_count = 0; |
Gaku0606 | 0:74d8e952a3bd | 530 | |
Gaku0606 | 0:74d8e952a3bd | 531 | //$GPRMC, 002519.799, V, , , , , 0.00, 0.00, 060180, , , N * 4A |
Gaku0606 | 0:74d8e952a3bd | 532 | |
Gaku0606 | 0:74d8e952a3bd | 533 | // 0 , 1 , 2,3,4,5,6, 7 , 8 , 9 ,10,11 ,12 |
Gaku0606 | 0:74d8e952a3bd | 534 | //$GPRMC,233514.000,A,3022.5291,N,13057.6141,E,0.41,335.09,030316,,,A*6C |
Gaku0606 | 0:74d8e952a3bd | 535 | |
Gaku0606 | 0:74d8e952a3bd | 536 | // 0 , 1 ,2, 3 ,4, 5 ,6, 7 , 8 , 9 ,1, 1 ,1 |
Gaku0606 | 0:74d8e952a3bd | 537 | // 0 1 2 |
Gaku0606 | 0:74d8e952a3bd | 538 | while (1){ |
Gaku0606 | 2:7be89bab6db9 | 539 | switch (tok_count){ |
Gaku0606 | 2:7be89bab6db9 | 540 | case 1://時分秒 |
Gaku0606 | 2:7be89bab6db9 | 541 | strcpy(zikann, tok);//zikann = "233514.000\0" |
Gaku0606 | 2:7be89bab6db9 | 542 | break; |
Gaku0606 | 2:7be89bab6db9 | 543 | case 2://有効か無効か判定 |
Gaku0606 | 2:7be89bab6db9 | 544 | if (strncmp(tok, "V", 1) == 0) return false; |
Gaku0606 | 2:7be89bab6db9 | 545 | else if (strcmp(tok, "A") == 0) rmc_flag = true; |
Gaku0606 | 2:7be89bab6db9 | 546 | break; |
Gaku0606 | 2:7be89bab6db9 | 547 | case 3://緯度 |
Gaku0606 | 2:7be89bab6db9 | 548 | strcpy(ido, tok); |
Gaku0606 | 2:7be89bab6db9 | 549 | break; |
Gaku0606 | 2:7be89bab6db9 | 550 | case 4://北緯か南緯か |
Gaku0606 | 2:7be89bab6db9 | 551 | if (strcmp(tok, "S") == 0) latSign = true; |
Gaku0606 | 2:7be89bab6db9 | 552 | break; |
Gaku0606 | 2:7be89bab6db9 | 553 | case 5://経度 |
Gaku0606 | 2:7be89bab6db9 | 554 | strcpy(keido, tok); |
Gaku0606 | 2:7be89bab6db9 | 555 | break; |
Gaku0606 | 2:7be89bab6db9 | 556 | case 6://東経か西経か |
Gaku0606 | 2:7be89bab6db9 | 557 | if (strcmp(tok, "W") == 0) lonSign = true; |
Gaku0606 | 2:7be89bab6db9 | 558 | break; |
Gaku0606 | 2:7be89bab6db9 | 559 | case 7://速度(キロノット) |
Gaku0606 | 2:7be89bab6db9 | 560 | strcpy(speed, tok); |
Gaku0606 | 2:7be89bab6db9 | 561 | break; |
Gaku0606 | 2:7be89bab6db9 | 562 | case 8://角度 |
Gaku0606 | 2:7be89bab6db9 | 563 | strcpy(angle, tok); |
Gaku0606 | 2:7be89bab6db9 | 564 | break; |
Gaku0606 | 2:7be89bab6db9 | 565 | case 9://日付 |
Gaku0606 | 2:7be89bab6db9 | 566 | strcpy(hizuke, tok);//hizuke = "030316\0" |
Gaku0606 | 2:7be89bab6db9 | 567 | break; |
Gaku0606 | 0:74d8e952a3bd | 568 | } |
Gaku0606 | 2:7be89bab6db9 | 569 | |
Gaku0606 | 0:74d8e952a3bd | 570 | tok = strtok(NULL, ",");//comma = ",\0" |
Gaku0606 | 0:74d8e952a3bd | 571 | tok_count++; |
Gaku0606 | 0:74d8e952a3bd | 572 | if (tok == NULL) break; |
Gaku0606 | 0:74d8e952a3bd | 573 | } |
Gaku0606 | 0:74d8e952a3bd | 574 | if (rmc_flag){ |
Gaku0606 | 0:74d8e952a3bd | 575 | rmc_flag = false; |
Gaku0606 | 0:74d8e952a3bd | 576 | //ddmmyy |
Gaku0606 | 0:74d8e952a3bd | 577 | int dmy = 0; |
Gaku0606 | 0:74d8e952a3bd | 578 | dmy = atoi(hizuke); |
Gaku0606 | 0:74d8e952a3bd | 579 | day = dmy / 10000; //030316 / 10000 = 3 |
Gaku0606 | 0:74d8e952a3bd | 580 | month = (dmy - day * 10000) / 100;//30316 - 30000 = 316, 316/100= 3 |
Gaku0606 | 0:74d8e952a3bd | 581 | year = dmy - month * 100 - day * 10000 + 2000; |
Gaku0606 | 0:74d8e952a3bd | 582 | |
Gaku0606 | 0:74d8e952a3bd | 583 | //hhmmss.ss |
Gaku0606 | 0:74d8e952a3bd | 584 | seconds = atof(zikann);//sec = 233514.000 |
Gaku0606 | 0:74d8e952a3bd | 585 | hour = (int)(seconds / 10000.0); //233514/10000=(int)23 |
Gaku0606 | 0:74d8e952a3bd | 586 | minutes = (seconds - hour * 10000) / 100;//233514.0-230000=3514.0,3514.0/100=35 |
Gaku0606 | 12:935b21d30ec2 | 587 | seconds = seconds - ((float)minutes * 100.0 + (float)hour * 10000.0); |
Gaku0606 | 0:74d8e952a3bd | 588 | |
Gaku0606 | 0:74d8e952a3bd | 589 | // getLonLatのとき有効 |
Gaku0606 | 0:74d8e952a3bd | 590 | Y_m = atof(ido);//*Y_m = 3457.5571 |
Gaku0606 | 0:74d8e952a3bd | 591 | Y = (int)(Y_m / 100.0);//Y = 34.0 |
Gaku0606 | 0:74d8e952a3bd | 592 | Y_m = Y_m - Y * 100.0;//Y_m = 3457.5571 - 34*100 = 57.5571 |
Gaku0606 | 0:74d8e952a3bd | 593 | |
Gaku0606 | 0:74d8e952a3bd | 594 | X_m = atof(keido);//*X = 13057.6142 |
Gaku0606 | 0:74d8e952a3bd | 595 | X = (int)(X_m / 100.0);//X = 130.0 |
Gaku0606 | 0:74d8e952a3bd | 596 | X_m = X_m - X * 100.0;//X_m = 13057.6142 - 13000.0 = 57.6142 |
Gaku0606 | 0:74d8e952a3bd | 597 | |
Gaku0606 | 0:74d8e952a3bd | 598 | //GPS calculation |
Gaku0606 | 0:74d8e952a3bd | 599 | longitude = X + X_m / 60.0;// 34.959285 |
Gaku0606 | 0:74d8e952a3bd | 600 | latitude = Y + Y_m / 60.0;//137.090290 |
Gaku0606 | 0:74d8e952a3bd | 601 | |
Gaku0606 | 2:7be89bab6db9 | 602 | if (lonSign) longitude *= (-1);//西経なら経度を負に |
Gaku0606 | 2:7be89bab6db9 | 603 | if (latSign) latitude *= (-1);//南緯なら緯度を負に |
Gaku0606 | 0:74d8e952a3bd | 604 | knot = atof(speed); |
Gaku0606 | 0:74d8e952a3bd | 605 | degree = atof(angle); |
Gaku0606 | 14:23611bb30bc8 | 606 | //printf("\r\n%f\t%f\r\n",knot, degree); |
Gaku0606 | 8:3f32df2b66c0 | 607 | if(longitude > 100.0 && longitude < 150.0 && latitude > 30.0 && latitude < 50.0){ |
Gaku0606 | 8:3f32df2b66c0 | 608 | return true; |
Gaku0606 | 8:3f32df2b66c0 | 609 | } |
Gaku0606 | 8:3f32df2b66c0 | 610 | else return false; |
Gaku0606 | 0:74d8e952a3bd | 611 | } |
Gaku0606 | 4:758f97bee95a | 612 | else{ |
Gaku0606 | 4:758f97bee95a | 613 | return false; |
Gaku0606 | 4:758f97bee95a | 614 | } |
Gaku0606 | 2:7be89bab6db9 | 615 | //} |
Gaku0606 | 2:7be89bab6db9 | 616 | //else return false; |
Gaku0606 | 0:74d8e952a3bd | 617 | } |
Gaku0606 | 0:74d8e952a3bd | 618 | |
Gaku0606 | 2:7be89bab6db9 | 619 | bool GPS_interrupt::processGPGGA(char *line){ |
Gaku0606 | 2:7be89bab6db9 | 620 | |
Gaku0606 | 2:7be89bab6db9 | 621 | char *tok; //strtokで帰ってくる文字列のポインター |
Gaku0606 | 2:7be89bab6db9 | 622 | bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue |
Gaku0606 | 2:7be89bab6db9 | 623 | //bool result = false; |
Gaku0606 | 2:7be89bab6db9 | 624 | char num[4] = "";//衛星数の文字データ |
Gaku0606 | 2:7be89bab6db9 | 625 | char kaibatsu[8], g_height[8]; |
Gaku0606 | 2:7be89bab6db9 | 626 | //char data_GPS[256] =""; |
Gaku0606 | 2:7be89bab6db9 | 627 | //char angle[8], speed[8]; |
Gaku0606 | 2:7be89bab6db9 | 628 | char ido[16] = "", keido[16] = ""; |
Gaku0606 | 2:7be89bab6db9 | 629 | |
Gaku0606 | 2:7be89bab6db9 | 630 | double X = 0, Y = 0; |
Gaku0606 | 2:7be89bab6db9 | 631 | double X_m = 0, Y_m = 0;//GPS座標の"分"の部分 |
Gaku0606 | 2:7be89bab6db9 | 632 | //char zikann[11], hizuke[7]; |
Gaku0606 | 2:7be89bab6db9 | 633 | char status[2]; |
Gaku0606 | 2:7be89bab6db9 | 634 | |
Gaku0606 | 2:7be89bab6db9 | 635 | bool gga_flag = false; |
Gaku0606 | 2:7be89bab6db9 | 636 | //bool rmc_flag = false; |
Gaku0606 | 2:7be89bab6db9 | 637 | char tok_count = 0; |
Gaku0606 | 2:7be89bab6db9 | 638 | |
Gaku0606 | 4:758f97bee95a | 639 | //gga_initialize(); |
Gaku0606 | 2:7be89bab6db9 | 640 | |
Gaku0606 | 2:7be89bab6db9 | 641 | //データ文字列の先頭から6文字目までが$GPGLLなら0が返ってくる |
Gaku0606 | 2:7be89bab6db9 | 642 | //if (strncmp(data_GPS, "$GPGGA", 6) == 0){ |
Gaku0606 | 2:7be89bab6db9 | 643 | |
Gaku0606 | 2:7be89bab6db9 | 644 | tok = strtok(line, ","); |
Gaku0606 | 2:7be89bab6db9 | 645 | tok_count = 0; |
Gaku0606 | 2:7be89bab6db9 | 646 | |
Gaku0606 | 2:7be89bab6db9 | 647 | //$GPGGA, 002519.799, , , , , 0, 0, , , M, , M, , *40 |
Gaku0606 | 2:7be89bab6db9 | 648 | |
Gaku0606 | 2:7be89bab6db9 | 649 | // 0 , 1 , , , , , 2, 3, , , 4, , 5, , 6 |
Gaku0606 | 2:7be89bab6db9 | 650 | |
Gaku0606 | 2:7be89bab6db9 | 651 | //$GPGGA,233515.000,3022.5292,N,13057.6142,E,1,9,0.88,11.3,M,29.3,M,,*63 |
Gaku0606 | 2:7be89bab6db9 | 652 | |
Gaku0606 | 2:7be89bab6db9 | 653 | // 0 , 1 , 2 ,3, 4 ,5,6,7, 8 , 9 ,1, 11 ,1 |
Gaku0606 | 2:7be89bab6db9 | 654 | // 0 2 |
Gaku0606 | 2:7be89bab6db9 | 655 | while (1){ |
Gaku0606 | 2:7be89bab6db9 | 656 | switch (tok_count){ |
Gaku0606 | 2:7be89bab6db9 | 657 | case 2://緯度 |
Gaku0606 | 2:7be89bab6db9 | 658 | strcpy(ido, tok);//ido = "30" |
Gaku0606 | 2:7be89bab6db9 | 659 | if (strcmp(ido, "0") == 0) return false;//GPS無効 |
Gaku0606 | 2:7be89bab6db9 | 660 | break; |
Gaku0606 | 2:7be89bab6db9 | 661 | case 3://北緯か南緯か |
Gaku0606 | 2:7be89bab6db9 | 662 | if (strcmp(tok, "S") == 0) latSign = true; |
Gaku0606 | 2:7be89bab6db9 | 663 | break; |
Gaku0606 | 2:7be89bab6db9 | 664 | case 4: |
Gaku0606 | 2:7be89bab6db9 | 665 | strcpy(keido, tok);//keido = "130" |
Gaku0606 | 2:7be89bab6db9 | 666 | break; |
Gaku0606 | 2:7be89bab6db9 | 667 | case 5://東経か西経か |
Gaku0606 | 2:7be89bab6db9 | 668 | if (strcmp(tok, "W") == 0) lonSign = true; |
Gaku0606 | 2:7be89bab6db9 | 669 | break; |
Gaku0606 | 2:7be89bab6db9 | 670 | case 6: |
Gaku0606 | 2:7be89bab6db9 | 671 | strcpy(status, tok); |
Gaku0606 | 2:7be89bab6db9 | 672 | if (strcmp(status, "0") != 0) gga_flag = true;//GPS有効 |
Gaku0606 | 2:7be89bab6db9 | 673 | break; |
Gaku0606 | 2:7be89bab6db9 | 674 | case 7: |
Gaku0606 | 2:7be89bab6db9 | 675 | strcpy(num, tok); |
Gaku0606 | 2:7be89bab6db9 | 676 | break; |
Gaku0606 | 2:7be89bab6db9 | 677 | case 9: |
Gaku0606 | 2:7be89bab6db9 | 678 | strcpy(kaibatsu, tok); |
Gaku0606 | 2:7be89bab6db9 | 679 | break; |
Gaku0606 | 2:7be89bab6db9 | 680 | case 11: |
Gaku0606 | 2:7be89bab6db9 | 681 | strcpy(g_height, tok); |
Gaku0606 | 2:7be89bab6db9 | 682 | break; |
Gaku0606 | 2:7be89bab6db9 | 683 | } |
Gaku0606 | 2:7be89bab6db9 | 684 | tok = strtok(NULL, ",");//comma = ",\0" |
Gaku0606 | 2:7be89bab6db9 | 685 | tok_count++; |
Gaku0606 | 2:7be89bab6db9 | 686 | if (tok == NULL) break; |
Gaku0606 | 2:7be89bab6db9 | 687 | } |
Gaku0606 | 2:7be89bab6db9 | 688 | if (gga_flag){ |
Gaku0606 | 2:7be89bab6db9 | 689 | |
Gaku0606 | 2:7be89bab6db9 | 690 | height = atof(kaibatsu); |
Gaku0606 | 2:7be89bab6db9 | 691 | geoid = atof(g_height); |
Gaku0606 | 2:7be89bab6db9 | 692 | number = atoi(num); |
Gaku0606 | 2:7be89bab6db9 | 693 | /* |
Gaku0606 | 2:7be89bab6db9 | 694 | //ddmmyy |
Gaku0606 | 2:7be89bab6db9 | 695 | long dmy = 0; |
Gaku0606 | 2:7be89bab6db9 | 696 | dmy = atol(hizuke); |
Gaku0606 | 2:7be89bab6db9 | 697 | day = (int)(dmy / 10000); //030316 / 10000 = 3 |
Gaku0606 | 2:7be89bab6db9 | 698 | month = (int)(dmy / 100 - day * 100);//30316 - 30000 = 316, 316/100= 3 |
Gaku0606 | 2:7be89bab6db9 | 699 | year = dmy - month * 100 - (long)day * 10000 + 2000; |
Gaku0606 | 2:7be89bab6db9 | 700 | |
Gaku0606 | 2:7be89bab6db9 | 701 | //hhmmss.ss |
Gaku0606 | 2:7be89bab6db9 | 702 | seconds = atof(zikann);//sec = 233514.000 |
Gaku0606 | 2:7be89bab6db9 | 703 | hour = (int)(seconds / 10000.0); //233514/10000=(int)23 |
Gaku0606 | 2:7be89bab6db9 | 704 | minutes = (int)(seconds / 100.0 - hour * 100.0);//233514.0-230000=3514.0,3514.0/100=35 |
Gaku0606 | 2:7be89bab6db9 | 705 | seconds = seconds - ((double)minutes * 100.0 + (double)hour * 10000.0); |
Gaku0606 | 2:7be89bab6db9 | 706 | */ |
Gaku0606 | 2:7be89bab6db9 | 707 | // getLonLatのとき有効 |
Gaku0606 | 2:7be89bab6db9 | 708 | Y_m = atof(ido);//Y_m = 3457.5571 |
Gaku0606 | 2:7be89bab6db9 | 709 | Y = (int)(Y_m / 100.0);//Y = 34.0 |
Gaku0606 | 2:7be89bab6db9 | 710 | Y_m = Y_m - Y * 100.0;//Y_m = 3457.5571 - 34*100 = 57.5571 |
Gaku0606 | 2:7be89bab6db9 | 711 | |
Gaku0606 | 2:7be89bab6db9 | 712 | X_m = atof(keido);//X = 13057.6142 |
Gaku0606 | 2:7be89bab6db9 | 713 | X = (int)(X_m / 100.0);//X = 130.0 |
Gaku0606 | 2:7be89bab6db9 | 714 | X_m = X_m - X * 100.0;//X_m = 13057.6142 - 13000.0 = 57.6142 |
Gaku0606 | 2:7be89bab6db9 | 715 | |
Gaku0606 | 2:7be89bab6db9 | 716 | //GPS calculation |
Gaku0606 | 2:7be89bab6db9 | 717 | longitude = X + X_m / 60.0;// 34.959285 |
Gaku0606 | 2:7be89bab6db9 | 718 | latitude = Y + Y_m / 60.0;//137.090290 |
Gaku0606 | 2:7be89bab6db9 | 719 | if (lonSign == true) longitude = longitude * (-1);//西経なら経度を負に |
Gaku0606 | 2:7be89bab6db9 | 720 | if (latSign == true) latitude = latitude * (-1);//南緯なら緯度を負に |
Gaku0606 | 2:7be89bab6db9 | 721 | |
Gaku0606 | 8:3f32df2b66c0 | 722 | if(longitude > 100.0 && longitude < 150.0 && latitude > 30.0 && latitude < 50.0){ |
Gaku0606 | 8:3f32df2b66c0 | 723 | return true; |
Gaku0606 | 8:3f32df2b66c0 | 724 | } |
Gaku0606 | 8:3f32df2b66c0 | 725 | else{ |
Gaku0606 | 8:3f32df2b66c0 | 726 | return false; |
Gaku0606 | 8:3f32df2b66c0 | 727 | } |
Gaku0606 | 2:7be89bab6db9 | 728 | } |
Gaku0606 | 4:758f97bee95a | 729 | else{ |
Gaku0606 | 4:758f97bee95a | 730 | return false; |
Gaku0606 | 4:758f97bee95a | 731 | } |
Sigma884 | 15:6b1ed321c1be | 732 | //return false; |
Gaku0606 | 8:3f32df2b66c0 | 733 | } |
Gaku0606 | 8:3f32df2b66c0 | 734 | |
Sigma884 | 15:6b1ed321c1be | 735 | void GPS_interrupt::processPMTK(char *line){ |
Sigma884 | 15:6b1ed321c1be | 736 | |
Sigma884 | 15:6b1ed321c1be | 737 | char *tok; //strtokで帰ってくる文字列のポインター |
Sigma884 | 15:6b1ed321c1be | 738 | int tok_count = 0; |
Sigma884 | 15:6b1ed321c1be | 739 | |
Sigma884 | 15:6b1ed321c1be | 740 | char status[2]; |
Sigma884 | 15:6b1ed321c1be | 741 | char mode_change_s[5] = ""; |
Sigma884 | 15:6b1ed321c1be | 742 | int mode_change = 0; |
Sigma884 | 15:6b1ed321c1be | 743 | |
Sigma884 | 15:6b1ed321c1be | 744 | tok = strtok(line, ","); |
Sigma884 | 15:6b1ed321c1be | 745 | tok_count = 0; |
Sigma884 | 15:6b1ed321c1be | 746 | |
Sigma884 | 15:6b1ed321c1be | 747 | //$PMTK001, 334, 3*34 |
Sigma884 | 15:6b1ed321c1be | 748 | // 0 , 1 , 2 |
Sigma884 | 15:6b1ed321c1be | 749 | |
Sigma884 | 15:6b1ed321c1be | 750 | while (1){ |
Sigma884 | 15:6b1ed321c1be | 751 | switch (tok_count){ |
Sigma884 | 15:6b1ed321c1be | 752 | case 1: //変更内容 |
Sigma884 | 15:6b1ed321c1be | 753 | if(strcmp(tok, "220") == 0){ |
Sigma884 | 15:6b1ed321c1be | 754 | mode_change = 220; |
Sigma884 | 15:6b1ed321c1be | 755 | } |
Sigma884 | 15:6b1ed321c1be | 756 | else if(strcmp(tok, "314") == 0){ |
Sigma884 | 15:6b1ed321c1be | 757 | mode_change = 314; |
Sigma884 | 15:6b1ed321c1be | 758 | } |
Sigma884 | 15:6b1ed321c1be | 759 | break; |
Sigma884 | 15:6b1ed321c1be | 760 | |
Sigma884 | 15:6b1ed321c1be | 761 | case 2: |
Sigma884 | 15:6b1ed321c1be | 762 | if(tok[0] = '3'){ |
Sigma884 | 15:6b1ed321c1be | 763 | switch(mode_change){ |
Sigma884 | 15:6b1ed321c1be | 764 | case 220: |
Sigma884 | 15:6b1ed321c1be | 765 | flag_change_freq = 1; |
Sigma884 | 15:6b1ed321c1be | 766 | break; |
Sigma884 | 15:6b1ed321c1be | 767 | case 314: |
Sigma884 | 15:6b1ed321c1be | 768 | flag_change_data = 1; |
Sigma884 | 15:6b1ed321c1be | 769 | break; |
Sigma884 | 15:6b1ed321c1be | 770 | } |
Sigma884 | 15:6b1ed321c1be | 771 | } |
Sigma884 | 15:6b1ed321c1be | 772 | else{ |
Sigma884 | 15:6b1ed321c1be | 773 | switch(mode_change){ |
Sigma884 | 15:6b1ed321c1be | 774 | case 220: |
Sigma884 | 15:6b1ed321c1be | 775 | flag_change_freq = -1; |
Sigma884 | 15:6b1ed321c1be | 776 | break; |
Sigma884 | 15:6b1ed321c1be | 777 | case 314: |
Sigma884 | 15:6b1ed321c1be | 778 | flag_change_data = -1; |
Sigma884 | 15:6b1ed321c1be | 779 | break; |
Sigma884 | 15:6b1ed321c1be | 780 | } |
Sigma884 | 15:6b1ed321c1be | 781 | } |
Sigma884 | 15:6b1ed321c1be | 782 | break; |
Sigma884 | 15:6b1ed321c1be | 783 | } |
Sigma884 | 15:6b1ed321c1be | 784 | tok = strtok(NULL, ","); |
Sigma884 | 15:6b1ed321c1be | 785 | tok_count ++; |
Sigma884 | 15:6b1ed321c1be | 786 | if(tok == NULL){ |
Sigma884 | 15:6b1ed321c1be | 787 | break; |
Sigma884 | 15:6b1ed321c1be | 788 | } |
Sigma884 | 15:6b1ed321c1be | 789 | } |
Sigma884 | 15:6b1ed321c1be | 790 | } |
Sigma884 | 15:6b1ed321c1be | 791 | |
Sigma884 | 15:6b1ed321c1be | 792 | |
Sigma884 | 15:6b1ed321c1be | 793 | |
Gaku0606 | 12:935b21d30ec2 | 794 | float GPS_interrupt::Distance(double x, double y){ |
Gaku0606 | 2:7be89bab6db9 | 795 | |
Gaku0606 | 8:3f32df2b66c0 | 796 | double dLat = x - longitude;//相対経度 |
Gaku0606 | 8:3f32df2b66c0 | 797 | double dLng = y - latitude;//相対緯度 |
Gaku0606 | 8:3f32df2b66c0 | 798 | double radLat = latitude * GPS_PI / 180.0;//今の緯度をラジアンにしたもの |
Gaku0606 | 8:3f32df2b66c0 | 799 | double F = EIRTH_AspectRatioInverse; |
Gaku0606 | 8:3f32df2b66c0 | 800 | // 離心率の2乗 |
Gaku0606 | 8:3f32df2b66c0 | 801 | double E = ((2.0 * F) - 1.0) / (F * F); |
Gaku0606 | 8:3f32df2b66c0 | 802 | // π * 赤道半径 |
Gaku0606 | 8:3f32df2b66c0 | 803 | double PI_ER = GPS_PI * EARTH_EQUATOR_RADIUS; |
Gaku0606 | 8:3f32df2b66c0 | 804 | // 1 - e^2 sin^2 (θ) |
Gaku0606 | 8:3f32df2b66c0 | 805 | double TMP = 1.0 - E * sin(radLat) * sin(radLat); |
Gaku0606 | 8:3f32df2b66c0 | 806 | // 経度1°あたりの長さ |
Gaku0606 | 8:3f32df2b66c0 | 807 | double arc_lat = (PI_ER * (1.0 - E)) / ( 180.0 * pow(TMP, 1.5)); |
Gaku0606 | 8:3f32df2b66c0 | 808 | // 緯度1°あたりの長さ |
Gaku0606 | 8:3f32df2b66c0 | 809 | double arc_lng = (PI_ER * cos(radLat)) / (180.0 * pow(TMP, 0.5)); |
Gaku0606 | 8:3f32df2b66c0 | 810 | |
Gaku0606 | 8:3f32df2b66c0 | 811 | dLat *= arc_lat; |
Gaku0606 | 8:3f32df2b66c0 | 812 | dLng *= arc_lng; |
Gaku0606 | 12:935b21d30ec2 | 813 | return (float)sqrt(dLat * dLat + dLng * dLng); |
Gaku0606 | 2:7be89bab6db9 | 814 | } |