edfc96027240f49df7c0a675f88e470a8f404a20
[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         assert(pack);
263         assert(info);
264
265         nmea_zero_GPGSV(pack);
266
267         pack->present = info->present;
268         nmea_INFO_unset_present(&pack->present, SMASK);
269         if (nmea_INFO_is_present(info->present, SATINVIEW)) {
270                 int sit;
271                 int pit;
272                 int toskip;
273
274                 pack->sat_count = (info->satinfo.inview < NMEA_MAXSAT) ? info->satinfo.inview : NMEA_MAXSAT;
275                 pack->pack_count = nmea_gsv_npack(pack->sat_count);
276
277                 if (pack_idx >= pack->pack_count)
278                         pack->pack_index = pack->pack_count;
279                 else
280                         pack->pack_index = pack_idx + 1;
281
282                 /* now skip the first ((pack->pack_index - 1) * NMEA_SATINPACK) in view sats */
283                 toskip = ((pack->pack_index - 1) * NMEA_SATINPACK);
284                 sit = 0;
285                 while ((toskip > 0) && (sit < NMEA_MAXSAT)) {
286                         if (info->satinfo.sat[sit].id) {
287                                 toskip--;
288                         }
289                         sit++;
290                 }
291
292                 for (pit = 0; pit < NMEA_SATINPACK; sit++) {
293                         if (sit < NMEA_MAXSAT) {
294                                 if (info->satinfo.sat[sit].id) {
295                                         pack->sat_data[pit] = info->satinfo.sat[sit];
296                                         pit++;
297                                 }
298                         } else {
299                                 memset(&pack->sat_data[pit], 0, sizeof(pack->sat_data[pit]));
300                                 pit++;
301                         }
302                 }
303         }
304 }
305
306 /**
307  * Fill nmeaINFO structure from RMC packet structure
308  *
309  * @param pack a pointer to the packet structure
310  * @param info a pointer to the nmeaINFO structure
311  */
312 void nmea_GPRMC2info(const nmeaGPRMC *pack, nmeaINFO *info) {
313         assert(pack);
314         assert(info);
315
316         info->present |= pack->present;
317         nmea_INFO_set_present(&info->present, SMASK);
318         info->smask |= GPRMC;
319         if (nmea_INFO_is_present(pack->present, UTCDATE)) {
320                 info->utc.year = pack->utc.year;
321                 info->utc.mon = pack->utc.mon;
322                 info->utc.day = pack->utc.day;
323         }
324         if (nmea_INFO_is_present(pack->present, UTCTIME)) {
325                 info->utc.hour = pack->utc.hour;
326                 info->utc.min = pack->utc.min;
327                 info->utc.sec = pack->utc.sec;
328                 info->utc.hsec = pack->utc.hsec;
329         }
330         nmea_INFO_set_present(&info->present, SIG);
331         nmea_INFO_set_present(&info->present, FIX);
332         if (pack->status == 'A') {
333                 if (info->sig == NMEA_SIG_BAD) {
334                         info->sig = NMEA_SIG_MID;
335                 }
336                 if (info->fix == NMEA_FIX_BAD) {
337                         info->fix = NMEA_FIX_2D;
338                 }
339         } else {
340                 info->sig = NMEA_SIG_BAD;
341                 info->fix = NMEA_FIX_BAD;
342         }
343         if (nmea_INFO_is_present(pack->present, LAT)) {
344                 info->lat = ((pack->ns == 'N') ? pack->lat : -pack->lat);
345         }
346         if (nmea_INFO_is_present(pack->present, LON)) {
347                 info->lon = ((pack->ew == 'E') ? pack->lon : -pack->lon);
348         }
349         if (nmea_INFO_is_present(pack->present, SPEED)) {
350                 info->speed = pack->speed * NMEA_TUD_KNOTS;
351         }
352         if (nmea_INFO_is_present(pack->present, TRACK)) {
353                 info->track = pack->track;
354         }
355         if (nmea_INFO_is_present(pack->present, MAGVAR)) {
356                 info->magvar = ((pack->magvar_ew == 'E') ? pack->magvar : -pack->magvar);
357         }
358         /* mode is ignored */
359 }
360
361 /**
362  * Convert an nmeaINFO structure into an nmeaGPRMC structure
363  *
364  * @param info a pointer to the nmeaINFO structure
365  * @param pack a pointer to the nmeaGPRMC structure
366  */
367 void nmea_info2GPRMC(const nmeaINFO *info, nmeaGPRMC *pack) {
368         assert(pack);
369         assert(info);
370
371         nmea_zero_GPRMC(pack);
372
373         pack->present = info->present;
374         nmea_INFO_unset_present(&pack->present, SMASK);
375         if (nmea_INFO_is_present(info->present, UTCDATE)) {
376                 pack->utc.year = info->utc.year;
377                 pack->utc.mon = info->utc.mon;
378                 pack->utc.day = info->utc.day;
379         }
380         if (nmea_INFO_is_present(info->present, UTCTIME)) {
381                 pack->utc.hour = info->utc.hour;
382                 pack->utc.min = info->utc.min;
383                 pack->utc.sec = info->utc.sec;
384                 pack->utc.hsec = info->utc.hsec;
385         }
386         if (nmea_INFO_is_present(info->present, SIG)) {
387                 pack->status = ((info->sig != NMEA_SIG_BAD) ? 'A' : 'V');
388         } else {
389                 pack->status = 'V';
390         }
391         if (nmea_INFO_is_present(info->present, LAT)) {
392                 pack->lat = fabs(info->lat);
393                 pack->ns = ((info->lat > 0) ? 'N' : 'S');
394         }
395         if (nmea_INFO_is_present(info->present, LON)) {
396                 pack->lon = fabs(info->lon);
397                 pack->ew = ((info->lon > 0) ? 'E' : 'W');
398         }
399         if (nmea_INFO_is_present(info->present, SPEED)) {
400                 pack->speed = info->speed / NMEA_TUD_KNOTS;
401         }
402         if (nmea_INFO_is_present(info->present, TRACK)) {
403                 pack->track = info->track;
404         }
405         if (nmea_INFO_is_present(info->present, MAGVAR)) {
406                 pack->magvar = fabs(info->magvar);
407                 pack->magvar_ew = ((info->magvar > 0) ? 'E' : 'W');
408         }
409         if (nmea_INFO_is_present(info->present, SIG)) {
410                 pack->mode = ((info->sig != NMEA_SIG_BAD) ? 'A' : 'N');
411         } else {
412                 pack->mode = 'N';
413         }
414 }
415
416 /**
417  * Fill nmeaINFO structure from VTG packet structure
418  *
419  * @param pack a pointer to the packet structure
420  * @param info a pointer to the nmeaINFO structure
421  */
422 void nmea_GPVTG2info(const nmeaGPVTG *pack, nmeaINFO *info) {
423         assert(pack);
424         assert(info);
425
426         info->present |= pack->present;
427         nmea_INFO_set_present(&info->present, SMASK);
428         info->smask |= GPVTG;
429         if (nmea_INFO_is_present(pack->present, SPEED)) {
430                 info->speed = pack->spk;
431         }
432         if (nmea_INFO_is_present(pack->present, TRACK)) {
433                 info->track = pack->track;
434         }
435         if (nmea_INFO_is_present(pack->present, MTRACK)) {
436                 info->mtrack = pack->mtrack;
437         }
438 }
439
440 /**
441  * Convert an nmeaINFO structure into an nmeaGPVTG structure
442  *
443  * @param info a pointer to the nmeaINFO structure
444  * @param pack a pointer to the nmeaGPRMC structure
445  */
446 void nmea_info2GPVTG(const nmeaINFO *info, nmeaGPVTG *pack) {
447         assert(pack);
448         assert(info);
449
450         nmea_zero_GPVTG(pack); /* also sets up units */
451
452         pack->present = info->present;
453         nmea_INFO_unset_present(&pack->present, SMASK);
454         if (nmea_INFO_is_present(info->present, TRACK)) {
455                 pack->track = info->track;
456         }
457         if (nmea_INFO_is_present(info->present, MTRACK)) {
458                 pack->mtrack = info->mtrack;
459         }
460         if (nmea_INFO_is_present(info->present, SPEED)) {
461                 pack->spn = info->speed / NMEA_TUD_KNOTS;
462                 pack->spk = info->speed;
463         }
464 }