pud: update nmealib to 2.0.0
[olsrd.git] / lib / pud / nmealib / src / conversions.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/conversions.h>
19
20 #include <nmea/gmath.h>
21
22 #include <assert.h>
23 #include <string.h>
24 #include <math.h>
25
26 /**
27  * Determine the number of GSV sentences needed for a number of sats
28  *
29  * @param sats the number of sats
30  * @return the number of GSV sentences needed
31  */
32 int nmea_gsv_npack(int sats) {
33         int pack_count = sats / NMEA_SATINPACK;
34
35         if ((sats % NMEA_SATINPACK) > 0)
36                 pack_count++;
37
38         if (!pack_count)
39                 pack_count++;
40
41         return pack_count;
42 }
43
44 /**
45  * Fill nmeaINFO structure from GGA packet structure
46  *
47  * @param pack a pointer to the packet structure
48  * @param info a pointer to the nmeaINFO structure
49  */
50 void nmea_GPGGA2info(const nmeaGPGGA *pack, nmeaINFO *info) {
51         assert(pack);
52         assert(info);
53
54         info->present |= pack->present;
55         nmea_INFO_set_present(&info->present, SMASK);
56         info->smask |= GPGGA;
57         if (nmea_INFO_is_present(pack->present, UTCTIME)) {
58                 info->utc.hour = pack->utc.hour;
59                 info->utc.min = pack->utc.min;
60                 info->utc.sec = pack->utc.sec;
61                 info->utc.hsec = pack->utc.hsec;
62         }
63         if (nmea_INFO_is_present(pack->present, LAT)) {
64                 info->lat = ((pack->ns == 'N') ? pack->lat : -pack->lat);
65         }
66         if (nmea_INFO_is_present(pack->present, LON)) {
67                 info->lon = ((pack->ew == 'E') ? pack->lon : -pack->lon);
68         }
69         if (nmea_INFO_is_present(pack->present, SIG)) {
70                 info->sig = pack->sig;
71         }
72         if (nmea_INFO_is_present(pack->present, SATINUSECOUNT)) {
73                 info->satinfo.inuse = pack->satinuse;
74         }
75         if (nmea_INFO_is_present(pack->present, HDOP)) {
76                 info->HDOP = pack->HDOP;
77         }
78         if (nmea_INFO_is_present(pack->present, ELV)) {
79                 info->elv = pack->elv;
80         }
81         /* ignore diff and diff_units */
82         /* ignore dgps_age and dgps_sid */
83 }
84
85 /**
86  * Convert an nmeaINFO structure into an nmeaGPGGA structure
87  *
88  * @param info a pointer to the nmeaINFO structure
89  * @param pack a pointer to the nmeaGPGGA structure
90  */
91 void nmea_info2GPGGA(const nmeaINFO *info, nmeaGPGGA *pack) {
92         assert(pack);
93         assert(info);
94
95         nmea_zero_GPGGA(pack);
96
97         pack->present = info->present;
98         nmea_INFO_unset_present(&pack->present, SMASK);
99         if (nmea_INFO_is_present(info->present, UTCTIME)) {
100                 pack->utc.hour = info->utc.hour;
101                 pack->utc.min = info->utc.min;
102                 pack->utc.sec = info->utc.sec;
103                 pack->utc.hsec = info->utc.hsec;
104         }
105         if (nmea_INFO_is_present(info->present, LAT)) {
106                 pack->lat = fabs(info->lat);
107                 pack->ns = ((info->lat > 0) ? 'N' : 'S');
108         }
109         if (nmea_INFO_is_present(info->present, LON)) {
110                 pack->lon = fabs(info->lon);
111                 pack->ew = ((info->lon > 0) ? 'E' : 'W');
112         }
113         if (nmea_INFO_is_present(info->present, SIG)) {
114                 pack->sig = info->sig;
115         }
116         if (nmea_INFO_is_present(info->present, SATINUSECOUNT)) {
117                 pack->satinuse = info->satinfo.inuse;
118         }
119         if (nmea_INFO_is_present(info->present, HDOP)) {
120                 pack->HDOP = info->HDOP;
121         }
122         if (nmea_INFO_is_present(info->present, ELV)) {
123                 pack->elv = info->elv;
124                 pack->elv_units = 'M';
125         }
126         /* defaults for (ignored) diff and diff_units */
127         pack->diff = 0;
128         pack->diff_units = 'M';
129         /* defaults for (ignored) dgps_age and dgps_sid */
130         pack->dgps_age = 0;
131         pack->dgps_sid = 0;
132 }
133
134 /**
135  * Fill nmeaINFO structure from GSA packet structure
136  *
137  * @param pack a pointer to the packet structure
138  * @param info a pointer to the nmeaINFO structure
139  */
140 void nmea_GPGSA2info(const nmeaGPGSA *pack, nmeaINFO *info) {
141         int i = 0;
142
143         assert(pack);
144         assert(info);
145
146         info->present |= pack->present;
147         nmea_INFO_set_present(&info->present, SMASK);
148         info->smask |= GPGSA;
149         if (nmea_INFO_is_present(pack->present, FIX)) {
150                 /* fix_mode is ignored */
151                 info->fix = pack->fix_type;
152         }
153         if (nmea_INFO_is_present(pack->present, SATINUSE)) {
154                 info->satinfo.inuse = 0;
155                 for (i = 0; i < NMEA_MAXSAT; i++) {
156                         info->satinfo.in_use[i] = pack->sat_prn[i];
157                         if (pack->sat_prn[i]) {
158                                 info->satinfo.inuse++;
159                         }
160                 }
161                 nmea_INFO_set_present(&info->present, SATINUSECOUNT);
162         }
163         if (nmea_INFO_is_present(pack->present, PDOP)) {
164                 info->PDOP = pack->PDOP;
165         }
166         if (nmea_INFO_is_present(pack->present, HDOP)) {
167                 info->HDOP = pack->HDOP;
168         }
169         if (nmea_INFO_is_present(pack->present, VDOP)) {
170                 info->VDOP = pack->VDOP;
171         }
172 }
173
174 /**
175  * Convert an nmeaINFO structure into an nmeaGPGSA structure
176  *
177  * @param info a pointer to the nmeaINFO structure
178  * @param pack a pointer to the nmeaGPGSA structure
179  */
180 void nmea_info2GPGSA(const nmeaINFO *info, nmeaGPGSA *pack) {
181         assert(pack);
182         assert(info);
183
184         nmea_zero_GPGSA(pack);
185
186         pack->present = info->present;
187         nmea_INFO_unset_present(&pack->present, SMASK);
188         if (nmea_INFO_is_present(info->present, FIX)) {
189                 pack->fix_mode = 'A';
190                 pack->fix_type = info->fix;
191         }
192         if (nmea_INFO_is_present(info->present, SATINUSE)) {
193                 memcpy(pack->sat_prn, info->satinfo.in_use, sizeof(pack->sat_prn));
194         }
195         if (nmea_INFO_is_present(info->present, PDOP)) {
196                 pack->PDOP = info->PDOP;
197         }
198         if (nmea_INFO_is_present(info->present, HDOP)) {
199                 pack->HDOP = info->HDOP;
200         }
201         if (nmea_INFO_is_present(info->present, VDOP)) {
202                 pack->VDOP = info->VDOP;
203         }
204 }
205
206 /**
207  * Fill nmeaINFO structure from GSV packet structure
208  *
209  * @param pack a pointer to the packet structure
210  * @param info a pointer to the nmeaINFO structure
211  */
212 void nmea_GPGSV2info(const nmeaGPGSV *pack, nmeaINFO *info) {
213         int pack_index;
214
215         assert(pack);
216         assert(info);
217
218         pack_index = pack->pack_index;
219         if (pack_index < 1)
220                 pack_index = 1;
221
222         if (pack_index > pack->pack_count)
223                 pack_index = pack->pack_count;
224
225         if ((pack_index * NMEA_SATINPACK) > NMEA_MAXSAT)
226                 pack_index = NMEA_NSATPACKS;
227
228         info->present |= pack->present;
229         nmea_INFO_set_present(&info->present, SMASK);
230         info->smask |= GPGSV;
231         if (nmea_INFO_is_present(pack->present, SATINVIEW)) {
232                 int sat_index;
233
234                 /* index of 1st sat in pack */
235                 int sat_offset = (pack_index - 1) * NMEA_SATINPACK;
236                 /* the number of sats in this sentence */
237                 int sat_count = ((sat_offset + NMEA_SATINPACK) > pack->sat_count) ? (pack->sat_count - sat_offset) : NMEA_SATINPACK;
238
239                 for (sat_index = 0; sat_index < sat_count; sat_index++) {
240                         info->satinfo.sat[sat_offset + sat_index].id = pack->sat_data[sat_index].id;
241                         info->satinfo.sat[sat_offset + sat_index].elv = pack->sat_data[sat_index].elv;
242                         info->satinfo.sat[sat_offset + sat_index].azimuth = pack->sat_data[sat_index].azimuth;
243                         info->satinfo.sat[sat_offset + sat_index].sig = pack->sat_data[sat_index].sig;
244                 }
245
246                 info->satinfo.inview = pack->sat_count;
247         }
248 }
249
250 /**
251  * Convert an nmeaINFO structure into an nmeaGPGSV structure
252  *
253  * @param info a pointer to the nmeaINFO structure
254  * @param pack a pointer to the nmeaGPGSV structure
255  * @param pack_idx pack index (zero based)
256  */
257 void nmea_info2GPGSV(const nmeaINFO *info, nmeaGPGSV *pack, int pack_idx) {
258         assert(pack);
259         assert(info);
260
261         nmea_zero_GPGSV(pack);
262
263         pack->present = info->present;
264         nmea_INFO_unset_present(&pack->present, SMASK);
265         if (nmea_INFO_is_present(info->present, SATINVIEW)) {
266                 int sit;
267                 int pit;
268                 int toskip;
269
270                 pack->sat_count = (info->satinfo.inview < NMEA_MAXSAT) ? info->satinfo.inview : NMEA_MAXSAT;
271                 pack->pack_count = nmea_gsv_npack(pack->sat_count);
272
273                 if (pack_idx >= pack->pack_count)
274                         pack->pack_index = pack->pack_count;
275                 else
276                         pack->pack_index = pack_idx + 1;
277
278                 /* now skip the first ((pack->pack_index - 1) * NMEA_SATINPACK) in view sats */
279                 toskip = ((pack->pack_index - 1) * NMEA_SATINPACK);
280                 sit = 0;
281                 while ((toskip > 0) && (sit < NMEA_MAXSAT)) {
282                         if (info->satinfo.sat[sit].id) {
283                                 toskip--;
284                         }
285                         sit++;
286                 }
287
288                 for (pit = 0; pit < NMEA_SATINPACK; sit++) {
289                         if (sit < NMEA_MAXSAT) {
290                                 if (info->satinfo.sat[sit].id) {
291                                         pack->sat_data[pit] = info->satinfo.sat[sit];
292                                         pit++;
293                                 }
294                         } else {
295                                 memset(&pack->sat_data[pit], 0, sizeof(pack->sat_data[pit]));
296                                 pit++;
297                         }
298                 }
299         }
300 }
301
302 /**
303  * Fill nmeaINFO structure from RMC packet structure
304  *
305  * @param pack a pointer to the packet structure
306  * @param info a pointer to the nmeaINFO structure
307  */
308 void nmea_GPRMC2info(const nmeaGPRMC *pack, nmeaINFO *info) {
309         assert(pack);
310         assert(info);
311
312         info->present |= pack->present;
313         nmea_INFO_set_present(&info->present, SMASK);
314         info->smask |= GPRMC;
315         if (nmea_INFO_is_present(pack->present, UTCDATE)) {
316                 info->utc.year = pack->utc.year;
317                 info->utc.mon = pack->utc.mon;
318                 info->utc.day = pack->utc.day;
319         }
320         if (nmea_INFO_is_present(pack->present, UTCTIME)) {
321                 info->utc.hour = pack->utc.hour;
322                 info->utc.min = pack->utc.min;
323                 info->utc.sec = pack->utc.sec;
324                 info->utc.hsec = pack->utc.hsec;
325         }
326         nmea_INFO_set_present(&info->present, SIG);
327         nmea_INFO_set_present(&info->present, FIX);
328         if (pack->status == 'A') {
329                 if (info->sig == NMEA_SIG_BAD) {
330                         info->sig = NMEA_SIG_MID;
331                 }
332                 if (info->fix == NMEA_FIX_BAD) {
333                         info->fix = NMEA_FIX_2D;
334                 }
335         } else {
336                 info->sig = NMEA_SIG_BAD;
337                 info->fix = NMEA_FIX_BAD;
338         }
339         if (nmea_INFO_is_present(pack->present, LAT)) {
340                 info->lat = ((pack->ns == 'N') ? pack->lat : -pack->lat);
341         }
342         if (nmea_INFO_is_present(pack->present, LON)) {
343                 info->lon = ((pack->ew == 'E') ? pack->lon : -pack->lon);
344         }
345         if (nmea_INFO_is_present(pack->present, SPEED)) {
346                 info->speed = pack->speed * NMEA_TUD_KNOTS;
347         }
348         if (nmea_INFO_is_present(pack->present, TRACK)) {
349                 info->track = pack->track;
350         }
351         if (nmea_INFO_is_present(pack->present, MAGVAR)) {
352                 info->magvar = ((pack->magvar_ew == 'E') ? pack->magvar : -pack->magvar);
353         }
354         /* mode is ignored */
355 }
356
357 /**
358  * Convert an nmeaINFO structure into an nmeaGPRMC structure
359  *
360  * @param info a pointer to the nmeaINFO structure
361  * @param pack a pointer to the nmeaGPRMC structure
362  */
363 void nmea_info2GPRMC(const nmeaINFO *info, nmeaGPRMC *pack) {
364         assert(pack);
365         assert(info);
366
367         nmea_zero_GPRMC(pack);
368
369         pack->present = info->present;
370         nmea_INFO_unset_present(&pack->present, SMASK);
371         if (nmea_INFO_is_present(info->present, UTCDATE)) {
372                 pack->utc.year = info->utc.year;
373                 pack->utc.mon = info->utc.mon;
374                 pack->utc.day = info->utc.day;
375         }
376         if (nmea_INFO_is_present(info->present, UTCTIME)) {
377                 pack->utc.hour = info->utc.hour;
378                 pack->utc.min = info->utc.min;
379                 pack->utc.sec = info->utc.sec;
380                 pack->utc.hsec = info->utc.hsec;
381         }
382         if (nmea_INFO_is_present(info->present, SIG)) {
383                 pack->status = ((info->sig != NMEA_SIG_BAD) ? 'A' : 'V');
384         } else {
385                 pack->status = 'V';
386         }
387         if (nmea_INFO_is_present(info->present, LAT)) {
388                 pack->lat = fabs(info->lat);
389                 pack->ns = ((info->lat > 0) ? 'N' : 'S');
390         }
391         if (nmea_INFO_is_present(info->present, LON)) {
392                 pack->lon = fabs(info->lon);
393                 pack->ew = ((info->lon > 0) ? 'E' : 'W');
394         }
395         if (nmea_INFO_is_present(info->present, SPEED)) {
396                 pack->speed = info->speed / NMEA_TUD_KNOTS;
397         }
398         if (nmea_INFO_is_present(info->present, TRACK)) {
399                 pack->track = info->track;
400         }
401         if (nmea_INFO_is_present(info->present, MAGVAR)) {
402                 pack->magvar = fabs(info->magvar);
403                 pack->magvar_ew = ((info->magvar > 0) ? 'E' : 'W');
404         }
405         if (nmea_INFO_is_present(info->present, SIG)) {
406                 pack->mode = ((info->sig != NMEA_SIG_BAD) ? 'A' : 'N');
407         } else {
408                 pack->mode = 'N';
409         }
410 }
411
412 /**
413  * Fill nmeaINFO structure from VTG packet structure
414  *
415  * @param pack a pointer to the packet structure
416  * @param info a pointer to the nmeaINFO structure
417  */
418 void nmea_GPVTG2info(const nmeaGPVTG *pack, nmeaINFO *info) {
419         assert(pack);
420         assert(info);
421
422         info->present |= pack->present;
423         nmea_INFO_set_present(&info->present, SMASK);
424         info->smask |= GPVTG;
425         if (nmea_INFO_is_present(pack->present, SPEED)) {
426                 info->speed = pack->spk;
427         }
428         if (nmea_INFO_is_present(pack->present, TRACK)) {
429                 info->track = pack->track;
430         }
431         if (nmea_INFO_is_present(pack->present, MTRACK)) {
432                 info->mtrack = pack->mtrack;
433         }
434 }
435
436 /**
437  * Convert an nmeaINFO structure into an nmeaGPVTG structure
438  *
439  * @param info a pointer to the nmeaINFO structure
440  * @param pack a pointer to the nmeaGPRMC structure
441  */
442 void nmea_info2GPVTG(const nmeaINFO *info, nmeaGPVTG *pack) {
443         assert(pack);
444         assert(info);
445
446         nmea_zero_GPVTG(pack); /* also sets up units */
447
448         pack->present = info->present;
449         nmea_INFO_unset_present(&pack->present, SMASK);
450         if (nmea_INFO_is_present(info->present, TRACK)) {
451                 pack->track = info->track;
452         }
453         if (nmea_INFO_is_present(info->present, MTRACK)) {
454                 pack->mtrack = info->mtrack;
455         }
456         if (nmea_INFO_is_present(info->present, SPEED)) {
457                 pack->spn = info->speed / NMEA_TUD_KNOTS;
458                 pack->spk = info->speed;
459         }
460 }