X-Git-Url: https://codewiz.org/gitweb?a=blobdiff_plain;f=bertos%2Fnet%2Fnmea.c;h=23e6dbcdc8bcbf305d82d4212a2ecd48a7b117dd;hb=5d57564d326be3dc789a616fe3299295683fff07;hp=00c6d076e71a9d6fbc3f72b308a2e6c3dba83119;hpb=c0e8a84a7f0e404ccaab0bb0cb36253f945ea657;p=bertos.git

diff --git a/bertos/net/nmea.c b/bertos/net/nmea.c
index 00c6d076..23e6dbcd 100644
--- a/bertos/net/nmea.c
+++ b/bertos/net/nmea.c
@@ -33,6 +33,7 @@
  *
  * \author Daniele Basile <asterix@develer.com>
  *
+ * notest:avr
  */
 
 #include "nmea.h"
@@ -50,98 +51,94 @@
 #include <ctype.h>
 #include <time.h>
 #include <string.h>
+#include <stdlib.h>
 
 
-static uint32_t tokenToInt(const char *s)
+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)
+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;
+}
+
+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;
-    }
 
-	lat = tokenToInt(plat);
-	deg = lat / 1000000;
-	min = lat - deg * 1000000;
-	lat = deg * 1000000 + ((min * 5) + 1) / 3;
 
-	return ns * lat;
+	return ns * convertToDegree(plat);
 }
 
 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 {
+    else
         ew = -1;
-    }
-
-	lot = tokenToInt(plot);
-	deg = lot / 1000000;
-	min = lot - deg * 1000000;
-	lot = deg * 1000000 + ((min * 5) + 1) / 3;
 
-	return ew  * lot;
+	return ew * convertToDegree(plot);
 }
 
 static uint16_t nmea_altitude(const char *palt, const char *punits)
@@ -149,11 +146,9 @@ 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')
 	{
@@ -181,7 +176,7 @@ static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp)
 	memset(&tmr, 0, sizeof(tmr));
 	memset(&date, 0, sizeof(date));
 
-	LOG_INFO("time_s[%u],date[%u]\n", time_stamp, date_stamp);
+	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;
@@ -196,6 +191,7 @@ 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.
 	t.tm_mday = 1;
 	t.tm_mon = 1;
 	t.tm_year = 70;
@@ -232,26 +228,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);
 
@@ -269,26 +261,20 @@ int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
 	 */
     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), atoi(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      = 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);
 
@@ -307,18 +293,15 @@ int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
 	 */
     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
@@ -340,41 +323,30 @@ int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence)
 	 */
     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;
 }
@@ -384,8 +356,7 @@ 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);
 	}
 }