SAKURA Internet / gps

spiralray gps decoder

Dependents:   SakuraIO_Evaluation_Board_Standard SakuraIO_Evalution_Board_Standard sakuraio_plus_rental_server

gps.cpp@4:82d2fced184b, 2019-01-15 (annotated)

Committer:
misodengaku
Date:
Tue Jan 15 06:51:48 2019 +0000
Revision:
4:82d2fced184b
Parent:
2:fc3ce3241d89 
add GGA message support

Who changed what in which revision?

UserRevisionLine numberNew contents of line
misodengaku 0:8132a51314ec 1 #include "gps.h"
misodengaku 0:8132a51314ec 2
misodengaku 0:8132a51314ec 3 void GPSDecoder::set_nmea_message(char *nmea_msg)
misodengaku 0:8132a51314ec 4 {
misodengaku 0:8132a51314ec 5 strcpy(nmea_line, nmea_msg);
misodengaku 0:8132a51314ec 6 }
misodengaku 0:8132a51314ec 7
misodengaku 0:8132a51314ec 8 void GPSDecoder::decode()
misodengaku 0:8132a51314ec 9 {
misodengaku 0:8132a51314ec 10 char *nmea_lines[MAXTOKEN];
misodengaku 0:8132a51314ec 11 //printf("%s\r\n", nmea_msg);
misodengaku 0:8132a51314ec 12 if (validate() != 0) {
misodengaku 0:8132a51314ec 13 printf("invalid message\n");
misodengaku 0:8132a51314ec 14 return;
misodengaku 0:8132a51314ec 15 }
misodengaku 4:82d2fced184b 16
misodengaku 0:8132a51314ec 17 uint8_t nmea_line_index = split(nmea_line, ',', nmea_lines, MAXTOKEN);
misodengaku 4:82d2fced184b 18 #if 0
misodengaku 0:8132a51314ec 19 for (int i = 0; i < nmea_line_index; i++) {
misodengaku 0:8132a51314ec 20 printf("[%2d]%s\r\n", i, nmea_lines[i]);
misodengaku 0:8132a51314ec 21 }
misodengaku 0:8132a51314ec 22 #endif
misodengaku 0:8132a51314ec 23
misodengaku 0:8132a51314ec 24 if (strcmp((nmea_lines[0]) + 3, "RMC") == 0) {
misodengaku 0:8132a51314ec 25 double decimal_part[2] = {0};
misodengaku 0:8132a51314ec 26
misodengaku 0:8132a51314ec 27 /* 時刻のデコード */
misodengaku 0:8132a51314ec 28 hour = (uint8_t)str2int(nmea_lines[1], 2);
misodengaku 0:8132a51314ec 29 min = (uint8_t)str2int(nmea_lines[1] + 2, 2);
misodengaku 0:8132a51314ec 30 sec = (uint8_t)str2int(nmea_lines[1] + 4, 2);
misodengaku 0:8132a51314ec 31 msec = (uint16_t)str2int(nmea_lines[1] + 7, 3);
misodengaku 0:8132a51314ec 32
misodengaku 0:8132a51314ec 33 /* 日付のデコード */
misodengaku 0:8132a51314ec 34 day = (uint8_t)str2int(nmea_lines[9], 2);
misodengaku 0:8132a51314ec 35 month = (uint8_t)str2int(nmea_lines[9] + 2, 2);
misodengaku 0:8132a51314ec 36 year = (uint16_t)str2int(nmea_lines[9] + 4, 2) + 2000;
misodengaku 0:8132a51314ec 37
misodengaku 0:8132a51314ec 38 if (nmea_lines[2][0] == 'A') {
misodengaku 0:8132a51314ec 39 //printf("RMC mode: Information\r\n");
misodengaku 0:8132a51314ec 40 } else if (nmea_lines[2][0] == 'V') {
misodengaku 0:8132a51314ec 41 //printf("RMC mode: Warning\r\n");
misodengaku 0:8132a51314ec 42 } else {
misodengaku 0:8132a51314ec 43 printf("RMC mode: invalid");
misodengaku 0:8132a51314ec 44 return;
misodengaku 0:8132a51314ec 45 }
misodengaku 0:8132a51314ec 46 if (nmea_lines[2][0] == 'A') {
misodengaku 0:8132a51314ec 47 if (nmea_lines[2][0] == 'A') {
misodengaku 0:8132a51314ec 48 /* 位置情報のデコード */
misodengaku 0:8132a51314ec 49 decimal_part[0] = modf(strtof(nmea_lines[3], NULL), &latitude); // 秒
misodengaku 0:8132a51314ec 50 latitude /= 100;
misodengaku 0:8132a51314ec 51 decimal_part[1] = floor((modf(latitude, &latitude) * 100) + 0.5); // 分
misodengaku 0:8132a51314ec 52 latitude += (decimal_part[1] + decimal_part[0]) / 60;
misodengaku 0:8132a51314ec 53
misodengaku 0:8132a51314ec 54 if (nmea_lines[4][0] == 'S')
misodengaku 0:8132a51314ec 55 latitude = -latitude;
misodengaku 0:8132a51314ec 56
misodengaku 0:8132a51314ec 57 decimal_part[0] = modf(strtof(nmea_lines[5], NULL), &longitude); // 秒
misodengaku 0:8132a51314ec 58 longitude /= 100;
misodengaku 0:8132a51314ec 59 decimal_part[1] = floor((modf(longitude, &longitude) * 100) + 0.5); // 分
misodengaku 0:8132a51314ec 60 longitude += (decimal_part[1] + decimal_part[0]) / 60;
misodengaku 0:8132a51314ec 61
misodengaku 0:8132a51314ec 62 if (nmea_lines[6][0] == 'W')
misodengaku 0:8132a51314ec 63 longitude = -longitude;
misodengaku 0:8132a51314ec 64
misodengaku 0:8132a51314ec 65 /* 移動速度のデコード */
misodengaku 1:f949d6d3a7e7 66 speed = strtof(nmea_lines[7], NULL) * 1.852f; // knot to km/h
misodengaku 0:8132a51314ec 67
misodengaku 0:8132a51314ec 68 move_direction = strtof(nmea_lines[8], NULL);
misodengaku 0:8132a51314ec 69 }
misodengaku 0:8132a51314ec 70 } else {
misodengaku 0:8132a51314ec 71 longitude = 0;
misodengaku 0:8132a51314ec 72 latitude = 0;
misodengaku 0:8132a51314ec 73 }
misodengaku 4:82d2fced184b 74 } else if (strcmp((nmea_lines[0]) + 3, "GGA") == 0) {
misodengaku 4:82d2fced184b 75 double decimal_part[2] = {0};
misodengaku 4:82d2fced184b 76
misodengaku 4:82d2fced184b 77 /* 時刻のデコード */
misodengaku 4:82d2fced184b 78 hour = (uint8_t)str2int(nmea_lines[1], 2);
misodengaku 4:82d2fced184b 79 min = (uint8_t)str2int(nmea_lines[1] + 2, 2);
misodengaku 4:82d2fced184b 80 sec = (uint8_t)str2int(nmea_lines[1] + 4, 2);
misodengaku 4:82d2fced184b 81 msec = (uint16_t)str2int(nmea_lines[1] + 7, 3);
misodengaku 4:82d2fced184b 82
misodengaku 4:82d2fced184b 83 // GGAでは日付は不明
misodengaku 4:82d2fced184b 84
misodengaku 4:82d2fced184b 85 /* 位置情報のデコード */
misodengaku 4:82d2fced184b 86 decimal_part[0] = modf(strtof(nmea_lines[2], NULL), &latitude); // 秒
misodengaku 4:82d2fced184b 87 latitude /= 100;
misodengaku 4:82d2fced184b 88 decimal_part[1] = floor((modf(latitude, &latitude) * 100) + 0.5); // 分
misodengaku 4:82d2fced184b 89 latitude += (decimal_part[1] + decimal_part[0]) / 60;
misodengaku 4:82d2fced184b 90
misodengaku 4:82d2fced184b 91 if (nmea_lines[3][0] == 'S')
misodengaku 4:82d2fced184b 92 latitude = -latitude;
misodengaku 4:82d2fced184b 93
misodengaku 4:82d2fced184b 94 decimal_part[0] = modf(strtof(nmea_lines[4], NULL), &longitude); // 秒
misodengaku 4:82d2fced184b 95 longitude /= 100;
misodengaku 4:82d2fced184b 96 decimal_part[1] = floor((modf(longitude, &longitude) * 100) + 0.5); // 分
misodengaku 4:82d2fced184b 97 longitude += (decimal_part[1] + decimal_part[0]) / 60;
misodengaku 4:82d2fced184b 98
misodengaku 4:82d2fced184b 99 if (nmea_lines[5][0] == 'W')
misodengaku 4:82d2fced184b 100 longitude = -longitude;
misodengaku 4:82d2fced184b 101
misodengaku 4:82d2fced184b 102 // 移動速度と移動方向はGGAではわからない
misodengaku 4:82d2fced184b 103
misodengaku 0:8132a51314ec 104 }
misodengaku 0:8132a51314ec 105 }
misodengaku 0:8132a51314ec 106
misodengaku 0:8132a51314ec 107 // valid = 0
misodengaku 0:8132a51314ec 108 // invalid = other
misodengaku 0:8132a51314ec 109 uint8_t GPSDecoder::validate()
misodengaku 0:8132a51314ec 110 {
misodengaku 0:8132a51314ec 111 uint8_t checksum = 0, nmea_checksum, flag = 0;
misodengaku 0:8132a51314ec 112 char checksum_str[3] = {0};
misodengaku 0:8132a51314ec 113
misodengaku 0:8132a51314ec 114 if (nmea_line[0] != '$') {
misodengaku 4:82d2fced184b 115 // printf("invalid preamble %02X\n", nmea_line[0]);
misodengaku 0:8132a51314ec 116 return 1;
misodengaku 0:8132a51314ec 117 }
misodengaku 0:8132a51314ec 118
misodengaku 0:8132a51314ec 119 for (int i = 1; i < strlen((char *)nmea_line); i++) {
misodengaku 0:8132a51314ec 120 if (flag) {
misodengaku 0:8132a51314ec 121 // 2byte checksum
misodengaku 0:8132a51314ec 122 memcpy(checksum_str, (char *)nmea_line + i, 2);
misodengaku 0:8132a51314ec 123 nmea_checksum = (uint8_t)strtol(checksum_str, NULL, 16);
misodengaku 0:8132a51314ec 124 break;
misodengaku 0:8132a51314ec 125 } else {
misodengaku 0:8132a51314ec 126 if (nmea_line[i] == '*') {
misodengaku 0:8132a51314ec 127 flag = 1;
misodengaku 0:8132a51314ec 128 continue;
misodengaku 0:8132a51314ec 129 }
misodengaku 0:8132a51314ec 130 checksum = checksum ^ nmea_line[i];
misodengaku 0:8132a51314ec 131 }
misodengaku 0:8132a51314ec 132 }
misodengaku 0:8132a51314ec 133 if (checksum != nmea_checksum) {
misodengaku 4:82d2fced184b 134 // printf("invalid checksum\n");
misodengaku 0:8132a51314ec 135 return 2;
misodengaku 0:8132a51314ec 136 }
misodengaku 4:82d2fced184b 137 // printf("validation passed\n");
misodengaku 0:8132a51314ec 138 return 0;
misodengaku 0:8132a51314ec 139 }
misodengaku 0:8132a51314ec 140
misodengaku 0:8132a51314ec 141 long GPSDecoder::str2int(char *src_ptr, uint8_t len)
misodengaku 0:8132a51314ec 142 {
misodengaku 0:8132a51314ec 143 char tmp[16] = {0};
misodengaku 0:8132a51314ec 144 memcpy(tmp, src_ptr, len);
misodengaku 0:8132a51314ec 145 return strtol(tmp, NULL, 10);
misodengaku 0:8132a51314ec 146 }
misodengaku 0:8132a51314ec 147
misodengaku 0:8132a51314ec 148 int GPSDecoder::split(char *str, const char delim, char *token[], int max_item)
misodengaku 0:8132a51314ec 149 {
misodengaku 0:8132a51314ec 150 int cnt = 0;
misodengaku 0:8132a51314ec 151 int len = strlen(str);
misodengaku 0:8132a51314ec 152
misodengaku 0:8132a51314ec 153 token[cnt++] = str;
misodengaku 0:8132a51314ec 154 for (int i = 0; i < len; i++) {
misodengaku 0:8132a51314ec 155 if (str[i] == delim) {
misodengaku 0:8132a51314ec 156 str[i] = '\0';
misodengaku 0:8132a51314ec 157 if (cnt == max_item)
misodengaku 0:8132a51314ec 158 return cnt;
misodengaku 0:8132a51314ec 159 token[cnt++] = str + i + 1;
misodengaku 0:8132a51314ec 160 }
misodengaku 0:8132a51314ec 161 }
misodengaku 0:8132a51314ec 162
misodengaku 0:8132a51314ec 163 return cnt;
misodengaku 0:8132a51314ec 164 }
misodengaku 0:8132a51314ec 165
misodengaku 2:fc3ce3241d89 166 time_t GPSDecoder::get_unixtime()
misodengaku 2:fc3ce3241d89 167 {
misodengaku 2:fc3ce3241d89 168 struct tm t;
misodengaku 2:fc3ce3241d89 169 t.tm_sec = sec;
misodengaku 2:fc3ce3241d89 170 t.tm_min = min;
misodengaku 2:fc3ce3241d89 171 t.tm_hour = hour;
misodengaku 2:fc3ce3241d89 172 t.tm_mday = day;
misodengaku 2:fc3ce3241d89 173 t.tm_mon = month - 1;
misodengaku 2:fc3ce3241d89 174 t.tm_year = year - 1900;
misodengaku 4:82d2fced184b 175
misodengaku 2:fc3ce3241d89 176 return mktime(&t);
misodengaku 2:fc3ce3241d89 177 }
misodengaku 2:fc3ce3241d89 178
misodengaku 0:8132a51314ec 179 void GPSDecoder::dump_info(char *buf)
misodengaku 0:8132a51314ec 180 {
misodengaku 0:8132a51314ec 181 sprintf(buf, "%d/%02d/%02d %02d:%02d:%02d.%03d (UTC)\r\n", year, month, day, hour, min, sec, msec);
misodengaku 0:8132a51314ec 182 sprintf(buf, "%slatitude: %f\r\n", buf, latitude);
misodengaku 0:8132a51314ec 183 sprintf(buf, "%slongitude: %f\r\n", buf, longitude);
misodengaku 1:f949d6d3a7e7 184 sprintf(buf, "%sspeed %f km/h (%f knot)\r\n", buf, speed, speed / 1.852f);
misodengaku 0:8132a51314ec 185 sprintf(buf, "%sdirection %f degree\r\n", buf, move_direction);
misodengaku 0:8132a51314ec 186 }