polytech umpc project
Fork of SerialGPS by
SerialGPS.cpp
- Committer:
- raphaelbresson
- Date:
- 2017-02-08
- Revision:
- 2:bedf4d6d1755
- Parent:
- 1:a5b887e09aa4
File content as of revision 2:bedf4d6d1755:
/** * 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(500); } /** * 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; }