8403973c28ad88671c88e2d82f2ca57d7400ad4c
[bertos.git] / nmea.c
1 /**
2  * \file
3  * <!--
4  * This file is part of BeRTOS.
5  *
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.
10  *
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.
15  *
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
19  *
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.
28  *
29  * Copyright 2009 Develer S.r.l. (http://www.develer.com/)
30  *
31  * -->
32  *
33  * \brief NMEA parser implementation.
34  *
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.
46  *
47  *
48  * \author Daniele Basile <asterix@develer.com>
49  *
50  * notest:avr
51  */
52
53 #include "nmea.h"
54
55 #include "cfg/cfg_nmea.h"
56
57 #include <cfg/debug.h>
58
59 #define LOG_LEVEL  NMEA_LOG_LEVEL
60 #define LOG_FORMAT NMEA_LOG_FORMAT
61 #include <cfg/log.h>
62
63 #include <net/nmeap/inc/nmeap.h>
64
65 #include <ctype.h>
66 #include <time.h>
67 #include <string.h>
68 #include <stdlib.h>
69
70 /*
71  * Make conversion from one string to int.
72  *
73  * You can specify the precision if the string is a float
74  * number. The result is an int multiplied to 10^precision.
75  */
76 static uint32_t tokenToInt(const char *s, int precision)
77 {
78         uint32_t num = 0;
79         bool sep_found = false;
80         int i;
81
82         if (!s)
83                 return 0;
84
85         for(i = 0; i < NMEAP_MAX_SENTENCE_LENGTH; i++)
86         {
87                 char c = *s++;
88
89                 if (c == '.')
90                 {
91                         sep_found = true;
92                         continue;
93                 }
94
95                 if (c == '\0' || !isdigit(c) || (precision == 0 && sep_found))
96                         break;
97
98                 if (sep_found)
99                         precision--;
100
101                 num *= 10;
102                 num += c - '0';
103         }
104
105         while (precision--)
106                 num *= 10;
107
108         return num;
109 }
110
111 /*
112  * Convert a string to micro degree.
113  */
114 static udegree_t convertToDegree(const char *str)
115 {
116         uint32_t dec;
117         uint32_t deg;
118         uint32_t min;
119
120         if (*str == 0)
121         {
122         return 0;
123     }
124
125         dec = tokenToInt(str, 4);
126         deg = dec / 1000000;
127         min = dec - deg * 1000000;
128         dec = deg * 1000000 + ((min * 5) + 1) / 3;
129
130         return dec;
131 }
132
133 /*
134  * Retun latitude in micro degree from a string.
135  */
136 static udegree_t nmea_latitude(const char *plat, const char *phem)
137 {
138         int ns;
139
140     if (*phem == 0)
141         return 0;
142
143     /* north lat is +, south lat is - */
144         ns = (*phem == 'N') ? 1 : -1;
145
146
147         return ns * convertToDegree(plat);
148 }
149
150 /*
151  * Retun longitude in micro degree from a string.
152  */
153 static udegree_t nmea_longitude(const char *plot, const char *phem)
154 {
155         int ew;
156
157     if (*phem == 0)
158         return 0;
159
160     /* west long is negative, east long is positive */
161    ew = (*phem == 'E') ? 1 : -1;
162
163         return ew * convertToDegree(plot);
164 }
165
166 /*
167  * Return altitude in meter from a string.
168  *
169  */
170 static uint16_t nmea_altitude(const char *palt, const char *punits)
171 {
172         uint32_t alt;
173
174         if (*palt == 0)
175         return 0;
176
177         alt = atoi(palt);
178
179     if (*punits == 'F')
180         {
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 */
188
189     }
190
191         return alt;
192 }
193
194 /*
195  * Convert time and date stamp string to unix time.
196  */
197 static time_t timestampToSec(uint32_t time_stamp, uint32_t date_stamp)
198 {
199         struct tm t;
200         uint16_t msec;
201         uint16_t tmr[3];
202         uint16_t date[3];
203
204         memset(&t, 0, sizeof(t));
205         memset(&tmr, 0, sizeof(tmr));
206         memset(&date, 0, sizeof(date));
207
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;
212
213         for (int i = 0; i < 3; i++)
214         {
215                 all = res;
216                 res = all / 100;
217                 tmr[i]  = all - res * 100;
218                 LOG_INFO("t[%d]%d\n", tmr[i],i);
219         }
220
221         t.tm_sec = tmr[0] + (ROUND_UP(msec, 1000) / 1000);
222         t.tm_min = tmr[1];
223         t.tm_hour = tmr[2];
224         //If we not have refence data, we set as default 1/1/1970.
225         t.tm_mday = 1;
226         t.tm_mon = 0;
227         t.tm_year = 70;
228
229         if (date_stamp)
230         {
231                 res = all = date_stamp;
232                 for (int i = 0; i < 3; i++)
233                 {
234                         all = res;
235                         res = all / 100;
236                         date[i]  = all - res * 100;
237                         LOG_INFO("d[%d]%d\n", date[i],i);
238                 }
239                 t.tm_mday = date[2];
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;
244         }
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);
246
247         return  mktime(&t);
248 }
249
250 /**
251  *  Callout example for GGA data
252  */
253 void gpgga_callout(nmeap_context_t *context, void *data, void *user_data)
254 {
255         (void)context;
256         (void)user_data;
257         (void)data;
258         LOG_INFOB(
259                 NmeaGga *gga = (NmeaGga *)data;
260                 LOG_INFO("Found GPGGA message %ld %ld %d %lu %d %d %d %d\n",
261                                 (long)gga->latitude,
262                                 (long)gga->longitude,
263                                 gga->altitude,
264                                 gga->time,
265                                 gga->satellites,
266                                 gga->quality,
267                                 gga->hdop,
268                                 gga->geoid);
269         );
270 }
271
272 /**
273  * Callout example for RMC
274  */
275 void gprmc_callout(nmeap_context_t *context, void *data, void *user_data)
276 {
277         (void)context;
278         (void)user_data;
279         (void)data;
280     LOG_INFOB(
281                 NmeaRmc *rmc = (NmeaRmc *)data;
282
283                 LOG_INFO("Found GPRMC message %lu %c %ld %ld %d %d %d\n",
284                                 rmc->time,
285                                 rmc->warn,
286                                 (long)rmc->latitude,
287                                 (long)rmc->longitude,
288                                 rmc->speed,
289                                 rmc->course,
290                                 rmc->mag_var);
291         );
292 }
293
294 /**
295  * Callout example for GSV data
296  */
297 void gpgsv_callout(nmeap_context_t *context, void *data, void *user_data)
298 {
299         (void)context;
300         (void)user_data;
301         (void)data;
302         LOG_INFOB(
303                 NmeaGsv *gsv = (NmeaGsv *)data;
304
305             LOG_INFO("Found GPGSV message %d %d %d\n", gsv->tot_message, gsv->message_num, gsv->tot_svv);
306
307                 for (int i = 0; i < 4; i++)
308                         LOG_INFO("%d %d %d %d\n", gsv->info[i].sv_prn, gsv->info[i].elevation, gsv->info[i].azimut, gsv->info[i].snr);
309         );
310 }
311
312 /**
313  * Callout example for VTG data
314  */
315 void gpvtg_callout(nmeap_context_t *context, void *data, void *user_data)
316 {
317         (void)context;
318         (void)user_data;
319         (void)data;
320         LOG_INFOB(
321                 NmeaVtg *vtg = (NmeaVtg *)data;
322             LOG_INFO("Found GPVTG message %d %d %d\n", vtg->track_good, vtg->knot_speed, vtg->km_speed);
323         );
324 }
325
326
327
328 /**
329  * standard GPGGA sentence parser
330  */
331 int nmea_gpgga(nmeap_context_t *context, nmeap_sentence_t *sentence)
332 {
333         /*
334          * get pointer to sentence data
335          */
336         NmeaGga *gga = (NmeaGga *)sentence->data;
337
338         ASSERT(gga);
339         ASSERT(context->tokens >= 12);
340
341         gga->latitude   = nmea_latitude(context->token[2],context->token[3]);
342         gga->longitude  = nmea_longitude(context->token[4],context->token[5]);
343         gga->altitude   = nmea_altitude(context->token[9],context->token[10]);
344         gga->time       = timestampToSec(tokenToInt(context->token[1], 3), 0);
345         gga->satellites = atoi(context->token[7]);
346         gga->quality    = atoi(context->token[6]);
347         gga->hdop       = tokenToInt(context->token[8], 1);
348         gga->geoid      = nmea_altitude(context->token[11],context->token[12]);
349
350         /*
351          * if the sentence has a callout, call it
352          */
353
354         if (sentence->callout != 0)
355                 (*sentence->callout)(context, gga, context->user_data);
356
357         return NMEA_GPGGA;
358 }
359
360 /**
361  * standard GPRMCntence parser
362  */
363 int nmea_gprmc(nmeap_context_t *context, nmeap_sentence_t *sentence)
364 {
365
366     /*
367          * get pointer to sentence data
368          */
369     NmeaRmc *rmc = (NmeaRmc *)sentence->data;
370
371         ASSERT(rmc);
372         ASSERT(context->tokens >= 10);
373
374         /*
375          * extract data from the tokens
376          */
377         rmc->time       = timestampToSec(tokenToInt(context->token[1], 3), tokenToInt(context->token[9], 0));
378         rmc->warn       = *context->token[2];
379         rmc->latitude   = nmea_latitude(context->token[3],context->token[4]);
380         rmc->longitude  = nmea_longitude(context->token[5],context->token[6]);
381         rmc->speed      = atoi(context->token[7]);
382         rmc->course     = atoi(context->token[8]);
383         rmc->mag_var    = atoi(context->token[10]);
384
385     if (sentence->callout != 0)
386         (*sentence->callout)(context, rmc, context->user_data);
387
388     return NMEA_GPRMC;
389 }
390
391
392 /**
393  * standard GPVTG sentence parser
394  */
395 int nmea_gpvtg(nmeap_context_t *context, nmeap_sentence_t *sentence)
396 {
397
398     /*
399          * get pointer to sentence data
400          */
401     NmeaVtg *vtg = (NmeaVtg *)sentence->data;
402
403         ASSERT(vtg);
404         ASSERT(context->tokens >= 7);
405
406         /*
407          * extract data from the tokens
408          */
409         vtg->track_good  = atoi(context->token[1]);
410         vtg->knot_speed  = atoi(context->token[5]);
411         vtg->km_speed    = atoi(context->token[7]);
412
413     /*
414          * if the sentence has a callout, call it
415          */
416     if (sentence->callout != 0)
417         (*sentence->callout)(context, vtg, context->user_data);
418
419     return NMEA_GPVTG;
420 }
421
422 /**
423  * standard GPGDSV sentence parser
424  */
425 int nmea_gpgsv(nmeap_context_t *context, nmeap_sentence_t *sentence)
426 {
427
428     /*
429          * get pointer to sentence data
430          */
431     NmeaGsv *gsv = (NmeaGsv *)sentence->data;
432
433
434         /*
435          * extract data from the tokens
436          */
437         gsv->tot_message     = atoi(context->token[1]);
438         gsv->message_num     = atoi(context->token[2]);
439         gsv->tot_svv         = atoi(context->token[3]);
440
441         // Fill remaning member until we have token
442         int  j = 0;
443         for (int i = 4; i < context->tokens - 3; i += 4, j++)
444         {
445
446                 gsv->info[j].sv_prn     = atoi(context->token[i]);
447                 gsv->info[j].elevation  = atoi(context->token[i + 1]);
448                 gsv->info[j].azimut     = atoi(context->token[i + 2]);
449                 gsv->info[j].snr        = atoi(context->token[i + 3]);
450         }
451
452     /*
453          * if the sentence has a callout, call it
454          */
455     if (sentence->callout != 0)
456         (*sentence->callout)(context, gsv, context->user_data);
457
458     return NMEA_GPGSV;
459 }
460
461
462 /*
463  * Parse NMEA sentence from a channel.
464  */
465 void nmea_poll(nmeap_context_t *context, KFile *channel)
466 {
467         int c;
468         while ((c = kfile_getc(channel)) != EOF)
469         {
470                 nmeap_parse(context, c);
471         }
472 }
473