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