Merge branch 'release-0.6.6'
[olsrd.git] / lib / pud / nmealib / src / info.c
1 /*
2  * This file is part of nmealib.
3  *
4  * Copyright (c) 2008 Timur Sinitsyn
5  * Copyright (c) 2011 Ferry Huberts
6  *
7  * This library is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public
9  * License as published by the Free Software Foundation; either
10  * version 2.1 of the License, or (at your option) any later version.
11  *
12  * This library is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
15  * Lesser General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program. If not, see <http://www.gnu.org/licenses/>.
19  */
20
21 #include <nmea/info.h>
22
23 #include <nmea/sentence.h>
24 #include <nmea/gmath.h>
25
26 #include <stdbool.h>
27 #include <string.h>
28 #include <math.h>
29 #include <time.h>
30 #include <sys/time.h>
31 #include <assert.h>
32
33 /**
34  * Reset the time to now
35  *
36  * @param utc a pointer to the time structure
37  * @param present a pointer to a present field. when non-NULL then the UTCDATE
38  * and UTCTIME flags are set in it.
39  */
40 void nmea_time_now(nmeaTIME *utc, uint32_t * present) {
41         struct timeval tp;
42         struct tm tt;
43
44         assert(utc);
45
46         gettimeofday(&tp, NULL );
47         gmtime_r(&tp.tv_sec, &tt);
48
49         utc->year = tt.tm_year;
50         utc->mon = tt.tm_mon;
51         utc->day = tt.tm_mday;
52         utc->hour = tt.tm_hour;
53         utc->min = tt.tm_min;
54         utc->sec = tt.tm_sec;
55         utc->hsec = (tp.tv_usec / 10000);
56         if (present) {
57           nmea_INFO_set_present(present, UTCDATE | UTCTIME);
58         }
59 }
60
61 /**
62  * Clear an info structure.
63  * Resets the time to now, sets up the signal as BAD, the FIX as BAD, and
64  * signals presence of these fields.
65  * Resets all other fields to 0.
66  *
67  * @param info a pointer to the structure
68  */
69 void nmea_zero_INFO(nmeaINFO *info) {
70         if (!info) {
71                 return;
72         }
73
74         memset(info, 0, sizeof(nmeaINFO));
75         nmea_time_now(&info->utc, &info->present);
76
77         info->sig = NMEA_SIG_BAD;
78         nmea_INFO_set_present(&info->present, SIG);
79
80         info->fix = NMEA_FIX_BAD;
81         nmea_INFO_set_present(&info->present, FIX);
82 }
83
84 /**
85  * Determine if a nmeaINFO structure has a certain field (from the smask).
86  * Note: this is not the complete truth as fields may be absent in certain
87  * sentences and are presented as 'present' by this function.
88  *
89  * @deprecated use nmea_INFO_is_present instead
90  * @param smask the smask field
91  * @param fieldName use a name from nmeaINFO_FIELD
92  * @return a boolean, true when the structure has the requested field
93  */
94 bool nmea_INFO_is_present_smask(int smask, nmeaINFO_FIELD fieldName) {
95         switch (fieldName) {
96         case SMASK:
97                 return true;
98
99         case ELV:
100                 return ((smask & GPGGA) != 0);
101
102         case PDOP:
103         case VDOP:
104         case SATINUSE:
105                 return ((smask & GPGSA) != 0);
106
107         case SATINVIEW:
108                 return ((smask & GPGSV) != 0);
109
110         case UTCDATE:
111         case MAGVAR:
112                 return ((smask & GPRMC) != 0);
113
114         case MTRACK:
115                 return ((smask & GPVTG) != 0);
116
117         case SATINUSECOUNT:
118         case HDOP:
119                 return ((smask & (GPGGA | GPGSA)) != 0);
120
121         case UTCTIME:
122         case SIG:
123         case LAT:
124         case LON:
125                 return ((smask & (GPGGA | GPRMC)) != 0);
126
127         case FIX:
128                 return ((smask & (GPGSA | GPRMC)) != 0);
129
130         case SPEED:
131         case TRACK:
132                 return ((smask & (GPRMC | GPVTG)) != 0);
133
134         default:
135                 return false;
136         }
137 }
138
139 /**
140  * Determine if a nmeaINFO structure has a certain field
141  *
142  * @param present the presence field
143  * @param fieldName use a name from nmeaINFO_FIELD
144  * @return a boolean, true when the structure has the requested field
145  */
146 bool nmea_INFO_is_present(uint32_t present, nmeaINFO_FIELD fieldName) {
147         return ((present & fieldName) != 0);
148 }
149
150 /**
151  * Flag a nmeaINFO structure to contain a certain field
152  *
153  * @param present a pointer to the presence field
154  * @param fieldName use a name from nmeaINFO_FIELD
155  */
156 void nmea_INFO_set_present(uint32_t * present, nmeaINFO_FIELD fieldName) {
157         assert(present);
158         *present |= fieldName;
159 }
160
161 /**
162  * Flag a nmeaINFO structure to NOT contain a certain field
163  *
164  * @param present a pointer to the presence field
165  * @param fieldName use a name from nmeaINFO_FIELD
166  */
167 void nmea_INFO_unset_present(uint32_t * present, nmeaINFO_FIELD fieldName) {
168         assert(present);
169         *present &= ~fieldName;
170 }
171
172 /**
173  * Sanitise the NMEA info, make sure that:
174  * - sig is in the range [0, 8],
175  * - fix is in the range [1, 3],
176  * - DOPs are positive,
177  * - latitude is in the range [-9000, 9000],
178  * - longitude is in the range [-18000, 18000],
179  * - speed is positive,
180  * - track is in the range [0, 360>.
181  * - mtrack is in the range [0, 360>.
182  * - magvar is in the range [0, 360>.
183  * - satinfo:
184  *   - inuse and in_use are consistent (w.r.t. count)
185  *   - inview and sat are consistent (w.r.t. count/id)
186  *   - in_use and sat are consistent (w.r.t. count/id)
187  *   - elv is in the range [0, 90]
188  *   - azimuth is in the range [0, 359]
189  *   - sig is in the range [0, 99]
190  *
191  * Time is set to the current time when not present.
192  * Fields are reset to their defaults (0) when not signaled as being present.
193  *
194  * @param nmeaInfo
195  * the NMEA info structure to sanitise
196  */
197 void nmea_INFO_sanitise(nmeaINFO *nmeaInfo) {
198         double lat = 0;
199         double lon = 0;
200         double speed = 0;
201         double track = 0;
202         double mtrack = 0;
203         double magvar = 0;
204         bool latAdjusted = false;
205         bool lonAdjusted = false;
206         bool speedAdjusted = false;
207         bool trackAdjusted = false;
208         bool mtrackAdjusted = false;
209         bool magvarAdjusted = false;
210         nmeaTIME utc;
211         int inuseIndex;
212         int inviewIndex;
213
214         if (!nmeaInfo) {
215                 return;
216         }
217
218         nmeaInfo->present = nmeaInfo->present & NMEA_INFO_PRESENT_MASK;
219
220         if (!nmea_INFO_is_present(nmeaInfo->present, SMASK)) {
221                 nmeaInfo->smask = 0;
222         }
223
224         if (!nmea_INFO_is_present(nmeaInfo->present, UTCDATE) || !nmea_INFO_is_present(nmeaInfo->present, UTCTIME)) {
225                 nmea_time_now(&utc, NULL);
226         }
227
228         if (!nmea_INFO_is_present(nmeaInfo->present, UTCDATE)) {
229                 nmeaInfo->utc.year = utc.year;
230                 nmeaInfo->utc.mon = utc.mon;
231                 nmeaInfo->utc.day = utc.day;
232         }
233
234         if (!nmea_INFO_is_present(nmeaInfo->present, UTCTIME)) {
235                 nmeaInfo->utc.hour = utc.hour;
236                 nmeaInfo->utc.min = utc.min;
237                 nmeaInfo->utc.sec = utc.sec;
238                 nmeaInfo->utc.hsec = utc.hsec;
239         }
240
241         if (!nmea_INFO_is_present(nmeaInfo->present, SIG)) {
242                 nmeaInfo->sig = NMEA_SIG_BAD;
243         } else {
244                 if ((nmeaInfo->sig < NMEA_SIG_BAD) || (nmeaInfo->sig > NMEA_SIG_SIM)) {
245                         nmeaInfo->sig = NMEA_SIG_BAD;
246                 }
247         }
248
249         if (!nmea_INFO_is_present(nmeaInfo->present, FIX)) {
250                 nmeaInfo->fix = NMEA_FIX_BAD;
251         } else {
252                 if ((nmeaInfo->fix < NMEA_FIX_BAD) || (nmeaInfo->fix > NMEA_FIX_3D)) {
253                         nmeaInfo->fix = NMEA_FIX_BAD;
254                 }
255         }
256
257         if (!nmea_INFO_is_present(nmeaInfo->present, PDOP)) {
258                 nmeaInfo->PDOP = 0;
259         } else {
260                 nmeaInfo->PDOP = fabs(nmeaInfo->PDOP);
261         }
262
263         if (!nmea_INFO_is_present(nmeaInfo->present, HDOP)) {
264                 nmeaInfo->HDOP = 0;
265         } else {
266                 nmeaInfo->HDOP = fabs(nmeaInfo->HDOP);
267         }
268
269         if (!nmea_INFO_is_present(nmeaInfo->present, VDOP)) {
270                 nmeaInfo->VDOP = 0;
271         } else {
272                 nmeaInfo->VDOP = fabs(nmeaInfo->VDOP);
273         }
274
275         if (!nmea_INFO_is_present(nmeaInfo->present, LAT)) {
276                 nmeaInfo->lat = 0;
277         }
278
279         if (!nmea_INFO_is_present(nmeaInfo->present, LON)) {
280                 nmeaInfo->lon = 0;
281         }
282
283         if (!nmea_INFO_is_present(nmeaInfo->present, ELV)) {
284                 nmeaInfo->elv = 0;
285         }
286
287         if (!nmea_INFO_is_present(nmeaInfo->present, SPEED)) {
288                 nmeaInfo->speed = 0;
289         }
290
291         if (!nmea_INFO_is_present(nmeaInfo->present, TRACK)) {
292                 nmeaInfo->track = 0;
293         }
294
295         if (!nmea_INFO_is_present(nmeaInfo->present, MTRACK)) {
296                 nmeaInfo->mtrack = 0;
297         }
298
299         if (!nmea_INFO_is_present(nmeaInfo->present, MAGVAR)) {
300                 nmeaInfo->magvar = 0;
301         }
302
303         if (!nmea_INFO_is_present(nmeaInfo->present, SATINUSECOUNT)) {
304                 nmeaInfo->satinfo.inuse = 0;
305         }
306         if (!nmea_INFO_is_present(nmeaInfo->present, SATINUSE)) {
307                 memset(&nmeaInfo->satinfo.in_use, 0, sizeof(nmeaInfo->satinfo.in_use));
308         }
309         if (!nmea_INFO_is_present(nmeaInfo->present, SATINVIEW)) {
310                 nmeaInfo->satinfo.inview = 0;
311                 memset(&nmeaInfo->satinfo.sat, 0, sizeof(nmeaInfo->satinfo.sat));
312         }
313
314         /*
315          * lat
316          */
317
318         lat = nmeaInfo->lat;
319         lon = nmeaInfo->lon;
320
321         /* force lat in [-18000, 18000] */
322         while (lat < -18000.0) {
323                 lat += 36000.0;
324                 latAdjusted = true;
325         }
326         while (lat > 18000.0) {
327                 lat -= 36000.0;
328                 latAdjusted = true;
329         }
330
331         /* lat is now in [-18000, 18000] */
332
333         /* force lat from <9000, 18000] in [9000, 0] */
334         if (lat > 9000.0) {
335                 lat = 18000.0 - lat;
336                 lon += 18000.0;
337                 latAdjusted = true;
338                 lonAdjusted = true;
339         }
340
341         /* force lat from [-18000, -9000> in [0, -9000] */
342         if (lat < -9000.0) {
343                 lat = -18000.0 - lat;
344                 lon += 18000.0;
345                 latAdjusted = true;
346                 lonAdjusted = true;
347         }
348
349         /* lat is now in [-9000, 9000] */
350
351         if (latAdjusted) {
352                 nmeaInfo->lat = lat;
353         }
354
355         /*
356          * lon
357          */
358
359         /* force lon in [-18000, 18000] */
360         while (lon < -18000.0) {
361                 lon += 36000.0;
362                 lonAdjusted = true;
363         }
364         while (lon > 18000.0) {
365                 lon -= 36000.0;
366                 lonAdjusted = true;
367         }
368
369         /* lon is now in [-18000, 18000] */
370
371         if (lonAdjusted) {
372                 nmeaInfo->lon = lon;
373         }
374
375         /*
376          * speed
377          */
378
379         speed = nmeaInfo->speed;
380         track = nmeaInfo->track;
381         mtrack = nmeaInfo->mtrack;
382
383         if (speed < 0.0) {
384                 speed = -speed;
385                 track += 180.0;
386                 mtrack += 180.0;
387                 speedAdjusted = true;
388                 trackAdjusted = true;
389                 mtrackAdjusted = true;
390         }
391
392         /* speed is now in [0, max> */
393
394         if (speedAdjusted) {
395                 nmeaInfo->speed = speed;
396         }
397
398         /*
399          * track
400          */
401
402         /* force track in [0, 360> */
403         while (track < 0.0) {
404                 track += 360.0;
405                 trackAdjusted = true;
406         }
407         while (track >= 360.0) {
408                 track -= 360.0;
409                 trackAdjusted = true;
410         }
411
412         /* track is now in [0, 360> */
413
414         if (trackAdjusted) {
415                 nmeaInfo->track = track;
416         }
417
418         /*
419          * mtrack
420          */
421
422         /* force mtrack in [0, 360> */
423         while (mtrack < 0.0) {
424                 mtrack += 360.0;
425                 mtrackAdjusted = true;
426         }
427         while (mtrack >= 360.0) {
428                 mtrack -= 360.0;
429                 mtrackAdjusted = true;
430         }
431
432         /* mtrack is now in [0, 360> */
433
434         if (mtrackAdjusted) {
435                 nmeaInfo->mtrack = mtrack;
436         }
437
438         /*
439          * magvar
440          */
441
442         magvar = nmeaInfo->magvar;
443
444         /* force magvar in [0, 360> */
445         while (magvar < 0.0) {
446                 magvar += 360.0;
447                 magvarAdjusted = true;
448         }
449         while (magvar >= 360.0) {
450                 magvar -= 360.0;
451                 magvarAdjusted = true;
452         }
453
454         /* magvar is now in [0, 360> */
455
456         if (magvarAdjusted) {
457                 nmeaInfo->magvar = magvar;
458         }
459
460         /*
461          * satinfo
462          */
463
464         nmeaInfo->satinfo.inuse = 0;
465         for (inuseIndex = 0; inuseIndex < NMEA_MAXSAT; inuseIndex++) {
466                 if (nmeaInfo->satinfo.in_use[inuseIndex])
467                         nmeaInfo->satinfo.inuse++;
468         }
469
470         nmeaInfo->satinfo.inview = 0;
471         for (inviewIndex = 0; inviewIndex < NMEA_MAXSAT; inviewIndex++) {
472                 if (nmeaInfo->satinfo.sat[inviewIndex].id) {
473                         nmeaInfo->satinfo.inview++;
474
475                         /* force elv in [-180, 180] */
476                         while (nmeaInfo->satinfo.sat[inviewIndex].elv < -180) {
477                                 nmeaInfo->satinfo.sat[inviewIndex].elv += 360;
478                         }
479                         while (nmeaInfo->satinfo.sat[inviewIndex].elv > 180) {
480                                 nmeaInfo->satinfo.sat[inviewIndex].elv -= 360;
481                         }
482
483                         /* elv is now in [-180, 180] */
484
485                         /* force elv from <90, 180] in [90, 0] */
486                         if (nmeaInfo->satinfo.sat[inviewIndex].elv > 90) {
487                                 nmeaInfo->satinfo.sat[inviewIndex].elv = 180 - nmeaInfo->satinfo.sat[inviewIndex].elv;
488                         }
489
490                         /* force elv from [-180, -90> in [0, -90] */
491                         if (nmeaInfo->satinfo.sat[inviewIndex].elv < -90) {
492                                 nmeaInfo->satinfo.sat[inviewIndex].elv = -180 - nmeaInfo->satinfo.sat[inviewIndex].elv;
493                         }
494
495                         /* elv is now in [-90, 90] */
496
497                         if (nmeaInfo->satinfo.sat[inviewIndex].elv < 0) {
498                                 nmeaInfo->satinfo.sat[inviewIndex].elv = -nmeaInfo->satinfo.sat[inviewIndex].elv;
499                         }
500
501                         /* elv is now in [0, 90] */
502
503                         /* force azimuth in [0, 360> */
504                         while (nmeaInfo->satinfo.sat[inviewIndex].azimuth < 0) {
505                                 nmeaInfo->satinfo.sat[inviewIndex].azimuth += 360;
506                         }
507                         while (nmeaInfo->satinfo.sat[inviewIndex].azimuth >= 360) {
508                                 nmeaInfo->satinfo.sat[inviewIndex].azimuth -= 360;
509                         }
510                         /* azimuth is now in [0, 360> */
511
512                         /* force sig in [0, 99] */
513                         if (nmeaInfo->satinfo.sat[inviewIndex].sig < 0)
514                                 nmeaInfo->satinfo.sat[inviewIndex].sig = 0;
515                         if (nmeaInfo->satinfo.sat[inviewIndex].sig > 99)
516                                 nmeaInfo->satinfo.sat[inviewIndex].sig = 99;
517                 }
518         }
519
520         /* make sure the in_use IDs map to sat IDs */
521         for (inuseIndex = 0; inuseIndex < NMEA_MAXSAT; inuseIndex++) {
522                 int inuseID = nmeaInfo->satinfo.in_use[inuseIndex];
523                 if (inuseID) {
524                         bool found = false;
525                         for (inviewIndex = 0; inviewIndex < NMEA_MAXSAT; inviewIndex++) {
526                                 int inviewID = nmeaInfo->satinfo.sat[inviewIndex].id;
527                                 if (inuseID == inviewID) {
528                                         found = true;
529                                         break;
530                                 }
531                         }
532                         if (!found) {
533                                 /* clear the id, did not find it */
534                                 nmeaInfo->satinfo.in_use[inuseIndex] = 0;
535                                 if (nmeaInfo->satinfo.inuse)
536                                         nmeaInfo->satinfo.inuse--;
537                         }
538                 }
539         }
540 }
541
542 /**
543  * Converts the position fields to degrees and DOP fields to meters so that
544  * all fields use normal metric units.
545  *
546  * @param nmeaInfo
547  * the nmeaINFO
548  */
549 void nmea_INFO_unit_conversion(nmeaINFO * nmeaInfo) {
550         if (!nmeaInfo) {
551                 return;
552         }
553
554         /* smask (already in correct format) */
555
556         /* utc (already in correct format) */
557
558         /* sig (already in correct format) */
559         /* fix (already in correct format) */
560
561         if (nmea_INFO_is_present(nmeaInfo->present, PDOP)) {
562                 nmeaInfo->PDOP = nmea_dop2meters(nmeaInfo->PDOP);
563         }
564
565         if (nmea_INFO_is_present(nmeaInfo->present, HDOP)) {
566                 nmeaInfo->HDOP = nmea_dop2meters(nmeaInfo->HDOP);
567         }
568
569         if (nmea_INFO_is_present(nmeaInfo->present, VDOP)) {
570                 nmeaInfo->VDOP = nmea_dop2meters(nmeaInfo->VDOP);
571         }
572
573         if (nmea_INFO_is_present(nmeaInfo->present, LAT)) {
574                 nmeaInfo->lat = nmea_ndeg2degree(nmeaInfo->lat);
575         }
576
577         if (nmea_INFO_is_present(nmeaInfo->present, LON)) {
578                 nmeaInfo->lon = nmea_ndeg2degree(nmeaInfo->lon);
579         }
580
581         /* elv (already in correct format) */
582         /* speed (already in correct format) */
583         /* track (already in correct format) */
584         /* mtrack (already in correct format) */
585         /* magvar (already in correct format) */
586
587         /* satinfo (already in correct format) */
588 }