SerialGPS.cpp

00001 /**
00002  * Serial GPS module interface driver class (Version 0.0.1)
00003  * This interface driver supports NMEA-0183 serial based modules.
00004  *
00005  * Copyright (C) 2010 Shinichiro Nakamura (CuBeatSystems)
00006  * http://shinta.main.jp/
00007  */
00008 #include "SerialGPS.h"
00009 
00010 /**
00011  * Create.
00012  *
00013  * @param tx A pin of transmit.
00014  * @param rx A pin of receive.
00015  * @param baud Baud rate. (Default = 9600)
00016  */
00017 SerialGPS::SerialGPS(PinName tx, PinName rx, int baud) : ser(tx, rx), cbfuncs(NULL) {
00018     ser.baud(baud);
00019     ser.setTimeout(50);
00020 }
00021 
00022 /**
00023  * Destroy.
00024  */
00025 SerialGPS::~SerialGPS() {
00026 }
00027 
00028 /**
00029  * Processing.
00030  */
00031 bool SerialGPS::processing() {
00032     /*
00033      * Read from a serial buffer.
00034      */
00035     static const int DATABUFSIZ = 128;
00036     char src[DATABUFSIZ];
00037     int cnt = 0;
00038     do {
00039         cnt = 0;
00040         bool done = false;
00041         do {
00042             int c = ser.getc();
00043             if (c < 0) {
00044                 return false;
00045             }
00046             if ((c == '\r') || (c == '\n')) {
00047                 done = true;
00048                 c = '\0';
00049             }
00050             src[cnt++] = c & 0xff;
00051         } while (!done);
00052     } while (cnt <= 1);
00053     
00054     /*
00055      * Return if the callback function does not exists.
00056      */
00057     if (cbfuncs == NULL) {
00058         return true;
00059     }
00060 
00061     /*
00062      * Call a function for logging data.
00063      */
00064     if (cbfuncs->cbfunc_log != NULL) {
00065         cbfuncs->cbfunc_log(src);
00066     }
00067 
00068     /*
00069      * Check a check sum for the data. The data format is '$[DATA]*CS'.
00070      */
00071     const size_t srclen = strlen(src);
00072     if ((src[0] == '$') && (src[srclen - 3] == '*')) {
00073         int cs_src;
00074         if (sscanf(src + srclen - 2, "%X", &cs_src) != 1) {
00075             printf("Invalid check sum data found.(%s)\n", src);
00076             return false;
00077         }
00078         uint8_t cs_cal = calcCheckSum(src + 1, srclen - 4);
00079         if ((uint8_t)cs_src != cs_cal) {
00080             printf("Illegal data found.(%s)\n", src);
00081             return false;
00082         }
00083     } else {
00084         printf("Invalid data format found.(%s)\n", src);
00085         return false;
00086     }
00087 
00088     /*
00089      * Parse a data.
00090      */
00091     int res = 0;
00092     char *p = src;
00093     char des[DATABUFSIZ];
00094     if ((p = parse(p, des, sizeof(des), ",*")) != NULL) {
00095         if (strcmp(des, "$GPGGA") == 0) {
00096             res = parseAndCallbackGGA(src, cbfuncs);
00097         } else if (strcmp(des, "$GPGSA") == 0) {
00098             res = parseAndCallbackGSA(src, cbfuncs);
00099         } else if (strcmp(des, "$GPRMC") == 0) {
00100             res = parseAndCallbackRMC(src, cbfuncs);
00101         } else if (strcmp(des, "$GPGSV") == 0) {
00102             res = parseAndCallbackGSV(src, cbfuncs);
00103         } else {
00104             res = parseAndCallbackUnknown(src, cbfuncs);
00105         }
00106     }
00107     return (res == 0) ? true : false;
00108 }
00109 
00110 /**
00111  * Attach a callback function.
00112  *
00113  * @param cbfuncs A pointer to a callback function structure.
00114  */
00115 void SerialGPS::attach(gps_callback_t *cbfuncs) {
00116     SerialGPS::cbfuncs = cbfuncs;
00117 }
00118 
00119 /**
00120  * Detach a callback function.
00121  */
00122 void SerialGPS::detach(void) {
00123     SerialGPS::cbfuncs = NULL;
00124 }
00125 
00126 /**
00127  * Parse a text string.
00128  */
00129 char * SerialGPS::parse(char *src, char *des, size_t dessiz, char *delim) {
00130     for (int i = 0; i < dessiz; i++, src++, des++) {
00131         if ((*src == '\0') || (*src == '\r') || (*src == '\n')) {
00132             *des = '\0';
00133             return NULL;
00134         }
00135         if (exists(*src, delim)) {
00136             *des = '\0';
00137             return ++src;
00138         }
00139         *des = *src;
00140     }
00141     des = '\0';
00142     return NULL;
00143 }
00144 
00145 int SerialGPS::parseAndCallbackGGA(char *src, gps_callback_t *cbfuncs) {
00146 
00147     if (cbfuncs->cbfunc_gga == NULL) {
00148         return -1;
00149     }
00150 
00151     char plist[PARAM_ARRAYSIZE][PARAM_TXTMAXLEN];
00152     char *p = src;
00153     int cnt = 0;
00154     while ((p = parse(p, plist[cnt], PARAM_TXTMAXLEN, ",*")) != NULL) {
00155         cnt++;
00156     }
00157 
00158     if (cnt == 15) {
00159         gps_gga_t data;
00160         data.hour = (plist[1][0] - '0') * 10 + (plist[1][1] - '0') * 1;
00161         data.min = (plist[1][2] - '0') * 10 + (plist[1][3] - '0') * 1;
00162         data.sec = (plist[1][4] - '0') * 10 + (plist[1][5] - '0') * 1;
00163 
00164         if (sscanf(plist[2], "%lf", &data.latitude) != 1) {
00165             return -2;
00166         }
00167         if (sscanf(plist[3], "%c", &data.ns) != 1) {
00168             return -3;
00169         }
00170         const char ns = data.ns;
00171         if ((ns != 'N') && (ns != 'S')) {
00172             return -4;
00173         }
00174         if (sscanf(plist[4], "%lf", &data.longitude) != 1) {
00175             return -5;
00176         }
00177         if (sscanf(plist[5], "%c", &data.ew) != 1) {
00178             return -6;
00179         }
00180         const char ew = data.ew;
00181         if ((ew != 'E') && (ew != 'W')) {
00182             return -7;
00183         }
00184 
00185         data.position_fix = atoi(plist[6]);
00186         data.satellites_used = atoi(plist[7]);
00187 
00188         if (sscanf(plist[8], "%lf", &data.hdop) != 1) {
00189             return -8;
00190         }
00191 
00192         data.altitude = atoi(plist[9]);
00193         if (strcmp(plist[10], "M") != 0) {
00194             return -9;
00195         }
00196 
00197         data.altitude = atoi(plist[11]);
00198         if (sscanf(plist[12], "%c", &data.altitude_unit) != 1) {
00199             return -10;
00200         }
00201 
00202         cbfuncs->cbfunc_gga(&data);
00203         return 0;
00204     }
00205     return -11;
00206 }
00207 
00208 int SerialGPS::parseAndCallbackGSA(char *src, gps_callback_t *cbfuncs) {
00209 
00210     if (cbfuncs->cbfunc_gsa == NULL) {
00211         return -1;
00212     }
00213 
00214     char plist[PARAM_ARRAYSIZE][PARAM_TXTMAXLEN];
00215     char *p = src;
00216     int cnt = 0;
00217     while ((p = parse(p, plist[cnt], PARAM_TXTMAXLEN, ",*")) != NULL) {
00218         cnt++;
00219     }
00220 
00221     if (cnt == 18) {
00222         gps_gsa_t data;
00223         data.selmode = plist[1][0];
00224         if ((data.selmode != 'A') && (data.selmode != 'M')) {
00225             return -2;
00226         }
00227 
00228         data.fix = atoi(plist[2]);
00229         if ((data.fix != 1) && (data.fix != 2) && (data.fix != 3)) {
00230             return -3;
00231         }
00232 
00233         cbfuncs->cbfunc_gsa(&data);
00234         return 0;
00235     }
00236     return -4;
00237 }
00238 
00239 int SerialGPS::parseAndCallbackRMC(char *src, gps_callback_t *cbfuncs) {
00240 
00241     if (cbfuncs->cbfunc_rmc == NULL) {
00242         return -1;
00243     }
00244 
00245     char plist[PARAM_ARRAYSIZE][PARAM_TXTMAXLEN];
00246     char *p = src;
00247     int cnt = 0;
00248     while ((p = parse(p, plist[cnt], PARAM_TXTMAXLEN, ",*")) != NULL) {
00249         cnt++;
00250     }
00251 
00252     if (cnt == 13) {
00253         gps_rmc_t data;
00254         data.hour = (plist[1][0] - '0') * 10 + (plist[1][1] - '0') * 1;
00255         data.min = (plist[1][2] - '0') * 10 + (plist[1][3] - '0') * 1;
00256         data.sec = (plist[1][4] - '0') * 10 + (plist[1][5] - '0') * 1;
00257 
00258         data.status = plist[2][0];
00259 
00260         if (sscanf(plist[3], "%lf", &data.nl) != 1) {
00261             return -2;
00262         }
00263         if (strcmp(plist[4], "N") != 0) {
00264             return -3;
00265         }
00266 
00267         if (sscanf(plist[5], "%lf", &data.el) != 1) {
00268             return -4;
00269         }
00270         if (strcmp(plist[6], "E") != 0) {
00271             return -5;
00272         }
00273 
00274         cbfuncs->cbfunc_rmc(&data);
00275         return 0;
00276     }
00277     return -4;
00278 }
00279 
00280 int SerialGPS::parseAndCallbackGSV(char *src, gps_callback_t *cbfuncs) {
00281 
00282     if (cbfuncs->cbfunc_gsv == NULL) {
00283         return -1;
00284     }
00285 
00286     char plist[PARAM_ARRAYSIZE][PARAM_TXTMAXLEN];
00287     char *p = src;
00288     int cnt = 0;
00289     while ((p = parse(p, plist[cnt], PARAM_TXTMAXLEN, ",*")) != NULL) {
00290         cnt++;
00291     }
00292 
00293     if (cnt == 20) {
00294         gps_gsv_t data;
00295         data.msgcnt = atoi(plist[1]);
00296 
00297         data.msgnum = atoi(plist[2]);
00298 
00299         data.satcnt = atoi(plist[3]);
00300 
00301         static const int SATINFOFS = 4;
00302         for (int i = 0; i < 4; i++) {
00303             data.satellite[i].num = atoi(plist[SATINFOFS + 0 * (i * 4)]);
00304             data.satellite[i].elevation = atoi(plist[SATINFOFS + 1 * (i * 4)]);
00305             data.satellite[i].azimuth = atoi(plist[SATINFOFS + 2 * (i * 4)]);
00306             data.satellite[i].snr = atoi(plist[SATINFOFS + 3 *(i * 4)]);
00307         }
00308 
00309         int chksum;
00310         if (sscanf(plist[20], "%x", &chksum) != 1) {
00311             return -2;
00312         }
00313 
00314         cbfuncs->cbfunc_gsv(&data);
00315         return 0;
00316     }
00317     return -3;
00318 }
00319 
00320 int SerialGPS::parseAndCallbackUnknown(char *src, gps_callback_t *cbfuncs) {
00321     return 0;
00322 }
00323 
00324 uint8_t SerialGPS::calcCheckSum(char *buf, size_t siz) {
00325     uint8_t cs = 0;
00326     for (int i = 0; i < siz; i++) {
00327         cs ^= buf[i];
00328     }
00329     return cs;
00330 }