Add server tcp api.
[bertos.git] / bertos / net / nmea.c
index d3937282df9b83b01a26f6c8936c49cfb6b76c56..f74879523e9bcf2f543c1520efc4e374e39e19af 100644 (file)
  * 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>
  *
 #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++;
+               unsigned 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;
 }
 
+/*
+ * Convert a string to micro degree.
+ */
 static udegree_t convertToDegree(const char *str)
 {
        uint32_t dec;
@@ -84,11 +118,9 @@ static udegree_t convertToDegree(const char *str)
        uint32_t min;
 
        if (*str == 0)
-       {
-        return 0;
-    }
+               return 0;
 
-       dec = tokenToInt(str);
+       dec = tokenToInt(str, 4);
        deg = dec / 1000000;
        min = dec - deg * 1000000;
        dec = deg * 1000000 + ((min * 5) + 1) / 3;
@@ -96,77 +128,70 @@ static udegree_t convertToDegree(const char *str)
        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;
-    }
-
+       if (*phem == 0)
+               return 0;
 
-    /* north lat is +, south lat is - */
-    if (*phem == 'N')
-       {
-        ns = 1;
-    }
-    else
-       {
-        ns = -1;
-    }
+       /* north lat is +, south lat is - */
+       ns = (*phem == 'N') ? 1 : -1;
 
 
        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;
 
-    /* west long is negative, east long is positive */
-    if (*phem == 'E')
-       {
-        ew = 1;
-    }
-    else {
-        ew = -1;
-    }
+       if (*phem == 0)
+               return 0;
 
-    if (*phem == 0)
-       {
-        return 0;
-    }
+       /* west long is negative, east long is positive */
+       ew = (*phem == 'E') ? 1 : -1;
 
        return ew * convertToDegree(plot);
 }
 
-static uint16_t nmea_altitude(const char *palt, const char *punits)
+/*
+ * Return altitude in meter from a string.
+ *
+ */
+static int32_t nmea_altitude(const char *palt, const char *punits)
 {
-       uint32_t alt;
+       int32_t alt;
 
        if (*palt == 0)
-       {
-        return 0;
-    }
+               return 0;
 
-       alt = tokenToInt(palt);
+       alt = atoi(palt);
 
-    if (*punits == 'F')
+       if (*punits == 'F')
        {
-        /* convert to feet */
-        /* alt = alt * 3.2808399 */
+               /* 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 */
+                       (alt >> 2) + /* 0.25 */
+                       (alt >> 6) + /* 0.015625 */
+                       (alt >> 7) + /* 0.0078125 */
+                       (alt >> 8); /* 0,00390625 */
 
-    }
+       }
 
        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;
@@ -182,6 +207,7 @@ static time_t timestampToSec(uint32_t time_stamp, uint32_t 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;
@@ -193,9 +219,9 @@ 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.
+       //If we do not have refence data, we set 1/1/1970 as default
        t.tm_mday = 1;
-       t.tm_mon = 1;
+       t.tm_mon = 0;
        t.tm_year = 70;
 
        if (date_stamp)
@@ -210,8 +236,8 @@ static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp)
                }
                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)..
+               // we should specify the number of years from 1900, but the year field
+               // is only two digits, so we add 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);
@@ -219,6 +245,83 @@ static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp)
        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
@@ -230,26 +333,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);
 
@@ -262,35 +361,29 @@ int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence)
 int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
 {
 
-    /*
+       /*
         * get pointer to sentence data
         */
-    NmeaRmc *rmc = (NmeaRmc *)sentence->data;
+       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), 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);
+       if (sentence->callout != 0)
+               (*sentence->callout)(context, rmc, context->user_data);
 
-    return NMEA_GPRMC;
+       return NMEA_GPRMC;
 }
 
 
@@ -300,31 +393,28 @@ int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
 int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
 {
 
-    /*
+       /*
         * get pointer to sentence data
         */
-    NmeaVtg *vtg = (NmeaVtg *)sentence->data;
+       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
         */
-    if (sentence->callout != 0)
-        (*sentence->callout)(context, vtg, context->user_data);
+       if (sentence->callout != 0)
+               (*sentence->callout)(context, vtg, context->user_data);
 
-    return NMEA_GPVTG;
+       return NMEA_GPVTG;
 }
 
 /**
@@ -332,57 +422,53 @@ int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
  */
 int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence)
 {
-
-    /*
+       /*
         * get pointer to sentence data
         */
-    NmeaGsv *gsv = (NmeaGsv *)sentence->data;
+       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);
+       if (sentence->callout != 0)
+               (*sentence->callout)(context, gsv, context->user_data);
 
-    return NMEA_GPGSV;
+       return NMEA_GPGSV;
 }
 
+
+/**
+ * Parse NMEA sentence from a channel.
+ */
 void nmea_poll(nmeap_context_t *context, KFile *channel)
 {
-       int c;
+       int c, e;
        while ((c = kfile_getc(channel)) != EOF)
-       {
                nmeap_parse(context, c);
+
+       if ((e = kfile_error(channel)))
+       {
+               LOG_ERR("ch error [%0X]\n", e);
+               kfile_clearerr(channel);
        }
 }