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