pud: update nmealib to 2.0.0
[olsrd.git] / lib / pud / nmealib / src / generator.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/generator.h>
19
20 #include <nmea/context.h>
21 #include <nmea/gmath.h>
22 #include <nmea/generate.h>
23
24 #include <stdlib.h>
25 #include <string.h>
26 #include <math.h>
27
28 /**
29  * Generate a random number in the range [min, max]
30  *
31  * @param min the minimum
32  * @param max the maximum
33  * @return a random number
34  */
35 static double nmea_random(const double min, const double max) {
36         static double rand_max = RAND_MAX;
37         double rand_val = rand();
38         double bounds = max - min;
39         return min + (rand_val * bounds) / rand_max;
40 }
41
42 /**
43  * Initialise the generator
44  *
45  * @param gen a pointer to the generator
46  * @param info a pointer to an nmeaINFO structure to use during generation
47  * (present and smask are preserved, other fields are reset before generation starts)
48  * @return 1 (true) on success, 0 (false) otherwise
49  */
50 int nmea_gen_init(nmeaGENERATOR *gen, nmeaINFO *info) {
51         int retval = 1;
52         int present = info->present;
53         int smask = info->smask;
54         nmeaGENERATOR *igen = gen;
55
56         nmea_zero_INFO(info);
57         info->present = present;
58         info->smask = smask;
59         nmea_INFO_set_present(&info->present, SMASK);
60
61         info->lat = NMEA_DEF_LAT;
62         info->lon = NMEA_DEF_LON;
63         nmea_INFO_set_present(&info->present, LAT);
64         nmea_INFO_set_present(&info->present, LON);
65
66         while (retval && igen) {
67                 if (igen->init_call)
68                         retval = (*igen->init_call)(igen, info);
69                 igen = igen->next;
70         }
71
72         return retval;
73 }
74
75 /**
76  * Loop the generator.
77  *
78  * @param gen a pointer to the generator
79  * @param info a pointer to an nmeaINFO structure to use during generation
80  * @return 1 (true) on success, 0 (false) otherwise
81  */
82 int nmea_gen_loop(nmeaGENERATOR *gen, nmeaINFO *info) {
83         int retVal = 1;
84
85         if (gen->loop_call)
86                 retVal = (*gen->loop_call)(gen, info);
87
88         if (retVal && gen->next)
89                 retVal = nmea_gen_loop(gen->next, info);
90
91         return retVal;
92 }
93
94 /**
95  * Reset the generator.
96  *
97  * @param gen a pointer to the generator
98  * @param info a pointer to an nmeaINFO structure to use during generation
99  * @return 1 (true) on success, 0 (false) otherwise
100  */
101 int nmea_gen_reset(nmeaGENERATOR *gen, nmeaINFO *info) {
102         int RetVal = 1;
103
104         if (gen->reset_call)
105                 RetVal = (*gen->reset_call)(gen, info);
106
107         return RetVal;
108 }
109
110 /**
111  * Destroy the generator.
112  *
113  * @param gen a pointer to the generator
114  */
115 void nmea_gen_destroy(nmeaGENERATOR *gen) {
116         if (gen->next) {
117                 nmea_gen_destroy(gen->next);
118                 gen->next = 0;
119         }
120
121         if (gen->destroy_call)
122                 (*gen->destroy_call)(gen);
123
124         free(gen);
125 }
126
127 /**
128  * Add a generator to the existing ones.
129  *
130  * @param to the generators to add to
131  * @param gen the generator to add
132  */
133 void nmea_gen_add(nmeaGENERATOR *to, nmeaGENERATOR *gen) {
134         nmeaGENERATOR * next = to;
135         while (next->next)
136                 next = to->next;
137
138         next->next = gen;
139 }
140
141 /**
142  * Run a new generation loop on the generator
143  *
144  * @param s a pointer to the string buffer in which to generate
145  * @param len the size of the buffer
146  * @param info a pointer to an nmeaINFO structure to use during generation
147  * @param gen a pointer to the generator
148  * @param generate_mask the smask of sentences to generate
149  * @return the total length of the generated sentences
150  */
151 int nmea_generate_from(char *s, int len, nmeaINFO *info, nmeaGENERATOR *gen, int generate_mask) {
152         int retval;
153
154         if ((retval = nmea_gen_loop(gen, info)))
155                 retval = nmea_generate(s, len, info, generate_mask);
156
157         return retval;
158 }
159
160 /*
161  * NOISE generator
162  */
163
164 /**
165  * NOISE Generator loop function.
166  * Does not touch smask and utc in info.
167  *
168  * @param gen a pointer to the generator
169  * @param info a pointer to an nmeaINFO structure to use during generation
170  * @return 1 (true) on success, 0 (false) otherwise
171  */
172 static int nmea_igen_noise_loop(nmeaGENERATOR *gen __attribute__ ((unused)), nmeaINFO *info) {
173         int it;
174         int in_use;
175
176         info->sig = lrint(nmea_random(1, 3));
177         info->fix = lrint(nmea_random(2, 3));
178         info->PDOP = nmea_random(0, 9);
179         info->HDOP = nmea_random(0, 9);
180         info->VDOP = nmea_random(0, 9);
181         info->lat = nmea_random(0, 100);
182         info->lon = nmea_random(0, 100);
183         info->elv = lrint(nmea_random(-100, 100));
184         info->speed = nmea_random(0, 100);
185         info->track = nmea_random(0, 360);
186         info->mtrack = nmea_random(0, 360);
187         info->magvar = nmea_random(0, 360);
188
189         nmea_INFO_set_present(&info->present, SIG);
190         nmea_INFO_set_present(&info->present, FIX);
191         nmea_INFO_set_present(&info->present, PDOP);
192         nmea_INFO_set_present(&info->present, HDOP);
193         nmea_INFO_set_present(&info->present, VDOP);
194         nmea_INFO_set_present(&info->present, LAT);
195         nmea_INFO_set_present(&info->present, LON);
196         nmea_INFO_set_present(&info->present, ELV);
197         nmea_INFO_set_present(&info->present, SPEED);
198         nmea_INFO_set_present(&info->present, TRACK);
199         nmea_INFO_set_present(&info->present, MTRACK);
200         nmea_INFO_set_present(&info->present, MAGVAR);
201
202         info->satinfo.inuse = 0;
203         info->satinfo.inview = 0;
204
205         for (it = 0; it < NMEA_MAXSAT; it++) {
206                 in_use = lrint(nmea_random(0, 3));
207                 info->satinfo.in_use[it] = in_use ? it : 0;
208                 info->satinfo.sat[it].id = it;
209                 info->satinfo.sat[it].elv = lrint(nmea_random(0, 90));
210                 info->satinfo.sat[it].azimuth = lrint(nmea_random(0, 359));
211                 info->satinfo.sat[it].sig = (int) (in_use ? nmea_random(40, 99) : nmea_random(0, 40));
212
213                 if (in_use)
214                         info->satinfo.inuse++;
215                 if (info->satinfo.sat[it].sig > 0)
216                         info->satinfo.inview++;
217         }
218
219         nmea_INFO_set_present(&info->present, SATINUSECOUNT);
220         nmea_INFO_set_present(&info->present, SATINUSE);
221         nmea_INFO_set_present(&info->present, SATINVIEW);
222
223         return 1;
224 }
225
226 /*
227  * STATIC generator
228  */
229
230 /**
231  * STATIC Generator loop function.
232  * Only touches utc in info.
233  *
234  * @param gen a pointer to the generator
235  * @param info a pointer to an nmeaINFO structure to use during generation
236  * @return 1 (true) on success, 0 (false) otherwise
237  */
238 static int nmea_igen_static_loop(nmeaGENERATOR *gen __attribute__ ((unused)), nmeaINFO *info) {
239         nmea_time_now(&info->utc, &info->present);
240         return 1;
241 }
242
243 /**
244  * STATIC Generator reset function.
245  * Resets only the satinfo to 4 sats in use and in view.
246  *
247  * @param gen a pointer to the generator
248  * @param info a pointer to an nmeaINFO structure to use during generation
249  * @return 1 (true) on success, 0 (false) otherwise
250  */
251 static int nmea_igen_static_reset(nmeaGENERATOR *gen __attribute__ ((unused)), nmeaINFO *info) {
252         info->satinfo.inuse = 4;
253         info->satinfo.inview = 4;
254
255         info->satinfo.in_use[0] = 1;
256         info->satinfo.sat[0].id = 1;
257         info->satinfo.sat[0].elv = 50;
258         info->satinfo.sat[0].azimuth = 0;
259         info->satinfo.sat[0].sig = 99;
260
261         info->satinfo.in_use[1] = 2;
262         info->satinfo.sat[1].id = 2;
263         info->satinfo.sat[1].elv = 50;
264         info->satinfo.sat[1].azimuth = 90;
265         info->satinfo.sat[1].sig = 99;
266
267         info->satinfo.in_use[2] = 3;
268         info->satinfo.sat[2].id = 3;
269         info->satinfo.sat[2].elv = 50;
270         info->satinfo.sat[2].azimuth = 180;
271         info->satinfo.sat[2].sig = 99;
272
273         info->satinfo.in_use[3] = 4;
274         info->satinfo.sat[3].id = 4;
275         info->satinfo.sat[3].elv = 50;
276         info->satinfo.sat[3].azimuth = 270;
277         info->satinfo.sat[3].sig = 99;
278
279         nmea_INFO_set_present(&info->present, SATINUSECOUNT);
280         nmea_INFO_set_present(&info->present, SATINUSE);
281         nmea_INFO_set_present(&info->present, SATINVIEW);
282
283         return 1;
284 }
285
286 /**
287  * STATIC Generator initialiser function.
288  * Only touches sig, fix and satinfo in info.
289  *
290  * @param gen a pointer to the generator
291  * @param info a pointer to an nmeaINFO structure to use during generation
292  * @return 1 (true) on success, 0 (false) otherwise
293  */
294 static int nmea_igen_static_init(nmeaGENERATOR *gen, nmeaINFO *info) {
295         info->sig = 3;
296         info->fix = 3;
297
298         nmea_INFO_set_present(&info->present, SIG);
299         nmea_INFO_set_present(&info->present, FIX);
300
301         nmea_igen_static_reset(gen, info);
302
303         return 1;
304 }
305
306 /*
307  * SAT_ROTATE generator
308  */
309
310 /**
311  * SAT_ROTATE Generator loop function.
312  *
313  * @param gen a pointer to the generator
314  * @param info a pointer to an nmeaINFO structure to use during generation
315  * @return 1 (true) on success, 0 (false) otherwise
316  */
317 static int nmea_igen_rotate_loop(nmeaGENERATOR *gen __attribute__ ((unused)), nmeaINFO *info) {
318         int it;
319         int count = info->satinfo.inview;
320         double deg = 360 / (count ? count : 1);
321         double srt = (count ? (info->satinfo.sat[0].azimuth) : 0) + 5;
322
323         nmea_time_now(&info->utc, &info->present);
324
325         for (it = 0; it < count; it++) {
326                 info->satinfo.sat[it].azimuth = (int) ((srt >= 360) ? srt - 360 : srt);
327                 srt += deg;
328         }
329
330         nmea_INFO_set_present(&info->present, SATINVIEW);
331
332         return 1;
333 }
334
335 /**
336  * SAT_ROTATE Generator reset function.
337  *
338  * @param gen a pointer to the generator
339  * @param info a pointer to an nmeaINFO structure to use during generation
340  * @return 1 (true) on success, 0 (false) otherwise
341  */
342 static int nmea_igen_rotate_reset(nmeaGENERATOR *gen __attribute__ ((unused)), nmeaINFO *info) {
343         int it;
344         double deg = 360 / 8;
345         double srt = 0;
346
347         info->satinfo.inuse = 8;
348         info->satinfo.inview = 8;
349
350         for (it = 0; it < info->satinfo.inview; it++) {
351                 info->satinfo.in_use[it] = it + 1;
352                 info->satinfo.sat[it].id = it + 1;
353                 info->satinfo.sat[it].elv = 5;
354                 info->satinfo.sat[it].azimuth = (int) srt;
355                 info->satinfo.sat[it].sig = 80;
356                 srt += deg;
357         }
358
359         nmea_INFO_set_present(&info->present, SATINUSECOUNT);
360         nmea_INFO_set_present(&info->present, SATINUSE);
361         nmea_INFO_set_present(&info->present, SATINVIEW);
362
363         return 1;
364 }
365
366 /**
367  * SAT_ROTATE Generator initialiser function.
368  * Only touches sig, fix and satinfo in info.
369  *
370  * @param gen a pointer to the generator
371  * @param info a pointer to an nmeaINFO structure to use during generation
372  * @return 1 (true) on success, 0 (false) otherwise
373  */
374 static int nmea_igen_rotate_init(nmeaGENERATOR *gen, nmeaINFO *info) {
375         info->sig = 3;
376         info->fix = 3;
377
378         nmea_INFO_set_present(&info->present, SIG);
379         nmea_INFO_set_present(&info->present, FIX);
380
381         nmea_igen_rotate_reset(gen, info);
382
383         return 1;
384 }
385
386 /*
387  * POS_RANDMOVE generator
388  */
389
390 /**
391  * POS_RANDMOVE Generator initialiser function.
392  * Only touches sig, fix, track, mtrack, magvar and speed in info.
393  *
394  * @param gen a pointer to the generator
395  * @param info a pointer to an nmeaINFO structure to use during generation
396  * @return 1 (true) on success, 0 (false) otherwise
397  */
398 static int nmea_igen_pos_rmove_init(nmeaGENERATOR *gen __attribute__ ((unused)), nmeaINFO *info) {
399         info->sig = 3;
400         info->fix = 3;
401         info->speed = 20;
402         info->track = 0;
403         info->mtrack = 0;
404         info->magvar = 0;
405
406         nmea_INFO_set_present(&info->present, SIG);
407         nmea_INFO_set_present(&info->present, FIX);
408         nmea_INFO_set_present(&info->present, SPEED);
409         nmea_INFO_set_present(&info->present, TRACK);
410         nmea_INFO_set_present(&info->present, MTRACK);
411         nmea_INFO_set_present(&info->present, MAGVAR);
412
413         return 1;
414 }
415
416 /**
417  * POS_RANDMOVE Generator loop function.
418  *
419  * @param gen a pointer to the generator
420  * @param info a pointer to an nmeaINFO structure to use during generation
421  * @return 1 (true) on success, 0 (false) otherwise
422  */
423 static int nmea_igen_pos_rmove_loop(nmeaGENERATOR *gen __attribute__ ((unused)), nmeaINFO *info) {
424         nmeaPOS crd;
425
426         info->track += nmea_random(-10, 10);
427         info->mtrack += nmea_random(-10, 10);
428         info->speed += nmea_random(-2, 3);
429
430         if (info->track < 0) {
431                 info->track = 359 + info->track;
432         }
433         if (info->track > 359) {
434                 info->track -= 359;
435         }
436         if (info->mtrack < 0) {
437                 info->mtrack = 359 + info->mtrack;
438         }
439         if (info->mtrack > 359) {
440                 info->mtrack -= 359;
441         }
442
443         if (info->speed > 40)
444                 info->speed = 40;
445         if (info->speed < 1)
446                 info->speed = 1;
447
448         nmea_info2pos(info, &crd);
449         nmea_move_horz(&crd, &crd, info->track, info->speed / 3600);
450         nmea_pos2info(&crd, info);
451
452         info->magvar = info->track;
453
454         nmea_INFO_set_present(&info->present, LAT);
455         nmea_INFO_set_present(&info->present, LON);
456         nmea_INFO_set_present(&info->present, SPEED);
457         nmea_INFO_set_present(&info->present, TRACK);
458         nmea_INFO_set_present(&info->present, MTRACK);
459         nmea_INFO_set_present(&info->present, MAGVAR);
460
461         return 1;
462 }
463
464 /**
465  * Create the generator.
466  *
467  * @param type the type of the generator to create (see nmeaGENTYPE)
468  * @param info a pointer to an nmeaINFO structure to use during generation
469  * @return the generator
470  */
471 static nmeaGENERATOR * __nmea_create_generator(const int type, nmeaINFO *info) {
472         nmeaGENERATOR *gen = 0;
473
474         switch (type) {
475         case NMEA_GEN_NOISE:
476                 if (!(gen = malloc(sizeof(nmeaGENERATOR))))
477                         nmea_error("__nmea_create_generator: insufficient memory!");
478                 else {
479                         memset(gen, 0, sizeof(nmeaGENERATOR));
480                         gen->loop_call = &nmea_igen_noise_loop;
481                 }
482                 break;
483         case NMEA_GEN_STATIC:
484         case NMEA_GEN_SAT_STATIC:
485                 if (!(gen = malloc(sizeof(nmeaGENERATOR))))
486                         nmea_error("__nmea_create_generator: insufficient memory!");
487                 else {
488                         memset(gen, 0, sizeof(nmeaGENERATOR));
489                         gen->init_call = &nmea_igen_static_init;
490                         gen->loop_call = &nmea_igen_static_loop;
491                         gen->reset_call = &nmea_igen_static_reset;
492                 }
493                 break;
494         case NMEA_GEN_SAT_ROTATE:
495                 if (!(gen = malloc(sizeof(nmeaGENERATOR))))
496                         nmea_error("__nmea_create_generator: insufficient memory!");
497                 else {
498                         memset(gen, 0, sizeof(nmeaGENERATOR));
499                         gen->init_call = &nmea_igen_rotate_init;
500                         gen->loop_call = &nmea_igen_rotate_loop;
501                         gen->reset_call = &nmea_igen_rotate_reset;
502                 }
503                 break;
504         case NMEA_GEN_POS_RANDMOVE:
505                 if (!(gen = malloc(sizeof(nmeaGENERATOR))))
506                         nmea_error("__nmea_create_generator: insufficient memory!");
507                 else {
508                         memset(gen, 0, sizeof(nmeaGENERATOR));
509                         gen->init_call = &nmea_igen_pos_rmove_init;
510                         gen->loop_call = &nmea_igen_pos_rmove_loop;
511                 }
512                 break;
513         default:
514                 /* case NMEA_GEN_ROTATE: */
515                 gen = __nmea_create_generator(NMEA_GEN_SAT_ROTATE, info);
516                 nmea_gen_add(gen, __nmea_create_generator(NMEA_GEN_POS_RANDMOVE, info));
517                 break;
518         };
519
520         return gen;
521 }
522
523 /**
524  * Create the generator and initialise it.
525  *
526  * @param type the type of the generator to create (see nmeaGENTYPE)
527  * @param info a pointer to an nmeaINFO structure to use during generation
528  * @return the generator
529  */
530 nmeaGENERATOR * nmea_create_generator(const int type, nmeaINFO *info) {
531         nmeaGENERATOR *gen = __nmea_create_generator(type, info);
532
533         if (gen)
534                 nmea_gen_init(gen, info);
535
536         return gen;
537 }