5cc21582cb310cfbc8dbd5f9dbee12ac0af24b8b
[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 <stdlib.h>
24 #include <string.h>
25
26 #include <nmea/context.h>
27 #include <nmea/generate.h>
28 #include <nmea/gmath.h>
29
30 double nmea_random(double min, double max)
31 {
32     static double rand_max = RAND_MAX;
33     double rand_val = rand();
34     double bounds = max - min;
35     return min + (rand_val * bounds) / rand_max;
36 }
37
38 /*
39  * low level
40  */
41
42 int nmea_gen_init(nmeaGENERATOR *gen, nmeaINFO *info)
43 {
44     int RetVal = 1; int smask = info->smask;
45     nmeaGENERATOR *igen = gen;
46
47     nmea_zero_INFO(info);
48     info->smask = smask;
49
50     info->lat = NMEA_DEF_LAT;
51     info->lon = NMEA_DEF_LON;
52
53     while(RetVal && igen)
54     {
55         if(igen->init_call)
56             RetVal = (*igen->init_call)(igen, info);
57         igen = igen->next;
58     }
59
60     return RetVal;
61 }
62
63 int nmea_gen_loop(nmeaGENERATOR *gen, nmeaINFO *info)
64 {
65     int RetVal = 1;
66
67     if(gen->loop_call)
68         RetVal = (*gen->loop_call)(gen, info);
69
70     if(RetVal && gen->next)
71         RetVal = nmea_gen_loop(gen->next, info);
72
73     return RetVal;
74 }
75
76 int nmea_gen_reset(nmeaGENERATOR *gen, nmeaINFO *info)
77 {
78     int RetVal = 1;
79
80     if(gen->reset_call)
81         RetVal = (*gen->reset_call)(gen, info);
82
83     return RetVal;
84 }
85
86 void nmea_gen_destroy(nmeaGENERATOR *gen)
87 {
88     if(gen->next)
89     {
90         nmea_gen_destroy(gen->next);
91         gen->next = 0;
92     }
93
94     if(gen->destroy_call)
95         (*gen->destroy_call)(gen);
96
97     free(gen);
98 }
99
100 void nmea_gen_add(nmeaGENERATOR *to, nmeaGENERATOR *gen)
101 {
102     if(to->next)
103         nmea_gen_add(to->next, gen);
104     else
105         to->next = gen;
106 }
107
108 int nmea_generate_from(
109     char *buff, int buff_sz,
110     nmeaINFO *info,
111     nmeaGENERATOR *gen,
112     int generate_mask
113     )
114 {
115     int retval;
116
117     if(0 != (retval = nmea_gen_loop(gen, info)))
118         retval = nmea_generate(buff, buff_sz, info, generate_mask);
119
120     return retval;
121 }
122
123 /*
124  * NOISE generator
125  */
126
127 int nmea_igen_noise_init(nmeaGENERATOR *gen __attribute__ ((unused)),
128                 nmeaINFO *info __attribute__ ((unused)))
129 {
130     return 1;
131 }
132
133 int nmea_igen_noise_loop(nmeaGENERATOR *gen __attribute__ ((unused)),
134                 nmeaINFO *info)
135 {
136     int it;
137     int in_use;
138
139     info->sig = (int)nmea_random(1, 3);
140     info->PDOP = nmea_random(0, 9);
141     info->HDOP = nmea_random(0, 9);
142     info->VDOP = nmea_random(0, 9);
143     info->fix = (int)nmea_random(2, 3);
144     info->lat = nmea_random(0, 100);
145     info->lon = nmea_random(0, 100);
146     info->speed = nmea_random(0, 100);
147     info->direction = nmea_random(0, 360);
148     info->declination = nmea_random(0, 360);
149     info->elv = (int)nmea_random(-100, 100);
150
151     info->satinfo.inuse = 0;
152     info->satinfo.inview = 0;
153
154     for(it = 0; it < 12; ++it)
155     {
156         info->satinfo.sat[it].id = it;
157         info->satinfo.sat[it].in_use = in_use = (int)nmea_random(0, 3);
158         info->satinfo.sat[it].elv = (int)nmea_random(0, 90);
159         info->satinfo.sat[it].azimuth = (int)nmea_random(0, 359);
160         info->satinfo.sat[it].sig = (int)(in_use?nmea_random(40, 99):nmea_random(0, 40));
161
162         if(in_use)
163             info->satinfo.inuse++;
164         if(info->satinfo.sat[it].sig > 0)
165             info->satinfo.inview++;
166     }
167
168     return 1;
169 }
170
171 int nmea_igen_noise_reset(nmeaGENERATOR *gen __attribute__ ((unused)),
172                 nmeaINFO *info __attribute__ ((unused)))
173 {
174     return 1;
175 }
176
177 /*
178  * STATIC generator
179  */
180
181 int nmea_igen_static_loop(nmeaGENERATOR *gen __attribute__ ((unused)),
182                 nmeaINFO *info)
183 {
184     nmea_time_now(&info->utc);
185     return 1;
186 };
187
188 int nmea_igen_static_reset(nmeaGENERATOR *gen __attribute__ ((unused)),
189                 nmeaINFO *info)
190 {
191     info->satinfo.inuse = 4;
192     info->satinfo.inview = 4;
193
194     info->satinfo.sat[0].id = 1;
195     info->satinfo.sat[0].in_use = 1;
196     info->satinfo.sat[0].elv = 50;
197     info->satinfo.sat[0].azimuth = 0;
198     info->satinfo.sat[0].sig = 99;
199
200     info->satinfo.sat[1].id = 2;
201     info->satinfo.sat[1].in_use = 1;
202     info->satinfo.sat[1].elv = 50;
203     info->satinfo.sat[1].azimuth = 90;
204     info->satinfo.sat[1].sig = 99;
205
206     info->satinfo.sat[2].id = 3;
207     info->satinfo.sat[2].in_use = 1;
208     info->satinfo.sat[2].elv = 50;
209     info->satinfo.sat[2].azimuth = 180;
210     info->satinfo.sat[2].sig = 99;
211
212     info->satinfo.sat[3].id = 4;
213     info->satinfo.sat[3].in_use = 1;
214     info->satinfo.sat[3].elv = 50;
215     info->satinfo.sat[3].azimuth = 270;
216     info->satinfo.sat[3].sig = 99;
217
218     return 1;
219 }
220
221 int nmea_igen_static_init(nmeaGENERATOR *gen, nmeaINFO *info)
222 {
223     info->sig = 3;
224     info->fix = 3;
225
226     nmea_igen_static_reset(gen, info);
227
228     return 1;
229 }
230
231 /*
232  * SAT_ROTATE generator
233  */
234
235 int nmea_igen_rotate_loop(nmeaGENERATOR *gen __attribute__ ((unused)),
236                 nmeaINFO *info)
237 {
238     int it;
239     int count = info->satinfo.inview;
240     double deg = 360 / (count?count:1);
241     double srt = (count?(info->satinfo.sat[0].azimuth):0) + 5;
242
243     nmea_time_now(&info->utc);
244
245     for(it = 0; it < count; ++it)
246     {
247         info->satinfo.sat[it].azimuth =
248             (int)((srt >= 360)?srt - 360:srt);
249         srt += deg;
250     }
251
252     return 1;
253 };
254
255 int nmea_igen_rotate_reset(nmeaGENERATOR *gen __attribute__ ((unused)),
256                 nmeaINFO *info)
257 {
258     int it;
259     double deg = 360 / 8;
260     double srt = 0;
261
262     info->satinfo.inuse = 8;
263     info->satinfo.inview = 8;
264
265     for(it = 0; it < info->satinfo.inview; ++it)
266     {
267         info->satinfo.sat[it].id = it + 1;
268         info->satinfo.sat[it].in_use = 1;
269         info->satinfo.sat[it].elv = 5;
270         info->satinfo.sat[it].azimuth = (int)srt;
271         info->satinfo.sat[it].sig = 80;
272         srt += deg;
273     }
274
275     return 1;
276 }
277
278 int nmea_igen_rotate_init(nmeaGENERATOR *gen, nmeaINFO *info)
279 {
280     info->sig = 3;
281     info->fix = 3;
282
283     nmea_igen_rotate_reset(gen, info);
284
285     return 1;
286 }
287
288 /*
289  * POS_RANDMOVE generator
290  */
291
292 int nmea_igen_pos_rmove_init(nmeaGENERATOR *gen __attribute__ ((unused)),
293                 nmeaINFO *info)
294 {    
295     info->sig = 3;
296     info->fix = 3;
297     info->direction = info->declination = 0;
298     info->speed = 20;
299     return 1;
300 }
301
302 int nmea_igen_pos_rmove_loop(nmeaGENERATOR *gen __attribute__ ((unused)),
303                 nmeaINFO *info)
304 {
305     nmeaPOS crd;
306
307     info->direction += nmea_random(-10, 10);
308     info->speed += nmea_random(-2, 3);
309
310     if(info->direction < 0)
311         info->direction = 359 + info->direction;
312     if(info->direction > 359)
313         info->direction -= 359;
314
315     if(info->speed > 40)
316         info->speed = 40;
317     if(info->speed < 1)
318         info->speed = 1;
319
320     nmea_info2pos(info, &crd);
321     nmea_move_horz(&crd, &crd, info->direction, info->speed / 3600);
322     nmea_pos2info(&crd, info);
323
324     info->declination = info->direction;
325
326     return 1;
327 };
328
329 int nmea_igen_pos_rmove_destroy(nmeaGENERATOR *gen __attribute__ ((unused)))
330 {
331     return 1;
332 };
333
334 /*
335  * generator create
336  */
337
338 nmeaGENERATOR * __nmea_create_generator(int type, nmeaINFO *info)
339 {
340     nmeaGENERATOR *gen = 0;
341
342     switch(type)
343     {
344     case NMEA_GEN_NOISE:
345         if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
346             nmea_error("Insufficient memory!");
347         else
348         {
349             memset(gen, 0, sizeof(nmeaGENERATOR));
350             gen->init_call = &nmea_igen_noise_init;
351             gen->loop_call = &nmea_igen_noise_loop;
352             gen->reset_call = &nmea_igen_noise_reset;
353         }
354         break;
355     case NMEA_GEN_STATIC:
356     case NMEA_GEN_SAT_STATIC:
357         if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
358             nmea_error("Insufficient memory!");
359         else
360         {
361             memset(gen, 0, sizeof(nmeaGENERATOR));
362             gen->init_call = &nmea_igen_static_init;
363             gen->loop_call = &nmea_igen_static_loop;
364             gen->reset_call = &nmea_igen_static_reset;
365         }
366         break;
367     case NMEA_GEN_SAT_ROTATE:
368         if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
369             nmea_error("Insufficient memory!");
370         else
371         {
372             memset(gen, 0, sizeof(nmeaGENERATOR));
373             gen->init_call = &nmea_igen_rotate_init;
374             gen->loop_call = &nmea_igen_rotate_loop;
375             gen->reset_call = &nmea_igen_rotate_reset;
376         }
377         break;
378     case NMEA_GEN_POS_RANDMOVE:
379         if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
380             nmea_error("Insufficient memory!");
381         else
382         {
383             memset(gen, 0, sizeof(nmeaGENERATOR));
384             gen->init_call = &nmea_igen_pos_rmove_init;
385             gen->loop_call = &nmea_igen_pos_rmove_loop;
386             gen->destroy_call = &nmea_igen_pos_rmove_destroy;
387         }
388         break;
389     default:
390     /* case NMEA_GEN_ROTATE: */
391         gen = __nmea_create_generator(NMEA_GEN_SAT_ROTATE, info);
392         nmea_gen_add(gen, __nmea_create_generator(NMEA_GEN_POS_RANDMOVE, info));
393         break;
394     };
395
396     return gen;
397 }
398
399 nmeaGENERATOR * nmea_create_generator(int type, nmeaINFO *info)
400 {
401     nmeaGENERATOR *gen = __nmea_create_generator(type, info);
402
403     if(gen)
404         nmea_gen_init(gen, info);
405
406     return gen;
407 }
408
409 void nmea_destroy_generator(nmeaGENERATOR *gen)
410 {
411     nmea_gen_destroy(gen);
412 }