4 * This file is part of BeRTOS.
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.
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.
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
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.
29 * Copyright 2009 Develer S.r.l. (http://www.develer.com/)
32 * \brief NMEA implementation.
34 * \author Daniele Basile <asterix@develer.com>
40 #include "cfg/cfg_nmea.h"
42 #include <cfg/debug.h>
44 #define LOG_LEVEL NMEA_LOG_LEVEL
45 #define LOG_FORMAT NMEA_LOG_FORMAT
48 #include <net/nmeap/inc/nmeap.h>
55 static uint32_t tokenToInt(const char *s)
62 for(i = 0; i < NMEAP_MAX_TOKENS; i++)
69 if (c == '\0' || !isdigit(c))
80 static udegree_t nmea_latitude(const char *plat, const char *phem)
96 /* north lat is +, south lat is - */
106 lat = tokenToInt(plat);
108 min = lat - deg * 1000000;
109 lat = deg * 1000000 + ((min * 5) + 1) / 3;
114 static udegree_t nmea_longitude(const char *plot, const char *phem)
130 /* west long is negative, east long is positive */
139 lot = tokenToInt(plot);
141 min = lot - deg * 1000000;
142 lot = deg * 1000000 + ((min * 5) + 1) / 3;
147 static uint16_t nmea_altitude(const char *palt, const char *punits)
156 alt = tokenToInt(palt);
160 /* convert to feet */
161 /* alt = alt * 3.2808399 */
162 alt = alt * 3 + /* 3.0 */
163 (alt >> 2) + /* 0.25 */
164 (alt >> 6) + /* 0.015625 */
165 (alt >> 7) + /* 0.0078125 */
166 (alt >> 8); /* 0,00390625 */
173 static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp)
180 memset(&t, 0, sizeof(t));
181 memset(&tmr, 0, sizeof(tmr));
182 memset(&date, 0, sizeof(date));
184 LOG_INFO("time_s[%u],date[%u]\n", time_stamp, date_stamp);
185 uint32_t res = time_stamp / 1000;
186 uint32_t all = time_stamp;
187 msec = all - res * 1000;
188 for (int i = 0; i < 3; i++)
192 tmr[i] = all - res * 100;
193 LOG_INFO("t[%d]%d\n", tmr[i],i);
196 t.tm_sec = tmr[0] + (ROUND_UP(msec, 1000) / 1000);
199 //If we not have refence data, we set as default 1/1/1970.
206 res = all = date_stamp;
207 for (int i = 0; i < 3; i++)
211 date[i] = all - res * 100;
212 LOG_INFO("d[%d]%d\n", date[i],i);
215 t.tm_mon = date[1] - 1; // time struct count month from 0 to 11;
216 // we should specific number of years from 1900, but the year field
217 // is only two cipher, so we sum 100 (2000 - 1900)..
218 t.tm_year = date[0] + 100;
220 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);
227 * standard GPGGA sentence parser
229 int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence)
232 * get pointer to sentence data
234 NmeaGga *gga = (NmeaGga *)sentence->data;
237 * if there is a data element, extract data from the tokens
241 gga->latitude = nmea_latitude(context->token[2],context->token[3]);
242 gga->longitude = nmea_longitude(context->token[4],context->token[5]);
243 gga->altitude = nmea_altitude(context->token[9],context->token[10]);
244 gga->time = timestampToSec(tokenToInt(context->token[1]), 0);
245 gga->satellites = tokenToInt(context->token[7]);
246 gga->quality = tokenToInt(context->token[6]);
247 gga->hdop = tokenToInt(context->token[8]);
248 gga->geoid = nmea_altitude(context->token[11],context->token[12]);
254 * if the sentence has a callout, call it
256 if (sentence->callout != 0)
257 (*sentence->callout)(context, gga, context->user_data);
263 * standard GPRMCntence parser
265 int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
269 * get pointer to sentence data
271 NmeaRmc *rmc = (NmeaRmc *)sentence->data;
274 * if there is a data element, use it
279 * extract data from the tokens
281 rmc->time = timestampToSec(tokenToInt(context->token[1]), tokenToInt(context->token[9]));
282 rmc->warn = *context->token[2];
283 rmc->latitude = nmea_latitude(context->token[3],context->token[4]);
284 rmc->longitude = nmea_longitude(context->token[5],context->token[6]);
285 rmc->speed = tokenToInt(context->token[7]);
286 rmc->course = tokenToInt(context->token[8]);
287 rmc->mag_var = tokenToInt(context->token[10]);
291 * if the sentence has a callout, call it
293 if (sentence->callout != 0)
294 (*sentence->callout)(context, rmc, context->user_data);
301 * standard GPVTG sentence parser
303 int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
307 * get pointer to sentence data
309 NmeaVtg *vtg = (NmeaVtg *)sentence->data;
312 * if there is a data element, use it
317 * extract data from the tokens
319 vtg->track_good = tokenToInt(context->token[1]);
320 vtg->knot_speed = tokenToInt(context->token[5]);
321 vtg->km_speed = tokenToInt(context->token[7]);
325 * if the sentence has a callout, call it
327 if (sentence->callout != 0)
328 (*sentence->callout)(context, vtg, context->user_data);
334 * standard GPGDSV sentence parser
336 int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence)
340 * get pointer to sentence data
342 NmeaGsv *gsv = (NmeaGsv *)sentence->data;
345 * if there is a data element, use it
350 * extract data from the tokens
352 gsv->tot_message = tokenToInt(context->token[1]);
353 gsv->message_num = tokenToInt(context->token[2]);
354 gsv->tot_svv = tokenToInt(context->token[3]);
355 gsv->sv_prn = tokenToInt(context->token[4]);
356 gsv->elevation = tokenToInt(context->token[5]);
357 gsv->azimut = tokenToInt(context->token[6]);
358 gsv->snr = tokenToInt(context->token[7]);
359 gsv->sv_prn2 = tokenToInt(context->token[8]);
360 gsv->elevation2 = tokenToInt(context->token[9]);
361 gsv->azimut2 = tokenToInt(context->token[10]);
362 gsv->snr2 = tokenToInt(context->token[11]);
363 gsv->sv_prn3 = tokenToInt(context->token[12]);
364 gsv->elevation3 = tokenToInt(context->token[13]);
365 gsv->azimut3 = tokenToInt(context->token[14]);
366 gsv->snr3 = tokenToInt(context->token[15]);
367 gsv->sv_prn4 = tokenToInt(context->token[16]);
368 gsv->elevation4 = tokenToInt(context->token[17]);
369 gsv->azimut4 = tokenToInt(context->token[18]);
370 gsv->snr4 = tokenToInt(context->token[19]);
375 * if the sentence has a callout, call it
377 if (sentence->callout != 0)
378 (*sentence->callout)(context, gsv, context->user_data);
383 void nmea_poll(nmeap_context_t *context, KFile *channel)
386 while ((c = kfile_getc(channel)) != EOF)
388 nmeap_parse(context, c);