pud: update nmealib to 2.0.0
[olsrd.git] / lib / pud / nmealib / src / generate.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/generate.h>
19
20 #include <nmea/tok.h>
21 #include <nmea/conversions.h>
22
23 #include <stdio.h>
24 #include <stdbool.h>
25
26 /**
27  * Generate a GPGGA sentence from an nmeaGPGGA structure
28  *
29  * @param s a pointer to the buffer to generate the string in
30  * @param len the size of the buffer
31  * @param pack the structure
32  * @return the length of the generated sentence
33  */
34 int nmea_gen_GPGGA(char *s, const int len, const nmeaGPGGA *pack) {
35         char sTime[16];
36         char sLat[16];
37         char sNs[2];
38         char sLon[16];
39         char sEw[2];
40         char sSig[4];
41         char sSatInUse[4];
42         char sHdop[16];
43         char sElv[16];
44         char sElvUnit[2];
45
46         sTime[0] = 0;
47         sLat[0] = 0;
48         sNs[0] = sNs[1] = 0;
49         sLon[0] = 0;
50         sEw[0] = sEw[1] = 0;
51         sSig[0] = 0;
52         sSatInUse[0] = 0;
53         sHdop[0] = 0;
54         sElv[0] = 0;
55         sElvUnit[0] = sElvUnit[1] = 0;
56
57         if (nmea_INFO_is_present(pack->present, UTCTIME)) {
58                 snprintf(&sTime[0], sizeof(sTime), "%02d%02d%02d.%02d", pack->utc.hour, pack->utc.min, pack->utc.sec,
59                                 pack->utc.hsec);
60         }
61         if (nmea_INFO_is_present(pack->present, LAT)) {
62                 snprintf(&sLat[0], sizeof(sLat), "%09.4f", pack->lat);
63                 sNs[0] = pack->ns;
64         }
65         if (nmea_INFO_is_present(pack->present, LON)) {
66                 snprintf(&sLon[0], sizeof(sLon), "%010.4f", pack->lon);
67                 sEw[0] = pack->ew;
68         }
69         if (nmea_INFO_is_present(pack->present, SIG)) {
70                 snprintf(&sSig[0], sizeof(sSig), "%1d", pack->sig);
71         }
72         if (nmea_INFO_is_present(pack->present, SATINUSECOUNT)) {
73                 snprintf(&sSatInUse[0], sizeof(sSatInUse), "%02d", pack->satinuse);
74         }
75         if (nmea_INFO_is_present(pack->present, HDOP)) {
76                 snprintf(&sHdop[0], sizeof(sHdop), "%03.1f", pack->HDOP);
77         }
78         if (nmea_INFO_is_present(pack->present, ELV)) {
79                 snprintf(&sElv[0], sizeof(sElv), "%03.1f", pack->elv);
80                 sElvUnit[0] = pack->elv_units;
81         }
82
83         return nmea_printf(s, len, "$GPGGA,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,,,,", &sTime[0], &sLat[0], &sNs[0],
84                         &sLon[0], &sEw[0], &sSig[0], &sSatInUse[0], &sHdop[0], &sElv[0], &sElvUnit[0]);
85 }
86
87 /**
88  * Generate a GPGSA sentence from an nmeaGPGSA structure
89  *
90  * @param s a pointer to the buffer to generate the string in
91  * @param len the size of the buffer
92  * @param pack the structure
93  * @return the length of the generated sentence
94  */
95 int nmea_gen_GPGSA(char *s, const int len, const nmeaGPGSA *pack) {
96         int i;
97         char sFixMode[2];
98         char sFixType[2];
99         char sSatPrn[(NMEA_MAXSAT * 4) + 1];
100         char sPdop[16];
101         char sHdop[16];
102         char sVdop[16];
103
104         char * psSatPrn = &sSatPrn[0];
105         int ssSatPrn = sizeof(sSatPrn) - 1;
106
107         bool satinuse = nmea_INFO_is_present(pack->present, SATINUSE);
108
109         sFixMode[0] = sFixMode[1] = 0;
110         sFixType[0] = sFixType[1] = 0;
111         sSatPrn[0] = 0;
112         sPdop[0] = 0;
113         sHdop[0] = 0;
114         sVdop[0] = 0;
115
116         if (nmea_INFO_is_present(pack->present, FIX)) {
117                 sFixMode[0] = pack->fix_mode;
118                 snprintf(&sFixType[0], sizeof(sFixType), "%1d", pack->fix_type);
119         }
120
121         for (i = 0; i < NMEA_MAXSAT; i++) {
122                 if (satinuse && pack->sat_prn[i]) {
123                         int cnt = snprintf(psSatPrn, ssSatPrn, "%d", pack->sat_prn[i]);
124                         if (cnt >= ssSatPrn) {
125                                 ssSatPrn = 0;
126                                 psSatPrn = &sSatPrn[sizeof(sSatPrn) - 1];
127                                 *psSatPrn = '\0';
128                                 break;
129                         } else {
130                                 ssSatPrn -= cnt;
131                                 psSatPrn += cnt;
132                         }
133                 }
134                 if (i < (NMEA_MAXSAT - 1)) {
135                         *psSatPrn = ',';
136                         psSatPrn++;
137                         ssSatPrn--;
138                         *psSatPrn = '\0';
139                 }
140         }
141
142         if (nmea_INFO_is_present(pack->present, PDOP)) {
143                 snprintf(&sPdop[0], sizeof(sPdop), "%03.1f", pack->PDOP);
144         }
145         if (nmea_INFO_is_present(pack->present, HDOP)) {
146                 snprintf(&sHdop[0], sizeof(sHdop), "%03.1f", pack->HDOP);
147         }
148         if (nmea_INFO_is_present(pack->present, VDOP)) {
149                 snprintf(&sVdop[0], sizeof(sVdop), "%03.1f", pack->VDOP);
150         }
151
152         return nmea_printf(s, len, "$GPGSA,%s,%s,%s,%s,%s,%s", &sFixMode[0], &sFixType[0], &sSatPrn[0], &sPdop[0],
153                         &sHdop[0], &sVdop[0]);
154 }
155
156 /**
157  * Generate a GPGSV sentence from an nmeaGPGSV structure
158  *
159  * @param s a pointer to the buffer to generate the string in
160  * @param len the size of the buffer
161  * @param pack the structure
162  * @return the length of the generated sentence
163  */
164 int nmea_gen_GPGSV(char *s, const int len, const nmeaGPGSV *pack) {
165         char sCount[2];
166         char sIndex[2];
167         char sSatCount[4];
168         char sSatInfo[(NMEA_SATINPACK * 4) + 1];
169         char * psSatInfo = &sSatInfo[0];
170         int ssSatInfo = sizeof(sSatInfo) - 1;
171         bool satinview = nmea_INFO_is_present(pack->present, SATINVIEW);
172         int i;
173
174         sCount[0] = 0;
175         sIndex[0] = 0;
176         sSatCount[0] = 0;
177         sSatInfo[0] = 0;
178
179         if (satinview) {
180                 snprintf(&sCount[0], sizeof(sCount), "%1d", pack->pack_count);
181                 snprintf(&sIndex[0], sizeof(sIndex), "%1d", pack->pack_index);
182                 snprintf(&sSatCount[0], sizeof(sSatCount), "%02d", pack->sat_count);
183         }
184         for (i = 0; i < NMEA_SATINPACK; i++) {
185                 int cnt = 0;
186                 if (satinview && pack->sat_data[i].id) {
187                         cnt = snprintf(psSatInfo, ssSatInfo, "%02d,%02d,%03d,%02d", pack->sat_data[i].id, pack->sat_data[i].elv,
188                                         pack->sat_data[i].azimuth, pack->sat_data[i].sig);
189                 } else {
190                         cnt = snprintf(psSatInfo, ssSatInfo, ",,,");
191                 }
192                 if (cnt >= ssSatInfo) {
193                         ssSatInfo = 0;
194                         psSatInfo = &sSatInfo[sizeof(sSatInfo) - 1];
195                         *psSatInfo = '\0';
196                         break;
197                 } else {
198                         ssSatInfo -= cnt;
199                         psSatInfo += cnt;
200                 }
201                 if (i < (NMEA_SATINPACK - 1)) {
202                         *psSatInfo = ',';
203                         psSatInfo++;
204                         ssSatInfo--;
205                         *psSatInfo = '\0';
206                 }
207         }
208
209         return nmea_printf(s, len, "$GPGSV,%s,%s,%s,%s", &sCount[0], &sIndex[0], &sSatCount[0], &sSatInfo[0]);
210 }
211
212 /**
213  * Generate a GPRMC sentence from an nmeaGPRMC structure
214  *
215  * @param s a pointer to the buffer to generate the string in
216  * @param len the size of the buffer
217  * @param pack the structure
218  * @return the length of the generated sentence
219  */
220 int nmea_gen_GPRMC(char *s, const int len, const nmeaGPRMC *pack) {
221         char sTime[16];
222         char sDate[16];
223         char sLat[16];
224         char sNs[2];
225         char sLon[16];
226         char sEw[2];
227         char sSpeed[16];
228         char sTrack[16];
229         char sMagvar[16];
230         char sMagvar_ew[2];
231
232         sTime[0] = 0;
233         sDate[0] = 0;
234         sLat[0] = 0;
235         sNs[0] = sNs[1] = 0;
236         sLon[0] = 0;
237         sEw[0] = sEw[1] = 0;
238         sSpeed[0] = 0;
239         sTrack[0] = 0;
240         sMagvar[0] = 0;
241         sMagvar_ew[0] = sMagvar_ew[1] = 0;
242
243         if (nmea_INFO_is_present(pack->present, UTCDATE)) {
244                 snprintf(&sDate[0], sizeof(sDate), "%02d%02d%02d", pack->utc.day, pack->utc.mon + 1, pack->utc.year - 100);
245         }
246         if (nmea_INFO_is_present(pack->present, UTCTIME)) {
247                 snprintf(&sTime[0], sizeof(sTime), "%02d%02d%02d.%02d", pack->utc.hour, pack->utc.min, pack->utc.sec,
248                                 pack->utc.hsec);
249         }
250         if (nmea_INFO_is_present(pack->present, LAT)) {
251                 snprintf(&sLat[0], sizeof(sLat), "%09.4f", pack->lat);
252                 sNs[0] = pack->ns;
253         }
254         if (nmea_INFO_is_present(pack->present, LON)) {
255                 snprintf(&sLon[0], sizeof(sLon), "%010.4f", pack->lon);
256                 sEw[0] = pack->ew;
257         }
258         if (nmea_INFO_is_present(pack->present, SPEED)) {
259                 snprintf(&sSpeed[0], sizeof(sSpeed), "%03.1f", pack->speed);
260         }
261         if (nmea_INFO_is_present(pack->present, TRACK)) {
262                 snprintf(&sTrack[0], sizeof(sTrack), "%03.1f", pack->track);
263         }
264         if (nmea_INFO_is_present(pack->present, MAGVAR)) {
265                 snprintf(&sMagvar[0], sizeof(sMagvar), "%03.1f", pack->magvar);
266                 sMagvar_ew[0] = pack->magvar_ew;
267         }
268
269         return nmea_printf(s, len, "$GPRMC,%s,%c,%s,%s,%s,%s,%s,%s,%s,%s,%s,%c", &sTime[0], pack->status, &sLat[0], &sNs[0],
270                         &sLon[0], &sEw[0], &sSpeed[0], &sTrack[0], &sDate[0], &sMagvar[0], &sMagvar_ew[0], pack->mode);
271 }
272
273 /**
274  * Generate a GPVTG sentence from an nmeaGPVTG structure
275  *
276  * @param s a pointer to the buffer to generate the string in
277  * @param len the size of the buffer
278  * @param pack the structure
279  * @return the length of the generated sentence
280  */
281 int nmea_gen_GPVTG(char *s, const int len, const nmeaGPVTG *pack) {
282         char sTrackT[16];
283         char sTrackM[16];
284         char sSpeedN[16];
285         char sSpeedK[16];
286         char sUnitT[2];
287         char sUnitM[2];
288         char sUnitN[2];
289         char sUnitK[2];
290
291         sTrackT[0] = 0;
292         sTrackM[0] = 0;
293         sSpeedN[0] = 0;
294         sSpeedK[0] = 0;
295         sUnitT[0] = sUnitT[1] = 0;
296         sUnitM[0] = sUnitM[1] = 0;
297         sUnitN[0] = sUnitN[1] = 0;
298         sUnitK[0] = sUnitK[1] = 0;
299
300         if (nmea_INFO_is_present(pack->present, TRACK)) {
301                 snprintf(&sTrackT[0], sizeof(sTrackT), "%03.1f", pack->track);
302                 sUnitT[0] = 'T';
303         }
304         if (nmea_INFO_is_present(pack->present, MTRACK)) {
305                 snprintf(&sTrackM[0], sizeof(sTrackM), "%03.1f", pack->mtrack);
306                 sUnitM[0] = 'M';
307         }
308         if (nmea_INFO_is_present(pack->present, SPEED)) {
309                 snprintf(&sSpeedN[0], sizeof(sSpeedN), "%03.1f", pack->spn);
310                 sUnitN[0] = 'N';
311                 snprintf(&sSpeedK[0], sizeof(sSpeedK), "%03.1f", pack->spk);
312                 sUnitK[0] = 'K';
313         }
314
315         return nmea_printf(s, len, "$GPVTG,%s,%s,%s,%s,%s,%s,%s,%s", &sTrackT[0], &sUnitT[0], &sTrackM[0],
316                         &sUnitM[0], &sSpeedN[0], &sUnitN[0], &sSpeedK[0], &sUnitK[0]);
317 }
318
319 /**
320  * Generate a number of sentences from an nmeaINFO structure.
321  *
322  * @param s a pointer to the buffer in which to generate the sentences
323  * @param len the size of the buffer
324  * @param info the structure
325  * @param generate_mask the mask of which sentences to generate
326  * @return the total length of the generated sentences
327  */
328 int nmea_generate(char *s, const int len, const nmeaINFO *info, const int generate_mask) {
329         int gen_count = 0;
330         int pack_mask = generate_mask;
331
332         if (!s || !len || !info || !generate_mask)
333                 return 0;
334
335         while (pack_mask) {
336                 if (pack_mask & GPGGA) {
337                         nmeaGPGGA gga;
338
339                         nmea_info2GPGGA(info, &gga);
340                         gen_count += nmea_gen_GPGGA(s + gen_count, len - gen_count, &gga);
341                         pack_mask &= ~GPGGA;
342                 } else if (pack_mask & GPGSA) {
343                         nmeaGPGSA gsa;
344
345                         nmea_info2GPGSA(info, &gsa);
346                         gen_count += nmea_gen_GPGSA(s + gen_count, len - gen_count, &gsa);
347                         pack_mask &= ~GPGSA;
348                 } else if (pack_mask & GPGSV) {
349                         nmeaGPGSV gsv;
350                         int gsv_it;
351                         int gsv_count = nmea_gsv_npack(info->satinfo.inview);
352
353                         for (gsv_it = 0; gsv_it < gsv_count && len - gen_count > 0; gsv_it++) {
354                                 nmea_info2GPGSV(info, &gsv, gsv_it);
355                                 gen_count += nmea_gen_GPGSV(s + gen_count, len - gen_count, &gsv);
356                         }
357                         pack_mask &= ~GPGSV;
358                 } else if (pack_mask & GPRMC) {
359                         nmeaGPRMC rmc;
360
361                         nmea_info2GPRMC(info, &rmc);
362                         gen_count += nmea_gen_GPRMC(s + gen_count, len - gen_count, &rmc);
363                         pack_mask &= ~GPRMC;
364                 } else if (pack_mask & GPVTG) {
365                         nmeaGPVTG vtg;
366
367                         nmea_info2GPVTG(info, &vtg);
368                         gen_count += nmea_gen_GPVTG(s + gen_count, len - gen_count, &vtg);
369                         pack_mask &= ~GPVTG;
370                 } else {
371                         /* no more known sentences to process */
372                         break;
373                 }
374
375                 if (len - gen_count <= 0)
376                         break;
377         }
378
379         return gen_count;
380 }