Shinichiro Nakamura / SerialGPS

SerialGPS.

Dependents:   SerialGPS_TestProgram Nucleo_SerialGPS_to_PC Nucleo_SerialGPS_to_PC SensorInterface ... more

SerialGPS.cpp@1:a5b887e09aa4, 2010-10-12 (annotated)

Committer:
shintamainjp
Date:
Tue Oct 12 22:22:28 2010 +0000
Revision:
1:a5b887e09aa4
Parent:
0:3d0c5d7e5d0a 
First version.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shintamainjp 0:3d0c5d7e5d0a 1 /**
shintamainjp 0:3d0c5d7e5d0a 2 * Serial GPS module interface driver class (Version 0.0.1)
shintamainjp 0:3d0c5d7e5d0a 3 * This interface driver supports NMEA-0183 serial based modules.
shintamainjp 0:3d0c5d7e5d0a 4 *
shintamainjp 0:3d0c5d7e5d0a 5 * Copyright (C) 2010 Shinichiro Nakamura (CuBeatSystems)
shintamainjp 0:3d0c5d7e5d0a 6 * http://shinta.main.jp/
shintamainjp 0:3d0c5d7e5d0a 7 */
shintamainjp 0:3d0c5d7e5d0a 8 #include "SerialGPS.h"
shintamainjp 0:3d0c5d7e5d0a 9
shintamainjp 0:3d0c5d7e5d0a 10 /**
shintamainjp 0:3d0c5d7e5d0a 11 * Create.
shintamainjp 0:3d0c5d7e5d0a 12 *
shintamainjp 0:3d0c5d7e5d0a 13 * @param tx A pin of transmit.
shintamainjp 0:3d0c5d7e5d0a 14 * @param rx A pin of receive.
shintamainjp 0:3d0c5d7e5d0a 15 * @param baud Baud rate. (Default = 9600)
shintamainjp 0:3d0c5d7e5d0a 16 */
shintamainjp 0:3d0c5d7e5d0a 17 SerialGPS::SerialGPS(PinName tx, PinName rx, int baud) : ser(tx, rx), cbfuncs(NULL) {
shintamainjp 0:3d0c5d7e5d0a 18 ser.baud(baud);
shintamainjp 0:3d0c5d7e5d0a 19 ser.setTimeout(50);
shintamainjp 0:3d0c5d7e5d0a 20 }
shintamainjp 0:3d0c5d7e5d0a 21
shintamainjp 0:3d0c5d7e5d0a 22 /**
shintamainjp 0:3d0c5d7e5d0a 23 * Destroy.
shintamainjp 0:3d0c5d7e5d0a 24 */
shintamainjp 0:3d0c5d7e5d0a 25 SerialGPS::~SerialGPS() {
shintamainjp 0:3d0c5d7e5d0a 26 }
shintamainjp 0:3d0c5d7e5d0a 27
shintamainjp 0:3d0c5d7e5d0a 28 /**
shintamainjp 0:3d0c5d7e5d0a 29 * Processing.
shintamainjp 0:3d0c5d7e5d0a 30 */
shintamainjp 0:3d0c5d7e5d0a 31 bool SerialGPS::processing() {
shintamainjp 0:3d0c5d7e5d0a 32 /*
shintamainjp 0:3d0c5d7e5d0a 33 * Read from a serial buffer.
shintamainjp 0:3d0c5d7e5d0a 34 */
shintamainjp 0:3d0c5d7e5d0a 35 static const int DATABUFSIZ = 128;
shintamainjp 0:3d0c5d7e5d0a 36 char src[DATABUFSIZ];
shintamainjp 0:3d0c5d7e5d0a 37 int cnt = 0;
shintamainjp 0:3d0c5d7e5d0a 38 do {
shintamainjp 0:3d0c5d7e5d0a 39 cnt = 0;
shintamainjp 0:3d0c5d7e5d0a 40 bool done = false;
shintamainjp 0:3d0c5d7e5d0a 41 do {
shintamainjp 0:3d0c5d7e5d0a 42 int c = ser.getc();
shintamainjp 0:3d0c5d7e5d0a 43 if (c < 0) {
shintamainjp 0:3d0c5d7e5d0a 44 return false;
shintamainjp 0:3d0c5d7e5d0a 45 }
shintamainjp 0:3d0c5d7e5d0a 46 if ((c == '\r') || (c == '\n')) {
shintamainjp 0:3d0c5d7e5d0a 47 done = true;
shintamainjp 0:3d0c5d7e5d0a 48 c = '\0';
shintamainjp 0:3d0c5d7e5d0a 49 }
shintamainjp 0:3d0c5d7e5d0a 50 src[cnt++] = c & 0xff;
shintamainjp 0:3d0c5d7e5d0a 51 } while (!done);
shintamainjp 0:3d0c5d7e5d0a 52 } while (cnt <= 1);
shintamainjp 1:a5b887e09aa4 53
shintamainjp 0:3d0c5d7e5d0a 54 /*
shintamainjp 0:3d0c5d7e5d0a 55 * Return if the callback function does not exists.
shintamainjp 0:3d0c5d7e5d0a 56 */
shintamainjp 0:3d0c5d7e5d0a 57 if (cbfuncs == NULL) {
shintamainjp 0:3d0c5d7e5d0a 58 return true;
shintamainjp 0:3d0c5d7e5d0a 59 }
shintamainjp 0:3d0c5d7e5d0a 60
shintamainjp 0:3d0c5d7e5d0a 61 /*
shintamainjp 0:3d0c5d7e5d0a 62 * Call a function for logging data.
shintamainjp 0:3d0c5d7e5d0a 63 */
shintamainjp 0:3d0c5d7e5d0a 64 if (cbfuncs->cbfunc_log != NULL) {
shintamainjp 0:3d0c5d7e5d0a 65 cbfuncs->cbfunc_log(src);
shintamainjp 0:3d0c5d7e5d0a 66 }
shintamainjp 0:3d0c5d7e5d0a 67
shintamainjp 0:3d0c5d7e5d0a 68 /*
shintamainjp 0:3d0c5d7e5d0a 69 * Check a check sum for the data. The data format is '$[DATA]*CS'.
shintamainjp 0:3d0c5d7e5d0a 70 */
shintamainjp 0:3d0c5d7e5d0a 71 const size_t srclen = strlen(src);
shintamainjp 0:3d0c5d7e5d0a 72 if ((src[0] == '$') && (src[srclen - 3] == '*')) {
shintamainjp 0:3d0c5d7e5d0a 73 int cs_src;
shintamainjp 0:3d0c5d7e5d0a 74 if (sscanf(src + srclen - 2, "%X", &cs_src) != 1) {
shintamainjp 0:3d0c5d7e5d0a 75 printf("Invalid check sum data found.(%s)\n", src);
shintamainjp 0:3d0c5d7e5d0a 76 return false;
shintamainjp 0:3d0c5d7e5d0a 77 }
shintamainjp 0:3d0c5d7e5d0a 78 uint8_t cs_cal = calcCheckSum(src + 1, srclen - 4);
shintamainjp 0:3d0c5d7e5d0a 79 if ((uint8_t)cs_src != cs_cal) {
shintamainjp 0:3d0c5d7e5d0a 80 printf("Illegal data found.(%s)\n", src);
shintamainjp 0:3d0c5d7e5d0a 81 return false;
shintamainjp 0:3d0c5d7e5d0a 82 }
shintamainjp 0:3d0c5d7e5d0a 83 } else {
shintamainjp 0:3d0c5d7e5d0a 84 printf("Invalid data format found.(%s)\n", src);
shintamainjp 0:3d0c5d7e5d0a 85 return false;
shintamainjp 0:3d0c5d7e5d0a 86 }
shintamainjp 0:3d0c5d7e5d0a 87
shintamainjp 0:3d0c5d7e5d0a 88 /*
shintamainjp 0:3d0c5d7e5d0a 89 * Parse a data.
shintamainjp 0:3d0c5d7e5d0a 90 */
shintamainjp 0:3d0c5d7e5d0a 91 int res = 0;
shintamainjp 0:3d0c5d7e5d0a 92 char *p = src;
shintamainjp 0:3d0c5d7e5d0a 93 char des[DATABUFSIZ];
shintamainjp 0:3d0c5d7e5d0a 94 if ((p = parse(p, des, sizeof(des), ",*")) != NULL) {
shintamainjp 0:3d0c5d7e5d0a 95 if (strcmp(des, "$GPGGA") == 0) {
shintamainjp 0:3d0c5d7e5d0a 96 res = parseAndCallbackGGA(src, cbfuncs);
shintamainjp 0:3d0c5d7e5d0a 97 } else if (strcmp(des, "$GPGSA") == 0) {
shintamainjp 0:3d0c5d7e5d0a 98 res = parseAndCallbackGSA(src, cbfuncs);
shintamainjp 0:3d0c5d7e5d0a 99 } else if (strcmp(des, "$GPRMC") == 0) {
shintamainjp 0:3d0c5d7e5d0a 100 res = parseAndCallbackRMC(src, cbfuncs);
shintamainjp 0:3d0c5d7e5d0a 101 } else if (strcmp(des, "$GPGSV") == 0) {
shintamainjp 0:3d0c5d7e5d0a 102 res = parseAndCallbackGSV(src, cbfuncs);
shintamainjp 0:3d0c5d7e5d0a 103 } else {
shintamainjp 0:3d0c5d7e5d0a 104 res = parseAndCallbackUnknown(src, cbfuncs);
shintamainjp 0:3d0c5d7e5d0a 105 }
shintamainjp 0:3d0c5d7e5d0a 106 }
shintamainjp 0:3d0c5d7e5d0a 107 return (res == 0) ? true : false;
shintamainjp 0:3d0c5d7e5d0a 108 }
shintamainjp 0:3d0c5d7e5d0a 109
shintamainjp 0:3d0c5d7e5d0a 110 /**
shintamainjp 0:3d0c5d7e5d0a 111 * Attach a callback function.
shintamainjp 0:3d0c5d7e5d0a 112 *
shintamainjp 0:3d0c5d7e5d0a 113 * @param cbfuncs A pointer to a callback function structure.
shintamainjp 0:3d0c5d7e5d0a 114 */
shintamainjp 0:3d0c5d7e5d0a 115 void SerialGPS::attach(gps_callback_t *cbfuncs) {
shintamainjp 0:3d0c5d7e5d0a 116 SerialGPS::cbfuncs = cbfuncs;
shintamainjp 0:3d0c5d7e5d0a 117 }
shintamainjp 0:3d0c5d7e5d0a 118
shintamainjp 0:3d0c5d7e5d0a 119 /**
shintamainjp 0:3d0c5d7e5d0a 120 * Detach a callback function.
shintamainjp 0:3d0c5d7e5d0a 121 */
shintamainjp 0:3d0c5d7e5d0a 122 void SerialGPS::detach(void) {
shintamainjp 0:3d0c5d7e5d0a 123 SerialGPS::cbfuncs = NULL;
shintamainjp 0:3d0c5d7e5d0a 124 }
shintamainjp 0:3d0c5d7e5d0a 125
shintamainjp 0:3d0c5d7e5d0a 126 /**
shintamainjp 0:3d0c5d7e5d0a 127 * Parse a text string.
shintamainjp 0:3d0c5d7e5d0a 128 */
shintamainjp 0:3d0c5d7e5d0a 129 char * SerialGPS::parse(char *src, char *des, size_t dessiz, char *delim) {
shintamainjp 0:3d0c5d7e5d0a 130 for (int i = 0; i < dessiz; i++, src++, des++) {
shintamainjp 0:3d0c5d7e5d0a 131 if ((*src == '\0') || (*src == '\r') || (*src == '\n')) {
shintamainjp 0:3d0c5d7e5d0a 132 *des = '\0';
shintamainjp 0:3d0c5d7e5d0a 133 return NULL;
shintamainjp 0:3d0c5d7e5d0a 134 }
shintamainjp 0:3d0c5d7e5d0a 135 if (exists(*src, delim)) {
shintamainjp 0:3d0c5d7e5d0a 136 *des = '\0';
shintamainjp 0:3d0c5d7e5d0a 137 return ++src;
shintamainjp 0:3d0c5d7e5d0a 138 }
shintamainjp 0:3d0c5d7e5d0a 139 *des = *src;
shintamainjp 0:3d0c5d7e5d0a 140 }
shintamainjp 0:3d0c5d7e5d0a 141 des = '\0';
shintamainjp 0:3d0c5d7e5d0a 142 return NULL;
shintamainjp 0:3d0c5d7e5d0a 143 }
shintamainjp 0:3d0c5d7e5d0a 144
shintamainjp 0:3d0c5d7e5d0a 145 int SerialGPS::parseAndCallbackGGA(char *src, gps_callback_t *cbfuncs) {
shintamainjp 0:3d0c5d7e5d0a 146
shintamainjp 0:3d0c5d7e5d0a 147 if (cbfuncs->cbfunc_gga == NULL) {
shintamainjp 0:3d0c5d7e5d0a 148 return -1;
shintamainjp 0:3d0c5d7e5d0a 149 }
shintamainjp 0:3d0c5d7e5d0a 150
shintamainjp 0:3d0c5d7e5d0a 151 char plist[PARAM_ARRAYSIZE][PARAM_TXTMAXLEN];
shintamainjp 0:3d0c5d7e5d0a 152 char *p = src;
shintamainjp 0:3d0c5d7e5d0a 153 int cnt = 0;
shintamainjp 0:3d0c5d7e5d0a 154 while ((p = parse(p, plist[cnt], PARAM_TXTMAXLEN, ",*")) != NULL) {
shintamainjp 0:3d0c5d7e5d0a 155 cnt++;
shintamainjp 0:3d0c5d7e5d0a 156 }
shintamainjp 0:3d0c5d7e5d0a 157
shintamainjp 0:3d0c5d7e5d0a 158 if (cnt == 15) {
shintamainjp 0:3d0c5d7e5d0a 159 gps_gga_t data;
shintamainjp 0:3d0c5d7e5d0a 160 data.hour = (plist[1][0] - '0') * 10 + (plist[1][1] - '0') * 1;
shintamainjp 0:3d0c5d7e5d0a 161 data.min = (plist[1][2] - '0') * 10 + (plist[1][3] - '0') * 1;
shintamainjp 0:3d0c5d7e5d0a 162 data.sec = (plist[1][4] - '0') * 10 + (plist[1][5] - '0') * 1;
shintamainjp 0:3d0c5d7e5d0a 163
shintamainjp 0:3d0c5d7e5d0a 164 if (sscanf(plist[2], "%lf", &data.latitude) != 1) {
shintamainjp 0:3d0c5d7e5d0a 165 return -2;
shintamainjp 0:3d0c5d7e5d0a 166 }
shintamainjp 0:3d0c5d7e5d0a 167 if (sscanf(plist[3], "%c", &data.ns) != 1) {
shintamainjp 0:3d0c5d7e5d0a 168 return -3;
shintamainjp 0:3d0c5d7e5d0a 169 }
shintamainjp 0:3d0c5d7e5d0a 170 const char ns = data.ns;
shintamainjp 0:3d0c5d7e5d0a 171 if ((ns != 'N') && (ns != 'S')) {
shintamainjp 0:3d0c5d7e5d0a 172 return -4;
shintamainjp 0:3d0c5d7e5d0a 173 }
shintamainjp 0:3d0c5d7e5d0a 174 if (sscanf(plist[4], "%lf", &data.longitude) != 1) {
shintamainjp 0:3d0c5d7e5d0a 175 return -5;
shintamainjp 0:3d0c5d7e5d0a 176 }
shintamainjp 0:3d0c5d7e5d0a 177 if (sscanf(plist[5], "%c", &data.ew) != 1) {
shintamainjp 0:3d0c5d7e5d0a 178 return -6;
shintamainjp 0:3d0c5d7e5d0a 179 }
shintamainjp 0:3d0c5d7e5d0a 180 const char ew = data.ew;
shintamainjp 0:3d0c5d7e5d0a 181 if ((ew != 'E') && (ew != 'W')) {
shintamainjp 0:3d0c5d7e5d0a 182 return -7;
shintamainjp 0:3d0c5d7e5d0a 183 }
shintamainjp 0:3d0c5d7e5d0a 184
shintamainjp 0:3d0c5d7e5d0a 185 data.position_fix = atoi(plist[6]);
shintamainjp 0:3d0c5d7e5d0a 186 data.satellites_used = atoi(plist[7]);
shintamainjp 0:3d0c5d7e5d0a 187
shintamainjp 0:3d0c5d7e5d0a 188 if (sscanf(plist[8], "%lf", &data.hdop) != 1) {
shintamainjp 0:3d0c5d7e5d0a 189 return -8;
shintamainjp 0:3d0c5d7e5d0a 190 }
shintamainjp 0:3d0c5d7e5d0a 191
shintamainjp 0:3d0c5d7e5d0a 192 data.altitude = atoi(plist[9]);
shintamainjp 0:3d0c5d7e5d0a 193 if (strcmp(plist[10], "M") != 0) {
shintamainjp 0:3d0c5d7e5d0a 194 return -9;
shintamainjp 0:3d0c5d7e5d0a 195 }
shintamainjp 0:3d0c5d7e5d0a 196
shintamainjp 0:3d0c5d7e5d0a 197 data.altitude = atoi(plist[11]);
shintamainjp 0:3d0c5d7e5d0a 198 if (sscanf(plist[12], "%c", &data.altitude_unit) != 1) {
shintamainjp 0:3d0c5d7e5d0a 199 return -10;
shintamainjp 0:3d0c5d7e5d0a 200 }
shintamainjp 0:3d0c5d7e5d0a 201
shintamainjp 0:3d0c5d7e5d0a 202 cbfuncs->cbfunc_gga(&data);
shintamainjp 0:3d0c5d7e5d0a 203 return 0;
shintamainjp 0:3d0c5d7e5d0a 204 }
shintamainjp 0:3d0c5d7e5d0a 205 return -11;
shintamainjp 0:3d0c5d7e5d0a 206 }
shintamainjp 0:3d0c5d7e5d0a 207
shintamainjp 0:3d0c5d7e5d0a 208 int SerialGPS::parseAndCallbackGSA(char *src, gps_callback_t *cbfuncs) {
shintamainjp 0:3d0c5d7e5d0a 209
shintamainjp 0:3d0c5d7e5d0a 210 if (cbfuncs->cbfunc_gsa == NULL) {
shintamainjp 0:3d0c5d7e5d0a 211 return -1;
shintamainjp 0:3d0c5d7e5d0a 212 }
shintamainjp 0:3d0c5d7e5d0a 213
shintamainjp 0:3d0c5d7e5d0a 214 char plist[PARAM_ARRAYSIZE][PARAM_TXTMAXLEN];
shintamainjp 0:3d0c5d7e5d0a 215 char *p = src;
shintamainjp 0:3d0c5d7e5d0a 216 int cnt = 0;
shintamainjp 0:3d0c5d7e5d0a 217 while ((p = parse(p, plist[cnt], PARAM_TXTMAXLEN, ",*")) != NULL) {
shintamainjp 0:3d0c5d7e5d0a 218 cnt++;
shintamainjp 0:3d0c5d7e5d0a 219 }
shintamainjp 0:3d0c5d7e5d0a 220
shintamainjp 0:3d0c5d7e5d0a 221 if (cnt == 18) {
shintamainjp 0:3d0c5d7e5d0a 222 gps_gsa_t data;
shintamainjp 0:3d0c5d7e5d0a 223 data.selmode = plist[1][0];
shintamainjp 0:3d0c5d7e5d0a 224 if ((data.selmode != 'A') && (data.selmode != 'M')) {
shintamainjp 0:3d0c5d7e5d0a 225 return -2;
shintamainjp 0:3d0c5d7e5d0a 226 }
shintamainjp 0:3d0c5d7e5d0a 227
shintamainjp 0:3d0c5d7e5d0a 228 data.fix = atoi(plist[2]);
shintamainjp 0:3d0c5d7e5d0a 229 if ((data.fix != 1) && (data.fix != 2) && (data.fix != 3)) {
shintamainjp 0:3d0c5d7e5d0a 230 return -3;
shintamainjp 0:3d0c5d7e5d0a 231 }
shintamainjp 0:3d0c5d7e5d0a 232
shintamainjp 0:3d0c5d7e5d0a 233 cbfuncs->cbfunc_gsa(&data);
shintamainjp 0:3d0c5d7e5d0a 234 return 0;
shintamainjp 0:3d0c5d7e5d0a 235 }
shintamainjp 0:3d0c5d7e5d0a 236 return -4;
shintamainjp 0:3d0c5d7e5d0a 237 }
shintamainjp 0:3d0c5d7e5d0a 238
shintamainjp 0:3d0c5d7e5d0a 239 int SerialGPS::parseAndCallbackRMC(char *src, gps_callback_t *cbfuncs) {
shintamainjp 0:3d0c5d7e5d0a 240
shintamainjp 0:3d0c5d7e5d0a 241 if (cbfuncs->cbfunc_rmc == NULL) {
shintamainjp 0:3d0c5d7e5d0a 242 return -1;
shintamainjp 0:3d0c5d7e5d0a 243 }
shintamainjp 0:3d0c5d7e5d0a 244
shintamainjp 0:3d0c5d7e5d0a 245 char plist[PARAM_ARRAYSIZE][PARAM_TXTMAXLEN];
shintamainjp 0:3d0c5d7e5d0a 246 char *p = src;
shintamainjp 0:3d0c5d7e5d0a 247 int cnt = 0;
shintamainjp 0:3d0c5d7e5d0a 248 while ((p = parse(p, plist[cnt], PARAM_TXTMAXLEN, ",*")) != NULL) {
shintamainjp 0:3d0c5d7e5d0a 249 cnt++;
shintamainjp 0:3d0c5d7e5d0a 250 }
shintamainjp 0:3d0c5d7e5d0a 251
shintamainjp 0:3d0c5d7e5d0a 252 if (cnt == 13) {
shintamainjp 0:3d0c5d7e5d0a 253 gps_rmc_t data;
shintamainjp 0:3d0c5d7e5d0a 254 data.hour = (plist[1][0] - '0') * 10 + (plist[1][1] - '0') * 1;
shintamainjp 0:3d0c5d7e5d0a 255 data.min = (plist[1][2] - '0') * 10 + (plist[1][3] - '0') * 1;
shintamainjp 0:3d0c5d7e5d0a 256 data.sec = (plist[1][4] - '0') * 10 + (plist[1][5] - '0') * 1;
shintamainjp 0:3d0c5d7e5d0a 257
shintamainjp 0:3d0c5d7e5d0a 258 data.status = plist[2][0];
shintamainjp 0:3d0c5d7e5d0a 259
shintamainjp 0:3d0c5d7e5d0a 260 if (sscanf(plist[3], "%lf", &data.nl) != 1) {
shintamainjp 0:3d0c5d7e5d0a 261 return -2;
shintamainjp 0:3d0c5d7e5d0a 262 }
shintamainjp 0:3d0c5d7e5d0a 263 if (strcmp(plist[4], "N") != 0) {
shintamainjp 0:3d0c5d7e5d0a 264 return -3;
shintamainjp 0:3d0c5d7e5d0a 265 }
shintamainjp 0:3d0c5d7e5d0a 266
shintamainjp 0:3d0c5d7e5d0a 267 if (sscanf(plist[5], "%lf", &data.el) != 1) {
shintamainjp 0:3d0c5d7e5d0a 268 return -4;
shintamainjp 0:3d0c5d7e5d0a 269 }
shintamainjp 0:3d0c5d7e5d0a 270 if (strcmp(plist[6], "E") != 0) {
shintamainjp 0:3d0c5d7e5d0a 271 return -5;
shintamainjp 0:3d0c5d7e5d0a 272 }
shintamainjp 0:3d0c5d7e5d0a 273
shintamainjp 0:3d0c5d7e5d0a 274 cbfuncs->cbfunc_rmc(&data);
shintamainjp 0:3d0c5d7e5d0a 275 return 0;
shintamainjp 0:3d0c5d7e5d0a 276 }
shintamainjp 0:3d0c5d7e5d0a 277 return -4;
shintamainjp 0:3d0c5d7e5d0a 278 }
shintamainjp 0:3d0c5d7e5d0a 279
shintamainjp 0:3d0c5d7e5d0a 280 int SerialGPS::parseAndCallbackGSV(char *src, gps_callback_t *cbfuncs) {
shintamainjp 0:3d0c5d7e5d0a 281
shintamainjp 0:3d0c5d7e5d0a 282 if (cbfuncs->cbfunc_gsv == NULL) {
shintamainjp 0:3d0c5d7e5d0a 283 return -1;
shintamainjp 0:3d0c5d7e5d0a 284 }
shintamainjp 0:3d0c5d7e5d0a 285
shintamainjp 0:3d0c5d7e5d0a 286 char plist[PARAM_ARRAYSIZE][PARAM_TXTMAXLEN];
shintamainjp 0:3d0c5d7e5d0a 287 char *p = src;
shintamainjp 0:3d0c5d7e5d0a 288 int cnt = 0;
shintamainjp 0:3d0c5d7e5d0a 289 while ((p = parse(p, plist[cnt], PARAM_TXTMAXLEN, ",*")) != NULL) {
shintamainjp 0:3d0c5d7e5d0a 290 cnt++;
shintamainjp 0:3d0c5d7e5d0a 291 }
shintamainjp 0:3d0c5d7e5d0a 292
shintamainjp 0:3d0c5d7e5d0a 293 if (cnt == 20) {
shintamainjp 0:3d0c5d7e5d0a 294 gps_gsv_t data;
shintamainjp 0:3d0c5d7e5d0a 295 data.msgcnt = atoi(plist[1]);
shintamainjp 0:3d0c5d7e5d0a 296
shintamainjp 0:3d0c5d7e5d0a 297 data.msgnum = atoi(plist[2]);
shintamainjp 0:3d0c5d7e5d0a 298
shintamainjp 0:3d0c5d7e5d0a 299 data.satcnt = atoi(plist[3]);
shintamainjp 0:3d0c5d7e5d0a 300
shintamainjp 0:3d0c5d7e5d0a 301 static const int SATINFOFS = 4;
shintamainjp 0:3d0c5d7e5d0a 302 for (int i = 0; i < 4; i++) {
shintamainjp 0:3d0c5d7e5d0a 303 data.satellite[i].num = atoi(plist[SATINFOFS + 0 * (i * 4)]);
shintamainjp 0:3d0c5d7e5d0a 304 data.satellite[i].elevation = atoi(plist[SATINFOFS + 1 * (i * 4)]);
shintamainjp 0:3d0c5d7e5d0a 305 data.satellite[i].azimuth = atoi(plist[SATINFOFS + 2 * (i * 4)]);
shintamainjp 0:3d0c5d7e5d0a 306 data.satellite[i].snr = atoi(plist[SATINFOFS + 3 *(i * 4)]);
shintamainjp 0:3d0c5d7e5d0a 307 }
shintamainjp 0:3d0c5d7e5d0a 308
shintamainjp 0:3d0c5d7e5d0a 309 int chksum;
shintamainjp 0:3d0c5d7e5d0a 310 if (sscanf(plist[20], "%x", &chksum) != 1) {
shintamainjp 0:3d0c5d7e5d0a 311 return -2;
shintamainjp 0:3d0c5d7e5d0a 312 }
shintamainjp 0:3d0c5d7e5d0a 313
shintamainjp 0:3d0c5d7e5d0a 314 cbfuncs->cbfunc_gsv(&data);
shintamainjp 0:3d0c5d7e5d0a 315 return 0;
shintamainjp 0:3d0c5d7e5d0a 316 }
shintamainjp 0:3d0c5d7e5d0a 317 return -3;
shintamainjp 0:3d0c5d7e5d0a 318 }
shintamainjp 0:3d0c5d7e5d0a 319
shintamainjp 0:3d0c5d7e5d0a 320 int SerialGPS::parseAndCallbackUnknown(char *src, gps_callback_t *cbfuncs) {
shintamainjp 0:3d0c5d7e5d0a 321 return 0;
shintamainjp 0:3d0c5d7e5d0a 322 }
shintamainjp 0:3d0c5d7e5d0a 323
shintamainjp 0:3d0c5d7e5d0a 324 uint8_t SerialGPS::calcCheckSum(char *buf, size_t siz) {
shintamainjp 0:3d0c5d7e5d0a 325 uint8_t cs = 0;
shintamainjp 0:3d0c5d7e5d0a 326 for (int i = 0; i < siz; i++) {
shintamainjp 0:3d0c5d7e5d0a 327 cs ^= buf[i];
shintamainjp 0:3d0c5d7e5d0a 328 }
shintamainjp 0:3d0c5d7e5d0a 329 return cs;
shintamainjp 0:3d0c5d7e5d0a 330 }