X-Git-Url: https://codewiz.org/gitweb?a=blobdiff_plain;f=bertos%2Fnet%2Fnmea.c;h=f74879523e9bcf2f543c1520efc4e374e39e19af;hb=1a8a2b9b57118be3f52d1261ebefac3875e67bcf;hp=d3937282df9b83b01a26f6c8936c49cfb6b76c56;hpb=9ebefafde06a6a7001e8fa631aedc91cf1dc5af3;p=bertos.git diff --git a/bertos/net/nmea.c b/bertos/net/nmea.c index d3937282..f7487952 100644 --- a/bertos/net/nmea.c +++ b/bertos/net/nmea.c @@ -29,7 +29,21 @@ * Copyright 2009 Develer S.r.l. (http://www.develer.com/) * * --> - * \brief NMEA implementation. + * + * \brief NMEA parser implementation. + * + * NMEA 0183 is acronym of National Marine Electronics Association that + * combined electrical and data specification for communication between marine + * electronic devices such as echo sounder, sonars, anemometer (wind speed and direction), + * gyrocompass, autopilot, GPS receivers and many other types of instruments. + * It has been defined by, and is controlled by, the U.S.-based National Marine + * Electronics Association. + * The NMEA 0183 standard uses a simple ASCII, serial communications protocol + * that defines how data is transmitted in a "sentence" from one "talker" + * to multiple "listeners" at a time. + * At the application layer, the standard also defines the contents of each sentence + * (message) type so that all listeners can parse messages accurately. + * * * \author Daniele Basile * @@ -51,32 +65,52 @@ #include #include #include +#include - -static uint32_t tokenToInt(const char *s) +/* + * Make conversion from one string to int. + * + * You can specify the precision if the string is a float + * number. The result is an int multiplied to 10^precision. + */ +static uint32_t tokenToInt(const char *s, int precision) { uint32_t num = 0; + bool sep_found = false; int i; - ASSERT(s); + if (!s) + return 0; - for(i = 0; i < NMEAP_MAX_TOKENS; i++) + for(i = 0; i < NMEAP_MAX_SENTENCE_LENGTH; i++) { - char c = *s++; + unsigned char c = *s++; if (c == '.') + { + sep_found = true; continue; + } - if (c == '\0' || !isdigit(c)) + if (c == '\0' || !isdigit(c) || (precision == 0 && sep_found)) break; + if (sep_found) + precision--; + num *= 10; num += c - '0'; } + while (precision--) + num *= 10; + return num; } +/* + * Convert a string to micro degree. + */ static udegree_t convertToDegree(const char *str) { uint32_t dec; @@ -84,11 +118,9 @@ static udegree_t convertToDegree(const char *str) uint32_t min; if (*str == 0) - { - return 0; - } + return 0; - dec = tokenToInt(str); + dec = tokenToInt(str, 4); deg = dec / 1000000; min = dec - deg * 1000000; dec = deg * 1000000 + ((min * 5) + 1) / 3; @@ -96,77 +128,70 @@ static udegree_t convertToDegree(const char *str) return dec; } +/* + * Retun latitude in micro degree from a string. + */ static udegree_t nmea_latitude(const char *plat, const char *phem) { int ns; - if (*phem == 0) - { - return 0; - } - + if (*phem == 0) + return 0; - /* north lat is +, south lat is - */ - if (*phem == 'N') - { - ns = 1; - } - else - { - ns = -1; - } + /* north lat is +, south lat is - */ + ns = (*phem == 'N') ? 1 : -1; return ns * convertToDegree(plat); } +/* + * Retun longitude in micro degree from a string. + */ static udegree_t nmea_longitude(const char *plot, const char *phem) { int ew; - /* west long is negative, east long is positive */ - if (*phem == 'E') - { - ew = 1; - } - else { - ew = -1; - } + if (*phem == 0) + return 0; - if (*phem == 0) - { - return 0; - } + /* west long is negative, east long is positive */ + ew = (*phem == 'E') ? 1 : -1; return ew * convertToDegree(plot); } -static uint16_t nmea_altitude(const char *palt, const char *punits) +/* + * Return altitude in meter from a string. + * + */ +static int32_t nmea_altitude(const char *palt, const char *punits) { - uint32_t alt; + int32_t alt; if (*palt == 0) - { - return 0; - } + return 0; - alt = tokenToInt(palt); + alt = atoi(palt); - if (*punits == 'F') + if (*punits == 'F') { - /* convert to feet */ - /* alt = alt * 3.2808399 */ + /* convert to feet */ + /* alt = alt * 3.2808399 */ alt = alt * 3 + /* 3.0 */ - (alt >> 2) + /* 0.25 */ - (alt >> 6) + /* 0.015625 */ - (alt >> 7) + /* 0.0078125 */ - (alt >> 8); /* 0,00390625 */ + (alt >> 2) + /* 0.25 */ + (alt >> 6) + /* 0.015625 */ + (alt >> 7) + /* 0.0078125 */ + (alt >> 8); /* 0,00390625 */ - } + } return alt; } +/* + * Convert time and date stamp string to unix time. + */ static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp) { struct tm t; @@ -182,6 +207,7 @@ static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp) uint32_t res = time_stamp / 1000; uint32_t all = time_stamp; msec = all - res * 1000; + for (int i = 0; i < 3; i++) { all = res; @@ -193,9 +219,9 @@ static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp) t.tm_sec = tmr[0] + (ROUND_UP(msec, 1000) / 1000); t.tm_min = tmr[1]; t.tm_hour = tmr[2]; - //If we not have refence data, we set as default 1/1/1970. + //If we do not have refence data, we set 1/1/1970 as default t.tm_mday = 1; - t.tm_mon = 1; + t.tm_mon = 0; t.tm_year = 70; if (date_stamp) @@ -210,8 +236,8 @@ static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp) } t.tm_mday = date[2]; t.tm_mon = date[1] - 1; // time struct count month from 0 to 11; - // we should specific number of years from 1900, but the year field - // is only two cipher, so we sum 100 (2000 - 1900).. + // we should specify the number of years from 1900, but the year field + // is only two digits, so we add 100 (2000 - 1900).. t.tm_year = date[0] + 100; } LOG_INFO("times=%d,%d,%d,%d,%d,%d\n",t.tm_sec, t.tm_min, t.tm_hour, t.tm_year, t.tm_mon, t.tm_mday); @@ -219,6 +245,83 @@ static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp) return mktime(&t); } +/** + * Callout example for GGA data + */ +void gpgga_callout(nmeap_context_t *context, void *data, void *user_data) +{ + (void)context; + (void)user_data; + (void)data; + LOG_INFOB( + NmeaGga *gga = (NmeaGga *)data; + LOG_INFO("Found GPGGA message %ld %ld %d %lu %d %d %d %d\n", + (long)gga->latitude, + (long)gga->longitude, + gga->altitude, + gga->time, + gga->satellites, + gga->quality, + gga->hdop, + gga->geoid); + ); +} + +/** + * Callout example for RMC + */ +void gprmc_callout(nmeap_context_t *context, void *data, void *user_data) +{ + (void)context; + (void)user_data; + (void)data; + LOG_INFOB( + NmeaRmc *rmc = (NmeaRmc *)data; + + LOG_INFO("Found GPRMC message %lu %c %ld %ld %d %d %d\n", + rmc->time, + rmc->warn, + (long)rmc->latitude, + (long)rmc->longitude, + rmc->speed, + rmc->course, + rmc->mag_var); + ); +} + +/** + * Callout example for GSV data + */ +void gpgsv_callout(nmeap_context_t *context, void *data, void *user_data) +{ + (void)context; + (void)user_data; + (void)data; + LOG_INFOB( + NmeaGsv *gsv = (NmeaGsv *)data; + + LOG_INFO("Found GPGSV message %d %d %d\n", gsv->tot_message, gsv->message_num, gsv->tot_svv); + + for (int i = 0; i < 4; i++) + LOG_INFO("%d %d %d %d\n", gsv->info[i].sv_prn, gsv->info[i].elevation, gsv->info[i].azimut, gsv->info[i].snr); + ); +} + +/** + * Callout example for VTG data + */ +void gpvtg_callout(nmeap_context_t *context, void *data, void *user_data) +{ + (void)context; + (void)user_data; + (void)data; + LOG_INFOB( + NmeaVtg *vtg = (NmeaVtg *)data; + LOG_INFO("Found GPVTG message %d %d %d\n", vtg->track_good, vtg->knot_speed, vtg->km_speed); + ); +} + + /** * standard GPGGA sentence parser @@ -230,26 +333,22 @@ int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence) */ NmeaGga *gga = (NmeaGga *)sentence->data; - /* - * if there is a data element, extract data from the tokens - */ - if (gga != 0) - { - gga->latitude = nmea_latitude(context->token[2],context->token[3]); - gga->longitude = nmea_longitude(context->token[4],context->token[5]); - gga->altitude = nmea_altitude(context->token[9],context->token[10]); - gga->time = timestampToSec(tokenToInt(context->token[1]), 0); - gga->satellites = tokenToInt(context->token[7]); - gga->quality = tokenToInt(context->token[6]); - gga->hdop = tokenToInt(context->token[8]); - gga->geoid = nmea_altitude(context->token[11],context->token[12]); - - } + ASSERT(gga); + ASSERT(context->tokens >= 12); + gga->latitude = nmea_latitude(context->token[2],context->token[3]); + gga->longitude = nmea_longitude(context->token[4],context->token[5]); + gga->altitude = nmea_altitude(context->token[9],context->token[10]); + gga->time = timestampToSec(tokenToInt(context->token[1], 3), 0); + gga->satellites = atoi(context->token[7]); + gga->quality = atoi(context->token[6]); + gga->hdop = tokenToInt(context->token[8], 1); + gga->geoid = nmea_altitude(context->token[11],context->token[12]); /* * if the sentence has a callout, call it */ + if (sentence->callout != 0) (*sentence->callout)(context, gga, context->user_data); @@ -262,35 +361,29 @@ int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence) int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence) { - /* + /* * get pointer to sentence data */ - NmeaRmc *rmc = (NmeaRmc *)sentence->data; + NmeaRmc *rmc = (NmeaRmc *)sentence->data; + + ASSERT(rmc); + ASSERT(context->tokens >= 10); /* - * if there is a data element, use it + * extract data from the tokens */ - if (rmc != 0) - { - /* - * extract data from the tokens - */ - rmc->time = timestampToSec(tokenToInt(context->token[1]), tokenToInt(context->token[9])); - rmc->warn = *context->token[2]; - rmc->latitude = nmea_latitude(context->token[3],context->token[4]); - rmc->longitude = nmea_longitude(context->token[5],context->token[6]); - rmc->speed = tokenToInt(context->token[7]); - rmc->course = tokenToInt(context->token[8]); - rmc->mag_var = tokenToInt(context->token[10]); - } + rmc->time = timestampToSec(tokenToInt(context->token[1], 3), tokenToInt(context->token[9], 0)); + rmc->warn = *context->token[2]; + rmc->latitude = nmea_latitude(context->token[3],context->token[4]); + rmc->longitude = nmea_longitude(context->token[5],context->token[6]); + rmc->speed = atoi(context->token[7]); + rmc->course = atoi(context->token[8]); + rmc->mag_var = atoi(context->token[10]); - /* - * if the sentence has a callout, call it - */ - if (sentence->callout != 0) - (*sentence->callout)(context, rmc, context->user_data); + if (sentence->callout != 0) + (*sentence->callout)(context, rmc, context->user_data); - return NMEA_GPRMC; + return NMEA_GPRMC; } @@ -300,31 +393,28 @@ int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence) int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence) { - /* + /* * get pointer to sentence data */ - NmeaVtg *vtg = (NmeaVtg *)sentence->data; + NmeaVtg *vtg = (NmeaVtg *)sentence->data; + + ASSERT(vtg); + ASSERT(context->tokens >= 7); /* - * if there is a data element, use it + * extract data from the tokens */ - if (vtg != 0) - { - /* - * extract data from the tokens - */ - vtg->track_good = tokenToInt(context->token[1]); - vtg->knot_speed = tokenToInt(context->token[5]); - vtg->km_speed = tokenToInt(context->token[7]); - } + vtg->track_good = atoi(context->token[1]); + vtg->knot_speed = atoi(context->token[5]); + vtg->km_speed = atoi(context->token[7]); - /* + /* * if the sentence has a callout, call it */ - if (sentence->callout != 0) - (*sentence->callout)(context, vtg, context->user_data); + if (sentence->callout != 0) + (*sentence->callout)(context, vtg, context->user_data); - return NMEA_GPVTG; + return NMEA_GPVTG; } /** @@ -332,57 +422,53 @@ int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence) */ int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence) { - - /* + /* * get pointer to sentence data */ - NmeaGsv *gsv = (NmeaGsv *)sentence->data; + NmeaGsv *gsv = (NmeaGsv *)sentence->data; + /* - * if there is a data element, use it + * extract data from the tokens */ - if (gsv != 0) + gsv->tot_message = atoi(context->token[1]); + gsv->message_num = atoi(context->token[2]); + gsv->tot_svv = atoi(context->token[3]); + + // Fill remaning member until we have token + int j = 0; + for (int i = 4; i < context->tokens - 3; i += 4, j++) { - /* - * extract data from the tokens - */ - gsv->tot_message = tokenToInt(context->token[1]); - gsv->message_num = tokenToInt(context->token[2]); - gsv->tot_svv = tokenToInt(context->token[3]); - gsv->sv_prn = tokenToInt(context->token[4]); - gsv->elevation = tokenToInt(context->token[5]); - gsv->azimut = tokenToInt(context->token[6]); - gsv->snr = tokenToInt(context->token[7]); - gsv->sv_prn2 = tokenToInt(context->token[8]); - gsv->elevation2 = tokenToInt(context->token[9]); - gsv->azimut2 = tokenToInt(context->token[10]); - gsv->snr2 = tokenToInt(context->token[11]); - gsv->sv_prn3 = tokenToInt(context->token[12]); - gsv->elevation3 = tokenToInt(context->token[13]); - gsv->azimut3 = tokenToInt(context->token[14]); - gsv->snr3 = tokenToInt(context->token[15]); - gsv->sv_prn4 = tokenToInt(context->token[16]); - gsv->elevation4 = tokenToInt(context->token[17]); - gsv->azimut4 = tokenToInt(context->token[18]); - gsv->snr4 = tokenToInt(context->token[19]); - } + gsv->info[j].sv_prn = atoi(context->token[i]); + gsv->info[j].elevation = atoi(context->token[i + 1]); + gsv->info[j].azimut = atoi(context->token[i + 2]); + gsv->info[j].snr = atoi(context->token[i + 3]); + } - /* + /* * if the sentence has a callout, call it */ - if (sentence->callout != 0) - (*sentence->callout)(context, gsv, context->user_data); + if (sentence->callout != 0) + (*sentence->callout)(context, gsv, context->user_data); - return NMEA_GPGSV; + return NMEA_GPGSV; } + +/** + * Parse NMEA sentence from a channel. + */ void nmea_poll(nmeap_context_t *context, KFile *channel) { - int c; + int c, e; while ((c = kfile_getc(channel)) != EOF) - { nmeap_parse(context, c); + + if ((e = kfile_error(channel))) + { + LOG_ERR("ch error [%0X]\n", e); + kfile_clearerr(channel); } }