From c0e8a84a7f0e404ccaab0bb0cb36253f945ea657 Mon Sep 17 00:00:00 2001 From: asterix Date: Mon, 5 Oct 2009 16:05:32 +0000 Subject: [PATCH] Clean code. Use correct type for all sentece structure. Add timestamp to set conversion function. git-svn-id: https://src.develer.com/svnoss/bertos/trunk@3022 38d2e660-2303-0410-9eaa-f027e97ec537 --- bertos/net/nmea.c | 77 +++++++++++++++++++++++++++++++++------ bertos/net/nmea.h | 93 +++++++++++++++++++++++++---------------------- 2 files changed, 114 insertions(+), 56 deletions(-) diff --git a/bertos/net/nmea.c b/bertos/net/nmea.c index f1925d34..00c6d076 100644 --- a/bertos/net/nmea.c +++ b/bertos/net/nmea.c @@ -48,6 +48,8 @@ #include #include +#include +#include static uint32_t tokenToInt(const char *s) @@ -142,7 +144,7 @@ static udegree_t nmea_longitude(const char *plot, const char *phem) return ew * lot; } -static cm_t nmea_altitude(const char *palt, const char *punits) +static uint16_t nmea_altitude(const char *palt, const char *punits) { uint32_t alt; @@ -168,6 +170,58 @@ static cm_t nmea_altitude(const char *palt, const char *punits) return alt; } +static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp) +{ + struct tm t; + uint16_t msec; + uint16_t tmr[3]; + uint16_t date[3]; + + memset(&t, 0, sizeof(t)); + memset(&tmr, 0, sizeof(tmr)); + memset(&date, 0, sizeof(date)); + + LOG_INFO("time_s[%u],date[%u]\n", time_stamp, 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; + res = all / 100; + tmr[i] = all - res * 100; + LOG_INFO("t[%d]%d\n", tmr[i],i); + } + + t.tm_sec = tmr[0] + (ROUND_UP(msec, 1000) / 1000); + t.tm_min = tmr[1]; + t.tm_hour = tmr[2]; + t.tm_mday = 1; + t.tm_mon = 1; + t.tm_year = 70; + + if (date_stamp) + { + res = all = date_stamp; + for (int i = 0; i < 3; i++) + { + all = res; + res = all / 100; + date[i] = all - res * 100; + LOG_INFO("d[%d]%d\n", date[i],i); + } + 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).. + 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); + + return mktime(&t); +} + + /** * standard GPGGA sentence parser */ @@ -186,7 +240,7 @@ int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence) 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->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]); @@ -199,7 +253,7 @@ int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence) * if the sentence has a callout, call it */ if (sentence->callout != 0) - (*sentence->callout)(context,gga,context->user_data); + (*sentence->callout)(context, gga, context->user_data); return NMEA_GPGGA; } @@ -223,13 +277,12 @@ int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence) /* * extract data from the tokens */ - rmc->time = tokenToInt(context->token[1]); + 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]) * 100; - rmc->course = tokenToInt(context->token[8]) * 100; - rmc->date = tokenToInt(context->token[9]); + rmc->speed = tokenToInt(context->token[7]); + rmc->course = tokenToInt(context->token[8]); rmc->mag_var = tokenToInt(context->token[10]); } @@ -237,7 +290,7 @@ int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence) * if the sentence has a callout, call it */ if (sentence->callout != 0) - (*sentence->callout)(context,rmc,context->user_data); + (*sentence->callout)(context, rmc, context->user_data); return NMEA_GPRMC; } @@ -262,16 +315,16 @@ int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence) /* * 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; + vtg->track_good = tokenToInt(context->token[1]); + vtg->knot_speed = tokenToInt(context->token[5]); + vtg->km_speed = tokenToInt(context->token[7]); } /* * if the sentence has a callout, call it */ if (sentence->callout != 0) - (*sentence->callout)(context,vtg,context->user_data); + (*sentence->callout)(context, vtg, context->user_data); return NMEA_GPVTG; } diff --git a/bertos/net/nmea.h b/bertos/net/nmea.h index fbfd0abb..28c7bf58 100644 --- a/bertos/net/nmea.h +++ b/bertos/net/nmea.h @@ -47,6 +47,8 @@ #include +#include + /* * Implemented NMEA parser strings. */ @@ -59,42 +61,45 @@ 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 /** + * Global Positioning System Fix Data. * Extracted data from a GGA message + * + * Note: time membert containt the second elapsed from 00:00:00 1/1/1970, + * becouse from nmea sentence we read only the time of UTC position, we + * not have any reference of date (day, month and year) so time is refered to + * the start of unix time. */ 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; + udegree_t latitude; /* Latitude (micro degree) */ + udegree_t longitude; /* Longitude (micro degree) */ + uint16_t altitude; /* Altitude (Meter) */ + time_t time; /* UTC of position (Unix time) */ + uint16_t satellites; /* Satellites are in view */ + uint16_t quality; /* Fix Quality: 0 = Invalid; 1 = GPS fix; 2 = DGPS fix; */ + uint16_t hdop; /* Relative accuracy of horizontal position (hdop * 10) */ + uint16_t geoid; /* Height of geoid above WGS84 ellipsoid (Meter) */ } NmeaGga; /** + * Recommended minimum specific GPS/Transit data. * Extracted data from an RMC message + * + * Note: RMC sentece contain also date stamp so, time contain real second elapsed + * from 0:00:00 1/1/1970. */ 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; + time_t time; /* UTC of position (Unix time) */ + char warn; /* Navigation receiver warning A = OK, V = warning */ + udegree_t latitude; /* Latitude (micro degree) */ + udegree_t longitude; /* Longitude (micro degree) */ + uint16_t speed; /* Speed over ground (knots) */ + degree_t course; /* Track made good in degrees True (degree) */ + degree_t mag_var; /* Magnetic variation degrees (degree) */ } NmeaRmc; /** @@ -102,9 +107,9 @@ typedef struct NmeaRmc */ typedef struct NmeaVtg { - mdegree_t track_good; - mknots_t knot_speed; - mkh_t km_speed; + degree_t track_good; /* True track made good (degree) */ + uint16_t knot_speed; /* Speed over ground (knots) */ + uint16_t km_speed; /* Speed over ground in kilometers/hour */ } NmeaVtg; /** @@ -112,25 +117,25 @@ typedef struct NmeaVtg */ 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; + uint16_t tot_message; + uint16_t message_num; + uint16_t tot_svv; + uint16_t sv_prn; + degree_t elevation; + degree_t azimut; + uint16_t snr; + uint16_t sv_prn2; + degree_t elevation2; + degree_t azimut2; + uint16_t snr2; + uint16_t sv_prn3; + degree_t elevation3; + degree_t azimut3; + uint16_t snr3; + uint16_t sv_prn4; + degree_t elevation4; + degree_t azimut4; + uint16_t snr4; } NmeaGsv; void nmea_poll(nmeap_context_t *context, KFile *channel); -- 2.25.1