PQ_Hybrid_Electrical_Equipment_Team / GPS_interrupt

2017年伊豆大島共同打ち上げ実験用電装モジュール搭載GPS測位プログラム

Dependents:   Hybrid_interruptGPS Hybrid_main_FirstEdtion rocket_logger_sinkan2018_v1 HYBRYD2018_IZU_ROCKET ... more

GPS_interrupt.cpp@11:1897b52fa8a1, 2017-11-25 (annotated)

Committer:
Gaku0606
Date:
Sat Nov 25 07:00:51 2017 +0000
Revision:
11:1897b52fa8a1
Parent:
10:a006445dc76d
Child:
12:935b21d30ec2
aaaa

Who changed what in which revision?

UserRevisionLine numberNew 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 }
Gaku0606 11:1897b52fa8a1 46 GPS_interrupt::GPS_interrupt(Serial *_gps){
Gaku0606 2:7be89bab6db9 47
Gaku0606 11:1897b52fa8a1 48 if(_gps == NULL){
Gaku0606 11:1897b52fa8a1 49 error("GPS UART BUS ERROR!!\r\n");
Gaku0606 11:1897b52fa8a1 50 }
Gaku0606 6:2f91c71d64b1 51 debugFlag = false;
Gaku0606 6:2f91c71d64b1 52 initialize();
Gaku0606 11:1897b52fa8a1 53 baudrate = 9600;//_baudrate;
Gaku0606 11:1897b52fa8a1 54 frequency = 1;
Gaku0606 6:2f91c71d64b1 55 gps_irq = this;
Gaku0606 11:1897b52fa8a1 56 gps = _gps;
Gaku0606 11:1897b52fa8a1 57 gps->baud(baudrate);
Gaku0606 6:2f91c71d64b1 58 gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq);
Gaku0606 6:2f91c71d64b1 59 }
Gaku0606 6:2f91c71d64b1 60
Gaku0606 11:1897b52fa8a1 61 GPS_interrupt::GPS_interrupt(Serial *_gps, int _baudrate){
Gaku0606 2:7be89bab6db9 62
Gaku0606 11:1897b52fa8a1 63 if(_gps == NULL){
Gaku0606 11:1897b52fa8a1 64 error("GPS UART BUS ERROR!!\r\n");
Gaku0606 11:1897b52fa8a1 65 }
Gaku0606 4:758f97bee95a 66 debugFlag = false;
Gaku0606 4:758f97bee95a 67 initialize();
Gaku0606 11:1897b52fa8a1 68 baudrate = _baudrate;
Gaku0606 11:1897b52fa8a1 69 //frequency = _frequency;
Gaku0606 0:74d8e952a3bd 70 gps_irq = this;
Gaku0606 0:74d8e952a3bd 71 gps = _gps;
Gaku0606 11:1897b52fa8a1 72 gps->baud(baudrate);
Gaku0606 11:1897b52fa8a1 73 /*if(baudrate == 9600){
Gaku0606 4:758f97bee95a 74 gps->printf("$PMTK251,9600*17\r\n");
Gaku0606 4:758f97bee95a 75 }
Gaku0606 4:758f97bee95a 76 else if(baudrate == 19200){
Gaku0606 4:758f97bee95a 77 gps->printf("$PMTK251,19200*22\r\n");
Gaku0606 4:758f97bee95a 78 }
Gaku0606 4:758f97bee95a 79 else if( baudrate == 38400){
Gaku0606 4:758f97bee95a 80 gps->printf("$PMTK251,38400*27\r\n");
Gaku0606 4:758f97bee95a 81 }
Gaku0606 4:758f97bee95a 82 else if(baudrate == 56700){
Gaku0606 4:758f97bee95a 83 gps->printf("$PMTK251,57600*2C\r\n");
Gaku0606 4:758f97bee95a 84 }
Gaku0606 4:758f97bee95a 85 else if(baudrate == 115200){
Gaku0606 4:758f97bee95a 86 gps->printf("$PMTK251,115200*1F\r\n");
Gaku0606 4:758f97bee95a 87 wait(0.1);
Gaku0606 4:758f97bee95a 88 gps->printf("$PMTK251,115200*1F\r\n");
Gaku0606 4:758f97bee95a 89 wait(0.2);
Gaku0606 4:758f97bee95a 90 gps->printf("$PMTK251,115200*1F\r\n");
Gaku0606 4:758f97bee95a 91 wait(0.2);
Gaku0606 4:758f97bee95a 92 }
Gaku0606 8:3f32df2b66c0 93 gps->baud(9600);
Gaku0606 11:1897b52fa8a1 94 */
Gaku0606 11:1897b52fa8a1 95 //baudrate = 9600;
Gaku0606 11:1897b52fa8a1 96 /*wait(0.1);
Gaku0606 4:758f97bee95a 97
Gaku0606 4:758f97bee95a 98 //gps->printf("$PMTK351,1*28\r\n");//$PMTK351,1*28<CR><LF> :Enable QZSS NMEA format
Gaku0606 2:7be89bab6db9 99 unsigned char checksum = 0;
Gaku0606 4:758f97bee95a 100 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 101 //$PMTK314,GLL,RMC,VTG,GGA,GSA,GSV,0,0,0,0,0,0,0,0,0,0,0,ZDA,MCHN,チェックサム
Gaku0606 4:758f97bee95a 102 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 103 wait(0.2);
Gaku0606 4:758f97bee95a 104 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 105 wait(0.2);
Gaku0606 4:758f97bee95a 106 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 107 wait(0.2);
Gaku0606 4:758f97bee95a 108 if(_frequency == 1) gps->printf("$PMTK220,1000*1F\r\n");
Gaku0606 4:758f97bee95a 109 else if(_frequency == 2) gps->printf("$PMTK220,500*2B\r\n");
Gaku0606 4:758f97bee95a 110 else if(_frequency == 3) gps->printf("$PMTK220,333*2D\r\n");
Gaku0606 4:758f97bee95a 111 else if(_frequency == 4) gps->printf("$PMTK220,250*29\r\n");
Gaku0606 4:758f97bee95a 112 else if(_frequency == 5) gps->printf("$PMTK220,200*2C\r\n");
Gaku0606 4:758f97bee95a 113 else if(_frequency == 10) gps->printf("$PMTK220,100*2F\r\n");
Gaku0606 4:758f97bee95a 114 wait(0.2);
Gaku0606 11:1897b52fa8a1 115 */
Gaku0606 0:74d8e952a3bd 116 gps->attach(gps_irq, &GPS_interrupt::gps_auto_receive, Serial::RxIrq);
Gaku0606 0:74d8e952a3bd 117 }
Gaku0606 0:74d8e952a3bd 118
Gaku0606 0:74d8e952a3bd 119 void GPS_interrupt::initialize(){
Gaku0606 0:74d8e952a3bd 120 latitude = 0.0f;
Gaku0606 0:74d8e952a3bd 121 longitude = 0.0f;
Gaku0606 0:74d8e952a3bd 122 year = 0;
Gaku0606 0:74d8e952a3bd 123 month = 0;
Gaku0606 0:74d8e952a3bd 124 day = 0;
Gaku0606 0:74d8e952a3bd 125 hour = 0;
Gaku0606 0:74d8e952a3bd 126 minutes = 0;
Gaku0606 0:74d8e952a3bd 127 seconds = 0;
Gaku0606 2:7be89bab6db9 128 knot = 0;
Gaku0606 2:7be89bab6db9 129 degree = 0;
Gaku0606 2:7be89bab6db9 130 number = 0;
Gaku0606 2:7be89bab6db9 131 height = 0;
Gaku0606 2:7be89bab6db9 132 geoid = 0;
Gaku0606 0:74d8e952a3bd 133 memset(gps_buffer_A, '\0', 128);
Gaku0606 0:74d8e952a3bd 134 memset(gps_buffer_B, '\0', 128);
Gaku0606 8:3f32df2b66c0 135 // memset(gps_buffer_C, '\0', 128);
Gaku0606 0:74d8e952a3bd 136 gps_readable = false;
Gaku0606 0:74d8e952a3bd 137 }
Gaku0606 2:7be89bab6db9 138 void GPS_interrupt::rmc_initialize(){
Gaku0606 2:7be89bab6db9 139 latitude = 0.0f;
Gaku0606 2:7be89bab6db9 140 longitude = 0.0f;
Gaku0606 2:7be89bab6db9 141 year = 0;
Gaku0606 2:7be89bab6db9 142 month = 0;
Gaku0606 2:7be89bab6db9 143 day = 0;
Gaku0606 2:7be89bab6db9 144 hour = 0;
Gaku0606 2:7be89bab6db9 145 minutes = 0;
Gaku0606 2:7be89bab6db9 146 seconds = 0;
Gaku0606 2:7be89bab6db9 147 knot = 0;
Gaku0606 2:7be89bab6db9 148 degree = 0;
Gaku0606 2:7be89bab6db9 149 }
Gaku0606 2:7be89bab6db9 150 void GPS_interrupt::gga_initialize(){
Gaku0606 2:7be89bab6db9 151 latitude = 0.0f;
Gaku0606 2:7be89bab6db9 152 longitude = 0.0f;
Gaku0606 2:7be89bab6db9 153 /*year = 0;
Gaku0606 2:7be89bab6db9 154 month = 0;
Gaku0606 2:7be89bab6db9 155 day = 0;
Gaku0606 2:7be89bab6db9 156 hour = 0;
Gaku0606 2:7be89bab6db9 157 minutes = 0;
Gaku0606 2:7be89bab6db9 158 seconds = 0;*/
Gaku0606 2:7be89bab6db9 159 number = 0;
Gaku0606 2:7be89bab6db9 160 height = 0;
Gaku0606 2:7be89bab6db9 161 geoid = 0;
Gaku0606 2:7be89bab6db9 162 }
Gaku0606 9:dab13bd20f43 163
Gaku0606 0:74d8e952a3bd 164 void GPS_interrupt::gps_auto_receive(){
Gaku0606 0:74d8e952a3bd 165
Gaku0606 0:74d8e952a3bd 166 static char str_temp[128] = {'\0'};
Gaku0606 8:3f32df2b66c0 167 static unsigned char temp = 0;
Gaku0606 0:74d8e952a3bd 168 static bool start_flag = false;
Gaku0606 8:3f32df2b66c0 169 static int current = 0;
Gaku0606 8:3f32df2b66c0 170 static char classify_ABC = 0;
Gaku0606 0:74d8e952a3bd 171 static char* save_buffer = gps_buffer_A;
Gaku0606 0:74d8e952a3bd 172
Gaku0606 0:74d8e952a3bd 173 temp = gps->getc();
Gaku0606 2:7be89bab6db9 174
Gaku0606 11:1897b52fa8a1 175 if(debugFlag) printf("%c", temp);
Gaku0606 2:7be89bab6db9 176
Gaku0606 0:74d8e952a3bd 177 if(temp == '$'){
Gaku0606 0:74d8e952a3bd 178 current = 1;
Gaku0606 0:74d8e952a3bd 179 start_flag = true;
Gaku0606 0:74d8e952a3bd 180 memset(save_buffer, '\0', 128);//初期化
Gaku0606 0:74d8e952a3bd 181 save_buffer[0] = '$';
Gaku0606 0:74d8e952a3bd 182 return;
Gaku0606 0:74d8e952a3bd 183 }
Gaku0606 0:74d8e952a3bd 184 if(start_flag){//1行スタート
Gaku0606 0:74d8e952a3bd 185 if(temp == '\r'){//1行終了
Gaku0606 0:74d8e952a3bd 186 save_buffer[current] = '\0';
Gaku0606 0:74d8e952a3bd 187 start_flag = false;
Gaku0606 0:74d8e952a3bd 188 current = 1;
Gaku0606 0:74d8e952a3bd 189 gps_read_buffer = save_buffer;
Gaku0606 2:7be89bab6db9 190
Gaku0606 2:7be89bab6db9 191 //debug(gps_read_buffer);
Gaku0606 8:3f32df2b66c0 192 //printf("%s\r\n", save_buffer);
Gaku0606 8:3f32df2b66c0 193 //printf("debug \"%s\"\r\n", gps_read_buffer);
Gaku0606 2:7be89bab6db9 194
Gaku0606 8:3f32df2b66c0 195 /*switch(classify_ABC){
Gaku0606 8:3f32df2b66c0 196 case 0:
Gaku0606 8:3f32df2b66c0 197 save_buffer = gps_buffer_B;
Gaku0606 8:3f32df2b66c0 198 classify_ABC = 1;
Gaku0606 8:3f32df2b66c0 199 break;
Gaku0606 8:3f32df2b66c0 200
Gaku0606 8:3f32df2b66c0 201 case 1:
Gaku0606 8:3f32df2b66c0 202 save_buffer = gps_buffer_C;//バッファ切換
Gaku0606 8:3f32df2b66c0 203 classify_ABC = 2;
Gaku0606 8:3f32df2b66c0 204 break;
Gaku0606 8:3f32df2b66c0 205
Gaku0606 8:3f32df2b66c0 206 case 2:
Gaku0606 8:3f32df2b66c0 207 save_buffer = gps_buffer_A;
Gaku0606 8:3f32df2b66c0 208 classify_ABC = 0;
Gaku0606 8:3f32df2b66c0 209 }*/
Gaku0606 8:3f32df2b66c0 210 if(classify_ABC == 0){
Gaku0606 8:3f32df2b66c0 211 save_buffer = gps_buffer_B;
Gaku0606 8:3f32df2b66c0 212 classify_ABC = 1;
Gaku0606 0:74d8e952a3bd 213 }
Gaku0606 8:3f32df2b66c0 214 else if(classify_ABC == 1){
Gaku0606 8:3f32df2b66c0 215 save_buffer = gps_buffer_A;//バッファ切換
Gaku0606 8:3f32df2b66c0 216 classify_ABC = 0;
Gaku0606 0:74d8e952a3bd 217 }
Gaku0606 8:3f32df2b66c0 218 memset(save_buffer, '\0', 128);
Gaku0606 7:4b893ac95ae1 219 //if(debugFlag) printf("debug \"%s\"\r\n", gps_read_buffer);
Gaku0606 6:2f91c71d64b1 220
Gaku0606 3:8e66ec281888 221 if((strncmp(gps_read_buffer, "$GPRMC", 6) == 0) || (strncmp(gps_read_buffer, "$GNRMC", 6) == 0)){
Gaku0606 2:7be89bab6db9 222 memset(str_temp, '\0', 128);
Gaku0606 0:74d8e952a3bd 223 strcpy(str_temp, gps_read_buffer);
Gaku0606 2:7be89bab6db9 224 if(processGPRMC(str_temp)){
Gaku0606 0:74d8e952a3bd 225 gps_readable = true;
Gaku0606 4:758f97bee95a 226 return;//データが取得できればここで終了
Gaku0606 0:74d8e952a3bd 227 }
Gaku0606 8:3f32df2b66c0 228 else{
Gaku0606 8:3f32df2b66c0 229 gps_readable = false;
Gaku0606 8:3f32df2b66c0 230 }
Gaku0606 0:74d8e952a3bd 231 }
Gaku0606 2:7be89bab6db9 232 else if(strncmp(gps_read_buffer, "$GPGGA", 6) == 0){
Gaku0606 2:7be89bab6db9 233 memset(str_temp, '\0', 128);
Gaku0606 2:7be89bab6db9 234 strcpy(str_temp, gps_read_buffer);
Gaku0606 2:7be89bab6db9 235 if(processGPGGA(str_temp)){
Gaku0606 2:7be89bab6db9 236 gps_readable = true;
Gaku0606 4:758f97bee95a 237 return;//データが取得できればここで終了
Gaku0606 4:758f97bee95a 238 }
Gaku0606 8:3f32df2b66c0 239 else{
Gaku0606 8:3f32df2b66c0 240 gps_readable = false;
Gaku0606 8:3f32df2b66c0 241 return;
Gaku0606 8:3f32df2b66c0 242 }
Gaku0606 2:7be89bab6db9 243 }
Gaku0606 2:7be89bab6db9 244 //else if(strncmp(gps_read_buffer, "$GPGSV", 6) == 0){
Gaku0606 2:7be89bab6db9 245 //printf("%s\r\n", gps_read_buffer);
Gaku0606 2:7be89bab6db9 246 //}
Gaku0606 0:74d8e952a3bd 247 }
Gaku0606 0:74d8e952a3bd 248 else{
Gaku0606 0:74d8e952a3bd 249 save_buffer[current] = temp;
Gaku0606 0:74d8e952a3bd 250 current++;
Gaku0606 0:74d8e952a3bd 251 if(current >= 127){
Gaku0606 0:74d8e952a3bd 252 current = 1;
Gaku0606 0:74d8e952a3bd 253 start_flag = false;
Gaku0606 8:3f32df2b66c0 254 memset(save_buffer, '\0', 128);
Gaku0606 0:74d8e952a3bd 255 }
Gaku0606 0:74d8e952a3bd 256 }
Gaku0606 0:74d8e952a3bd 257 }
Gaku0606 0:74d8e952a3bd 258 }
Gaku0606 0:74d8e952a3bd 259
Gaku0606 2:7be89bab6db9 260 bool GPS_interrupt::processGPRMC(char *line){
Gaku0606 0:74d8e952a3bd 261
Gaku0606 0:74d8e952a3bd 262 char *tok; //strtokで帰ってくる文字列のポインター
Gaku0606 0:74d8e952a3bd 263 bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue
Gaku0606 0:74d8e952a3bd 264 //bool result = false;
Gaku0606 0:74d8e952a3bd 265 //char num[4] = "";//衛星数の文字データ
Gaku0606 0:74d8e952a3bd 266 //char kaibatsu[8], g_height[8];
Gaku0606 0:74d8e952a3bd 267 //char data_GPS[256] ="";
Gaku0606 0:74d8e952a3bd 268 char angle[8], speed[8];
Gaku0606 0:74d8e952a3bd 269 char ido[16] = "", keido[16] = "";
Gaku0606 0:74d8e952a3bd 270
Gaku0606 2:7be89bab6db9 271 double X = 0, Y = 0;
Gaku0606 0:74d8e952a3bd 272 double X_m = 0, Y_m = 0;//GPS座標の"分"の部分
Gaku0606 0:74d8e952a3bd 273 char zikann[11], hizuke[7];
Gaku0606 0:74d8e952a3bd 274
Gaku0606 0:74d8e952a3bd 275 //bool gga_flag = false;
Gaku0606 0:74d8e952a3bd 276 bool rmc_flag = false;
Gaku0606 2:7be89bab6db9 277 char tok_count = 0;
Gaku0606 0:74d8e952a3bd 278
Gaku0606 4:758f97bee95a 279 //rmc_initialize();
Gaku0606 0:74d8e952a3bd 280
Gaku0606 0:74d8e952a3bd 281 //_____GPRMC___________________________________
Gaku0606 0:74d8e952a3bd 282 //データ文字列の先頭から6文字目までが$GPRMCなら０が返ってくる
Gaku0606 2:7be89bab6db9 283 //if (strncmp(line, "$GPRMC", 6) == 0){
Gaku0606 0:74d8e952a3bd 284
Gaku0606 0:74d8e952a3bd 285 tok = strtok(line, ",");
Gaku0606 2:7be89bab6db9 286 tok_count = 0;
Gaku0606 0:74d8e952a3bd 287
Gaku0606 0:74d8e952a3bd 288 //$GPRMC, 002519.799, V, , , , , 0.00, 0.00, 060180, , , N * 4A
Gaku0606 0:74d8e952a3bd 289
Gaku0606 0:74d8e952a3bd 290 // 0 , 1 , 2,3,4,5,6, 7 , 8 , 9 ,10,11 ,12
Gaku0606 0:74d8e952a3bd 291 //$GPRMC,233514.000,A,3022.5291,N,13057.6141,E,0.41,335.09,030316,,,A*6C
Gaku0606 0:74d8e952a3bd 292
Gaku0606 0:74d8e952a3bd 293 // 0 , 1 ,2, 3 ,4, 5 ,6, 7 , 8 , 9 ,1, 1 ,1
Gaku0606 0:74d8e952a3bd 294 // 0 1 2
Gaku0606 0:74d8e952a3bd 295 while (1){
Gaku0606 2:7be89bab6db9 296 switch (tok_count){
Gaku0606 2:7be89bab6db9 297 case 1://時分秒
Gaku0606 2:7be89bab6db9 298 strcpy(zikann, tok);//zikann = "233514.000\0"
Gaku0606 2:7be89bab6db9 299 break;
Gaku0606 2:7be89bab6db9 300 case 2://有効か無効か判定
Gaku0606 2:7be89bab6db9 301 if (strncmp(tok, "V", 1) == 0) return false;
Gaku0606 2:7be89bab6db9 302 else if (strcmp(tok, "A") == 0) rmc_flag = true;
Gaku0606 2:7be89bab6db9 303 break;
Gaku0606 2:7be89bab6db9 304 case 3://緯度
Gaku0606 2:7be89bab6db9 305 strcpy(ido, tok);
Gaku0606 2:7be89bab6db9 306 break;
Gaku0606 2:7be89bab6db9 307 case 4://北緯か南緯か
Gaku0606 2:7be89bab6db9 308 if (strcmp(tok, "S") == 0) latSign = true;
Gaku0606 2:7be89bab6db9 309 break;
Gaku0606 2:7be89bab6db9 310 case 5://経度
Gaku0606 2:7be89bab6db9 311 strcpy(keido, tok);
Gaku0606 2:7be89bab6db9 312 break;
Gaku0606 2:7be89bab6db9 313 case 6://東経か西経か
Gaku0606 2:7be89bab6db9 314 if (strcmp(tok, "W") == 0) lonSign = true;
Gaku0606 2:7be89bab6db9 315 break;
Gaku0606 2:7be89bab6db9 316 case 7://速度（キロノット）
Gaku0606 2:7be89bab6db9 317 strcpy(speed, tok);
Gaku0606 2:7be89bab6db9 318 break;
Gaku0606 2:7be89bab6db9 319 case 8://角度
Gaku0606 2:7be89bab6db9 320 strcpy(angle, tok);
Gaku0606 2:7be89bab6db9 321 break;
Gaku0606 2:7be89bab6db9 322 case 9://日付
Gaku0606 2:7be89bab6db9 323 strcpy(hizuke, tok);//hizuke = "030316\0"
Gaku0606 2:7be89bab6db9 324 break;
Gaku0606 0:74d8e952a3bd 325 }
Gaku0606 2:7be89bab6db9 326
Gaku0606 0:74d8e952a3bd 327 tok = strtok(NULL, ",");//comma = ",\0"
Gaku0606 0:74d8e952a3bd 328 tok_count++;
Gaku0606 0:74d8e952a3bd 329 if (tok == NULL) break;
Gaku0606 0:74d8e952a3bd 330 }
Gaku0606 0:74d8e952a3bd 331 if (rmc_flag){
Gaku0606 0:74d8e952a3bd 332 rmc_flag = false;
Gaku0606 0:74d8e952a3bd 333 //ddmmyy
Gaku0606 0:74d8e952a3bd 334 int dmy = 0;
Gaku0606 0:74d8e952a3bd 335 dmy = atoi(hizuke);
Gaku0606 0:74d8e952a3bd 336 day = dmy / 10000; //030316 / 10000 = 3
Gaku0606 0:74d8e952a3bd 337 month = (dmy - day * 10000) / 100;//30316 - 30000 = 316, 316/100= 3
Gaku0606 0:74d8e952a3bd 338 year = dmy - month * 100 - day * 10000 + 2000;
Gaku0606 0:74d8e952a3bd 339
Gaku0606 0:74d8e952a3bd 340 //hhmmss.ss
Gaku0606 0:74d8e952a3bd 341 seconds = atof(zikann);//sec = 233514.000
Gaku0606 0:74d8e952a3bd 342 hour = (int)(seconds / 10000.0); //233514/10000=(int)23
Gaku0606 0:74d8e952a3bd 343 minutes = (seconds - hour * 10000) / 100;//233514.0-230000=3514.0,3514.0/100=35
Gaku0606 0:74d8e952a3bd 344 seconds = seconds - ((double)minutes * 100.0 + (double)hour * 10000.0);
Gaku0606 0:74d8e952a3bd 345
Gaku0606 0:74d8e952a3bd 346 // getLonLatのとき有効
Gaku0606 0:74d8e952a3bd 347 Y_m = atof(ido);//*Y_m = 3457.5571
Gaku0606 0:74d8e952a3bd 348 Y = (int)(Y_m / 100.0);//Y = 34.0
Gaku0606 0:74d8e952a3bd 349 Y_m = Y_m - Y * 100.0;//Y_m = 3457.5571 - 34*100 = 57.5571
Gaku0606 0:74d8e952a3bd 350
Gaku0606 0:74d8e952a3bd 351 X_m = atof(keido);//*X = 13057.6142
Gaku0606 0:74d8e952a3bd 352 X = (int)(X_m / 100.0);//X = 130.0
Gaku0606 0:74d8e952a3bd 353 X_m = X_m - X * 100.0;//X_m = 13057.6142 - 13000.0 = 57.6142
Gaku0606 0:74d8e952a3bd 354
Gaku0606 0:74d8e952a3bd 355 //GPS calculation
Gaku0606 0:74d8e952a3bd 356 longitude = X + X_m / 60.0;// 34.959285
Gaku0606 0:74d8e952a3bd 357 latitude = Y + Y_m / 60.0;//137.090290
Gaku0606 0:74d8e952a3bd 358
Gaku0606 2:7be89bab6db9 359 if (lonSign) longitude *= (-1);//西経なら経度を負に
Gaku0606 2:7be89bab6db9 360 if (latSign) latitude *= (-1);//南緯なら緯度を負に
Gaku0606 0:74d8e952a3bd 361 knot = atof(speed);
Gaku0606 0:74d8e952a3bd 362 degree = atof(angle);
Gaku0606 8:3f32df2b66c0 363 if(longitude > 100.0 && longitude < 150.0 && latitude > 30.0 && latitude < 50.0){
Gaku0606 8:3f32df2b66c0 364 return true;
Gaku0606 8:3f32df2b66c0 365 }
Gaku0606 8:3f32df2b66c0 366 else return false;
Gaku0606 0:74d8e952a3bd 367 }
Gaku0606 4:758f97bee95a 368 else{
Gaku0606 4:758f97bee95a 369 return false;
Gaku0606 4:758f97bee95a 370 }
Gaku0606 2:7be89bab6db9 371 //}
Gaku0606 2:7be89bab6db9 372 //else return false;
Gaku0606 0:74d8e952a3bd 373 }
Gaku0606 0:74d8e952a3bd 374
Gaku0606 2:7be89bab6db9 375 bool GPS_interrupt::processGPGGA(char *line){
Gaku0606 2:7be89bab6db9 376
Gaku0606 2:7be89bab6db9 377 char *tok; //strtokで帰ってくる文字列のポインター
Gaku0606 2:7be89bab6db9 378 bool latSign = false, lonSign = false;//N,Eならfalse, S,Wならtrue
Gaku0606 2:7be89bab6db9 379 //bool result = false;
Gaku0606 2:7be89bab6db9 380 char num[4] = "";//衛星数の文字データ
Gaku0606 2:7be89bab6db9 381 char kaibatsu[8], g_height[8];
Gaku0606 2:7be89bab6db9 382 //char data_GPS[256] ="";
Gaku0606 2:7be89bab6db9 383 //char angle[8], speed[8];
Gaku0606 2:7be89bab6db9 384 char ido[16] = "", keido[16] = "";
Gaku0606 2:7be89bab6db9 385
Gaku0606 2:7be89bab6db9 386 double X = 0, Y = 0;
Gaku0606 2:7be89bab6db9 387 double X_m = 0, Y_m = 0;//GPS座標の"分"の部分
Gaku0606 2:7be89bab6db9 388 //char zikann[11], hizuke[7];
Gaku0606 2:7be89bab6db9 389 char status[2];
Gaku0606 2:7be89bab6db9 390
Gaku0606 2:7be89bab6db9 391 bool gga_flag = false;
Gaku0606 2:7be89bab6db9 392 //bool rmc_flag = false;
Gaku0606 2:7be89bab6db9 393 char tok_count = 0;
Gaku0606 2:7be89bab6db9 394
Gaku0606 4:758f97bee95a 395 //gga_initialize();
Gaku0606 2:7be89bab6db9 396
Gaku0606 2:7be89bab6db9 397 //データ文字列の先頭から6文字目までが$GPGLLなら０が返ってくる
Gaku0606 2:7be89bab6db9 398 //if (strncmp(data_GPS, "$GPGGA", 6) == 0){
Gaku0606 2:7be89bab6db9 399
Gaku0606 2:7be89bab6db9 400 tok = strtok(line, ",");
Gaku0606 2:7be89bab6db9 401 tok_count = 0;
Gaku0606 2:7be89bab6db9 402
Gaku0606 2:7be89bab6db9 403 //$GPGGA, 002519.799, , , , , 0, 0, , , M, , M, , *40
Gaku0606 2:7be89bab6db9 404
Gaku0606 2:7be89bab6db9 405 // 0 , 1 , , , , , 2, 3, , , 4, , 5, , 6
Gaku0606 2:7be89bab6db9 406
Gaku0606 2:7be89bab6db9 407 //$GPGGA,233515.000,3022.5292,N,13057.6142,E,1,9,0.88,11.3,M,29.3,M,,*63
Gaku0606 2:7be89bab6db9 408
Gaku0606 2:7be89bab6db9 409 // 0 , 1 , 2 ,3, 4 ,5,6,7, 8 , 9 ,1, 11 ,1
Gaku0606 2:7be89bab6db9 410 // 0 2
Gaku0606 2:7be89bab6db9 411 while (1){
Gaku0606 2:7be89bab6db9 412 switch (tok_count){
Gaku0606 2:7be89bab6db9 413 case 2://緯度
Gaku0606 2:7be89bab6db9 414 strcpy(ido, tok);//ido = "30"
Gaku0606 2:7be89bab6db9 415 if (strcmp(ido, "0") == 0) return false;//GPS無効
Gaku0606 2:7be89bab6db9 416 break;
Gaku0606 2:7be89bab6db9 417 case 3://北緯か南緯か
Gaku0606 2:7be89bab6db9 418 if (strcmp(tok, "S") == 0) latSign = true;
Gaku0606 2:7be89bab6db9 419 break;
Gaku0606 2:7be89bab6db9 420 case 4:
Gaku0606 2:7be89bab6db9 421 strcpy(keido, tok);//keido = "130"
Gaku0606 2:7be89bab6db9 422 break;
Gaku0606 2:7be89bab6db9 423 case 5://東経か西経か
Gaku0606 2:7be89bab6db9 424 if (strcmp(tok, "W") == 0) lonSign = true;
Gaku0606 2:7be89bab6db9 425 break;
Gaku0606 2:7be89bab6db9 426 case 6:
Gaku0606 2:7be89bab6db9 427 strcpy(status, tok);
Gaku0606 2:7be89bab6db9 428 if (strcmp(status, "0") != 0) gga_flag = true;//GPS有効
Gaku0606 2:7be89bab6db9 429 break;
Gaku0606 2:7be89bab6db9 430 case 7:
Gaku0606 2:7be89bab6db9 431 strcpy(num, tok);
Gaku0606 2:7be89bab6db9 432 break;
Gaku0606 2:7be89bab6db9 433 case 9:
Gaku0606 2:7be89bab6db9 434 strcpy(kaibatsu, tok);
Gaku0606 2:7be89bab6db9 435 break;
Gaku0606 2:7be89bab6db9 436 case 11:
Gaku0606 2:7be89bab6db9 437 strcpy(g_height, tok);
Gaku0606 2:7be89bab6db9 438 break;
Gaku0606 2:7be89bab6db9 439 }
Gaku0606 2:7be89bab6db9 440 tok = strtok(NULL, ",");//comma = ",\0"
Gaku0606 2:7be89bab6db9 441 tok_count++;
Gaku0606 2:7be89bab6db9 442 if (tok == NULL) break;
Gaku0606 2:7be89bab6db9 443 }
Gaku0606 2:7be89bab6db9 444 if (gga_flag){
Gaku0606 2:7be89bab6db9 445
Gaku0606 2:7be89bab6db9 446 height = atof(kaibatsu);
Gaku0606 2:7be89bab6db9 447 geoid = atof(g_height);
Gaku0606 2:7be89bab6db9 448 number = atoi(num);
Gaku0606 2:7be89bab6db9 449 /*
Gaku0606 2:7be89bab6db9 450 //ddmmyy
Gaku0606 2:7be89bab6db9 451 long dmy = 0;
Gaku0606 2:7be89bab6db9 452 dmy = atol(hizuke);
Gaku0606 2:7be89bab6db9 453 day = (int)(dmy / 10000); //030316 / 10000 = 3
Gaku0606 2:7be89bab6db9 454 month = (int)(dmy / 100 - day * 100);//30316 - 30000 = 316, 316/100= 3
Gaku0606 2:7be89bab6db9 455 year = dmy - month * 100 - (long)day * 10000 + 2000;
Gaku0606 2:7be89bab6db9 456
Gaku0606 2:7be89bab6db9 457 //hhmmss.ss
Gaku0606 2:7be89bab6db9 458 seconds = atof(zikann);//sec = 233514.000
Gaku0606 2:7be89bab6db9 459 hour = (int)(seconds / 10000.0); //233514/10000=(int)23
Gaku0606 2:7be89bab6db9 460 minutes = (int)(seconds / 100.0 - hour * 100.0);//233514.0-230000=3514.0,3514.0/100=35
Gaku0606 2:7be89bab6db9 461 seconds = seconds - ((double)minutes * 100.0 + (double)hour * 10000.0);
Gaku0606 2:7be89bab6db9 462 */
Gaku0606 2:7be89bab6db9 463 // getLonLatのとき有効
Gaku0606 2:7be89bab6db9 464 Y_m = atof(ido);//Y_m = 3457.5571
Gaku0606 2:7be89bab6db9 465 Y = (int)(Y_m / 100.0);//Y = 34.0
Gaku0606 2:7be89bab6db9 466 Y_m = Y_m - Y * 100.0;//Y_m = 3457.5571 - 34*100 = 57.5571
Gaku0606 2:7be89bab6db9 467
Gaku0606 2:7be89bab6db9 468 X_m = atof(keido);//X = 13057.6142
Gaku0606 2:7be89bab6db9 469 X = (int)(X_m / 100.0);//X = 130.0
Gaku0606 2:7be89bab6db9 470 X_m = X_m - X * 100.0;//X_m = 13057.6142 - 13000.0 = 57.6142
Gaku0606 2:7be89bab6db9 471
Gaku0606 2:7be89bab6db9 472 //GPS calculation
Gaku0606 2:7be89bab6db9 473 longitude = X + X_m / 60.0;// 34.959285
Gaku0606 2:7be89bab6db9 474 latitude = Y + Y_m / 60.0;//137.090290
Gaku0606 2:7be89bab6db9 475 if (lonSign == true) longitude = longitude * (-1);//西経なら経度を負に
Gaku0606 2:7be89bab6db9 476 if (latSign == true) latitude = latitude * (-1);//南緯なら緯度を負に
Gaku0606 2:7be89bab6db9 477
Gaku0606 8:3f32df2b66c0 478 if(longitude > 100.0 && longitude < 150.0 && latitude > 30.0 && latitude < 50.0){
Gaku0606 8:3f32df2b66c0 479 return true;
Gaku0606 8:3f32df2b66c0 480 }
Gaku0606 8:3f32df2b66c0 481 else{
Gaku0606 8:3f32df2b66c0 482 return false;
Gaku0606 8:3f32df2b66c0 483 }
Gaku0606 2:7be89bab6db9 484 }
Gaku0606 4:758f97bee95a 485 else{
Gaku0606 4:758f97bee95a 486 return false;
Gaku0606 4:758f97bee95a 487 }
Gaku0606 8:3f32df2b66c0 488 return false;
Gaku0606 8:3f32df2b66c0 489 }
Gaku0606 8:3f32df2b66c0 490
Gaku0606 8:3f32df2b66c0 491 double GPS_interrupt::Distance(double x, double y){
Gaku0606 2:7be89bab6db9 492
Gaku0606 8:3f32df2b66c0 493 double dLat = x - longitude;//相対経度
Gaku0606 8:3f32df2b66c0 494 double dLng = y - latitude;//相対緯度
Gaku0606 8:3f32df2b66c0 495 double radLat = latitude * GPS_PI / 180.0;//今の緯度をラジアンにしたもの
Gaku0606 8:3f32df2b66c0 496 double F = EIRTH_AspectRatioInverse;
Gaku0606 8:3f32df2b66c0 497 // 離心率の2乗
Gaku0606 8:3f32df2b66c0 498 double E = ((2.0 * F) - 1.0) / (F * F);
Gaku0606 8:3f32df2b66c0 499 // π * 赤道半径
Gaku0606 8:3f32df2b66c0 500 double PI_ER = GPS_PI * EARTH_EQUATOR_RADIUS;
Gaku0606 8:3f32df2b66c0 501 // 1 - e^2 sin^2 (θ)
Gaku0606 8:3f32df2b66c0 502 double TMP = 1.0 - E * sin(radLat) * sin(radLat);
Gaku0606 8:3f32df2b66c0 503 // 経度1°あたりの長さ
Gaku0606 8:3f32df2b66c0 504 double arc_lat = (PI_ER * (1.0 - E)) / ( 180.0 * pow(TMP, 1.5));
Gaku0606 8:3f32df2b66c0 505 // 緯度1°あたりの長さ
Gaku0606 8:3f32df2b66c0 506 double arc_lng = (PI_ER * cos(radLat)) / (180.0 * pow(TMP, 0.5));
Gaku0606 8:3f32df2b66c0 507
Gaku0606 8:3f32df2b66c0 508 dLat *= arc_lat;
Gaku0606 8:3f32df2b66c0 509 dLng *= arc_lng;
Gaku0606 8:3f32df2b66c0 510 return sqrt(dLat * dLat + dLng * dLng);
Gaku0606 2:7be89bab6db9 511 }