spiralray gps decoder
Dependents: SakuraIO_Evaluation_Board_Standard SakuraIO_Evalution_Board_Standard sakuraio_plus_rental_server
gps.cpp
- Committer:
- misodengaku
- Date:
- 2019-01-15
- Revision:
- 4:82d2fced184b
- Parent:
- 2:fc3ce3241d89
File content as of revision 4:82d2fced184b:
#include "gps.h" void GPSDecoder::set_nmea_message(char *nmea_msg) { strcpy(nmea_line, nmea_msg); } void GPSDecoder::decode() { char *nmea_lines[MAXTOKEN]; //printf("%s\r\n", nmea_msg); if (validate() != 0) { printf("invalid message\n"); return; } uint8_t nmea_line_index = split(nmea_line, ',', nmea_lines, MAXTOKEN); #if 0 for (int i = 0; i < nmea_line_index; i++) { printf("[%2d]%s\r\n", i, nmea_lines[i]); } #endif if (strcmp((nmea_lines[0]) + 3, "RMC") == 0) { double decimal_part[2] = {0}; /* 時刻のデコード */ hour = (uint8_t)str2int(nmea_lines[1], 2); min = (uint8_t)str2int(nmea_lines[1] + 2, 2); sec = (uint8_t)str2int(nmea_lines[1] + 4, 2); msec = (uint16_t)str2int(nmea_lines[1] + 7, 3); /* 日付のデコード */ day = (uint8_t)str2int(nmea_lines[9], 2); month = (uint8_t)str2int(nmea_lines[9] + 2, 2); year = (uint16_t)str2int(nmea_lines[9] + 4, 2) + 2000; if (nmea_lines[2][0] == 'A') { //printf("RMC mode: Information\r\n"); } else if (nmea_lines[2][0] == 'V') { //printf("RMC mode: Warning\r\n"); } else { printf("RMC mode: invalid"); return; } if (nmea_lines[2][0] == 'A') { if (nmea_lines[2][0] == 'A') { /* 位置情報のデコード */ decimal_part[0] = modf(strtof(nmea_lines[3], NULL), &latitude); // 秒 latitude /= 100; decimal_part[1] = floor((modf(latitude, &latitude) * 100) + 0.5); // 分 latitude += (decimal_part[1] + decimal_part[0]) / 60; if (nmea_lines[4][0] == 'S') latitude = -latitude; decimal_part[0] = modf(strtof(nmea_lines[5], NULL), &longitude); // 秒 longitude /= 100; decimal_part[1] = floor((modf(longitude, &longitude) * 100) + 0.5); // 分 longitude += (decimal_part[1] + decimal_part[0]) / 60; if (nmea_lines[6][0] == 'W') longitude = -longitude; /* 移動速度のデコード */ speed = strtof(nmea_lines[7], NULL) * 1.852f; // knot to km/h move_direction = strtof(nmea_lines[8], NULL); } } else { longitude = 0; latitude = 0; } } else if (strcmp((nmea_lines[0]) + 3, "GGA") == 0) { double decimal_part[2] = {0}; /* 時刻のデコード */ hour = (uint8_t)str2int(nmea_lines[1], 2); min = (uint8_t)str2int(nmea_lines[1] + 2, 2); sec = (uint8_t)str2int(nmea_lines[1] + 4, 2); msec = (uint16_t)str2int(nmea_lines[1] + 7, 3); // GGAでは日付は不明 /* 位置情報のデコード */ decimal_part[0] = modf(strtof(nmea_lines[2], NULL), &latitude); // 秒 latitude /= 100; decimal_part[1] = floor((modf(latitude, &latitude) * 100) + 0.5); // 分 latitude += (decimal_part[1] + decimal_part[0]) / 60; if (nmea_lines[3][0] == 'S') latitude = -latitude; decimal_part[0] = modf(strtof(nmea_lines[4], NULL), &longitude); // 秒 longitude /= 100; decimal_part[1] = floor((modf(longitude, &longitude) * 100) + 0.5); // 分 longitude += (decimal_part[1] + decimal_part[0]) / 60; if (nmea_lines[5][0] == 'W') longitude = -longitude; // 移動速度と移動方向はGGAではわからない } } // valid = 0 // invalid = other uint8_t GPSDecoder::validate() { uint8_t checksum = 0, nmea_checksum, flag = 0; char checksum_str[3] = {0}; if (nmea_line[0] != '$') { // printf("invalid preamble %02X\n", nmea_line[0]); return 1; } for (int i = 1; i < strlen((char *)nmea_line); i++) { if (flag) { // 2byte checksum memcpy(checksum_str, (char *)nmea_line + i, 2); nmea_checksum = (uint8_t)strtol(checksum_str, NULL, 16); break; } else { if (nmea_line[i] == '*') { flag = 1; continue; } checksum = checksum ^ nmea_line[i]; } } if (checksum != nmea_checksum) { // printf("invalid checksum\n"); return 2; } // printf("validation passed\n"); return 0; } long GPSDecoder::str2int(char *src_ptr, uint8_t len) { char tmp[16] = {0}; memcpy(tmp, src_ptr, len); return strtol(tmp, NULL, 10); } int GPSDecoder::split(char *str, const char delim, char *token[], int max_item) { int cnt = 0; int len = strlen(str); token[cnt++] = str; for (int i = 0; i < len; i++) { if (str[i] == delim) { str[i] = '\0'; if (cnt == max_item) return cnt; token[cnt++] = str + i + 1; } } return cnt; } time_t GPSDecoder::get_unixtime() { struct tm t; t.tm_sec = sec; t.tm_min = min; t.tm_hour = hour; t.tm_mday = day; t.tm_mon = month - 1; t.tm_year = year - 1900; return mktime(&t); } void GPSDecoder::dump_info(char *buf) { sprintf(buf, "%d/%02d/%02d %02d:%02d:%02d.%03d (UTC)\r\n", year, month, day, hour, min, sec, msec); sprintf(buf, "%slatitude: %f\r\n", buf, latitude); sprintf(buf, "%slongitude: %f\r\n", buf, longitude); sprintf(buf, "%sspeed %f km/h (%f knot)\r\n", buf, speed, speed / 1.852f); sprintf(buf, "%sdirection %f degree\r\n", buf, move_direction); }