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