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