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