+++ /dev/null
-/**
- * \file
- * <!--
- * This file is part of BeRTOS.
- *
- * Bertos is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- *
- * As a special exception, you may use this file as part of a free software
- * library without restriction. Specifically, if other files instantiate
- * templates or use macros or inline functions from this file, or you compile
- * this file and link it with other files to produce an executable, this
- * file does not by itself cause the resulting executable to be covered by
- * the GNU General Public License. This exception does not however
- * invalidate any other reasons why the executable file might be covered by
- * the GNU General Public License.
- *
- * Copyright 2009 Develer S.r.l. (http://www.develer.com/)
- *
- * -->
- * \brief NMEA implementation.
- *
- * \author Daniele Basile <asterix@develer.com>
- *
- */
-
-#include "nmea.h"
-
-#include "cfg/cfg_nmea.h"
-
-#include <cfg/debug.h>
-
-#define LOG_LEVEL NMEA_LOG_LEVEL
-#define LOG_FORMAT NMEA_LOG_FORMAT
-#include <cfg/log.h>
-
-#include <net/nmeap/inc/nmeap.h>
-
-#include <ctype.h>
-
-
-static uint32_t tokenToInt(const char *s)
-{
- uint32_t num = 0;
- int i;
-
- ASSERT(s);
-
- for(i = 0; i < NMEAP_MAX_TOKENS; i++)
- {
- char c = *s++;
-
- if (c == '.')
- continue;
-
- if (c == '\0' || !isdigit(c))
- break;
-
- num *= 10;
- num += c - '0';
- }
-
- return num;
-}
-
-
-static udegree_t nmea_latitude(const char *plat, const char *phem)
-{
- int ns;
- uint32_t lat;
- uint32_t deg;
- uint32_t min;
-
- if (*plat == 0)
- {
- return 0;
- }
- if (*phem == 0)
- {
- return 0;
- }
-
- /* north lat is +, south lat is - */
- if (*phem == 'N')
- {
- ns = 1;
- }
- else
- {
- ns = -1;
- }
-
- lat = tokenToInt(plat);
- deg = lat / 1000000;
- min = lat - deg * 1000000;
- lat = deg * 1000000 + ((min * 5) + 1) / 3;
-
- return ns * lat;
-}
-
-static udegree_t nmea_longitude(const char *plot, const char *phem)
-{
- int ew;
- uint32_t lot;
- uint32_t deg;
- uint32_t min;
-
- if (*plot == 0)
- {
- return 0;
- }
- if (*phem == 0)
- {
- return 0;
- }
-
- /* west long is negative, east long is positive */
- if (*phem == 'E')
- {
- ew = 1;
- }
- else {
- ew = -1;
- }
-
- lot = tokenToInt(plot);
- deg = lot / 1000000;
- min = lot - deg * 1000000;
- lot = deg * 1000000 + ((min * 5) + 1) / 3;
-
- return ew * lot;
-}
-
-static cm_t nmea_altitude(const char *palt, const char *punits)
-{
- uint32_t alt;
-
- if (*palt == 0)
- {
- return 0;
- }
-
- alt = tokenToInt(palt);
-
- if (*punits == 'F')
- {
- /* convert to feet */
- /* alt = alt * 3.2808399 */
- alt = alt * 3 + /* 3.0 */
- (alt >> 2) + /* 0.25 */
- (alt >> 6) + /* 0.015625 */
- (alt >> 7) + /* 0.0078125 */
- (alt >> 8); /* 0,00390625 */
-
- }
-
- return alt;
-}
-
-/**
- * standard GPGGA sentence parser
- */
-int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence)
-{
- /*
- * get pointer to sentence data
- */
- NmeaGga *gga = (NmeaGga *)sentence->data;
-
- /*
- * if there is a data element, extract data from the tokens
- */
- if (gga != 0)
- {
- gga->latitude = nmea_latitude(context->token[2],context->token[3]);
- gga->longitude = nmea_longitude(context->token[4],context->token[5]);
- gga->altitude = nmea_altitude(context->token[9],context->token[10]);
- gga->time = tokenToInt(context->token[1]);
- gga->satellites = tokenToInt(context->token[7]);
- gga->quality = tokenToInt(context->token[6]);
- gga->hdop = tokenToInt(context->token[8]);
- gga->geoid = nmea_altitude(context->token[11],context->token[12]);
-
- }
-
-
- /*
- * if the sentence has a callout, call it
- */
- if (sentence->callout != 0)
- (*sentence->callout)(context,gga,context->user_data);
-
- return NMEA_GPGGA;
-}
-
-/**
- * standard GPRMCntence parser
- */
-int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
-{
-
- /*
- * get pointer to sentence data
- */
- NmeaRmc *rmc = (NmeaRmc *)sentence->data;
-
- /*
- * if there is a data element, use it
- */
- if (rmc != 0)
- {
- /*
- * extract data from the tokens
- */
- rmc->time = tokenToInt(context->token[1]);
- rmc->warn = *context->token[2];
- rmc->latitude = nmea_latitude(context->token[3],context->token[4]);
- rmc->longitude = nmea_longitude(context->token[5],context->token[6]);
- rmc->speed = tokenToInt(context->token[7]) * 100;
- rmc->course = tokenToInt(context->token[8]) * 100;
- rmc->date = tokenToInt(context->token[9]);
- rmc->mag_var = tokenToInt(context->token[10]);
- }
-
- /*
- * if the sentence has a callout, call it
- */
- if (sentence->callout != 0)
- (*sentence->callout)(context,rmc,context->user_data);
-
- return NMEA_GPRMC;
-}
-
-
-/**
- * standard GPVTG sentence parser
- */
-int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
-{
-
- /*
- * get pointer to sentence data
- */
- NmeaVtg *vtg = (NmeaVtg *)sentence->data;
-
- /*
- * if there is a data element, use it
- */
- if (vtg != 0)
- {
- /*
- * extract data from the tokens
- */
- vtg->track_good = tokenToInt(context->token[1]) * 100;
- vtg->knot_speed = tokenToInt(context->token[5]) * 100;
- vtg->km_speed = tokenToInt(context->token[7]) * 100;
- }
-
- /*
- * if the sentence has a callout, call it
- */
- if (sentence->callout != 0)
- (*sentence->callout)(context,vtg,context->user_data);
-
- return NMEA_GPVTG;
-}
-
-/**
- * standard GPGDSV sentence parser
- */
-int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence)
-{
-
- /*
- * get pointer to sentence data
- */
- NmeaGsv *gsv = (NmeaGsv *)sentence->data;
-
- /*
- * if there is a data element, use it
- */
- if (gsv != 0)
- {
- /*
- * extract data from the tokens
- */
- gsv->tot_message = tokenToInt(context->token[1]);
- gsv->message_num = tokenToInt(context->token[2]);
- gsv->tot_svv = tokenToInt(context->token[3]);
- gsv->sv_prn = tokenToInt(context->token[4]);
- gsv->elevation = tokenToInt(context->token[5]);
- gsv->azimut = tokenToInt(context->token[6]);
- gsv->snr = tokenToInt(context->token[7]);
- gsv->sv_prn2 = tokenToInt(context->token[8]);
- gsv->elevation2 = tokenToInt(context->token[9]);
- gsv->azimut2 = tokenToInt(context->token[10]);
- gsv->snr2 = tokenToInt(context->token[11]);
- gsv->sv_prn3 = tokenToInt(context->token[12]);
- gsv->elevation3 = tokenToInt(context->token[13]);
- gsv->azimut3 = tokenToInt(context->token[14]);
- gsv->snr3 = tokenToInt(context->token[15]);
- gsv->sv_prn4 = tokenToInt(context->token[16]);
- gsv->elevation4 = tokenToInt(context->token[17]);
- gsv->azimut4 = tokenToInt(context->token[18]);
- gsv->snr4 = tokenToInt(context->token[19]);
- }
-
-
- /*
- * if the sentence has a callout, call it
- */
- if (sentence->callout != 0)
- (*sentence->callout)(context,gsv,context->user_data);
-
- return NMEA_GPGSV;
-}
-
-void nmea_poll(nmeap_context_t *context, KFile *channel)
-{
- int c;
- while ((c = kfile_getc(channel)) != EOF)
- {
- if (nmeap_parse(context, c) == -1)
- break;
- }
-}
-
projects / bertos.git / blobdiff