BeRTOS
nmea.c
Go to the documentation of this file.
00001 
00053 #include "nmea.h"
00054 
00055 #include "cfg/cfg_nmea.h"
00056 
00057 #include <cfg/debug.h>
00058 
00059 #define LOG_LEVEL  NMEA_LOG_LEVEL
00060 #define LOG_FORMAT NMEA_LOG_FORMAT
00061 #include <cfg/log.h>
00062 
00063 #include <net/nmeap/inc/nmeap.h>
00064 
00065 #include <ctype.h>
00066 #include <time.h>
00067 #include <string.h>
00068 #include <stdlib.h>
00069 
00070 /*
00071  * Make conversion from one string to int.
00072  *
00073  * You can specify the precision if the string is a float
00074  * number. The result is an int multiplied to 10^precision.
00075  */
00076 static uint32_t tokenToInt(const char *s, int precision)
00077 {
00078     uint32_t num = 0;
00079     bool sep_found = false;
00080     int i;
00081 
00082     if (!s)
00083         return 0;
00084 
00085     for(i = 0; i < NMEAP_MAX_SENTENCE_LENGTH; i++)
00086     {
00087         unsigned char c = *s++;
00088 
00089         if (c == '.')
00090         {
00091             sep_found = true;
00092             continue;
00093         }
00094 
00095         if (c == '\0' || !isdigit(c) || (precision == 0 && sep_found))
00096             break;
00097 
00098         if (sep_found)
00099             precision--;
00100 
00101         num *= 10;
00102         num += c - '0';
00103     }
00104 
00105     while (precision--)
00106         num *= 10;
00107 
00108     return num;
00109 }
00110 
00111 /*
00112  * Convert a string to micro degree.
00113  */
00114 static udegree_t convertToDegree(const char *str)
00115 {
00116     uint32_t dec;
00117     uint32_t deg;
00118     uint32_t min;
00119 
00120     if (*str == 0)
00121         return 0;
00122 
00123     dec = tokenToInt(str, 4);
00124     deg = dec / 1000000;
00125     min = dec - deg * 1000000;
00126     dec = deg * 1000000 + ((min * 5) + 1) / 3;
00127 
00128     return dec;
00129 }
00130 
00131 /*
00132  * Retun latitude in micro degree from a string.
00133  */
00134 static udegree_t nmea_latitude(const char *plat, const char *phem)
00135 {
00136     int ns;
00137 
00138     if (*phem == 0)
00139         return 0;
00140 
00141     /* north lat is +, south lat is - */
00142     ns = (*phem == 'N') ? 1 : -1;
00143 
00144 
00145     return ns * convertToDegree(plat);
00146 }
00147 
00148 /*
00149  * Retun longitude in micro degree from a string.
00150  */
00151 static udegree_t nmea_longitude(const char *plot, const char *phem)
00152 {
00153     int ew;
00154 
00155     if (*phem == 0)
00156         return 0;
00157 
00158     /* west long is negative, east long is positive */
00159     ew = (*phem == 'E') ? 1 : -1;
00160 
00161     return ew * convertToDegree(plot);
00162 }
00163 
00164 /*
00165  * Return altitude in meter from a string.
00166  *
00167  */
00168 static int32_t nmea_altitude(const char *palt, const char *punits)
00169 {
00170     int32_t alt;
00171 
00172     if (*palt == 0)
00173         return 0;
00174 
00175     alt = atoi(palt);
00176 
00177     if (*punits == 'F')
00178     {
00179         /* convert to feet */
00180         /* alt = alt * 3.2808399 */
00181         alt = alt * 3 +  /* 3.0 */
00182             (alt >> 2) + /* 0.25 */
00183             (alt >> 6) + /* 0.015625 */
00184             (alt >> 7) + /* 0.0078125 */
00185             (alt >> 8); /* 0,00390625 */
00186 
00187     }
00188 
00189     return alt;
00190 }
00191 
00192 /*
00193  * Convert time and date stamp string to unix time.
00194  */
00195 static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp)
00196 {
00197     struct tm t;
00198     uint16_t msec;
00199     uint16_t tmr[3];
00200     uint16_t date[3];
00201 
00202     memset(&t, 0, sizeof(t));
00203     memset(&tmr, 0, sizeof(tmr));
00204     memset(&date, 0, sizeof(date));
00205 
00206     LOG_INFO("time_s[%lu],date[%lu]\n", (long)time_stamp, (long)date_stamp);
00207     uint32_t res = time_stamp / 1000;
00208     uint32_t all = time_stamp;
00209     msec = all - res * 1000;
00210 
00211     for (int i = 0; i < 3; i++)
00212     {
00213         all = res;
00214         res = all / 100;
00215         tmr[i]  = all - res * 100;
00216         LOG_INFO("t[%d]%d\n", tmr[i],i);
00217     }
00218 
00219     t.tm_sec = tmr[0] + (ROUND_UP(msec, 1000) / 1000);
00220     t.tm_min = tmr[1];
00221     t.tm_hour = tmr[2];
00222     //If we do not have refence data, we set 1/1/1970 as default
00223     t.tm_mday = 1;
00224     t.tm_mon = 0;
00225     t.tm_year = 70;
00226 
00227     if (date_stamp)
00228     {
00229         res = all = date_stamp;
00230         for (int i = 0; i < 3; i++)
00231         {
00232             all = res;
00233             res = all / 100;
00234             date[i]  = all - res * 100;
00235             LOG_INFO("d[%d]%d\n", date[i],i);
00236         }
00237         t.tm_mday = date[2];
00238         t.tm_mon = date[1] - 1; // time struct count month from 0 to 11;
00239         // we should specify the number of years from 1900, but the year field
00240         // is only two digits, so we add 100 (2000 - 1900)..
00241         t.tm_year = date[0] + 100;
00242     }
00243     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);
00244 
00245     return  mktime(&t);
00246 }
00247 
00251 void gpgga_callout(nmeap_context_t *context, void *data, void *user_data)
00252 {
00253     (void)context;
00254     (void)user_data;
00255     (void)data;
00256     LOG_INFOB(
00257         NmeaGga *gga = (NmeaGga *)data;
00258         LOG_INFO("Found GPGGA message %ld %ld %d %lu %d %d %d %d\n",
00259             (long)gga->latitude,
00260             (long)gga->longitude,
00261             gga->altitude,
00262             gga->time,
00263             gga->satellites,
00264             gga->quality,
00265             gga->hdop,
00266             gga->geoid);
00267     );
00268 }
00269 
00273 void gprmc_callout(nmeap_context_t *context, void *data, void *user_data)
00274 {
00275     (void)context;
00276     (void)user_data;
00277     (void)data;
00278     LOG_INFOB(
00279         NmeaRmc *rmc = (NmeaRmc *)data;
00280 
00281         LOG_INFO("Found GPRMC message %lu %c %ld %ld %d %d %d\n",
00282             rmc->time,
00283             rmc->warn,
00284             (long)rmc->latitude,
00285             (long)rmc->longitude,
00286             rmc->speed,
00287             rmc->course,
00288             rmc->mag_var);
00289     );
00290 }
00291 
00295 void gpgsv_callout(nmeap_context_t *context, void *data, void *user_data)
00296 {
00297     (void)context;
00298     (void)user_data;
00299     (void)data;
00300     LOG_INFOB(
00301         NmeaGsv *gsv = (NmeaGsv *)data;
00302 
00303         LOG_INFO("Found GPGSV message %d %d %d\n", gsv->tot_message, gsv->message_num, gsv->tot_svv);
00304 
00305         for (int i = 0; i < 4; i++)
00306             LOG_INFO("%d %d %d %d\n", gsv->info[i].sv_prn, gsv->info[i].elevation, gsv->info[i].azimut, gsv->info[i].snr);
00307     );
00308 }
00309 
00313 void gpvtg_callout(nmeap_context_t *context, void *data, void *user_data)
00314 {
00315     (void)context;
00316     (void)user_data;
00317     (void)data;
00318     LOG_INFOB(
00319         NmeaVtg *vtg = (NmeaVtg *)data;
00320         LOG_INFO("Found GPVTG message %d %d %d\n", vtg->track_good, vtg->knot_speed, vtg->km_speed);
00321     );
00322 }
00323 
00324 
00325 
00329 int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence)
00330 {
00331     /*
00332      * get pointer to sentence data
00333      */
00334     NmeaGga *gga = (NmeaGga *)sentence->data;
00335 
00336     ASSERT(gga);
00337     ASSERT(context->tokens >= 12);
00338 
00339     gga->latitude   = nmea_latitude(context->token[2],context->token[3]);
00340     gga->longitude  = nmea_longitude(context->token[4],context->token[5]);
00341     gga->altitude   = nmea_altitude(context->token[9],context->token[10]);
00342     gga->time       = timestampToSec(tokenToInt(context->token[1], 3), 0);
00343     gga->satellites = atoi(context->token[7]);
00344     gga->quality    = atoi(context->token[6]);
00345     gga->hdop       = tokenToInt(context->token[8], 1);
00346     gga->geoid      = nmea_altitude(context->token[11],context->token[12]);
00347 
00348     /*
00349      * if the sentence has a callout, call it
00350      */
00351 
00352     if (sentence->callout != 0)
00353         (*sentence->callout)(context, gga, context->user_data);
00354 
00355     return NMEA_GPGGA;
00356 }
00357 
00361 int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
00362 {
00363 
00364     /*
00365      * get pointer to sentence data
00366      */
00367     NmeaRmc *rmc = (NmeaRmc *)sentence->data;
00368 
00369     ASSERT(rmc);
00370     ASSERT(context->tokens >= 10);
00371 
00372     /*
00373      * extract data from the tokens
00374      */
00375     rmc->time       = timestampToSec(tokenToInt(context->token[1], 3), tokenToInt(context->token[9], 0));
00376     rmc->warn       = *context->token[2];
00377     rmc->latitude   = nmea_latitude(context->token[3],context->token[4]);
00378     rmc->longitude  = nmea_longitude(context->token[5],context->token[6]);
00379     rmc->speed      = atoi(context->token[7]);
00380     rmc->course     = atoi(context->token[8]);
00381     rmc->mag_var    = atoi(context->token[10]);
00382 
00383     if (sentence->callout != 0)
00384         (*sentence->callout)(context, rmc, context->user_data);
00385 
00386     return NMEA_GPRMC;
00387 }
00388 
00389 
00393 int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
00394 {
00395 
00396     /*
00397      * get pointer to sentence data
00398      */
00399     NmeaVtg *vtg = (NmeaVtg *)sentence->data;
00400 
00401     ASSERT(vtg);
00402     ASSERT(context->tokens >= 7);
00403 
00404     /*
00405      * extract data from the tokens
00406      */
00407     vtg->track_good  = atoi(context->token[1]);
00408     vtg->knot_speed  = atoi(context->token[5]);
00409     vtg->km_speed    = atoi(context->token[7]);
00410 
00411     /*
00412      * if the sentence has a callout, call it
00413      */
00414     if (sentence->callout != 0)
00415         (*sentence->callout)(context, vtg, context->user_data);
00416 
00417     return NMEA_GPVTG;
00418 }
00419 
00423 int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence)
00424 {
00425     /*
00426      * get pointer to sentence data
00427      */
00428     NmeaGsv *gsv = (NmeaGsv *)sentence->data;
00429 
00430 
00431     /*
00432      * extract data from the tokens
00433      */
00434     gsv->tot_message     = atoi(context->token[1]);
00435     gsv->message_num     = atoi(context->token[2]);
00436     gsv->tot_svv         = atoi(context->token[3]);
00437 
00438     // Fill remaning member until we have token
00439     int  j = 0;
00440     for (int i = 4; i < context->tokens - 3; i += 4, j++)
00441     {
00442 
00443         gsv->info[j].sv_prn     = atoi(context->token[i]);
00444         gsv->info[j].elevation  = atoi(context->token[i + 1]);
00445         gsv->info[j].azimut     = atoi(context->token[i + 2]);
00446         gsv->info[j].snr        = atoi(context->token[i + 3]);
00447     }
00448 
00449     /*
00450      * if the sentence has a callout, call it
00451      */
00452     if (sentence->callout != 0)
00453         (*sentence->callout)(context, gsv, context->user_data);
00454 
00455     return NMEA_GPGSV;
00456 }
00457 
00458 
00462 void nmea_poll(nmeap_context_t *context, KFile *channel)
00463 {
00464     int c, e;
00465     while ((c = kfile_getc(channel)) != EOF)
00466         nmeap_parse(context, c);
00467 
00468     if ((e = kfile_error(channel)))
00469     {
00470         LOG_ERR("ch error [%0X]\n", e);
00471         kfile_clearerr(channel);
00472     }
00473 }
00474