dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity
px4-firmware/apps/gps/nmealib/generator.c

400 lines
8.8 KiB
C
Raw Blame History

/*
*
* NMEA library
* URL: http://nmea.sourceforge.net
* Author: Tim (xtimor@gmail.com)
* Licence: http://www.gnu.org/licenses/lgpl.html
* $Id: generator.c 17 2008-03-11 11:56:11Z xtimor $
*
*/
#include "nmea/gmath.h"
#include "nmea/generate.h"
#include "nmea/generator.h"
#include "nmea/context.h"
#include <string.h>
#include <stdlib.h>
#if defined(NMEA_WIN) && defined(_MSC_VER)
# pragma warning(disable: 4100) /* unreferenced formal parameter */
#endif
float nmea_random(float min, float max)
{
static float rand_max = MAX_RAND;//RAND_MAX; //nuttx defines MAX_RAND instead of RAND_MAX
float rand_val = rand();
float bounds = max - min;
return min + (rand_val * bounds) / rand_max;
}
/*
* low level
*/
int nmea_gen_init(nmeaGENERATOR *gen, nmeaINFO *info)
{
int RetVal = 1; int smask = info->smask;
nmeaGENERATOR *igen = gen;
nmea_zero_INFO(info);
info->smask = smask;
info->lat = NMEA_DEF_LAT;
info->lon = NMEA_DEF_LON;
while(RetVal && igen)
{
if(igen->init_call)
RetVal = (*igen->init_call)(igen, info);
igen = igen->next;
}
return RetVal;
}
int nmea_gen_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
int RetVal = 1;
if(gen->loop_call)
RetVal = (*gen->loop_call)(gen, info);
if(RetVal && gen->next)
RetVal = nmea_gen_loop(gen->next, info);
return RetVal;
}
int nmea_gen_reset(nmeaGENERATOR *gen, nmeaINFO *info)
{
int RetVal = 1;
if(gen->reset_call)
RetVal = (*gen->reset_call)(gen, info);
return RetVal;
}
void nmea_gen_destroy(nmeaGENERATOR *gen)
{
if(gen->next)
{
nmea_gen_destroy(gen->next);
gen->next = 0;
}
if(gen->destroy_call)
(*gen->destroy_call)(gen);
free(gen);
}
void nmea_gen_add(nmeaGENERATOR *to, nmeaGENERATOR *gen)
{
if(to->next)
nmea_gen_add(to->next, gen);
else
to->next = gen;
}
int nmea_generate_from(
char *buff, int buff_sz,
nmeaINFO *info,
nmeaGENERATOR *gen,
int generate_mask
)
{
int retval;
if(0 != (retval = nmea_gen_loop(gen, info)))
retval = nmea_generate(buff, buff_sz, info, generate_mask);
return retval;
}
/*
* NOISE generator
*/
int nmea_igen_noise_init(nmeaGENERATOR *gen, nmeaINFO *info)
{
return 1;
}
int nmea_igen_noise_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
int it;
int in_use;
info->sig = (int)nmea_random(1, 3);
info->PDOP = nmea_random(0, 9);
info->HDOP = nmea_random(0, 9);
info->VDOP = nmea_random(0, 9);
info->fix = (int)nmea_random(2, 3);
info->lat = nmea_random(0, 100);
info->lon = nmea_random(0, 100);
info->speed = nmea_random(0, 100);
info->direction = nmea_random(0, 360);
info->declination = nmea_random(0, 360);
info->elv = (int)nmea_random(-100, 100);
info->satinfo.inuse = 0;
info->satinfo.inview = 0;
for(it = 0; it < 12; ++it)
{
info->satinfo.sat[it].id = it;
info->satinfo.sat[it].in_use = in_use = (int)nmea_random(0, 3);
info->satinfo.sat[it].elv = (int)nmea_random(0, 90);
info->satinfo.sat[it].azimuth = (int)nmea_random(0, 359);
info->satinfo.sat[it].sig = (int)(in_use?nmea_random(40, 99):nmea_random(0, 40));
if(in_use)
info->satinfo.inuse++;
if(info->satinfo.sat[it].sig > 0)
info->satinfo.inview++;
}
return 1;
}
int nmea_igen_noise_reset(nmeaGENERATOR *gen, nmeaINFO *info)
{
return 1;
}
/*
* STATIC generator
*/
int nmea_igen_static_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
nmea_time_now(&info->utc);
return 1;
};
int nmea_igen_static_reset(nmeaGENERATOR *gen, nmeaINFO *info)
{
info->satinfo.inuse = 4;
info->satinfo.inview = 4;
info->satinfo.sat[0].id = 1;
info->satinfo.sat[0].in_use = 1;
info->satinfo.sat[0].elv = 50;
info->satinfo.sat[0].azimuth = 0;
info->satinfo.sat[0].sig = 99;
info->satinfo.sat[1].id = 2;
info->satinfo.sat[1].in_use = 1;
info->satinfo.sat[1].elv = 50;
info->satinfo.sat[1].azimuth = 90;
info->satinfo.sat[1].sig = 99;
info->satinfo.sat[2].id = 3;
info->satinfo.sat[2].in_use = 1;
info->satinfo.sat[2].elv = 50;
info->satinfo.sat[2].azimuth = 180;
info->satinfo.sat[2].sig = 99;
info->satinfo.sat[3].id = 4;
info->satinfo.sat[3].in_use = 1;
info->satinfo.sat[3].elv = 50;
info->satinfo.sat[3].azimuth = 270;
info->satinfo.sat[3].sig = 99;
return 1;
}
int nmea_igen_static_init(nmeaGENERATOR *gen, nmeaINFO *info)
{
info->sig = 3;
info->fix = 3;
nmea_igen_static_reset(gen, info);
return 1;
}
/*
* SAT_ROTATE generator
*/
int nmea_igen_rotate_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
int it;
int count = info->satinfo.inview;
float deg = 360 / (count?count:1);
float srt = (count?(info->satinfo.sat[0].azimuth):0) + 5;
nmea_time_now(&info->utc);
for(it = 0; it < count; ++it)
{
info->satinfo.sat[it].azimuth =
(int)((srt >= 360)?srt - 360:srt);
srt += deg;
}
return 1;
};
int nmea_igen_rotate_reset(nmeaGENERATOR *gen, nmeaINFO *info)
{
int it;
float deg = 360 / 8;
float srt = 0;
info->satinfo.inuse = 8;
info->satinfo.inview = 8;
for(it = 0; it < info->satinfo.inview; ++it)
{
info->satinfo.sat[it].id = it + 1;
info->satinfo.sat[it].in_use = 1;
info->satinfo.sat[it].elv = 5;
info->satinfo.sat[it].azimuth = (int)srt;
info->satinfo.sat[it].sig = 80;
srt += deg;
}
return 1;
}
int nmea_igen_rotate_init(nmeaGENERATOR *gen, nmeaINFO *info)
{
info->sig = 3;
info->fix = 3;
nmea_igen_rotate_reset(gen, info);
return 1;
}
/*
* POS_RANDMOVE generator
*/
int nmea_igen_pos_rmove_init(nmeaGENERATOR *gen, nmeaINFO *info)
{
info->sig = 3;
info->fix = 3;
info->direction = info->declination = 0;
info->speed = 20;
return 1;
}
int nmea_igen_pos_rmove_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
nmeaPOS crd;
info->direction += nmea_random(-10, 10);
info->speed += nmea_random(-2, 3);
if(info->direction < 0)
info->direction = 359 + info->direction;
if(info->direction > 359)
info->direction -= 359;
if(info->speed > 40)
info->speed = 40;
if(info->speed < 1)
info->speed = 1;
nmea_info2pos(info, &crd);
nmea_move_horz(&crd, &crd, info->direction, info->speed / 3600);
nmea_pos2info(&crd, info);
info->declination = info->direction;
return 1;
};
int nmea_igen_pos_rmove_destroy(nmeaGENERATOR *gen)
{
return 1;
};
/*
* generator create
*/
nmeaGENERATOR * __nmea_create_generator(int type, nmeaINFO *info)
{
nmeaGENERATOR *gen = 0;
switch(type)
{
case NMEA_GEN_NOISE:
if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
nmea_error("Insufficient memory!");
else
{
memset(gen, 0, sizeof(nmeaGENERATOR));
gen->init_call = &nmea_igen_noise_init;
gen->loop_call = &nmea_igen_noise_loop;
gen->reset_call = &nmea_igen_noise_reset;
}
break;
case NMEA_GEN_STATIC:
case NMEA_GEN_SAT_STATIC:
if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
nmea_error("Insufficient memory!");
else
{
memset(gen, 0, sizeof(nmeaGENERATOR));
gen->init_call = &nmea_igen_static_init;
gen->loop_call = &nmea_igen_static_loop;
gen->reset_call = &nmea_igen_static_reset;
}
break;
case NMEA_GEN_SAT_ROTATE:
if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
nmea_error("Insufficient memory!");
else
{
memset(gen, 0, sizeof(nmeaGENERATOR));
gen->init_call = &nmea_igen_rotate_init;
gen->loop_call = &nmea_igen_rotate_loop;
gen->reset_call = &nmea_igen_rotate_reset;
}
break;
case NMEA_GEN_POS_RANDMOVE:
if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
nmea_error("Insufficient memory!");
else
{
memset(gen, 0, sizeof(nmeaGENERATOR));
gen->init_call = &nmea_igen_pos_rmove_init;
gen->loop_call = &nmea_igen_pos_rmove_loop;
gen->destroy_call = &nmea_igen_pos_rmove_destroy;
}
break;
case NMEA_GEN_ROTATE:
gen = __nmea_create_generator(NMEA_GEN_SAT_ROTATE, info);
nmea_gen_add(gen, __nmea_create_generator(NMEA_GEN_POS_RANDMOVE, info));
break;
};
return gen;
}
nmeaGENERATOR * nmea_create_generator(int type, nmeaINFO *info)
{
nmeaGENERATOR *gen = __nmea_create_generator(type, info);
if(gen)
nmea_gen_init(gen, info);
return gen;
}
void nmea_destroy_generator(nmeaGENERATOR *gen)
{
nmea_gen_destroy(gen);
}
#if defined(NMEA_WIN) && defined(_MSC_VER)
# pragma warning(default: 4100)
#endif
View Git Blame Copy Permalink