7cd20d4025ea6d20cb0aa3a5e51b03836857c5c4
[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 #include <nmea/time.h>
26
27 #include <string.h>
28 #include <math.h>
29
30 void nmea_zero_INFO(nmeaINFO *info)
31 {
32         if (!info) {
33                 return;
34         }
35
36     memset(info, 0, sizeof(nmeaINFO));
37     nmea_time_now(&info->utc);
38     info->sig = NMEA_SIG_BAD;
39     info->fix = NMEA_FIX_BAD;
40 }
41
42 /**
43  * Determine whether a given nmeaINFO structure has a certain field.
44  *
45  * nmeaINFO dependencies:
46  <pre>
47  field/sentence GPGGA   GPGSA   GPGSV   GPRMC   GPVTG
48  smask:         x       x       x       x       x
49  utc:           x                       x
50  sig:           x                       x
51  fix:                   x               x
52  PDOP:                  x
53  HDOP:          x       x
54  VDOP:                  x
55  lat:           x                       x
56  lon:           x                       x
57  elv:           x
58  speed:                                 x       x
59  direction:                             x       x
60  declination:                                   x
61  satinfo:               x       x
62  </pre>
63  *
64  * @param smask
65  * the smask of a nmeaINFO structure
66  * @param fieldName
67  * the field name
68  *
69  * @return
70  * - true when the nmeaINFO structure has the field
71  * - false otherwise
72  */
73 bool nmea_INFO_has_field(int smask, nmeaINFO_FIELD fieldName) {
74         switch (fieldName) {
75                 case SMASK:
76                         return true;
77
78                 case UTC:
79                 case SIG:
80                 case LAT:
81                 case LON:
82                         return ((smask & (GPGGA | GPRMC)) != 0);
83
84                 case FIX:
85                         return ((smask & (GPGSA | GPRMC)) != 0);
86
87                 case PDOP:
88                 case VDOP:
89                         return ((smask & GPGSA) != 0);
90
91                 case HDOP:
92                         return ((smask & (GPGGA | GPGSA)) != 0);
93
94                 case ELV:
95                         return ((smask & GPGGA) != 0);
96
97                 case SPEED:
98                 case DIRECTION:
99                         return ((smask & (GPRMC | GPVTG)) != 0);
100
101                 case DECLINATION:
102                         return ((smask & GPVTG) != 0);
103
104                 case SATINFO:
105                         return ((smask & (GPGSA | GPGSV)) != 0);
106
107                 default:
108                         return false;
109         }
110 }
111
112 /**
113  * Sanitise the NMEA info, make sure that:
114  * - latitude is in the range [-9000, 9000],
115  * - longitude is in the range [-18000, 18000],
116  * - DOPs are positive,
117  * - speed is positive,
118  * - direction is in the range [0, 360>.
119  *
120  * Time is set to the current time when not present.
121  *
122  * When a field is not present then it is reset to its default (NMEA_SIG_BAD,
123  * NMEA_FIX_BAD, 0).
124  *
125  * Satinfo is not touched.
126  *
127  * @param nmeaInfo
128  * the NMEA info structure to sanitise
129  */
130 void nmea_INFO_sanitise(nmeaINFO *nmeaInfo) {
131         double lat = 0;
132         double lon = 0;
133         double speed = 0;
134         double direction = 0;
135         bool latAdjusted = false;
136         bool lonAdjusted = false;
137         bool speedAdjusted = false;
138         bool directionAdjusted = false;
139
140         if (!nmeaInfo) {
141                 return;
142         }
143
144         if (!nmea_INFO_has_field(nmeaInfo->smask, UTC)) {
145                 nmea_time_now(&nmeaInfo->utc);
146         }
147
148         if (!nmea_INFO_has_field(nmeaInfo->smask, SIG)) {
149                 nmeaInfo->sig = NMEA_SIG_BAD;
150         }
151
152         if (!nmea_INFO_has_field(nmeaInfo->smask, FIX)) {
153                 nmeaInfo->fix = NMEA_FIX_BAD;
154         }
155
156         if (!nmea_INFO_has_field(nmeaInfo->smask, PDOP)) {
157                 nmeaInfo->PDOP = 0;
158         } else {
159                 nmeaInfo->PDOP = fabs(nmeaInfo->PDOP);
160         }
161
162         if (!nmea_INFO_has_field(nmeaInfo->smask, HDOP)) {
163                 nmeaInfo->HDOP = 0;
164         } else {
165                 nmeaInfo->HDOP = fabs(nmeaInfo->HDOP);
166         }
167
168         if (!nmea_INFO_has_field(nmeaInfo->smask, VDOP)) {
169                 nmeaInfo->VDOP = 0;
170         } else {
171                 nmeaInfo->VDOP = fabs(nmeaInfo->VDOP);
172         }
173
174         if (!nmea_INFO_has_field(nmeaInfo->smask, LAT)) {
175                 nmeaInfo->lat = 0;
176         }
177
178         if (!nmea_INFO_has_field(nmeaInfo->smask, LON)) {
179                 nmeaInfo->lon = 0;
180         }
181
182         if (!nmea_INFO_has_field(nmeaInfo->smask, ELV)) {
183                 nmeaInfo->elv = 0;
184         }
185
186         if (!nmea_INFO_has_field(nmeaInfo->smask, SPEED)) {
187                 nmeaInfo->speed = 0;
188         }
189
190         if (!nmea_INFO_has_field(nmeaInfo->smask, DIRECTION)) {
191                 nmeaInfo->direction = 0;
192         }
193
194         if (!nmea_INFO_has_field(nmeaInfo->smask, DECLINATION)) {
195                 nmeaInfo->declination = 0;
196         }
197
198         /* satinfo is not used */
199
200         /*
201          * lat
202          */
203
204         lat = nmeaInfo->lat;
205         lon = nmeaInfo->lon;
206
207         /* force lat in [-18000, 18000] */
208         while (lat < -18000.0) {
209                 lat += 36000.0;
210                 latAdjusted = true;
211         }
212         while (lat > 18000.0) {
213                 lat -= 36000.0;
214                 latAdjusted = true;
215         }
216
217         /* lat is now in [-18000, 18000] */
218
219         /* force lat from <9000, 18000] in [9000, 0] */
220         if (lat > 9000.0) {
221                 lat = 18000.0 - lat;
222                 lon += 18000.0;
223                 latAdjusted = true;
224                 lonAdjusted = true;
225         }
226
227         /* force lat from [-18000, -9000> in [0, -9000] */
228         if (lat < -9000.0) {
229                 lat = -18000.0 - lat;
230                 lon += 18000.0;
231                 latAdjusted = true;
232                 lonAdjusted = true;
233         }
234
235         /* lat is now in [-9000, 9000] */
236
237         if (latAdjusted) {
238                 nmeaInfo->lat = lat;
239         }
240
241         /*
242          * lon
243          */
244
245         /* force lon in [-18000, 18000] */
246         while (lon < -18000.0) {
247                 lon += 36000.0;
248                 lonAdjusted = true;
249         }
250         while (lon > 18000.0) {
251                 lon -= 36000.0;
252                 lonAdjusted = true;
253         }
254
255         /* lon is now in [-18000, 18000] */
256
257         if (lonAdjusted) {
258                 nmeaInfo->lon = lon;
259         }
260
261         /*
262          * speed
263          */
264
265         speed = nmeaInfo->speed;
266         direction = nmeaInfo->direction;
267
268         if (speed < 0.0) {
269                 speed = -speed;
270                 direction += 180.0;
271                 speedAdjusted = true;
272                 directionAdjusted = true;
273         }
274
275         /* speed is now in [0, max> */
276
277         if (speedAdjusted) {
278                 nmeaInfo->speed = speed;
279         }
280
281         /*
282          * direction
283          */
284
285         /* force direction in [0, 360> */
286         while (direction < 0.0) {
287                 direction += 360.0;
288                 directionAdjusted = true;
289         }
290         while (direction >= 360.0) {
291                 direction -= 360.0;
292                 directionAdjusted = true;
293         }
294
295         /* direction is now in [0, 360> */
296
297         if (directionAdjusted) {
298                 nmeaInfo->direction = direction;
299         }
300 }
301
302 /**
303  * Converts the position fields to degrees and DOP fields to meters so that
304  * all fields use normal metric units.
305  *
306  * @param nmeaInfo
307  * the nmeaINFO
308  */
309 void nmea_INFO_unit_conversion(nmeaINFO * nmeaInfo) {
310         if (!nmeaInfo) {
311                 return;
312         }
313
314         /* smask (already in correct format) */
315
316         /* utc (already in correct format) */
317
318         /* sig (already in correct format) */
319         /* fix (already in correct format) */
320
321         if (nmea_INFO_has_field(nmeaInfo->smask, PDOP)) {
322                 nmeaInfo->PDOP = nmea_dop2meters(nmeaInfo->PDOP);
323         }
324
325         if (nmea_INFO_has_field(nmeaInfo->smask, HDOP)) {
326                 nmeaInfo->HDOP = nmea_dop2meters(nmeaInfo->HDOP);
327         }
328
329         if (nmea_INFO_has_field(nmeaInfo->smask, VDOP)) {
330                 nmeaInfo->VDOP = nmea_dop2meters(nmeaInfo->VDOP);
331         }
332
333         if (nmea_INFO_has_field(nmeaInfo->smask, LAT)) {
334                 nmeaInfo->lat = nmea_ndeg2degree(nmeaInfo->lat);
335         }
336
337         if (nmea_INFO_has_field(nmeaInfo->smask, LON)) {
338                 nmeaInfo->lon = nmea_ndeg2degree(nmeaInfo->lon);
339         }
340
341         /* elv (already in correct format) */
342         /* speed (already in correct format) */
343         /* direction (already in correct format) */
344         /* declination (already in correct format) */
345
346         /* satinfo (not used) */
347 }