spiralray gps decoder
Dependents: SakuraIO_Evaluation_Board_Standard SakuraIO_Evalution_Board_Standard sakuraio_plus_rental_server
gps.cpp@0:8132a51314ec, 2017-11-29 (annotated)
- Committer:
- misodengaku
- Date:
- Wed Nov 29 05:37:22 2017 +0000
- Revision:
- 0:8132a51314ec
- Child:
- 1:f949d6d3a7e7
init
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:8132a51314ec | 66 | speed = strtof(nmea_lines[7], NULL); |
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 | 0:8132a51314ec | 133 | void GPSDecoder::dump_info(char *buf) |
misodengaku | 0:8132a51314ec | 134 | { |
misodengaku | 0:8132a51314ec | 135 | sprintf(buf, "%d/%02d/%02d %02d:%02d:%02d.%03d (UTC)\r\n", year, month, day, hour, min, sec, msec); |
misodengaku | 0:8132a51314ec | 136 | sprintf(buf, "%slatitude: %f\r\n", buf, latitude); |
misodengaku | 0:8132a51314ec | 137 | sprintf(buf, "%slongitude: %f\r\n", buf, longitude); |
misodengaku | 0:8132a51314ec | 138 | sprintf(buf, "%sspeed %f km/h (%f knot)\r\n", buf, speed * 1.852f, speed); |
misodengaku | 0:8132a51314ec | 139 | sprintf(buf, "%sdirection %f degree\r\n", buf, move_direction); |
misodengaku | 0:8132a51314ec | 140 | } |