Diff: SerialGPS.cpp

Revision:

0:3d0c5d7e5d0a

Child:

1:a5b887e09aa4

diff -r 000000000000 -r 3d0c5d7e5d0a SerialGPS.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialGPS.cpp	Mon Sep 20 06:03:38 2010 +0000
@@ -0,0 +1,330 @@
+/**
+ * Serial GPS module interface driver class (Version 0.0.1)
+ * This interface driver supports NMEA-0183 serial based modules.
+ *
+ * Copyright (C) 2010 Shinichiro Nakamura (CuBeatSystems)
+ * http://shinta.main.jp/
+ */
+#include "SerialGPS.h"
+
+/**
+ * Create.
+ *
+ * @param tx A pin of transmit.
+ * @param rx A pin of receive.
+ * @param baud Baud rate. (Default = 9600)
+ */
+SerialGPS::SerialGPS(PinName tx, PinName rx, int baud) : ser(tx, rx), cbfuncs(NULL) {
+    ser.baud(baud);
+    ser.setTimeout(50);
+}
+
+/**
+ * Destroy.
+ */
+SerialGPS::~SerialGPS() {
+}
+
+/**
+ * Processing.
+ */
+bool SerialGPS::processing() {
+    /*
+     * Read from a serial buffer.
+     */
+    static const int DATABUFSIZ = 128;
+    char src[DATABUFSIZ];
+    int cnt = 0;
+    do {
+        cnt = 0;
+        bool done = false;
+        do {
+            int c = ser.getc();
+            if (c < 0) {
+                return false;
+            }
+            if ((c == '\r') || (c == '\n')) {
+                done = true;
+                c = '\0';
+            }
+            src[cnt++] = c & 0xff;
+        } while (!done);
+    } while (cnt <= 1);
+
+    /*
+     * Return if the callback function does not exists.
+     */
+    if (cbfuncs == NULL) {
+        return true;
+    }
+
+    /*
+     * Call a function for logging data.
+     */
+    if (cbfuncs->cbfunc_log != NULL) {
+        cbfuncs->cbfunc_log(src);
+    }
+
+    /*
+     * Check a check sum for the data. The data format is '$[DATA]*CS'.
+     */
+    const size_t srclen = strlen(src);
+    if ((src[0] == '$') && (src[srclen - 3] == '*')) {
+        int cs_src;
+        if (sscanf(src + srclen - 2, "%X", &cs_src) != 1) {
+            printf("Invalid check sum data found.(%s)\n", src);
+            return false;
+        }
+        uint8_t cs_cal = calcCheckSum(src + 1, srclen - 4);
+        if ((uint8_t)cs_src != cs_cal) {
+            printf("Illegal data found.(%s)\n", src);
+            return false;
+        }
+    } else {
+        printf("Invalid data format found.(%s)\n", src);
+        return false;
+    }
+
+    /*
+     * Parse a data.
+     */
+    int res = 0;
+    char *p = src;
+    char des[DATABUFSIZ];
+    if ((p = parse(p, des, sizeof(des), ",*")) != NULL) {
+        if (strcmp(des, "$GPGGA") == 0) {
+            res = parseAndCallbackGGA(src, cbfuncs);
+        } else if (strcmp(des, "$GPGSA") == 0) {
+            res = parseAndCallbackGSA(src, cbfuncs);
+        } else if (strcmp(des, "$GPRMC") == 0) {
+            res = parseAndCallbackRMC(src, cbfuncs);
+        } else if (strcmp(des, "$GPGSV") == 0) {
+            res = parseAndCallbackGSV(src, cbfuncs);
+        } else {
+            res = parseAndCallbackUnknown(src, cbfuncs);
+        }
+    }
+    return (res == 0) ? true : false;
+}
+
+/**
+ * Attach a callback function.
+ *
+ * @param cbfuncs A pointer to a callback function structure.
+ */
+void SerialGPS::attach(gps_callback_t *cbfuncs) {
+    SerialGPS::cbfuncs = cbfuncs;
+}
+
+/**
+ * Detach a callback function.
+ */
+void SerialGPS::detach(void) {
+    SerialGPS::cbfuncs = NULL;
+}
+
+/**
+ * Parse a text string.
+ */
+char * SerialGPS::parse(char *src, char *des, size_t dessiz, char *delim) {
+    for (int i = 0; i < dessiz; i++, src++, des++) {
+        if ((*src == '\0') || (*src == '\r') || (*src == '\n')) {
+            *des = '\0';
+            return NULL;
+        }
+        if (exists(*src, delim)) {
+            *des = '\0';
+            return ++src;
+        }
+        *des = *src;
+    }
+    des = '\0';
+    return NULL;
+}
+
+int SerialGPS::parseAndCallbackGGA(char *src, gps_callback_t *cbfuncs) {
+
+    if (cbfuncs->cbfunc_gga == NULL) {
+        return -1;
+    }
+
+    char plist[PARAM_ARRAYSIZE][PARAM_TXTMAXLEN];
+    char *p = src;
+    int cnt = 0;
+    while ((p = parse(p, plist[cnt], PARAM_TXTMAXLEN, ",*")) != NULL) {
+        cnt++;
+    }
+
+    if (cnt == 15) {
+        gps_gga_t data;
+        data.hour = (plist[1][0] - '0') * 10 + (plist[1][1] - '0') * 1;
+        data.min = (plist[1][2] - '0') * 10 + (plist[1][3] - '0') * 1;
+        data.sec = (plist[1][4] - '0') * 10 + (plist[1][5] - '0') * 1;
+
+        if (sscanf(plist[2], "%lf", &data.latitude) != 1) {
+            return -2;
+        }
+        if (sscanf(plist[3], "%c", &data.ns) != 1) {
+            return -3;
+        }
+        const char ns = data.ns;
+        if ((ns != 'N') && (ns != 'S')) {
+            return -4;
+        }
+        if (sscanf(plist[4], "%lf", &data.longitude) != 1) {
+            return -5;
+        }
+        if (sscanf(plist[5], "%c", &data.ew) != 1) {
+            return -6;
+        }
+        const char ew = data.ew;
+        if ((ew != 'E') && (ew != 'W')) {
+            return -7;
+        }
+
+        data.position_fix = atoi(plist[6]);
+        data.satellites_used = atoi(plist[7]);
+
+        if (sscanf(plist[8], "%lf", &data.hdop) != 1) {
+            return -8;
+        }
+
+        data.altitude = atoi(plist[9]);
+        if (strcmp(plist[10], "M") != 0) {
+            return -9;
+        }
+
+        data.altitude = atoi(plist[11]);
+        if (sscanf(plist[12], "%c", &data.altitude_unit) != 1) {
+            return -10;
+        }
+
+        cbfuncs->cbfunc_gga(&data);
+        return 0;
+    }
+    return -11;
+}
+
+int SerialGPS::parseAndCallbackGSA(char *src, gps_callback_t *cbfuncs) {
+
+    if (cbfuncs->cbfunc_gsa == NULL) {
+        return -1;
+    }
+
+    char plist[PARAM_ARRAYSIZE][PARAM_TXTMAXLEN];
+    char *p = src;
+    int cnt = 0;
+    while ((p = parse(p, plist[cnt], PARAM_TXTMAXLEN, ",*")) != NULL) {
+        cnt++;
+    }
+
+    if (cnt == 18) {
+        gps_gsa_t data;
+        data.selmode = plist[1][0];
+        if ((data.selmode != 'A') && (data.selmode != 'M')) {
+            return -2;
+        }
+
+        data.fix = atoi(plist[2]);
+        if ((data.fix != 1) && (data.fix != 2) && (data.fix != 3)) {
+            return -3;
+        }
+
+        cbfuncs->cbfunc_gsa(&data);
+        return 0;
+    }
+    return -4;
+}
+
+int SerialGPS::parseAndCallbackRMC(char *src, gps_callback_t *cbfuncs) {
+
+    if (cbfuncs->cbfunc_rmc == NULL) {
+        return -1;
+    }
+
+    char plist[PARAM_ARRAYSIZE][PARAM_TXTMAXLEN];
+    char *p = src;
+    int cnt = 0;
+    while ((p = parse(p, plist[cnt], PARAM_TXTMAXLEN, ",*")) != NULL) {
+        cnt++;
+    }
+
+    if (cnt == 13) {
+        gps_rmc_t data;
+        data.hour = (plist[1][0] - '0') * 10 + (plist[1][1] - '0') * 1;
+        data.min = (plist[1][2] - '0') * 10 + (plist[1][3] - '0') * 1;
+        data.sec = (plist[1][4] - '0') * 10 + (plist[1][5] - '0') * 1;
+
+        data.status = plist[2][0];
+
+        if (sscanf(plist[3], "%lf", &data.nl) != 1) {
+            return -2;
+        }
+        if (strcmp(plist[4], "N") != 0) {
+            return -3;
+        }
+
+        if (sscanf(plist[5], "%lf", &data.el) != 1) {
+            return -4;
+        }
+        if (strcmp(plist[6], "E") != 0) {
+            return -5;
+        }
+
+        cbfuncs->cbfunc_rmc(&data);
+        return 0;
+    }
+    return -4;
+}
+
+int SerialGPS::parseAndCallbackGSV(char *src, gps_callback_t *cbfuncs) {
+
+    if (cbfuncs->cbfunc_gsv == NULL) {
+        return -1;
+    }
+
+    char plist[PARAM_ARRAYSIZE][PARAM_TXTMAXLEN];
+    char *p = src;
+    int cnt = 0;
+    while ((p = parse(p, plist[cnt], PARAM_TXTMAXLEN, ",*")) != NULL) {
+        cnt++;
+    }
+
+    if (cnt == 20) {
+        gps_gsv_t data;
+        data.msgcnt = atoi(plist[1]);
+
+        data.msgnum = atoi(plist[2]);
+
+        data.satcnt = atoi(plist[3]);
+
+        static const int SATINFOFS = 4;
+        for (int i = 0; i < 4; i++) {
+            data.satellite[i].num = atoi(plist[SATINFOFS + 0 * (i * 4)]);
+            data.satellite[i].elevation = atoi(plist[SATINFOFS + 1 * (i * 4)]);
+            data.satellite[i].azimuth = atoi(plist[SATINFOFS + 2 * (i * 4)]);
+            data.satellite[i].snr = atoi(plist[SATINFOFS + 3 *(i * 4)]);
+        }
+
+        int chksum;
+        if (sscanf(plist[20], "%x", &chksum) != 1) {
+            return -2;
+        }
+
+        cbfuncs->cbfunc_gsv(&data);
+        return 0;
+    }
+    return -3;
+}
+
+int SerialGPS::parseAndCallbackUnknown(char *src, gps_callback_t *cbfuncs) {
+    return 0;
+}
+
+uint8_t SerialGPS::calcCheckSum(char *buf, size_t siz) {
+    uint8_t cs = 0;
+    for (int i = 0; i < siz; i++) {
+        cs ^= buf[i];
+    }
+    return cs;
+}