SAKURA Internet / gps

spiralray gps decoder

Dependents:   SakuraIO_Evaluation_Board_Standard SakuraIO_Evalution_Board_Standard sakuraio_plus_rental_server

gps.cpp@2:fc3ce3241d89, 2017-11-29 (annotated)

Committer:
misodengaku
Date:
Wed Nov 29 06:23:34 2017 +0000
Revision:
2:fc3ce3241d89
Parent:
1:f949d6d3a7e7
Child:
4:82d2fced184b
unixtime

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 0:8132a51314ec 16
misodengaku 0:8132a51314ec 17 uint8_t nmea_line_index = split(nmea_line, ',', nmea_lines, MAXTOKEN);
misodengaku 0:8132a51314ec 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 0:8132a51314ec 74 }
misodengaku 0:8132a51314ec 75 }
misodengaku 0:8132a51314ec 76
misodengaku 0:8132a51314ec 77 // valid = 0
misodengaku 0:8132a51314ec 78 // invalid = other
misodengaku 0:8132a51314ec 79 uint8_t GPSDecoder::validate()
misodengaku 0:8132a51314ec 80 {
misodengaku 0:8132a51314ec 81 uint8_t checksum = 0, nmea_checksum, flag = 0;
misodengaku 0:8132a51314ec 82 char checksum_str[3] = {0};
misodengaku 0:8132a51314ec 83
misodengaku 0:8132a51314ec 84 if (nmea_line[0] != '$') {
misodengaku 0:8132a51314ec 85 return 1;
misodengaku 0:8132a51314ec 86 }
misodengaku 0:8132a51314ec 87
misodengaku 0:8132a51314ec 88 for (int i = 1; i < strlen((char *)nmea_line); i++) {
misodengaku 0:8132a51314ec 89 if (flag) {
misodengaku 0:8132a51314ec 90 // 2byte checksum
misodengaku 0:8132a51314ec 91 memcpy(checksum_str, (char *)nmea_line + i, 2);
misodengaku 0:8132a51314ec 92 nmea_checksum = (uint8_t)strtol(checksum_str, NULL, 16);
misodengaku 0:8132a51314ec 93 break;
misodengaku 0:8132a51314ec 94 } else {
misodengaku 0:8132a51314ec 95 if (nmea_line[i] == '*') {
misodengaku 0:8132a51314ec 96 flag = 1;
misodengaku 0:8132a51314ec 97 continue;
misodengaku 0:8132a51314ec 98 }
misodengaku 0:8132a51314ec 99 checksum = checksum ^ nmea_line[i];
misodengaku 0:8132a51314ec 100 }
misodengaku 0:8132a51314ec 101 }
misodengaku 0:8132a51314ec 102 if (checksum != nmea_checksum) {
misodengaku 0:8132a51314ec 103 return 2;
misodengaku 0:8132a51314ec 104 }
misodengaku 0:8132a51314ec 105 return 0;
misodengaku 0:8132a51314ec 106 }
misodengaku 0:8132a51314ec 107
misodengaku 0:8132a51314ec 108 long GPSDecoder::str2int(char *src_ptr, uint8_t len)
misodengaku 0:8132a51314ec 109 {
misodengaku 0:8132a51314ec 110 char tmp[16] = {0};
misodengaku 0:8132a51314ec 111 memcpy(tmp, src_ptr, len);
misodengaku 0:8132a51314ec 112 return strtol(tmp, NULL, 10);
misodengaku 0:8132a51314ec 113 }
misodengaku 0:8132a51314ec 114
misodengaku 0:8132a51314ec 115 int GPSDecoder::split(char *str, const char delim, char *token[], int max_item)
misodengaku 0:8132a51314ec 116 {
misodengaku 0:8132a51314ec 117 int cnt = 0;
misodengaku 0:8132a51314ec 118 int len = strlen(str);
misodengaku 0:8132a51314ec 119
misodengaku 0:8132a51314ec 120 token[cnt++] = str;
misodengaku 0:8132a51314ec 121 for (int i = 0; i < len; i++) {
misodengaku 0:8132a51314ec 122 if (str[i] == delim) {
misodengaku 0:8132a51314ec 123 str[i] = '\0';
misodengaku 0:8132a51314ec 124 if (cnt == max_item)
misodengaku 0:8132a51314ec 125 return cnt;
misodengaku 0:8132a51314ec 126 token[cnt++] = str + i + 1;
misodengaku 0:8132a51314ec 127 }
misodengaku 0:8132a51314ec 128 }
misodengaku 0:8132a51314ec 129
misodengaku 0:8132a51314ec 130 return cnt;
misodengaku 0:8132a51314ec 131 }
misodengaku 0:8132a51314ec 132
misodengaku 2:fc3ce3241d89 133 time_t GPSDecoder::get_unixtime()
misodengaku 2:fc3ce3241d89 134 {
misodengaku 2:fc3ce3241d89 135 struct tm t;
misodengaku 2:fc3ce3241d89 136 t.tm_sec = sec;
misodengaku 2:fc3ce3241d89 137 t.tm_min = min;
misodengaku 2:fc3ce3241d89 138 t.tm_hour = hour;
misodengaku 2:fc3ce3241d89 139 t.tm_mday = day;
misodengaku 2:fc3ce3241d89 140 t.tm_mon = month - 1;
misodengaku 2:fc3ce3241d89 141 t.tm_year = year - 1900;
misodengaku 2:fc3ce3241d89 142
misodengaku 2:fc3ce3241d89 143 return mktime(&t);
misodengaku 2:fc3ce3241d89 144 }
misodengaku 2:fc3ce3241d89 145
misodengaku 0:8132a51314ec 146 void GPSDecoder::dump_info(char *buf)
misodengaku 0:8132a51314ec 147 {
misodengaku 0:8132a51314ec 148 sprintf(buf, "%d/%02d/%02d %02d:%02d:%02d.%03d (UTC)\r\n", year, month, day, hour, min, sec, msec);
misodengaku 0:8132a51314ec 149 sprintf(buf, "%slatitude: %f\r\n", buf, latitude);
misodengaku 0:8132a51314ec 150 sprintf(buf, "%slongitude: %f\r\n", buf, longitude);
misodengaku 1:f949d6d3a7e7 151 sprintf(buf, "%sspeed %f km/h (%f knot)\r\n", buf, speed, speed / 1.852f);
misodengaku 0:8132a51314ec 152 sprintf(buf, "%sdirection %f degree\r\n", buf, move_direction);
misodengaku 0:8132a51314ec 153 }