475 lines
11 KiB
C
475 lines
11 KiB
C
/**
|
|
* \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 parser implementation.
|
|
*
|
|
* NMEA 0183 is acronym of National Marine Electronics Association that
|
|
* combined electrical and data specification for communication between marine
|
|
* electronic devices such as echo sounder, sonars, anemometer (wind speed and direction),
|
|
* gyrocompass, autopilot, GPS receivers and many other types of instruments.
|
|
* It has been defined by, and is controlled by, the U.S.-based National Marine
|
|
* Electronics Association.
|
|
* The NMEA 0183 standard uses a simple ASCII, serial communications protocol
|
|
* that defines how data is transmitted in a "sentence" from one "talker"
|
|
* to multiple "listeners" at a time.
|
|
* At the application layer, the standard also defines the contents of each sentence
|
|
* (message) type so that all listeners can parse messages accurately.
|
|
*
|
|
*
|
|
* \author Daniele Basile <asterix@develer.com>
|
|
*
|
|
* notest:avr
|
|
*/
|
|
|
|
#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>
|
|
#include <time.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
|
|
/*
|
|
* Make conversion from one string to int.
|
|
*
|
|
* You can specify the precision if the string is a float
|
|
* number. The result is an int multiplied to 10^precision.
|
|
*/
|
|
static uint32_t tokenToInt(const char *s, int precision)
|
|
{
|
|
uint32_t num = 0;
|
|
bool sep_found = false;
|
|
int i;
|
|
|
|
if (!s)
|
|
return 0;
|
|
|
|
for(i = 0; i < NMEAP_MAX_SENTENCE_LENGTH; i++)
|
|
{
|
|
unsigned char c = *s++;
|
|
|
|
if (c == '.')
|
|
{
|
|
sep_found = true;
|
|
continue;
|
|
}
|
|
|
|
if (c == '\0' || !isdigit(c) || (precision == 0 && sep_found))
|
|
break;
|
|
|
|
if (sep_found)
|
|
precision--;
|
|
|
|
num *= 10;
|
|
num += c - '0';
|
|
}
|
|
|
|
while (precision--)
|
|
num *= 10;
|
|
|
|
return num;
|
|
}
|
|
|
|
/*
|
|
* Convert a string to micro degree.
|
|
*/
|
|
static udegree_t convertToDegree(const char *str)
|
|
{
|
|
uint32_t dec;
|
|
uint32_t deg;
|
|
uint32_t min;
|
|
|
|
if (*str == 0)
|
|
return 0;
|
|
|
|
dec = tokenToInt(str, 4);
|
|
deg = dec / 1000000;
|
|
min = dec - deg * 1000000;
|
|
dec = deg * 1000000 + ((min * 5) + 1) / 3;
|
|
|
|
return dec;
|
|
}
|
|
|
|
/*
|
|
* Retun latitude in micro degree from a string.
|
|
*/
|
|
static udegree_t nmea_latitude(const char *plat, const char *phem)
|
|
{
|
|
int ns;
|
|
|
|
if (*phem == 0)
|
|
return 0;
|
|
|
|
/* north lat is +, south lat is - */
|
|
ns = (*phem == 'N') ? 1 : -1;
|
|
|
|
|
|
return ns * convertToDegree(plat);
|
|
}
|
|
|
|
/*
|
|
* Retun longitude in micro degree from a string.
|
|
*/
|
|
static udegree_t nmea_longitude(const char *plot, const char *phem)
|
|
{
|
|
int ew;
|
|
|
|
if (*phem == 0)
|
|
return 0;
|
|
|
|
/* west long is negative, east long is positive */
|
|
ew = (*phem == 'E') ? 1 : -1;
|
|
|
|
return ew * convertToDegree(plot);
|
|
}
|
|
|
|
/*
|
|
* Return altitude in meter from a string.
|
|
*
|
|
*/
|
|
static int32_t nmea_altitude(const char *palt, const char *punits)
|
|
{
|
|
int32_t alt;
|
|
|
|
if (*palt == 0)
|
|
return 0;
|
|
|
|
alt = atoi(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;
|
|
}
|
|
|
|
/*
|
|
* Convert time and date stamp string to unix time.
|
|
*/
|
|
static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp)
|
|
{
|
|
struct tm t;
|
|
uint16_t msec;
|
|
uint16_t tmr[3];
|
|
uint16_t date[3];
|
|
|
|
memset(&t, 0, sizeof(t));
|
|
memset(&tmr, 0, sizeof(tmr));
|
|
memset(&date, 0, sizeof(date));
|
|
|
|
LOG_INFO("time_s[%lu],date[%lu]\n", (long)time_stamp, (long)date_stamp);
|
|
uint32_t res = time_stamp / 1000;
|
|
uint32_t all = time_stamp;
|
|
msec = all - res * 1000;
|
|
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
all = res;
|
|
res = all / 100;
|
|
tmr[i] = all - res * 100;
|
|
LOG_INFO("t[%d]%d\n", tmr[i],i);
|
|
}
|
|
|
|
t.tm_sec = tmr[0] + (ROUND_UP(msec, 1000) / 1000);
|
|
t.tm_min = tmr[1];
|
|
t.tm_hour = tmr[2];
|
|
//If we do not have refence data, we set 1/1/1970 as default
|
|
t.tm_mday = 1;
|
|
t.tm_mon = 0;
|
|
t.tm_year = 70;
|
|
|
|
if (date_stamp)
|
|
{
|
|
res = all = date_stamp;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
all = res;
|
|
res = all / 100;
|
|
date[i] = all - res * 100;
|
|
LOG_INFO("d[%d]%d\n", date[i],i);
|
|
}
|
|
t.tm_mday = date[2];
|
|
t.tm_mon = date[1] - 1; // time struct count month from 0 to 11;
|
|
// we should specify the number of years from 1900, but the year field
|
|
// is only two digits, so we add 100 (2000 - 1900)..
|
|
t.tm_year = date[0] + 100;
|
|
}
|
|
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);
|
|
|
|
return mktime(&t);
|
|
}
|
|
|
|
/**
|
|
* Callout example for GGA data
|
|
*/
|
|
void gpgga_callout(nmeap_context_t *context, void *data, void *user_data)
|
|
{
|
|
(void)context;
|
|
(void)user_data;
|
|
(void)data;
|
|
LOG_INFOB(
|
|
NmeaGga *gga = (NmeaGga *)data;
|
|
LOG_INFO("Found GPGGA message %ld %ld %d %lu %d %d %d %d\n",
|
|
(long)gga->latitude,
|
|
(long)gga->longitude,
|
|
gga->altitude,
|
|
gga->time,
|
|
gga->satellites,
|
|
gga->quality,
|
|
gga->hdop,
|
|
gga->geoid);
|
|
);
|
|
}
|
|
|
|
/**
|
|
* Callout example for RMC
|
|
*/
|
|
void gprmc_callout(nmeap_context_t *context, void *data, void *user_data)
|
|
{
|
|
(void)context;
|
|
(void)user_data;
|
|
(void)data;
|
|
LOG_INFOB(
|
|
NmeaRmc *rmc = (NmeaRmc *)data;
|
|
|
|
LOG_INFO("Found GPRMC message %lu %c %ld %ld %d %d %d\n",
|
|
rmc->time,
|
|
rmc->warn,
|
|
(long)rmc->latitude,
|
|
(long)rmc->longitude,
|
|
rmc->speed,
|
|
rmc->course,
|
|
rmc->mag_var);
|
|
);
|
|
}
|
|
|
|
/**
|
|
* Callout example for GSV data
|
|
*/
|
|
void gpgsv_callout(nmeap_context_t *context, void *data, void *user_data)
|
|
{
|
|
(void)context;
|
|
(void)user_data;
|
|
(void)data;
|
|
LOG_INFOB(
|
|
NmeaGsv *gsv = (NmeaGsv *)data;
|
|
|
|
LOG_INFO("Found GPGSV message %d %d %d\n", gsv->tot_message, gsv->message_num, gsv->tot_svv);
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
LOG_INFO("%d %d %d %d\n", gsv->info[i].sv_prn, gsv->info[i].elevation, gsv->info[i].azimut, gsv->info[i].snr);
|
|
);
|
|
}
|
|
|
|
/**
|
|
* Callout example for VTG data
|
|
*/
|
|
void gpvtg_callout(nmeap_context_t *context, void *data, void *user_data)
|
|
{
|
|
(void)context;
|
|
(void)user_data;
|
|
(void)data;
|
|
LOG_INFOB(
|
|
NmeaVtg *vtg = (NmeaVtg *)data;
|
|
LOG_INFO("Found GPVTG message %d %d %d\n", vtg->track_good, vtg->knot_speed, vtg->km_speed);
|
|
);
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* 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;
|
|
|
|
ASSERT(gga);
|
|
ASSERT(context->tokens >= 12);
|
|
|
|
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 = timestampToSec(tokenToInt(context->token[1], 3), 0);
|
|
gga->satellites = atoi(context->token[7]);
|
|
gga->quality = atoi(context->token[6]);
|
|
gga->hdop = tokenToInt(context->token[8], 1);
|
|
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;
|
|
|
|
ASSERT(rmc);
|
|
ASSERT(context->tokens >= 10);
|
|
|
|
/*
|
|
* extract data from the tokens
|
|
*/
|
|
rmc->time = timestampToSec(tokenToInt(context->token[1], 3), tokenToInt(context->token[9], 0));
|
|
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 = atoi(context->token[7]);
|
|
rmc->course = atoi(context->token[8]);
|
|
rmc->mag_var = atoi(context->token[10]);
|
|
|
|
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;
|
|
|
|
ASSERT(vtg);
|
|
ASSERT(context->tokens >= 7);
|
|
|
|
/*
|
|
* extract data from the tokens
|
|
*/
|
|
vtg->track_good = atoi(context->token[1]);
|
|
vtg->knot_speed = atoi(context->token[5]);
|
|
vtg->km_speed = atoi(context->token[7]);
|
|
|
|
/*
|
|
* 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;
|
|
|
|
|
|
/*
|
|
* extract data from the tokens
|
|
*/
|
|
gsv->tot_message = atoi(context->token[1]);
|
|
gsv->message_num = atoi(context->token[2]);
|
|
gsv->tot_svv = atoi(context->token[3]);
|
|
|
|
// Fill remaning member until we have token
|
|
int j = 0;
|
|
for (int i = 4; i < context->tokens - 3; i += 4, j++)
|
|
{
|
|
|
|
gsv->info[j].sv_prn = atoi(context->token[i]);
|
|
gsv->info[j].elevation = atoi(context->token[i + 1]);
|
|
gsv->info[j].azimut = atoi(context->token[i + 2]);
|
|
gsv->info[j].snr = atoi(context->token[i + 3]);
|
|
}
|
|
|
|
/*
|
|
* if the sentence has a callout, call it
|
|
*/
|
|
if (sentence->callout != 0)
|
|
(*sentence->callout)(context, gsv, context->user_data);
|
|
|
|
return NMEA_GPGSV;
|
|
}
|
|
|
|
|
|
/**
|
|
* Parse NMEA sentence from a channel.
|
|
*/
|
|
void nmea_poll(nmeap_context_t *context, KFile *channel)
|
|
{
|
|
int c, e;
|
|
while ((c = kfile_getc(channel)) != EOF)
|
|
nmeap_parse(context, c);
|
|
|
|
if ((e = kfile_error(channel)))
|
|
{
|
|
LOG_ERR("ch error [%0X]\n", e);
|
|
kfile_clearerr(channel);
|
|
}
|
|
}
|
|
|