From: asterix Date: Mon, 5 Oct 2009 08:27:16 +0000 (+0000) Subject: Move in correct dir nmea module. X-Git-Tag: 2.3.0~64 X-Git-Url: https://codewiz.org/gitweb?a=commitdiff_plain;h=5ccf8a1433d28ad1f858b590830e76186cccd926;p=bertos.git Move in correct dir nmea module. git-svn-id: https://src.develer.com/svnoss/bertos/trunk@3017 38d2e660-2303-0410-9eaa-f027e97ec537 --- diff --git a/bertos/net/nmea.c b/bertos/net/nmea.c new file mode 100644 index 00000000..f1925d34 --- /dev/null +++ b/bertos/net/nmea.c @@ -0,0 +1,338 @@ +/** + * \file + * + * \brief NMEA implementation. + * + * \author Daniele Basile + * + */ + +#include "nmea.h" + +#include "cfg/cfg_nmea.h" + +#include + +#define LOG_LEVEL NMEA_LOG_LEVEL +#define LOG_FORMAT NMEA_LOG_FORMAT +#include + +#include + +#include + + +static uint32_t tokenToInt(const char *s) +{ + uint32_t num = 0; + int i; + + ASSERT(s); + + for(i = 0; i < NMEAP_MAX_TOKENS; i++) + { + char c = *s++; + + if (c == '.') + continue; + + if (c == '\0' || !isdigit(c)) + break; + + num *= 10; + num += c - '0'; + } + + return num; +} + + +static udegree_t nmea_latitude(const char *plat, const char *phem) +{ + int ns; + uint32_t lat; + uint32_t deg; + uint32_t min; + + if (*plat == 0) + { + return 0; + } + if (*phem == 0) + { + return 0; + } + + /* north lat is +, south lat is - */ + if (*phem == 'N') + { + ns = 1; + } + else + { + ns = -1; + } + + lat = tokenToInt(plat); + deg = lat / 1000000; + min = lat - deg * 1000000; + lat = deg * 1000000 + ((min * 5) + 1) / 3; + + return ns * lat; +} + +static udegree_t nmea_longitude(const char *plot, const char *phem) +{ + int ew; + uint32_t lot; + uint32_t deg; + uint32_t min; + + if (*plot == 0) + { + return 0; + } + if (*phem == 0) + { + return 0; + } + + /* west long is negative, east long is positive */ + if (*phem == 'E') + { + ew = 1; + } + else { + ew = -1; + } + + lot = tokenToInt(plot); + deg = lot / 1000000; + min = lot - deg * 1000000; + lot = deg * 1000000 + ((min * 5) + 1) / 3; + + return ew * lot; +} + +static cm_t nmea_altitude(const char *palt, const char *punits) +{ + uint32_t alt; + + if (*palt == 0) + { + return 0; + } + + alt = tokenToInt(palt); + + if (*punits == 'F') + { + /* 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 */ + + } + + return alt; +} + +/** + * standard GPGGA sentence parser + */ +int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence) +{ + /* + * get pointer to sentence data + */ + 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 = tokenToInt(context->token[1]); + 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]); + + } + + + /* + * if the sentence has a callout, call it + */ + if (sentence->callout != 0) + (*sentence->callout)(context,gga,context->user_data); + + return NMEA_GPGGA; +} + +/** + * standard GPRMCntence parser + */ +int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence) +{ + + /* + * get pointer to sentence data + */ + NmeaRmc *rmc = (NmeaRmc *)sentence->data; + + /* + * if there is a data element, use it + */ + if (rmc != 0) + { + /* + * extract data from the tokens + */ + rmc->time = tokenToInt(context->token[1]); + 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]) * 100; + rmc->course = tokenToInt(context->token[8]) * 100; + rmc->date = tokenToInt(context->token[9]); + rmc->mag_var = tokenToInt(context->token[10]); + } + + /* + * if the sentence has a callout, call it + */ + if (sentence->callout != 0) + (*sentence->callout)(context,rmc,context->user_data); + + return NMEA_GPRMC; +} + + +/** + * standard GPVTG sentence parser + */ +int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence) +{ + + /* + * get pointer to sentence data + */ + NmeaVtg *vtg = (NmeaVtg *)sentence->data; + + /* + * if there is a data element, use it + */ + if (vtg != 0) + { + /* + * extract data from the tokens + */ + vtg->track_good = tokenToInt(context->token[1]) * 100; + vtg->knot_speed = tokenToInt(context->token[5]) * 100; + vtg->km_speed = tokenToInt(context->token[7]) * 100; + } + + /* + * if the sentence has a callout, call it + */ + if (sentence->callout != 0) + (*sentence->callout)(context,vtg,context->user_data); + + return NMEA_GPVTG; +} + +/** + * standard GPGDSV sentence parser + */ +int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence) +{ + + /* + * get pointer to sentence data + */ + NmeaGsv *gsv = (NmeaGsv *)sentence->data; + + /* + * if there is a data element, use it + */ + if (gsv != 0) + { + /* + * 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]); + } + + + /* + * if the sentence has a callout, call it + */ + if (sentence->callout != 0) + (*sentence->callout)(context,gsv,context->user_data); + + return NMEA_GPGSV; +} + +void nmea_poll(nmeap_context_t *context, KFile *channel) +{ + int c; + while ((c = kfile_getc(channel)) != EOF) + { + if (nmeap_parse(context, c) == -1) + break; + } +} + diff --git a/bertos/net/nmea.h b/bertos/net/nmea.h new file mode 100644 index 00000000..8c28b658 --- /dev/null +++ b/bertos/net/nmea.h @@ -0,0 +1,143 @@ +/** + * \file + * + * \brief NMEA Parser + * + * \author Daniele Basile + * + * $WIZ$ module_name = "nmea" + * $WIZ$ module_configuration = "bertos/cfg/cfg_nmea.h" + * $WIZ$ module_depends = "kfile" + */ + +#ifndef NET_NMEA_H +#define NET_NMEA_H + +#include "cfg/cfg_nmea.h" + +#include + +#include + +/* + * Implemented NMEA parser strings. + */ +#define NMEA_GPGGA 1 // GGA MESSAGE ID +#define NMEA_GPRMC 2 // RMC MESSAGE ID +#define NMEA_GPVTG 3 // VTG MESSAGE ID +#define NMEA_GPGSV 4 // GSV MESSAGE ID + +// Standart type to rappresent fiels. +typedef uint32_t udegree_t; // Micro degrees +typedef uint32_t mdegree_t; // Milli degrees +typedef uint16_t degree_t; // Degrees +typedef uint16_t cm_t; // Centimeter +typedef uint32_t sec_t; // Seconds +typedef uint32_t str_time_t; // Time format HH:MM:SS +typedef uint32_t mknots_t; // Milli knots +typedef uint32_t mkh_t; // Milli kilometers/hour +typedef uint16_t number_t; // Pure number + + +/** + * Extracted data from a GGA message + */ +typedef struct NmeaGga +{ + udegree_t latitude; + udegree_t longitude; + cm_t altitude; + sec_t time; + number_t satellites; + number_t quality; + udegree_t hdop; + udegree_t geoid; +} NmeaGga; + +/** + * Extracted data from an RMC message + */ +typedef struct NmeaRmc +{ + str_time_t time; + char warn; + udegree_t latitude; + udegree_t longitude; + mknots_t speed; + mdegree_t course; + str_time_t date; + mdegree_t mag_var; +} NmeaRmc; + +/** + * Extracted data from an vtg message + */ +typedef struct NmeaVtg +{ + mdegree_t track_good; + mknots_t knot_speed; + mkh_t km_speed; +} NmeaVtg; + +/** + * Extracted data from an gsv message + */ +typedef struct NmeaGsv +{ + number_t tot_message; + number_t message_num; + number_t tot_svv; + number_t sv_prn; + degree_t elevation; + degree_t azimut; + number_t snr; + number_t sv_prn2; + degree_t elevation2; + degree_t azimut2; + number_t snr2; + number_t sv_prn3; + degree_t elevation3; + degree_t azimut3; + number_t snr3; + number_t sv_prn4; + degree_t elevation4; + degree_t azimut4; + number_t snr4; +} NmeaGsv; + +void nmea_poll(nmeap_context_t *context, KFile *channel); + +int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence); +int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence); +int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence); +int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence); + +#endif /* NET_NMEA_H */ diff --git a/nmea.c b/nmea.c deleted file mode 100644 index f1925d34..00000000 --- a/nmea.c +++ /dev/null @@ -1,338 +0,0 @@ -/** - * \file - * - * \brief NMEA implementation. - * - * \author Daniele Basile - * - */ - -#include "nmea.h" - -#include "cfg/cfg_nmea.h" - -#include - -#define LOG_LEVEL NMEA_LOG_LEVEL -#define LOG_FORMAT NMEA_LOG_FORMAT -#include - -#include - -#include - - -static uint32_t tokenToInt(const char *s) -{ - uint32_t num = 0; - int i; - - ASSERT(s); - - for(i = 0; i < NMEAP_MAX_TOKENS; i++) - { - char c = *s++; - - if (c == '.') - continue; - - if (c == '\0' || !isdigit(c)) - break; - - num *= 10; - num += c - '0'; - } - - return num; -} - - -static udegree_t nmea_latitude(const char *plat, const char *phem) -{ - int ns; - uint32_t lat; - uint32_t deg; - uint32_t min; - - if (*plat == 0) - { - return 0; - } - if (*phem == 0) - { - return 0; - } - - /* north lat is +, south lat is - */ - if (*phem == 'N') - { - ns = 1; - } - else - { - ns = -1; - } - - lat = tokenToInt(plat); - deg = lat / 1000000; - min = lat - deg * 1000000; - lat = deg * 1000000 + ((min * 5) + 1) / 3; - - return ns * lat; -} - -static udegree_t nmea_longitude(const char *plot, const char *phem) -{ - int ew; - uint32_t lot; - uint32_t deg; - uint32_t min; - - if (*plot == 0) - { - return 0; - } - if (*phem == 0) - { - return 0; - } - - /* west long is negative, east long is positive */ - if (*phem == 'E') - { - ew = 1; - } - else { - ew = -1; - } - - lot = tokenToInt(plot); - deg = lot / 1000000; - min = lot - deg * 1000000; - lot = deg * 1000000 + ((min * 5) + 1) / 3; - - return ew * lot; -} - -static cm_t nmea_altitude(const char *palt, const char *punits) -{ - uint32_t alt; - - if (*palt == 0) - { - return 0; - } - - alt = tokenToInt(palt); - - if (*punits == 'F') - { - /* 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 */ - - } - - return alt; -} - -/** - * standard GPGGA sentence parser - */ -int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence) -{ - /* - * get pointer to sentence data - */ - 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 = tokenToInt(context->token[1]); - 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]); - - } - - - /* - * if the sentence has a callout, call it - */ - if (sentence->callout != 0) - (*sentence->callout)(context,gga,context->user_data); - - return NMEA_GPGGA; -} - -/** - * standard GPRMCntence parser - */ -int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence) -{ - - /* - * get pointer to sentence data - */ - NmeaRmc *rmc = (NmeaRmc *)sentence->data; - - /* - * if there is a data element, use it - */ - if (rmc != 0) - { - /* - * extract data from the tokens - */ - rmc->time = tokenToInt(context->token[1]); - 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]) * 100; - rmc->course = tokenToInt(context->token[8]) * 100; - rmc->date = tokenToInt(context->token[9]); - rmc->mag_var = tokenToInt(context->token[10]); - } - - /* - * if the sentence has a callout, call it - */ - if (sentence->callout != 0) - (*sentence->callout)(context,rmc,context->user_data); - - return NMEA_GPRMC; -} - - -/** - * standard GPVTG sentence parser - */ -int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence) -{ - - /* - * get pointer to sentence data - */ - NmeaVtg *vtg = (NmeaVtg *)sentence->data; - - /* - * if there is a data element, use it - */ - if (vtg != 0) - { - /* - * extract data from the tokens - */ - vtg->track_good = tokenToInt(context->token[1]) * 100; - vtg->knot_speed = tokenToInt(context->token[5]) * 100; - vtg->km_speed = tokenToInt(context->token[7]) * 100; - } - - /* - * if the sentence has a callout, call it - */ - if (sentence->callout != 0) - (*sentence->callout)(context,vtg,context->user_data); - - return NMEA_GPVTG; -} - -/** - * standard GPGDSV sentence parser - */ -int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence) -{ - - /* - * get pointer to sentence data - */ - NmeaGsv *gsv = (NmeaGsv *)sentence->data; - - /* - * if there is a data element, use it - */ - if (gsv != 0) - { - /* - * 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]); - } - - - /* - * if the sentence has a callout, call it - */ - if (sentence->callout != 0) - (*sentence->callout)(context,gsv,context->user_data); - - return NMEA_GPGSV; -} - -void nmea_poll(nmeap_context_t *context, KFile *channel) -{ - int c; - while ((c = kfile_getc(channel)) != EOF) - { - if (nmeap_parse(context, c) == -1) - break; - } -} - diff --git a/nmea.h b/nmea.h deleted file mode 100644 index 8c28b658..00000000 --- a/nmea.h +++ /dev/null @@ -1,143 +0,0 @@ -/** - * \file - * - * \brief NMEA Parser - * - * \author Daniele Basile - * - * $WIZ$ module_name = "nmea" - * $WIZ$ module_configuration = "bertos/cfg/cfg_nmea.h" - * $WIZ$ module_depends = "kfile" - */ - -#ifndef NET_NMEA_H -#define NET_NMEA_H - -#include "cfg/cfg_nmea.h" - -#include - -#include - -/* - * Implemented NMEA parser strings. - */ -#define NMEA_GPGGA 1 // GGA MESSAGE ID -#define NMEA_GPRMC 2 // RMC MESSAGE ID -#define NMEA_GPVTG 3 // VTG MESSAGE ID -#define NMEA_GPGSV 4 // GSV MESSAGE ID - -// Standart type to rappresent fiels. -typedef uint32_t udegree_t; // Micro degrees -typedef uint32_t mdegree_t; // Milli degrees -typedef uint16_t degree_t; // Degrees -typedef uint16_t cm_t; // Centimeter -typedef uint32_t sec_t; // Seconds -typedef uint32_t str_time_t; // Time format HH:MM:SS -typedef uint32_t mknots_t; // Milli knots -typedef uint32_t mkh_t; // Milli kilometers/hour -typedef uint16_t number_t; // Pure number - - -/** - * Extracted data from a GGA message - */ -typedef struct NmeaGga -{ - udegree_t latitude; - udegree_t longitude; - cm_t altitude; - sec_t time; - number_t satellites; - number_t quality; - udegree_t hdop; - udegree_t geoid; -} NmeaGga; - -/** - * Extracted data from an RMC message - */ -typedef struct NmeaRmc -{ - str_time_t time; - char warn; - udegree_t latitude; - udegree_t longitude; - mknots_t speed; - mdegree_t course; - str_time_t date; - mdegree_t mag_var; -} NmeaRmc; - -/** - * Extracted data from an vtg message - */ -typedef struct NmeaVtg -{ - mdegree_t track_good; - mknots_t knot_speed; - mkh_t km_speed; -} NmeaVtg; - -/** - * Extracted data from an gsv message - */ -typedef struct NmeaGsv -{ - number_t tot_message; - number_t message_num; - number_t tot_svv; - number_t sv_prn; - degree_t elevation; - degree_t azimut; - number_t snr; - number_t sv_prn2; - degree_t elevation2; - degree_t azimut2; - number_t snr2; - number_t sv_prn3; - degree_t elevation3; - degree_t azimut3; - number_t snr3; - number_t sv_prn4; - degree_t elevation4; - degree_t azimut4; - number_t snr4; -} NmeaGsv; - -void nmea_poll(nmeap_context_t *context, KFile *channel); - -int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence); -int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence); -int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence); -int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence); - -#endif /* NET_NMEA_H */