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 static uint32_t tokenToInt(const char *s, int precision)
  72 {
  73         uint32_t num = 0;
  74         bool sep_found = false;
  75         int i;
  76
  77         if (!s)
  78                 return 0;
  79
  80         for(i = 0; i < NMEAP_MAX_SENTENCE_LENGTH; i++)
  81         {
  82                 char c = *s++;
  83
  84                 if (c == '.')
  85                 {
  86                         sep_found = true;
  87                         continue;
  88                 }
  89
  90                 if (c == '\0' || !isdigit(c) || (precision == 0 && sep_found))
  91                         break;
  92
  93                 if (sep_found)
  94                         precision--;
  95
  96                 num *= 10;
  97                 num += c - '0';
  98         }
  99
 100         while (precision--)
 101                 num *= 10;
 102
 103         return num;
 104 }
 105
 106 static udegree_t convertToDegree(const char *str)
 107 {
 108         uint32_t dec;
 109         uint32_t deg;
 110         uint32_t min;
 111
 112         if (*str == 0)
 113         {
 114         return 0;
 115     }
 116
 117         dec = tokenToInt(str, 4);
 118         deg = dec / 1000000;
 119         min = dec - deg * 1000000;
 120         dec = deg * 1000000 + ((min * 5) + 1) / 3;
 121
 122         return dec;
 123 }
 124
 125 static udegree_t nmea_latitude(const char *plat, const char *phem)
 126 {
 127         int ns;
 128
 129     if (*phem == 0)
 130         return 0;
 131
 132     /* north lat is +, south lat is - */
 133     if (*phem == 'N')
 134         ns = 1;
 135     else
 136         ns = -1;
 137
 138
 139         return ns * convertToDegree(plat);
 140 }
 141
 142 static udegree_t nmea_longitude(const char *plot, const char *phem)
 143 {
 144         int ew;
 145
 146     if (*phem == 0)
 147         return 0;
 148
 149     /* west long is negative, east long is positive */
 150     if (*phem == 'E')
 151         ew = 1;
 152     else
 153         ew = -1;
 154
 155         return ew * convertToDegree(plot);
 156 }
 157
 158 static uint16_t nmea_altitude(const char *palt, const char *punits)
 159 {
 160         uint32_t alt;
 161
 162         if (*palt == 0)
 163         return 0;
 164
 165         alt = atoi(palt);
 166
 167     if (*punits == 'F')
 168         {
 169         /* convert to feet */
 170         /* alt = alt * 3.2808399 */
 171                 alt = alt * 3 +  /* 3.0 */
 172                           (alt >> 2) + /* 0.25 */
 173                           (alt >> 6) + /* 0.015625 */
 174                           (alt >> 7) + /* 0.0078125 */
 175                           (alt >> 8); /* 0,00390625 */
 176
 177     }
 178
 179         return alt;
 180 }
 181
 182 static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp)
 183 {
 184         struct tm t;
 185         uint16_t msec;
 186         uint16_t tmr[3];
 187         uint16_t date[3];
 188
 189         memset(&t, 0, sizeof(t));
 190         memset(&tmr, 0, sizeof(tmr));
 191         memset(&date, 0, sizeof(date));
 192
 193         LOG_INFO("time_s[%lu],date[%lu]\n", (long)time_stamp, (long)date_stamp);
 194         uint32_t res = time_stamp / 1000;
 195         uint32_t all = time_stamp;
 196         msec = all - res * 1000;
 197
 198         for (int i = 0; i < 3; i++)
 199         {
 200                 all = res;
 201                 res = all / 100;
 202                 tmr[i]  = all - res * 100;
 203                 LOG_INFO("t[%d]%d\n", tmr[i],i);
 204         }
 205
 206         t.tm_sec = tmr[0] + (ROUND_UP(msec, 1000) / 1000);
 207         t.tm_min = tmr[1];
 208         t.tm_hour = tmr[2];
 209         //If we not have refence data, we set as default 1/1/1970.
 210         t.tm_mday = 1;
 211         t.tm_mon = 0;
 212         t.tm_year = 70;
 213
 214         if (date_stamp)
 215         {
 216                 res = all = date_stamp;
 217                 for (int i = 0; i < 3; i++)
 218                 {
 219                         all = res;
 220                         res = all / 100;
 221                         date[i]  = all - res * 100;
 222                         LOG_INFO("d[%d]%d\n", date[i],i);
 223                 }
 224                 t.tm_mday = date[2];
 225                 t.tm_mon = date[1] - 1; // time struct count month from 0 to 11;
 226                 // we should specific number of years from 1900, but the year field
 227                 // is only two cipher, so we sum 100 (2000 - 1900)..
 228                 t.tm_year = date[0] + 100;
 229         }
 230         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);
 231
 232         return  mktime(&t);
 233 }
 234
 235 /**
 236  *  Callout example for GGA data
 237  */
 238 void gpgga_callout(nmeap_context_t *context, void *data, void *user_data)
 239 {
 240         (void)context;
 241         (void)user_data;
 242         NmeaGga *gga = (NmeaGga *)data;
 243
 244     LOG_INFO("Found GPGGA message %ld %ld %d %lu %d %d %d %d\n",
 245             (long)gga->latitude,
 246             (long)gga->longitude,
 247             gga->altitude,
 248             gga->time,
 249             gga->satellites,
 250             gga->quality,
 251             gga->hdop,
 252             gga->geoid);
 253 }
 254
 255 /**
 256  * Callout example for RMC
 257  */
 258 void gprmc_callout(nmeap_context_t *context, void *data, void *user_data)
 259 {
 260         (void)context;
 261         (void)user_data;
 262     NmeaRmc *rmc = (NmeaRmc *)data;
 263
 264         LOG_INFO("Found GPRMC message %lu %c %ld %ld %d %d %d\n",
 265             rmc->time,
 266             rmc->warn,
 267             (long)rmc->latitude,
 268             (long)rmc->longitude,
 269             rmc->speed,
 270             rmc->course,
 271             rmc->mag_var);
 272 }
 273
 274 /**
 275  * Callout example for GSV data
 276  */
 277 void gpgsv_callout(nmeap_context_t *context, void *data, void *user_data)
 278 {
 279         (void)context;
 280         (void)user_data;
 281         NmeaGsv *gsv = (NmeaGsv *)data;
 282
 283     LOG_INFO("Found GPGSV message %d %d %d\n", gsv->tot_message, gsv->message_num, gsv->tot_svv);
 284
 285         for (int i = 0; i < 4; i++)
 286             LOG_INFO("%d %d %d %d\n", gsv->info[i].sv_prn, gsv->info[i].elevation, gsv->info[i].azimut, gsv->info[i].snr);
 287 }
 288
 289 /**
 290  * Callout example for VTG data
 291  */
 292 void gpvtg_callout(nmeap_context_t *context, void *data, void *user_data)
 293 {
 294         (void)context;
 295         (void)user_data;
 296         NmeaVtg *vtg = (NmeaVtg *)data;
 297
 298     LOG_INFO("Found GPVTG message %d %d %d\n", vtg->track_good, vtg->knot_speed, vtg->km_speed);
 299 }
 300
 301
 302
 303 /**
 304  * standard GPGGA sentence parser
 305  */
 306 int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence)
 307 {
 308         /*
 309          * get pointer to sentence data
 310          */
 311         NmeaGga *gga = (NmeaGga *)sentence->data;
 312
 313         ASSERT(gga);
 314         ASSERT(context->tokens >= 12);
 315
 316         gga->latitude   = nmea_latitude(context->token[2],context->token[3]);
 317         gga->longitude  = nmea_longitude(context->token[4],context->token[5]);
 318         gga->altitude   = nmea_altitude(context->token[9],context->token[10]);
 319         gga->time       = timestampToSec(tokenToInt(context->token[1], 3), 0);
 320         gga->satellites = atoi(context->token[7]);
 321         gga->quality    = atoi(context->token[6]);
 322         gga->hdop       = tokenToInt(context->token[8], 1);
 323         gga->geoid      = nmea_altitude(context->token[11],context->token[12]);
 324
 325         /*
 326          * if the sentence has a callout, call it
 327          */
 328
 329         if (sentence->callout != 0)
 330                 (*sentence->callout)(context, gga, context->user_data);
 331
 332         return NMEA_GPGGA;
 333 }
 334
 335 /**
 336  * standard GPRMCntence parser
 337  */
 338 int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
 339 {
 340
 341     /*
 342          * get pointer to sentence data
 343          */
 344     NmeaRmc *rmc = (NmeaRmc *)sentence->data;
 345
 346         ASSERT(rmc);
 347         ASSERT(context->tokens >= 10);
 348
 349         /*
 350          * extract data from the tokens
 351          */
 352         rmc->time       = timestampToSec(tokenToInt(context->token[1], 3), tokenToInt(context->token[9], 0));
 353         rmc->warn       = *context->token[2];
 354         rmc->latitude   = nmea_latitude(context->token[3],context->token[4]);
 355         rmc->longitude  = nmea_longitude(context->token[5],context->token[6]);
 356         rmc->speed      = atoi(context->token[7]);
 357         rmc->course     = atoi(context->token[8]);
 358         rmc->mag_var    = atoi(context->token[10]);
 359
 360     if (sentence->callout != 0)
 361         (*sentence->callout)(context, rmc, context->user_data);
 362
 363     return NMEA_GPRMC;
 364 }
 365
 366
 367 /**
 368  * standard GPVTG sentence parser
 369  */
 370 int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
 371 {
 372
 373     /*
 374          * get pointer to sentence data
 375          */
 376     NmeaVtg *vtg = (NmeaVtg *)sentence->data;
 377
 378         ASSERT(vtg);
 379         ASSERT(context->tokens >= 7);
 380
 381         /*
 382          * extract data from the tokens
 383          */
 384         vtg->track_good  = atoi(context->token[1]);
 385         vtg->knot_speed  = atoi(context->token[5]);
 386         vtg->km_speed    = atoi(context->token[7]);
 387
 388     /*
 389          * if the sentence has a callout, call it
 390          */
 391     if (sentence->callout != 0)
 392         (*sentence->callout)(context, vtg, context->user_data);
 393
 394     return NMEA_GPVTG;
 395 }
 396
 397 /**
 398  * standard GPGDSV sentence parser
 399  */
 400 int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence)
 401 {
 402
 403     /*
 404          * get pointer to sentence data
 405          */
 406     NmeaGsv *gsv = (NmeaGsv *)sentence->data;
 407
 408
 409         /*
 410          * extract data from the tokens
 411          */
 412         gsv->tot_message     = atoi(context->token[1]);
 413         gsv->message_num     = atoi(context->token[2]);
 414         gsv->tot_svv         = atoi(context->token[3]);
 415
 416         // Fill remaning member until we have token
 417         int  j = 0;
 418         for (int i = 4; i < context->tokens - 3; i += 4, j++)
 419         {
 420
 421                 gsv->info[j].sv_prn     = atoi(context->token[i]);
 422                 gsv->info[j].elevation  = atoi(context->token[i + 1]);
 423                 gsv->info[j].azimut     = atoi(context->token[i + 2]);
 424                 gsv->info[j].snr        = atoi(context->token[i + 3]);
 425         }
 426
 427     /*
 428          * if the sentence has a callout, call it
 429          */
 430     if (sentence->callout != 0)
 431         (*sentence->callout)(context, gsv, context->user_data);
 432
 433     return NMEA_GPGSV;
 434 }
 435
 436 void nmea_poll(nmeap_context_t *context, KFile *channel)
 437 {
 438         int c;
 439         while ((c = kfile_getc(channel)) != EOF)
 440         {
 441                 nmeap_parse(context, c);
 442         }
 443 }
 444