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