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
  52
  53 static uint32_t tokenToInt(const char *s)
  54 {
  55         uint32_t num = 0;
  56         int i;
  57
  58         ASSERT(s);
  59
  60         for(i = 0; i < NMEAP_MAX_TOKENS; i++)
  61         {
  62                 char c = *s++;
  63
  64                 if (c == '.')
  65                         continue;
  66
  67                 if (c == '\0' || !isdigit(c))
  68                         break;
  69
  70                 num *= 10;
  71                 num += c - '0';
  72         }
  73
  74         return num;
  75 }
  76
  77
  78 static udegree_t nmea_latitude(const char *plat, const char *phem)
  79 {
  80         int ns;
  81         uint32_t lat;
  82         uint32_t deg;
  83         uint32_t min;
  84
  85         if (*plat == 0)
  86         {
  87         return 0;
  88     }
  89     if (*phem == 0)
  90         {
  91         return 0;
  92     }
  93
  94     /* north lat is +, south lat is - */
  95     if (*phem == 'N')
  96         {
  97         ns = 1;
  98     }
  99     else
 100         {
 101         ns = -1;
 102     }
 103
 104         lat = tokenToInt(plat);
 105         deg = lat / 1000000;
 106         min = lat - deg * 1000000;
 107         lat = deg * 1000000 + ((min * 5) + 1) / 3;
 108
 109         return ns * lat;
 110 }
 111
 112 static udegree_t nmea_longitude(const char *plot, const char *phem)
 113 {
 114         int ew;
 115         uint32_t lot;
 116         uint32_t deg;
 117         uint32_t min;
 118
 119         if (*plot == 0)
 120         {
 121         return 0;
 122     }
 123     if (*phem == 0)
 124         {
 125         return 0;
 126     }
 127
 128     /* west long is negative, east long is positive */
 129     if (*phem == 'E')
 130         {
 131         ew = 1;
 132     }
 133     else {
 134         ew = -1;
 135     }
 136
 137         lot = tokenToInt(plot);
 138         deg = lot / 1000000;
 139         min = lot - deg * 1000000;
 140         lot = deg * 1000000 + ((min * 5) + 1) / 3;
 141
 142         return ew  * lot;
 143 }
 144
 145 static cm_t nmea_altitude(const char *palt, const char *punits)
 146 {
 147         uint32_t alt;
 148
 149         if (*palt == 0)
 150         {
 151         return 0;
 152     }
 153
 154         alt = tokenToInt(palt);
 155
 156     if (*punits == 'F')
 157         {
 158         /* convert to feet */
 159         /* alt = alt * 3.2808399 */
 160                 alt = alt * 3 +  /* 3.0 */
 161                           (alt >> 2) + /* 0.25 */
 162                           (alt >> 6) + /* 0.015625 */
 163                           (alt >> 7) + /* 0.0078125 */
 164                           (alt >> 8); /* 0,00390625 */
 165
 166     }
 167
 168         return alt;
 169 }
 170
 171 /**
 172  * standard GPGGA sentence parser
 173  */
 174 int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence)
 175 {
 176         /*
 177          * get pointer to sentence data
 178          */
 179         NmeaGga *gga = (NmeaGga *)sentence->data;
 180
 181         /*
 182          * if there is a data element, extract data from the tokens
 183          */
 184         if (gga != 0)
 185         {
 186                 gga->latitude   = nmea_latitude(context->token[2],context->token[3]);
 187                 gga->longitude  = nmea_longitude(context->token[4],context->token[5]);
 188                 gga->altitude   = nmea_altitude(context->token[9],context->token[10]);
 189                 gga->time       = tokenToInt(context->token[1]);
 190                 gga->satellites = tokenToInt(context->token[7]);
 191                 gga->quality    = tokenToInt(context->token[6]);
 192                 gga->hdop       = tokenToInt(context->token[8]);
 193                 gga->geoid      = nmea_altitude(context->token[11],context->token[12]);
 194
 195         }
 196
 197
 198         /*
 199          * if the sentence has a callout, call it
 200          */
 201         if (sentence->callout != 0)
 202                 (*sentence->callout)(context,gga,context->user_data);
 203
 204         return NMEA_GPGGA;
 205 }
 206
 207 /**
 208  * standard GPRMCntence parser
 209  */
 210 int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
 211 {
 212
 213     /*
 214          * get pointer to sentence data
 215          */
 216     NmeaRmc *rmc = (NmeaRmc *)sentence->data;
 217
 218         /*
 219          * if there is a data element, use it
 220          */
 221         if (rmc != 0)
 222         {
 223                 /*
 224                  * extract data from the tokens
 225                  */
 226                 rmc->time       = tokenToInt(context->token[1]);
 227                 rmc->warn       = *context->token[2];
 228                 rmc->latitude   = nmea_latitude(context->token[3],context->token[4]);
 229                 rmc->longitude  = nmea_longitude(context->token[5],context->token[6]);
 230                 rmc->speed      = tokenToInt(context->token[7]) * 100;
 231                 rmc->course     = tokenToInt(context->token[8]) * 100;
 232                 rmc->date       = tokenToInt(context->token[9]);
 233                 rmc->mag_var    = tokenToInt(context->token[10]);
 234         }
 235
 236     /*
 237          * if the sentence has a callout, call it
 238          */
 239     if (sentence->callout != 0)
 240         (*sentence->callout)(context,rmc,context->user_data);
 241
 242     return NMEA_GPRMC;
 243 }
 244
 245
 246 /**
 247  * standard GPVTG sentence parser
 248  */
 249 int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
 250 {
 251
 252     /*
 253          * get pointer to sentence data
 254          */
 255     NmeaVtg *vtg = (NmeaVtg *)sentence->data;
 256
 257         /*
 258          * if there is a data element, use it
 259          */
 260         if (vtg != 0)
 261         {
 262                 /*
 263                  * extract data from the tokens
 264                  */
 265                 vtg->track_good  = tokenToInt(context->token[1]) * 100;
 266                 vtg->knot_speed  = tokenToInt(context->token[5]) * 100;
 267                 vtg->km_speed    = tokenToInt(context->token[7]) * 100;
 268         }
 269
 270     /*
 271          * if the sentence has a callout, call it
 272          */
 273     if (sentence->callout != 0)
 274         (*sentence->callout)(context,vtg,context->user_data);
 275
 276     return NMEA_GPVTG;
 277 }
 278
 279 /**
 280  * standard GPGDSV sentence parser
 281  */
 282 int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence)
 283 {
 284
 285     /*
 286          * get pointer to sentence data
 287          */
 288     NmeaGsv *gsv = (NmeaGsv *)sentence->data;
 289
 290         /*
 291          * if there is a data element, use it
 292          */
 293         if (gsv != 0)
 294         {
 295                 /*
 296                  * extract data from the tokens
 297                  */
 298                 gsv->tot_message     = tokenToInt(context->token[1]);
 299                 gsv->message_num     = tokenToInt(context->token[2]);
 300                 gsv->tot_svv         = tokenToInt(context->token[3]);
 301                 gsv->sv_prn          = tokenToInt(context->token[4]);
 302                 gsv->elevation       = tokenToInt(context->token[5]);
 303                 gsv->azimut          = tokenToInt(context->token[6]);
 304                 gsv->snr             = tokenToInt(context->token[7]);
 305                 gsv->sv_prn2         = tokenToInt(context->token[8]);
 306                 gsv->elevation2      = tokenToInt(context->token[9]);
 307                 gsv->azimut2         = tokenToInt(context->token[10]);
 308                 gsv->snr2            = tokenToInt(context->token[11]);
 309                 gsv->sv_prn3         = tokenToInt(context->token[12]);
 310                 gsv->elevation3      = tokenToInt(context->token[13]);
 311                 gsv->azimut3         = tokenToInt(context->token[14]);
 312                 gsv->snr3            = tokenToInt(context->token[15]);
 313                 gsv->sv_prn4         = tokenToInt(context->token[16]);
 314                 gsv->elevation4      = tokenToInt(context->token[17]);
 315                 gsv->azimut4         = tokenToInt(context->token[18]);
 316                 gsv->snr4            = tokenToInt(context->token[19]);
 317         }
 318
 319
 320     /*
 321          * if the sentence has a callout, call it
 322          */
 323     if (sentence->callout != 0)
 324         (*sentence->callout)(context,gsv,context->user_data);
 325
 326     return NMEA_GPGSV;
 327 }
 328
 329 void nmea_poll(nmeap_context_t *context, KFile *channel)
 330 {
 331         int c;
 332         while ((c = kfile_getc(channel)) != EOF)
 333         {
 334                 if (nmeap_parse(context, c) == -1)
 335                         break;
 336         }
 337 }
 338