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  * \brief NMEA implementation.
  33  *
  34  * \author Daniele Basile <asterix@develer.com>
  35  *
  36  */
  37
  38 #include "nmea.h"
  39
  40 #include "cfg/cfg_nmea.h"
  41
  42 #include <cfg/debug.h>
  43
  44 #define LOG_LEVEL  NMEA_LOG_LEVEL
  45 #define LOG_FORMAT NMEA_LOG_FORMAT
  46 #include <cfg/log.h>
  47
  48 #include <net/nmeap/inc/nmeap.h>
  49
  50 #include <ctype.h>
  51 #include <time.h>
  52 #include <string.h>
  53
  54
  55 static uint32_t tokenToInt(const char *s)
  56 {
  57         uint32_t num = 0;
  58         int i;
  59
  60         ASSERT(s);
  61
  62         for(i = 0; i < NMEAP_MAX_TOKENS; i++)
  63         {
  64                 char c = *s++;
  65
  66                 if (c == '.')
  67                         continue;
  68
  69                 if (c == '\0' || !isdigit(c))
  70                         break;
  71
  72                 num *= 10;
  73                 num += c - '0';
  74         }
  75
  76         return num;
  77 }
  78
  79
  80 static udegree_t nmea_latitude(const char *plat, const char *phem)
  81 {
  82         int ns;
  83         uint32_t lat;
  84         uint32_t deg;
  85         uint32_t min;
  86
  87         if (*plat == 0)
  88         {
  89         return 0;
  90     }
  91     if (*phem == 0)
  92         {
  93         return 0;
  94     }
  95
  96     /* north lat is +, south lat is - */
  97     if (*phem == 'N')
  98         {
  99         ns = 1;
 100     }
 101     else
 102         {
 103         ns = -1;
 104     }
 105
 106         lat = tokenToInt(plat);
 107         deg = lat / 1000000;
 108         min = lat - deg * 1000000;
 109         lat = deg * 1000000 + ((min * 5) + 1) / 3;
 110
 111         return ns * lat;
 112 }
 113
 114 static udegree_t nmea_longitude(const char *plot, const char *phem)
 115 {
 116         int ew;
 117         uint32_t lot;
 118         uint32_t deg;
 119         uint32_t min;
 120
 121         if (*plot == 0)
 122         {
 123         return 0;
 124     }
 125     if (*phem == 0)
 126         {
 127         return 0;
 128     }
 129
 130     /* west long is negative, east long is positive */
 131     if (*phem == 'E')
 132         {
 133         ew = 1;
 134     }
 135     else {
 136         ew = -1;
 137     }
 138
 139         lot = tokenToInt(plot);
 140         deg = lot / 1000000;
 141         min = lot - deg * 1000000;
 142         lot = deg * 1000000 + ((min * 5) + 1) / 3;
 143
 144         return ew  * lot;
 145 }
 146
 147 static uint16_t nmea_altitude(const char *palt, const char *punits)
 148 {
 149         uint32_t alt;
 150
 151         if (*palt == 0)
 152         {
 153         return 0;
 154     }
 155
 156         alt = tokenToInt(palt);
 157
 158     if (*punits == 'F')
 159         {
 160         /* convert to feet */
 161         /* alt = alt * 3.2808399 */
 162                 alt = alt * 3 +  /* 3.0 */
 163                           (alt >> 2) + /* 0.25 */
 164                           (alt >> 6) + /* 0.015625 */
 165                           (alt >> 7) + /* 0.0078125 */
 166                           (alt >> 8); /* 0,00390625 */
 167
 168     }
 169
 170         return alt;
 171 }
 172
 173 static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp)
 174 {
 175         struct tm t;
 176         uint16_t msec;
 177         uint16_t tmr[3];
 178         uint16_t date[3];
 179
 180         memset(&t, 0, sizeof(t));
 181         memset(&tmr, 0, sizeof(tmr));
 182         memset(&date, 0, sizeof(date));
 183
 184         LOG_INFO("time_s[%u],date[%u]\n", time_stamp, date_stamp);
 185         uint32_t res = time_stamp / 1000;
 186         uint32_t all = time_stamp;
 187         msec = all - res * 1000;
 188         for (int i = 0; i < 3; i++)
 189         {
 190                 all = res;
 191                 res = all / 100;
 192                 tmr[i]  = all - res * 100;
 193                 LOG_INFO("t[%d]%d\n", tmr[i],i);
 194         }
 195
 196         t.tm_sec = tmr[0] + (ROUND_UP(msec, 1000) / 1000);
 197         t.tm_min = tmr[1];
 198         t.tm_hour = tmr[2];
 199         //If we not have refence data, we set as default 1/1/1970.
 200         t.tm_mday = 1;
 201         t.tm_mon = 1;
 202         t.tm_year = 70;
 203
 204         if (date_stamp)
 205         {
 206                 res = all = date_stamp;
 207                 for (int i = 0; i < 3; i++)
 208                 {
 209                         all = res;
 210                         res = all / 100;
 211                         date[i]  = all - res * 100;
 212                         LOG_INFO("d[%d]%d\n", date[i],i);
 213                 }
 214                 t.tm_mday = date[2];
 215                 t.tm_mon = date[1] - 1; // time struct count month from 0 to 11;
 216                 // we should specific number of years from 1900, but the year field
 217                 // is only two cipher, so we sum 100 (2000 - 1900)..
 218                 t.tm_year = date[0] + 100;
 219         }
 220         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);
 221
 222         return  mktime(&t);
 223 }
 224
 225
 226 /**
 227  * standard GPGGA sentence parser
 228  */
 229 int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence)
 230 {
 231         /*
 232          * get pointer to sentence data
 233          */
 234         NmeaGga *gga = (NmeaGga *)sentence->data;
 235
 236         /*
 237          * if there is a data element, extract data from the tokens
 238          */
 239         if (gga != 0)
 240         {
 241                 gga->latitude   = nmea_latitude(context->token[2],context->token[3]);
 242                 gga->longitude  = nmea_longitude(context->token[4],context->token[5]);
 243                 gga->altitude   = nmea_altitude(context->token[9],context->token[10]);
 244                 gga->time       = timestampToSec(tokenToInt(context->token[1]), 0);
 245                 gga->satellites = tokenToInt(context->token[7]);
 246                 gga->quality    = tokenToInt(context->token[6]);
 247                 gga->hdop       = tokenToInt(context->token[8]);
 248                 gga->geoid      = nmea_altitude(context->token[11],context->token[12]);
 249
 250         }
 251
 252
 253         /*
 254          * if the sentence has a callout, call it
 255          */
 256         if (sentence->callout != 0)
 257                 (*sentence->callout)(context, gga, context->user_data);
 258
 259         return NMEA_GPGGA;
 260 }
 261
 262 /**
 263  * standard GPRMCntence parser
 264  */
 265 int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
 266 {
 267
 268     /*
 269          * get pointer to sentence data
 270          */
 271     NmeaRmc *rmc = (NmeaRmc *)sentence->data;
 272
 273         /*
 274          * if there is a data element, use it
 275          */
 276         if (rmc != 0)
 277         {
 278                 /*
 279                  * extract data from the tokens
 280                  */
 281                 rmc->time       = timestampToSec(tokenToInt(context->token[1]), tokenToInt(context->token[9]));
 282                 rmc->warn       = *context->token[2];
 283                 rmc->latitude   = nmea_latitude(context->token[3],context->token[4]);
 284                 rmc->longitude  = nmea_longitude(context->token[5],context->token[6]);
 285                 rmc->speed      = tokenToInt(context->token[7]);
 286                 rmc->course     = tokenToInt(context->token[8]);
 287                 rmc->mag_var    = tokenToInt(context->token[10]);
 288         }
 289
 290     /*
 291          * if the sentence has a callout, call it
 292          */
 293     if (sentence->callout != 0)
 294         (*sentence->callout)(context, rmc, context->user_data);
 295
 296     return NMEA_GPRMC;
 297 }
 298
 299
 300 /**
 301  * standard GPVTG sentence parser
 302  */
 303 int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
 304 {
 305
 306     /*
 307          * get pointer to sentence data
 308          */
 309     NmeaVtg *vtg = (NmeaVtg *)sentence->data;
 310
 311         /*
 312          * if there is a data element, use it
 313          */
 314         if (vtg != 0)
 315         {
 316                 /*
 317                  * extract data from the tokens
 318                  */
 319                 vtg->track_good  = tokenToInt(context->token[1]);
 320                 vtg->knot_speed  = tokenToInt(context->token[5]);
 321                 vtg->km_speed    = tokenToInt(context->token[7]);
 322         }
 323
 324     /*
 325          * if the sentence has a callout, call it
 326          */
 327     if (sentence->callout != 0)
 328         (*sentence->callout)(context, vtg, context->user_data);
 329
 330     return NMEA_GPVTG;
 331 }
 332
 333 /**
 334  * standard GPGDSV sentence parser
 335  */
 336 int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence)
 337 {
 338
 339     /*
 340          * get pointer to sentence data
 341          */
 342     NmeaGsv *gsv = (NmeaGsv *)sentence->data;
 343
 344         /*
 345          * if there is a data element, use it
 346          */
 347         if (gsv != 0)
 348         {
 349                 /*
 350                  * extract data from the tokens
 351                  */
 352                 gsv->tot_message     = tokenToInt(context->token[1]);
 353                 gsv->message_num     = tokenToInt(context->token[2]);
 354                 gsv->tot_svv         = tokenToInt(context->token[3]);
 355                 gsv->sv_prn          = tokenToInt(context->token[4]);
 356                 gsv->elevation       = tokenToInt(context->token[5]);
 357                 gsv->azimut          = tokenToInt(context->token[6]);
 358                 gsv->snr             = tokenToInt(context->token[7]);
 359                 gsv->sv_prn2         = tokenToInt(context->token[8]);
 360                 gsv->elevation2      = tokenToInt(context->token[9]);
 361                 gsv->azimut2         = tokenToInt(context->token[10]);
 362                 gsv->snr2            = tokenToInt(context->token[11]);
 363                 gsv->sv_prn3         = tokenToInt(context->token[12]);
 364                 gsv->elevation3      = tokenToInt(context->token[13]);
 365                 gsv->azimut3         = tokenToInt(context->token[14]);
 366                 gsv->snr3            = tokenToInt(context->token[15]);
 367                 gsv->sv_prn4         = tokenToInt(context->token[16]);
 368                 gsv->elevation4      = tokenToInt(context->token[17]);
 369                 gsv->azimut4         = tokenToInt(context->token[18]);
 370                 gsv->snr4            = tokenToInt(context->token[19]);
 371         }
 372
 373
 374     /*
 375          * if the sentence has a callout, call it
 376          */
 377     if (sentence->callout != 0)
 378         (*sentence->callout)(context, gsv, context->user_data);
 379
 380     return NMEA_GPGSV;
 381 }
 382
 383 void nmea_poll(nmeap_context_t *context, KFile *channel)
 384 {
 385         int c;
 386         while ((c = kfile_getc(channel)) != EOF)
 387         {
 388                 nmeap_parse(context, c);
 389         }
 390 }
 391