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