bertos/net/nmea.c

   1 /**
   2  * \file
   3  * <!--
   4  * This file is part of BeRTOS.
   5  *
   6  * Bertos is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, write to the Free Software
  18  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19  *
  20  * As a special exception, you may use this file as part of a free software
  21  * library without restriction.  Specifically, if other files instantiate
  22  * templates or use macros or inline functions from this file, or you compile
  23  * this file and link it with other files to produce an executable, this
  24  * file does not by itself cause the resulting executable to be covered by
  25  * the GNU General Public License.  This exception does not however
  26  * invalidate any other reasons why the executable file might be covered by
  27  * the GNU General Public License.
  28  *
  29  * Copyright 2009 Develer S.r.l. (http://www.develer.com/)
  30  *
  31  * -->
  32  *
  33  * \brief NMEA parser implementation.
  34  *
  35  * NMEA 0183 is acronym of National Marine Electronics Association that
  36  * combined electrical and data specification for communication between marine
  37  * electronic devices such as echo sounder, sonars, anemometer (wind speed and direction),
  38  * gyrocompass, autopilot, GPS receivers and many other types of instruments.
  39  * It has been defined by, and is controlled by, the U.S.-based National Marine
  40  * Electronics Association.
  41  * The NMEA 0183 standard uses a simple ASCII, serial communications protocol
  42  * that defines how data is transmitted in a "sentence" from one "talker"
  43  * to multiple "listeners" at a time.
  44  * At the application layer, the standard also defines the contents of each sentence
  45  * (message) type so that all listeners can parse messages accurately.
  46  *
  47  *
  48  * \author Daniele Basile <asterix@develer.com>
  49  *
  50  * notest:avr
  51  */
  52
  53 #include "nmea.h"
  54
  55 #include "cfg/cfg_nmea.h"
  56
  57 #include <cfg/debug.h>
  58
  59 #define LOG_LEVEL  NMEA_LOG_LEVEL
  60 #define LOG_FORMAT NMEA_LOG_FORMAT
  61 #include <cfg/log.h>
  62
  63 #include <net/nmeap/inc/nmeap.h>
  64
  65 #include <ctype.h>
  66 #include <time.h>
  67 #include <string.h>
  68 #include <stdlib.h>
  69
  70 /*
  71  * Make conversion from one string to int.
  72  *
  73  * You can specify the precision if the string is a float
  74  * number. The result is an int multiplied to 10^precision.
  75  */
  76 static uint32_t tokenToInt(const char *s, int precision)
  77 {
  78         uint32_t num = 0;
  79         bool sep_found = false;
  80         int i;
  81
  82         if (!s)
  83                 return 0;
  84
  85         for(i = 0; i < NMEAP_MAX_SENTENCE_LENGTH; i++)
  86         {
  87                 unsigned char c = *s++;
  88
  89                 if (c == '.')
  90                 {
  91                         sep_found = true;
  92                         continue;
  93                 }
  94
  95                 if (c == '\0' || !isdigit(c) || (precision == 0 && sep_found))
  96                         break;
  97
  98                 if (sep_found)
  99                         precision--;
 100
 101                 num *= 10;
 102                 num += c - '0';
 103         }
 104
 105         while (precision--)
 106                 num *= 10;
 107
 108         return num;
 109 }
 110
 111 /*
 112  * Convert a string to micro degree.
 113  */
 114 static udegree_t convertToDegree(const char *str)
 115 {
 116         uint32_t dec;
 117         uint32_t deg;
 118         uint32_t min;
 119
 120         if (*str == 0)
 121                 return 0;
 122
 123         dec = tokenToInt(str, 4);
 124         deg = dec / 1000000;
 125         min = dec - deg * 1000000;
 126         dec = deg * 1000000 + ((min * 5) + 1) / 3;
 127
 128         return dec;
 129 }
 130
 131 /*
 132  * Retun latitude in micro degree from a string.
 133  */
 134 static udegree_t nmea_latitude(const char *plat, const char *phem)
 135 {
 136         int ns;
 137
 138         if (*phem == 0)
 139                 return 0;
 140
 141         /* north lat is +, south lat is - */
 142         ns = (*phem == 'N') ? 1 : -1;
 143
 144
 145         return ns * convertToDegree(plat);
 146 }
 147
 148 /*
 149  * Retun longitude in micro degree from a string.
 150  */
 151 static udegree_t nmea_longitude(const char *plot, const char *phem)
 152 {
 153         int ew;
 154
 155         if (*phem == 0)
 156                 return 0;
 157
 158         /* west long is negative, east long is positive */
 159         ew = (*phem == 'E') ? 1 : -1;
 160
 161         return ew * convertToDegree(plot);
 162 }
 163
 164 /*
 165  * Return altitude in meter from a string.
 166  *
 167  */
 168 static int32_t nmea_altitude(const char *palt, const char *punits)
 169 {
 170         int32_t alt;
 171
 172         if (*palt == 0)
 173                 return 0;
 174
 175         alt = atoi(palt);
 176
 177         if (*punits == 'F')
 178         {
 179                 /* convert to feet */
 180                 /* alt = alt * 3.2808399 */
 181                 alt = alt * 3 +  /* 3.0 */
 182                         (alt >> 2) + /* 0.25 */
 183                         (alt >> 6) + /* 0.015625 */
 184                         (alt >> 7) + /* 0.0078125 */
 185                         (alt >> 8); /* 0,00390625 */
 186
 187         }
 188
 189         return alt;
 190 }
 191
 192 /*
 193  * Convert time and date stamp string to unix time.
 194  */
 195 static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp)
 196 {
 197         struct tm t;
 198         uint16_t msec;
 199         uint16_t tmr[3];
 200         uint16_t date[3];
 201
 202         memset(&t, 0, sizeof(t));
 203         memset(&tmr, 0, sizeof(tmr));
 204         memset(&date, 0, sizeof(date));
 205
 206         LOG_INFO("time_s[%lu],date[%lu]\n", (long)time_stamp, (long)date_stamp);
 207         uint32_t res = time_stamp / 1000;
 208         uint32_t all = time_stamp;
 209         msec = all - res * 1000;
 210
 211         for (int i = 0; i < 3; i++)
 212         {
 213                 all = res;
 214                 res = all / 100;
 215                 tmr[i]  = all - res * 100;
 216                 LOG_INFO("t[%d]%d\n", tmr[i],i);
 217         }
 218
 219         t.tm_sec = tmr[0] + (ROUND_UP(msec, 1000) / 1000);
 220         t.tm_min = tmr[1];
 221         t.tm_hour = tmr[2];
 222         //If we do not have refence data, we set 1/1/1970 as default
 223         t.tm_mday = 1;
 224         t.tm_mon = 0;
 225         t.tm_year = 70;
 226
 227         if (date_stamp)
 228         {
 229                 res = all = date_stamp;
 230                 for (int i = 0; i < 3; i++)
 231                 {
 232                         all = res;
 233                         res = all / 100;
 234                         date[i]  = all - res * 100;
 235                         LOG_INFO("d[%d]%d\n", date[i],i);
 236                 }
 237                 t.tm_mday = date[2];
 238                 t.tm_mon = date[1] - 1; // time struct count month from 0 to 11;
 239                 // we should specify the number of years from 1900, but the year field
 240                 // is only two digits, so we add 100 (2000 - 1900)..
 241                 t.tm_year = date[0] + 100;
 242         }
 243         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);
 244
 245         return  mktime(&t);
 246 }
 247
 248 /**
 249  *  Callout example for GGA data
 250  */
 251 void gpgga_callout(nmeap_context_t *context, void *data, void *user_data)
 252 {
 253         (void)context;
 254         (void)user_data;
 255         (void)data;
 256         LOG_INFOB(
 257                 NmeaGga *gga = (NmeaGga *)data;
 258                 LOG_INFO("Found GPGGA message %ld %ld %d %lu %d %d %d %d\n",
 259                         (long)gga->latitude,
 260                         (long)gga->longitude,
 261                         gga->altitude,
 262                         gga->time,
 263                         gga->satellites,
 264                         gga->quality,
 265                         gga->hdop,
 266                         gga->geoid);
 267         );
 268 }
 269
 270 /**
 271  * Callout example for RMC
 272  */
 273 void gprmc_callout(nmeap_context_t *context, void *data, void *user_data)
 274 {
 275         (void)context;
 276         (void)user_data;
 277         (void)data;
 278         LOG_INFOB(
 279                 NmeaRmc *rmc = (NmeaRmc *)data;
 280
 281                 LOG_INFO("Found GPRMC message %lu %c %ld %ld %d %d %d\n",
 282                         rmc->time,
 283                         rmc->warn,
 284                         (long)rmc->latitude,
 285                         (long)rmc->longitude,
 286                         rmc->speed,
 287                         rmc->course,
 288                         rmc->mag_var);
 289         );
 290 }
 291
 292 /**
 293  * Callout example for GSV data
 294  */
 295 void gpgsv_callout(nmeap_context_t *context, void *data, void *user_data)
 296 {
 297         (void)context;
 298         (void)user_data;
 299         (void)data;
 300         LOG_INFOB(
 301                 NmeaGsv *gsv = (NmeaGsv *)data;
 302
 303                 LOG_INFO("Found GPGSV message %d %d %d\n", gsv->tot_message, gsv->message_num, gsv->tot_svv);
 304
 305                 for (int i = 0; i < 4; i++)
 306                         LOG_INFO("%d %d %d %d\n", gsv->info[i].sv_prn, gsv->info[i].elevation, gsv->info[i].azimut, gsv->info[i].snr);
 307         );
 308 }
 309
 310 /**
 311  * Callout example for VTG data
 312  */
 313 void gpvtg_callout(nmeap_context_t *context, void *data, void *user_data)
 314 {
 315         (void)context;
 316         (void)user_data;
 317         (void)data;
 318         LOG_INFOB(
 319                 NmeaVtg *vtg = (NmeaVtg *)data;
 320                 LOG_INFO("Found GPVTG message %d %d %d\n", vtg->track_good,     vtg->knot_speed, vtg->km_speed);
 321         );
 322 }
 323
 324
 325
 326 /**
 327  * standard GPGGA sentence parser
 328  */
 329 int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence)
 330 {
 331         /*
 332          * get pointer to sentence data
 333          */
 334         NmeaGga *gga = (NmeaGga *)sentence->data;
 335
 336         ASSERT(gga);
 337         ASSERT(context->tokens >= 12);
 338
 339         gga->latitude   = nmea_latitude(context->token[2],context->token[3]);
 340         gga->longitude  = nmea_longitude(context->token[4],context->token[5]);
 341         gga->altitude   = nmea_altitude(context->token[9],context->token[10]);
 342         gga->time       = timestampToSec(tokenToInt(context->token[1], 3), 0);
 343         gga->satellites = atoi(context->token[7]);
 344         gga->quality    = atoi(context->token[6]);
 345         gga->hdop       = tokenToInt(context->token[8], 1);
 346         gga->geoid      = nmea_altitude(context->token[11],context->token[12]);
 347
 348         /*
 349          * if the sentence has a callout, call it
 350          */
 351
 352         if (sentence->callout != 0)
 353                 (*sentence->callout)(context, gga, context->user_data);
 354
 355         return NMEA_GPGGA;
 356 }
 357
 358 /**
 359  * standard GPRMCntence parser
 360  */
 361 int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
 362 {
 363
 364         /*
 365          * get pointer to sentence data
 366          */
 367         NmeaRmc *rmc = (NmeaRmc *)sentence->data;
 368
 369         ASSERT(rmc);
 370         ASSERT(context->tokens >= 10);
 371
 372         /*
 373          * extract data from the tokens
 374          */
 375         rmc->time       = timestampToSec(tokenToInt(context->token[1], 3), tokenToInt(context->token[9], 0));
 376         rmc->warn       = *context->token[2];
 377         rmc->latitude   = nmea_latitude(context->token[3],context->token[4]);
 378         rmc->longitude  = nmea_longitude(context->token[5],context->token[6]);
 379         rmc->speed      = atoi(context->token[7]);
 380         rmc->course     = atoi(context->token[8]);
 381         rmc->mag_var    = atoi(context->token[10]);
 382
 383         if (sentence->callout != 0)
 384                 (*sentence->callout)(context, rmc, context->user_data);
 385
 386         return NMEA_GPRMC;
 387 }
 388
 389
 390 /**
 391  * standard GPVTG sentence parser
 392  */
 393 int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
 394 {
 395
 396         /*
 397          * get pointer to sentence data
 398          */
 399         NmeaVtg *vtg = (NmeaVtg *)sentence->data;
 400
 401         ASSERT(vtg);
 402         ASSERT(context->tokens >= 7);
 403
 404         /*
 405          * extract data from the tokens
 406          */
 407         vtg->track_good  = atoi(context->token[1]);
 408         vtg->knot_speed  = atoi(context->token[5]);
 409         vtg->km_speed    = atoi(context->token[7]);
 410
 411         /*
 412          * if the sentence has a callout, call it
 413          */
 414         if (sentence->callout != 0)
 415                 (*sentence->callout)(context, vtg, context->user_data);
 416
 417         return NMEA_GPVTG;
 418 }
 419
 420 /**
 421  * standard GPGDSV sentence parser
 422  */
 423 int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence)
 424 {
 425         /*
 426          * get pointer to sentence data
 427          */
 428         NmeaGsv *gsv = (NmeaGsv *)sentence->data;
 429
 430
 431         /*
 432          * extract data from the tokens
 433          */
 434         gsv->tot_message     = atoi(context->token[1]);
 435         gsv->message_num     = atoi(context->token[2]);
 436         gsv->tot_svv         = atoi(context->token[3]);
 437
 438         // Fill remaning member until we have token
 439         int  j = 0;
 440         for (int i = 4; i < context->tokens - 3; i += 4, j++)
 441         {
 442
 443                 gsv->info[j].sv_prn     = atoi(context->token[i]);
 444                 gsv->info[j].elevation  = atoi(context->token[i + 1]);
 445                 gsv->info[j].azimut     = atoi(context->token[i + 2]);
 446                 gsv->info[j].snr        = atoi(context->token[i + 3]);
 447         }
 448
 449         /*
 450          * if the sentence has a callout, call it
 451          */
 452         if (sentence->callout != 0)
 453                 (*sentence->callout)(context, gsv, context->user_data);
 454
 455         return NMEA_GPGSV;
 456 }
 457
 458
 459 /**
 460  * Parse NMEA sentence from a channel.
 461  */
 462 void nmea_poll(nmeap_context_t *context, KFile *channel)
 463 {
 464         int c, e;
 465         while ((c = kfile_getc(channel)) != EOF)
 466                 nmeap_parse(context, c);
 467
 468         if ((e = kfile_error(channel)))
 469         {
 470                 LOG_ERR("ch error [%0X]\n", e);
 471                 kfile_clearerr(channel);
 472         }
 473 }
 474