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/)
33 * \brief NMEA parser implementation.
35 * NMEA 0183 is acronym of National Marine Electronics Association that
36 * combined electrical and data specification for communication between marine
37 * electronic devices such as echo sounder, sonars, anemometer (wind speed and direction),
38 * gyrocompass, autopilot, GPS receivers and many other types of instruments.
39 * It has been defined by, and is controlled by, the U.S.-based National Marine
40 * Electronics Association.
41 * The NMEA 0183 standard uses a simple ASCII, serial communications protocol
42 * that defines how data is transmitted in a "sentence" from one "talker"
43 * to multiple "listeners" at a time.
44 * At the application layer, the standard also defines the contents of each sentence
45 * (message) type so that all listeners can parse messages accurately.
48 * \author Daniele Basile <asterix@develer.com>
55 #include "cfg/cfg_nmea.h"
57 #include <cfg/debug.h>
59 #define LOG_LEVEL NMEA_LOG_LEVEL
60 #define LOG_FORMAT NMEA_LOG_FORMAT
63 #include <net/nmeap/inc/nmeap.h>
71 * Make conversion from one string to int.
73 * You can specify the precision if the string is a float
74 * number. The result is an int multiplied to 10^precision.
76 static uint32_t tokenToInt(const char *s, int precision)
79 bool sep_found = false;
85 for(i = 0; i < NMEAP_MAX_SENTENCE_LENGTH; i++)
95 if (c == '\0' || !isdigit(c) || (precision == 0 && sep_found))
112 * Convert a string to micro degree.
114 static udegree_t convertToDegree(const char *str)
125 dec = tokenToInt(str, 4);
127 min = dec - deg * 1000000;
128 dec = deg * 1000000 + ((min * 5) + 1) / 3;
134 * Retun latitude in micro degree from a string.
136 static udegree_t nmea_latitude(const char *plat, const char *phem)
143 /* north lat is +, south lat is - */
144 ns = (*phem == 'N') ? 1 : -1;
147 return ns * convertToDegree(plat);
151 * Retun longitude in micro degree from a string.
153 static udegree_t nmea_longitude(const char *plot, const char *phem)
160 /* west long is negative, east long is positive */
161 ew = (*phem == 'E') ? 1 : -1;
163 return ew * convertToDegree(plot);
167 * Return altitude in meter from a string.
170 static uint16_t nmea_altitude(const char *palt, const char *punits)
181 /* convert to feet */
182 /* alt = alt * 3.2808399 */
183 alt = alt * 3 + /* 3.0 */
184 (alt >> 2) + /* 0.25 */
185 (alt >> 6) + /* 0.015625 */
186 (alt >> 7) + /* 0.0078125 */
187 (alt >> 8); /* 0,00390625 */
195 * Convert time and date stamp string to unix time.
197 static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp)
204 memset(&t, 0, sizeof(t));
205 memset(&tmr, 0, sizeof(tmr));
206 memset(&date, 0, sizeof(date));
208 LOG_INFO("time_s[%lu],date[%lu]\n", (long)time_stamp, (long)date_stamp);
209 uint32_t res = time_stamp / 1000;
210 uint32_t all = time_stamp;
211 msec = all - res * 1000;
213 for (int i = 0; i < 3; i++)
217 tmr[i] = all - res * 100;
218 LOG_INFO("t[%d]%d\n", tmr[i],i);
221 t.tm_sec = tmr[0] + (ROUND_UP(msec, 1000) / 1000);
224 //If we not have refence data, we set as default 1/1/1970.
231 res = all = date_stamp;
232 for (int i = 0; i < 3; i++)
236 date[i] = all - res * 100;
237 LOG_INFO("d[%d]%d\n", date[i],i);
240 t.tm_mon = date[1] - 1; // time struct count month from 0 to 11;
241 // we should specific number of years from 1900, but the year field
242 // is only two cipher, so we sum 100 (2000 - 1900)..
243 t.tm_year = date[0] + 100;
245 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);
251 * Callout example for GGA data
253 void gpgga_callout(nmeap_context_t *context, void *data, void *user_data)
258 LOG_INFOB(NmeaGga *gga = (NmeaGga *)data;);
260 LOG_INFO("Found GPGGA message %ld %ld %d %lu %d %d %d %d\n",
262 (long)gga->longitude,
272 * Callout example for RMC
274 void gprmc_callout(nmeap_context_t *context, void *data, void *user_data)
279 LOG_INFOB(NmeaRmc *rmc = (NmeaRmc *)data;);
281 LOG_INFO("Found GPRMC message %lu %c %ld %ld %d %d %d\n",
285 (long)rmc->longitude,
292 * Callout example for GSV data
294 void gpgsv_callout(nmeap_context_t *context, void *data, void *user_data)
299 LOG_INFOB(NmeaGsv *gsv = (NmeaGsv *)data;);
301 LOG_INFO("Found GPGSV message %d %d %d\n", gsv->tot_message, gsv->message_num, gsv->tot_svv);
303 for (int i = 0; i < 4; i++)
304 LOG_INFO("%d %d %d %d\n", gsv->info[i].sv_prn, gsv->info[i].elevation, gsv->info[i].azimut, gsv->info[i].snr);
308 * Callout example for VTG data
310 void gpvtg_callout(nmeap_context_t *context, void *data, void *user_data)
315 LOG_INFOB(NmeaVtg *vtg = (NmeaVtg *)data;);
317 LOG_INFO("Found GPVTG message %d %d %d\n", vtg->track_good, vtg->knot_speed, vtg->km_speed);
323 * standard GPGGA sentence parser
325 int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence)
328 * get pointer to sentence data
330 NmeaGga *gga = (NmeaGga *)sentence->data;
333 ASSERT(context->tokens >= 12);
335 gga->latitude = nmea_latitude(context->token[2],context->token[3]);
336 gga->longitude = nmea_longitude(context->token[4],context->token[5]);
337 gga->altitude = nmea_altitude(context->token[9],context->token[10]);
338 gga->time = timestampToSec(tokenToInt(context->token[1], 3), 0);
339 gga->satellites = atoi(context->token[7]);
340 gga->quality = atoi(context->token[6]);
341 gga->hdop = tokenToInt(context->token[8], 1);
342 gga->geoid = nmea_altitude(context->token[11],context->token[12]);
345 * if the sentence has a callout, call it
348 if (sentence->callout != 0)
349 (*sentence->callout)(context, gga, context->user_data);
355 * standard GPRMCntence parser
357 int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
361 * get pointer to sentence data
363 NmeaRmc *rmc = (NmeaRmc *)sentence->data;
366 ASSERT(context->tokens >= 10);
369 * extract data from the tokens
371 rmc->time = timestampToSec(tokenToInt(context->token[1], 3), tokenToInt(context->token[9], 0));
372 rmc->warn = *context->token[2];
373 rmc->latitude = nmea_latitude(context->token[3],context->token[4]);
374 rmc->longitude = nmea_longitude(context->token[5],context->token[6]);
375 rmc->speed = atoi(context->token[7]);
376 rmc->course = atoi(context->token[8]);
377 rmc->mag_var = atoi(context->token[10]);
379 if (sentence->callout != 0)
380 (*sentence->callout)(context, rmc, context->user_data);
387 * standard GPVTG sentence parser
389 int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
393 * get pointer to sentence data
395 NmeaVtg *vtg = (NmeaVtg *)sentence->data;
398 ASSERT(context->tokens >= 7);
401 * extract data from the tokens
403 vtg->track_good = atoi(context->token[1]);
404 vtg->knot_speed = atoi(context->token[5]);
405 vtg->km_speed = atoi(context->token[7]);
408 * if the sentence has a callout, call it
410 if (sentence->callout != 0)
411 (*sentence->callout)(context, vtg, context->user_data);
417 * standard GPGDSV sentence parser
419 int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence)
423 * get pointer to sentence data
425 NmeaGsv *gsv = (NmeaGsv *)sentence->data;
429 * extract data from the tokens
431 gsv->tot_message = atoi(context->token[1]);
432 gsv->message_num = atoi(context->token[2]);
433 gsv->tot_svv = atoi(context->token[3]);
435 // Fill remaning member until we have token
437 for (int i = 4; i < context->tokens - 3; i += 4, j++)
440 gsv->info[j].sv_prn = atoi(context->token[i]);
441 gsv->info[j].elevation = atoi(context->token[i + 1]);
442 gsv->info[j].azimut = atoi(context->token[i + 2]);
443 gsv->info[j].snr = atoi(context->token[i + 3]);
447 * if the sentence has a callout, call it
449 if (sentence->callout != 0)
450 (*sentence->callout)(context, gsv, context->user_data);
457 * Parse NMEA sentence from a channel.
459 void nmea_poll(nmeap_context_t *context, KFile *channel)
462 while ((c = kfile_getc(channel)) != EOF)
464 nmeap_parse(context, c);