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>
53 static uint32_t tokenToInt(const char *s)
60 for(i = 0; i < NMEAP_MAX_TOKENS; i++)
67 if (c == '\0' || !isdigit(c))
78 static udegree_t nmea_latitude(const char *plat, const char *phem)
94 /* north lat is +, south lat is - */
104 lat = tokenToInt(plat);
106 min = lat - deg * 1000000;
107 lat = deg * 1000000 + ((min * 5) + 1) / 3;
112 static udegree_t nmea_longitude(const char *plot, const char *phem)
128 /* west long is negative, east long is positive */
137 lot = tokenToInt(plot);
139 min = lot - deg * 1000000;
140 lot = deg * 1000000 + ((min * 5) + 1) / 3;
145 static cm_t nmea_altitude(const char *palt, const char *punits)
154 alt = tokenToInt(palt);
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 */
172 * standard GPGGA sentence parser
174 int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence)
177 * get pointer to sentence data
179 NmeaGga *gga = (NmeaGga *)sentence->data;
182 * if there is a data element, extract data from the tokens
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]);
199 * if the sentence has a callout, call it
201 if (sentence->callout != 0)
202 (*sentence->callout)(context,gga,context->user_data);
208 * standard GPRMCntence parser
210 int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
214 * get pointer to sentence data
216 NmeaRmc *rmc = (NmeaRmc *)sentence->data;
219 * if there is a data element, use it
224 * extract data from the tokens
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]);
237 * if the sentence has a callout, call it
239 if (sentence->callout != 0)
240 (*sentence->callout)(context,rmc,context->user_data);
247 * standard GPVTG sentence parser
249 int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
253 * get pointer to sentence data
255 NmeaVtg *vtg = (NmeaVtg *)sentence->data;
258 * if there is a data element, use it
263 * extract data from the tokens
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;
271 * if the sentence has a callout, call it
273 if (sentence->callout != 0)
274 (*sentence->callout)(context,vtg,context->user_data);
280 * standard GPGDSV sentence parser
282 int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence)
286 * get pointer to sentence data
288 NmeaGsv *gsv = (NmeaGsv *)sentence->data;
291 * if there is a data element, use it
296 * extract data from the tokens
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]);
321 * if the sentence has a callout, call it
323 if (sentence->callout != 0)
324 (*sentence->callout)(context,gsv,context->user_data);
329 void nmea_poll(nmeap_context_t *context, KFile *channel)
332 while ((c = kfile_getc(channel)) != EOF)
334 if (nmeap_parse(context, c) == -1)