* 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 <asterix@develer.com>
*
+ * notest:avr
*/
#include "nmea.h"
#include <net/nmeap/inc/nmeap.h>
#include <ctype.h>
+#include <time.h>
+#include <string.h>
+#include <stdlib.h>
-
-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++;
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;
}
-
-static udegree_t nmea_latitude(const char *plat, const char *phem)
+/*
+ * Convert a string to micro degree.
+ */
+static udegree_t convertToDegree(const char *str)
{
- int ns;
- uint32_t lat;
+ uint32_t dec;
uint32_t deg;
uint32_t min;
- if (*plat == 0)
+ if (*str == 0)
{
return 0;
}
+
+ dec = tokenToInt(str, 4);
+ deg = dec / 1000000;
+ min = dec - deg * 1000000;
+ dec = deg * 1000000 + ((min * 5) + 1) / 3;
+
+ 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;
- }
/* north lat is +, south lat is - */
- if (*phem == 'N')
- {
- ns = 1;
- }
- else
- {
- ns = -1;
- }
+ ns = (*phem == 'N') ? 1 : -1;
- lat = tokenToInt(plat);
- deg = lat / 1000000;
- min = lat - deg * 1000000;
- lat = deg * 1000000 + ((min * 5) + 1) / 3;
- return ns * lat;
+ 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;
- 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;
+ ew = (*phem == 'E') ? 1 : -1;
- return ew * lot;
+ return ew * convertToDegree(plot);
}
-static cm_t nmea_altitude(const char *palt, const char *punits)
+/*
+ * Return altitude in meter from a string.
+ *
+ */
+static uint16_t nmea_altitude(const char *palt, const char *punits)
{
uint32_t alt;
if (*palt == 0)
- {
return 0;
- }
- alt = tokenToInt(palt);
+ alt = atoi(palt);
if (*punits == 'F')
{
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;
+ 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[%lu],date[%lu]\n", (long)time_stamp, (long)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];
+ //If we not have refence data, we set as default 1/1/1970.
+ t.tm_mday = 1;
+ t.tm_mon = 0;
+ 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);
+}
+
+/**
+ * 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
*/
*/
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]);
-
- }
+ 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);
+ (*sentence->callout)(context, gga, context->user_data);
return NMEA_GPGGA;
}
*/
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 = 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]);
- }
+ 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);
+ (*sentence->callout)(context, rmc, context->user_data);
return NMEA_GPRMC;
}
*/
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]) * 100;
- vtg->knot_speed = tokenToInt(context->token[5]) * 100;
- vtg->km_speed = tokenToInt(context->token[7]) * 100;
- }
+ 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);
+ (*sentence->callout)(context, vtg, context->user_data);
return NMEA_GPVTG;
}
*/
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);
+ (*sentence->callout)(context, gsv, context->user_data);
return NMEA_GPGSV;
}
+
+/*
+ * Parse NMEA sentence from a channel.
+ */
void nmea_poll(nmeap_context_t *context, KFile *channel)
{
int c;
while ((c = kfile_getc(channel)) != EOF)
{
- if (nmeap_parse(context, c) == -1)
- break;
+ nmeap_parse(context, c);
}
}
projects / bertos.git / blobdiff