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