Diff: gps.cpp

Revision:

0:8132a51314ec

Child:

1:f949d6d3a7e7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gps.cpp	Wed Nov 29 05:37:22 2017 +0000
@@ -0,0 +1,140 @@
+#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);
+
+                move_direction = strtof(nmea_lines[8], NULL);
+            }
+        } else {
+            longitude = 0;
+            latitude = 0;
+        }
+    }
+}
+
+// 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] != '$') {
+        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) {
+        return 2;
+    }
+    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;
+}
+
+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 * 1.852f, speed);
+    sprintf(buf, "%sdirection %f degree\r\n", buf, move_direction);
+}