gps.cpp

00001 #include "gps.h"
00002 
00003 void GPSDecoder::set_nmea_message(char *nmea_msg)
00004 {
00005     strcpy(nmea_line, nmea_msg);
00006 }
00007 
00008 void GPSDecoder::decode()
00009 {
00010     char *nmea_lines[MAXTOKEN];
00011     //printf("%s\r\n", nmea_msg);
00012     if (validate() != 0) {
00013         printf("invalid message\n");
00014         return;
00015     }
00016 
00017     uint8_t nmea_line_index = split(nmea_line, ',', nmea_lines, MAXTOKEN);
00018 #if 0
00019     for (int i = 0; i < nmea_line_index; i++) {
00020         printf("[%2d]%s\r\n", i, nmea_lines[i]);
00021     }
00022 #endif
00023 
00024     if (strcmp((nmea_lines[0]) + 3, "RMC") == 0) {
00025         double decimal_part[2] = {0};
00026 
00027         /* 時刻のデコード */
00028         hour = (uint8_t)str2int(nmea_lines[1], 2);
00029         min = (uint8_t)str2int(nmea_lines[1] + 2, 2);
00030         sec = (uint8_t)str2int(nmea_lines[1] + 4, 2);
00031         msec = (uint16_t)str2int(nmea_lines[1] + 7, 3);
00032 
00033         /* 日付のデコード */
00034         day = (uint8_t)str2int(nmea_lines[9], 2);
00035         month = (uint8_t)str2int(nmea_lines[9] + 2, 2);
00036         year = (uint16_t)str2int(nmea_lines[9] + 4, 2) + 2000;
00037 
00038         if (nmea_lines[2][0] == 'A') {
00039             //printf("RMC mode: Information\r\n");
00040         } else if (nmea_lines[2][0] == 'V') {
00041             //printf("RMC mode: Warning\r\n");
00042         } else {
00043             printf("RMC mode: invalid");
00044             return;
00045         }
00046         if (nmea_lines[2][0] == 'A') {
00047             if (nmea_lines[2][0] == 'A') {
00048                 /* 位置情報のデコード */
00049                 decimal_part[0] = modf(strtof(nmea_lines[3], NULL), &latitude); // 秒
00050                 latitude /= 100;
00051                 decimal_part[1] = floor((modf(latitude, &latitude) * 100) + 0.5); // 分
00052                 latitude += (decimal_part[1] + decimal_part[0]) / 60;
00053 
00054                 if (nmea_lines[4][0] == 'S')
00055                     latitude = -latitude;
00056 
00057                 decimal_part[0] = modf(strtof(nmea_lines[5], NULL), &longitude); // 秒
00058                 longitude /= 100;
00059                 decimal_part[1] = floor((modf(longitude, &longitude) * 100) + 0.5); // 分
00060                 longitude += (decimal_part[1] + decimal_part[0]) / 60;
00061 
00062                 if (nmea_lines[6][0] == 'W')
00063                     longitude = -longitude;
00064 
00065                 /* 移動速度のデコード */
00066                 speed = strtof(nmea_lines[7], NULL) * 1.852f; // knot to km/h
00067 
00068                 move_direction = strtof(nmea_lines[8], NULL);
00069             }
00070         } else {
00071             longitude = 0;
00072             latitude = 0;
00073         }
00074     } else if (strcmp((nmea_lines[0]) + 3, "GGA") == 0) {
00075         double decimal_part[2] = {0};
00076 
00077         /* 時刻のデコード */
00078         hour = (uint8_t)str2int(nmea_lines[1], 2);
00079         min = (uint8_t)str2int(nmea_lines[1] + 2, 2);
00080         sec = (uint8_t)str2int(nmea_lines[1] + 4, 2);
00081         msec = (uint16_t)str2int(nmea_lines[1] + 7, 3);
00082 
00083         // GGAでは日付は不明
00084 
00085         /* 位置情報のデコード */
00086         decimal_part[0] = modf(strtof(nmea_lines[2], NULL), &latitude); // 秒
00087         latitude /= 100;
00088         decimal_part[1] = floor((modf(latitude, &latitude) * 100) + 0.5); // 分
00089         latitude += (decimal_part[1] + decimal_part[0]) / 60;
00090 
00091         if (nmea_lines[3][0] == 'S')
00092             latitude = -latitude;
00093 
00094         decimal_part[0] = modf(strtof(nmea_lines[4], NULL), &longitude); // 秒
00095         longitude /= 100;
00096         decimal_part[1] = floor((modf(longitude, &longitude) * 100) + 0.5); // 分
00097         longitude += (decimal_part[1] + decimal_part[0]) / 60;
00098 
00099         if (nmea_lines[5][0] == 'W')
00100             longitude = -longitude;
00101 
00102         // 移動速度と移動方向はGGAではわからない
00103 
00104     }
00105 }
00106 
00107 // valid = 0
00108 // invalid = other
00109 uint8_t GPSDecoder::validate()
00110 {
00111     uint8_t checksum = 0, nmea_checksum, flag = 0;
00112     char checksum_str[3] = {0};
00113 
00114     if (nmea_line[0] != '$') {
00115 //        printf("invalid preamble %02X\n", nmea_line[0]);
00116         return 1;
00117     }
00118 
00119     for (int i = 1; i < strlen((char *)nmea_line); i++) {
00120         if (flag) {
00121             // 2byte checksum
00122             memcpy(checksum_str, (char *)nmea_line + i, 2);
00123             nmea_checksum = (uint8_t)strtol(checksum_str, NULL, 16);
00124             break;
00125         } else {
00126             if (nmea_line[i] == '*') {
00127                 flag = 1;
00128                 continue;
00129             }
00130             checksum = checksum ^ nmea_line[i];
00131         }
00132     }
00133     if (checksum != nmea_checksum) {
00134 //        printf("invalid checksum\n");
00135         return 2;
00136     }
00137 //    printf("validation passed\n");
00138     return 0;
00139 }
00140 
00141 long GPSDecoder::str2int(char *src_ptr, uint8_t len)
00142 {
00143     char tmp[16] = {0};
00144     memcpy(tmp, src_ptr, len);
00145     return strtol(tmp, NULL, 10);
00146 }
00147 
00148 int GPSDecoder::split(char *str, const char delim, char *token[], int max_item)
00149 {
00150     int cnt = 0;
00151     int len = strlen(str);
00152 
00153     token[cnt++] = str;
00154     for (int i = 0; i < len; i++) {
00155         if (str[i] == delim) {
00156             str[i] = '\0';
00157             if (cnt == max_item)
00158                 return cnt;
00159             token[cnt++] = str + i + 1;
00160         }
00161     }
00162 
00163     return cnt;
00164 }
00165 
00166 time_t GPSDecoder::get_unixtime()
00167 {
00168     struct tm t;
00169     t.tm_sec = sec;
00170     t.tm_min = min;
00171     t.tm_hour = hour;
00172     t.tm_mday = day;
00173     t.tm_mon = month - 1;
00174     t.tm_year = year - 1900;
00175 
00176     return mktime(&t);
00177 }
00178 
00179 void GPSDecoder::dump_info(char *buf)
00180 {
00181     sprintf(buf, "%d/%02d/%02d %02d:%02d:%02d.%03d (UTC)\r\n", year, month, day, hour, min, sec, msec);
00182     sprintf(buf, "%slatitude: %f\r\n", buf, latitude);
00183     sprintf(buf, "%slongitude: %f\r\n", buf, longitude);
00184     sprintf(buf, "%sspeed %f km/h (%f knot)\r\n", buf, speed, speed / 1.852f);
00185     sprintf(buf, "%sdirection %f degree\r\n", buf, move_direction);
00186 }