projects / bertos.git / blobdiff
? search:
summary | shortlog | log | commit | commitdiff | tree
raw | inline | side by side
Reformat code.
[bertos.git] / bertos / net / nmea.c
diff --git a/bertos/net/nmea.c b/bertos/net/nmea.c
index a9684a9ec126a3803953fcabf1501b95230672f8..93985e79b3db0456d29c6d98fa7d31c871606556 100644 (file)
--- 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,29 +51,41 @@
 #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;
 }
 
@@ -87,10 +100,12 @@ static udegree_t convertToDegree(const char *str)
         return 0;
     }
 
-       dec = tokenToInt(str);
+       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)
@@ -98,20 +113,13 @@ 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;
-    }
 
 
        return ns * convertToDegree(plat);
@@ -121,19 +129,14 @@ static udegree_t nmea_longitude(const char *plot, const char *phem)
 {
        int ew;
 
+    if (*phem == 0)
+        return 0;
+
     /* west long is negative, east long is positive */
     if (*phem == 'E')
-       {
         ew = 1;
-    }
-    else {
+    else
         ew = -1;
-    }
-
-    if (*phem == 0)
-       {
-        return 0;
-    }
 
        return ew * convertToDegree(plot);
 }
@@ -143,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')
        {
@@ -175,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;
@@ -227,26 +228,24 @@ int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence)
         */
        NmeaGga *gga = (NmeaGga *)sentence->data;
 
+       ASSERT(gga);
+
        /*
         * 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]);
-
-       }
-
+       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);
 
@@ -272,13 +271,13 @@ int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
                /*
                 * extract data from the tokens
                 */
-               rmc->time       = timestampToSec(tokenToInt(context->token[1]), tokenToInt(context->token[9]));
+               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      = tokenToInt(context->token[7]);
-               rmc->course     = tokenToInt(context->token[8]);
-               rmc->mag_var    = tokenToInt(context->token[10]);
+               rmc->speed      = atoi(context->token[7]);
+               rmc->course     = atoi(context->token[8]);
+               rmc->mag_var    = atoi(context->token[10]);
        }
 
     /*
@@ -310,9 +309,9 @@ int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
                /*
                 * 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]);
        }
 
     /*
@@ -343,25 +342,25 @@ int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence)
                /*
                 * 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->tot_message     = atoi(context->token[1]);
+               gsv->message_num     = atoi(context->token[2]);
+               gsv->tot_svv         = atoi(context->token[3]);
+               gsv->sv_prn          = atoi(context->token[4]);
+               gsv->elevation       = atoi(context->token[5]);
+               gsv->azimut          = atoi(context->token[6]);
+               gsv->snr             = atoi(context->token[7]);
+               gsv->sv_prn2         = atoi(context->token[8]);
+               gsv->elevation2      = atoi(context->token[9]);
+               gsv->azimut2         = atoi(context->token[10]);
+               gsv->snr2            = atoi(context->token[11]);
+               gsv->sv_prn3         = atoi(context->token[12]);
+               gsv->elevation3      = atoi(context->token[13]);
+               gsv->azimut3         = atoi(context->token[14]);
+               gsv->snr3            = atoi(context->token[15]);
+               gsv->sv_prn4         = atoi(context->token[16]);
+               gsv->elevation4      = atoi(context->token[17]);
+               gsv->azimut4         = atoi(context->token[18]);
+               gsv->snr4            = atoi(context->token[19]);
        }
 
 
A free, realtime operating system for embedded devices - http://bertos.org/