diff --git a/conf/autopilot/booz2_simulator_nps.makefile b/conf/autopilot/booz2_simulator_nps.makefile index ed5024ca56..b39158d4fb 100644 --- a/conf/autopilot/booz2_simulator_nps.makefile +++ b/conf/autopilot/booz2_simulator_nps.makefile @@ -11,6 +11,8 @@ JSBSIM_LIB = $(JSBSIM_ROOT)/lib SRC_BOOZ=booz SRC_BOOZ_SIM = $(SRC_BOOZ)/arch/sim +NPSDIR = $(SIMDIR)/nps + sim.ARCHDIR = $(ARCHI) sim.ARCH = sitl @@ -20,26 +22,24 @@ sim.TARGETDIR = sim sim.CFLAGS += -DSITL sim.CFLAGS += `pkg-config glib-2.0 --cflags` -I /usr/include/meschach sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lmeschach -lpcre -lglibivy -sim.CFLAGS += -I$(SIMDIR) -I/usr/local/include -I$(JSBSIM_INC) +sim.CFLAGS += -I$(NPSDIR) -I/usr/local/include -I$(JSBSIM_INC) sim.LDFLAGS += -L$(JSBSIM_LIB) -lJSBSim sim.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ_SIM) -I../simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps -sim.srcs = $(SIMDIR)/nps_main.c \ - $(SIMDIR)/nps_fdm_jsbsim.c \ - $(SIMDIR)/nps_random.c \ - $(SIMDIR)/booz_r250.c \ - $(SIMDIR)/booz_randlcg.c \ - $(SIMDIR)/nps_sensors.c \ - $(SIMDIR)/nps_sensor_gyro.c \ - $(SIMDIR)/nps_sensor_accel.c \ - $(SIMDIR)/nps_sensor_mag.c \ - $(SIMDIR)/nps_sensor_baro.c \ - $(SIMDIR)/nps_sensor_gps.c \ - $(SIMDIR)/nps_radio_control.c \ - $(SIMDIR)/nps_autopilot_booz.c \ - $(SIMDIR)/nps_ivy.c \ - $(SIMDIR)/nps_flightgear.c \ +sim.srcs = $(NPSDIR)/nps_main.c \ + $(NPSDIR)/nps_fdm_jsbsim.c \ + $(NPSDIR)/nps_random.c \ + $(NPSDIR)/nps_sensors.c \ + $(NPSDIR)/nps_sensor_gyro.c \ + $(NPSDIR)/nps_sensor_accel.c \ + $(NPSDIR)/nps_sensor_mag.c \ + $(NPSDIR)/nps_sensor_baro.c \ + $(NPSDIR)/nps_sensor_gps.c \ + $(NPSDIR)/nps_radio_control.c \ + $(NPSDIR)/nps_autopilot_booz.c \ + $(NPSDIR)/nps_ivy.c \ + $(NPSDIR)/nps_flightgear.c \ sim.srcs += math/pprz_trig_int.c \ diff --git a/sw/simulator/nps_atmosphere.h b/sw/simulator/nps/nps_atmosphere.h similarity index 100% rename from sw/simulator/nps_atmosphere.h rename to sw/simulator/nps/nps_atmosphere.h diff --git a/sw/simulator/nps_autopilot.h b/sw/simulator/nps/nps_autopilot.h similarity index 100% rename from sw/simulator/nps_autopilot.h rename to sw/simulator/nps/nps_autopilot.h diff --git a/sw/simulator/nps_autopilot_booz.c b/sw/simulator/nps/nps_autopilot_booz.c similarity index 100% rename from sw/simulator/nps_autopilot_booz.c rename to sw/simulator/nps/nps_autopilot_booz.c diff --git a/sw/simulator/nps_fdm.h b/sw/simulator/nps/nps_fdm.h similarity index 100% rename from sw/simulator/nps_fdm.h rename to sw/simulator/nps/nps_fdm.h diff --git a/sw/simulator/nps_fdm_jsbsim.c b/sw/simulator/nps/nps_fdm_jsbsim.c similarity index 100% rename from sw/simulator/nps_fdm_jsbsim.c rename to sw/simulator/nps/nps_fdm_jsbsim.c diff --git a/sw/simulator/nps_flightgear.c b/sw/simulator/nps/nps_flightgear.c similarity index 100% rename from sw/simulator/nps_flightgear.c rename to sw/simulator/nps/nps_flightgear.c diff --git a/sw/simulator/nps_flightgear.h b/sw/simulator/nps/nps_flightgear.h similarity index 100% rename from sw/simulator/nps_flightgear.h rename to sw/simulator/nps/nps_flightgear.h diff --git a/sw/simulator/nps_ivy.c b/sw/simulator/nps/nps_ivy.c similarity index 100% rename from sw/simulator/nps_ivy.c rename to sw/simulator/nps/nps_ivy.c diff --git a/sw/simulator/nps_ivy.h b/sw/simulator/nps/nps_ivy.h similarity index 100% rename from sw/simulator/nps_ivy.h rename to sw/simulator/nps/nps_ivy.h diff --git a/sw/simulator/nps_main.c b/sw/simulator/nps/nps_main.c similarity index 100% rename from sw/simulator/nps_main.c rename to sw/simulator/nps/nps_main.c diff --git a/sw/simulator/nps_radio_control.c b/sw/simulator/nps/nps_radio_control.c similarity index 100% rename from sw/simulator/nps_radio_control.c rename to sw/simulator/nps/nps_radio_control.c diff --git a/sw/simulator/nps_radio_control.h b/sw/simulator/nps/nps_radio_control.h similarity index 100% rename from sw/simulator/nps_radio_control.h rename to sw/simulator/nps/nps_radio_control.h diff --git a/sw/simulator/nps/nps_random.c b/sw/simulator/nps/nps_random.c new file mode 100644 index 0000000000..315aa14d6e --- /dev/null +++ b/sw/simulator/nps/nps_random.c @@ -0,0 +1,227 @@ +#include "nps_random.h" + + +#include +#include + +/* + * R250 + * Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast + * Shift-Register Sequence Random Number Generator", + * Journal of Computational Physics, V.40 + * + */ + +static void r250_init(int seed); +//static unsigned int r250( void ); +static double dr250( void ); + +/* + * randclg + * Linear Congruential Method, the "minimal standard generator" + * Park & Miller, 1988, Comm of the ACM, 31(10), pp. 1192-1201 + * + */ + +static long set_seed(long); +//static long get_seed(void); +static unsigned long int randlcg(void); + + + + + +void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev) { + vect->x += get_gaussian_noise() * std_dev->x; + vect->y += get_gaussian_noise() * std_dev->y; + vect->z += get_gaussian_noise() * std_dev->z; +} + +/* + http://www.taygeta.com/random/gaussian.html +*/ + + +double get_gaussian_noise(void) { + + double x1; + static int nb_call = 0; + static double x2, w; + if (nb_call==0) r250_init(0); + nb_call++; + if (nb_call%2) { + do { + x1 = 2.0 * dr250() - 1.0; + x2 = 2.0 * dr250() - 1.0; + w = x1 * x1 + x2 * x2; + } while ( w >= 1.0 ); + + w = sqrt( (-2.0 * log( w ) ) / w ); + return x1 * w; + } + else + return x2 * w; +} + + + + +/* + * R250 + * Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast + * Shift-Register Sequence Random Number Generator", + * Journal of Computational Physics, V.40 + * + */ + +/* defines to allow for 16 or 32 bit integers */ +#define BITS 31 + + +#if WORD_BIT == 32 +#ifndef BITS +#define BITS 32 +#endif +#else +#ifndef BITS +#define BITS 16 +#endif +#endif + +#if BITS == 31 +#define MSB 0x40000000L +#define ALL_BITS 0x7fffffffL +#define HALF_RANGE 0x20000000L +#define STEP 7 +#endif + +#if BITS == 32 +#define MSB 0x80000000L +#define ALL_BITS 0xffffffffL +#define HALF_RANGE 0x40000000L +#define STEP 7 +#endif + +#if BITS == 16 +#define MSB 0x8000 +#define ALL_BITS 0xffff +#define HALF_RANGE 0x4000 +#define STEP 11 +#endif + +static unsigned int r250_buffer[ 250 ]; +static int r250_index; + +static void r250_init(int sd) { + int j, k; + unsigned int mask, msb; + set_seed( sd ); + + r250_index = 0; + for (j = 0; j < 250; j++) /* fill r250 buffer with BITS-1 bit values */ + r250_buffer[j] = randlcg(); + + for (j = 0; j < 250; j++) /* set some MSBs to 1 */ + if ( randlcg() > HALF_RANGE ) + r250_buffer[j] |= MSB; + + msb = MSB; /* turn on diagonal bit */ + mask = ALL_BITS; /* turn off the leftmost bits */ + + for (j = 0; j < BITS; j++) { + k = STEP * j + 3; /* select a word to operate on */ + r250_buffer[k] &= mask; /* turn off bits left of the diagonal */ + r250_buffer[k] |= msb; /* turn on the diagonal bit */ + mask >>= 1; + msb >>= 1; + } + +} + +#if 0 +/* returns a random unsigned integer */ +static unsigned int r250(void) { + register int j; + register unsigned int new_rand; + + if ( r250_index >= 147 ) + j = r250_index - 147; /* wrap pointer around */ + else + j = r250_index + 103; + + new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ]; + r250_buffer[ r250_index ] = new_rand; + + if ( r250_index >= 249 ) /* increment pointer for next time */ + r250_index = 0; + else + r250_index++; + + return new_rand; + +} +#endif + +/* returns a random double in range 0..1 */ +static double dr250(void) { + register int j; + register unsigned int new_rand; + + if ( r250_index >= 147 ) + j = r250_index - 147; /* wrap pointer around */ + else + j = r250_index + 103; + + new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ]; + r250_buffer[ r250_index ] = new_rand; + + if ( r250_index >= 249 ) /* increment pointer for next time */ + r250_index = 0; + else + r250_index++; + + return (double)new_rand / ALL_BITS; + +} + +/* + * randclg + * Linear Congruential Method, the "minimal standard generator" + * Park & Miller, 1988, Comm of the ACM, 31(10), pp. 1192-1201 + * + */ + +static long int quotient = LONG_MAX / 16807L; +static long int my_remainder = LONG_MAX % 16807L; + +static long int seed_val = 1L; + +static long set_seed(long int sd) { + return seed_val = sd; +} + +//static long get_seed(void) { +// return seed_val; +//} + +/* returns a random unsigned integer */ +unsigned long int randlcg() { + if ( seed_val <= quotient ) + seed_val = (seed_val * 16807L) % LONG_MAX; + else + { + long int high_part = seed_val / quotient; + long int low_part = seed_val % quotient; + + long int test = 16807L * low_part - my_remainder * high_part; + + if ( test > 0 ) + seed_val = test; + else + seed_val = test + LONG_MAX; + + } + + return seed_val; +} + diff --git a/sw/simulator/nps_random.h b/sw/simulator/nps/nps_random.h similarity index 100% rename from sw/simulator/nps_random.h rename to sw/simulator/nps/nps_random.h diff --git a/sw/simulator/nps_sensor_accel.c b/sw/simulator/nps/nps_sensor_accel.c similarity index 100% rename from sw/simulator/nps_sensor_accel.c rename to sw/simulator/nps/nps_sensor_accel.c diff --git a/sw/simulator/nps_sensor_accel.h b/sw/simulator/nps/nps_sensor_accel.h similarity index 100% rename from sw/simulator/nps_sensor_accel.h rename to sw/simulator/nps/nps_sensor_accel.h diff --git a/sw/simulator/nps_sensor_baro.c b/sw/simulator/nps/nps_sensor_baro.c similarity index 100% rename from sw/simulator/nps_sensor_baro.c rename to sw/simulator/nps/nps_sensor_baro.c diff --git a/sw/simulator/nps_sensor_baro.h b/sw/simulator/nps/nps_sensor_baro.h similarity index 100% rename from sw/simulator/nps_sensor_baro.h rename to sw/simulator/nps/nps_sensor_baro.h diff --git a/sw/simulator/nps_sensor_gps.c b/sw/simulator/nps/nps_sensor_gps.c similarity index 100% rename from sw/simulator/nps_sensor_gps.c rename to sw/simulator/nps/nps_sensor_gps.c diff --git a/sw/simulator/nps_sensor_gps.h b/sw/simulator/nps/nps_sensor_gps.h similarity index 100% rename from sw/simulator/nps_sensor_gps.h rename to sw/simulator/nps/nps_sensor_gps.h diff --git a/sw/simulator/nps_sensor_gyro.c b/sw/simulator/nps/nps_sensor_gyro.c similarity index 100% rename from sw/simulator/nps_sensor_gyro.c rename to sw/simulator/nps/nps_sensor_gyro.c diff --git a/sw/simulator/nps_sensor_gyro.h b/sw/simulator/nps/nps_sensor_gyro.h similarity index 100% rename from sw/simulator/nps_sensor_gyro.h rename to sw/simulator/nps/nps_sensor_gyro.h diff --git a/sw/simulator/nps_sensor_mag.c b/sw/simulator/nps/nps_sensor_mag.c similarity index 100% rename from sw/simulator/nps_sensor_mag.c rename to sw/simulator/nps/nps_sensor_mag.c diff --git a/sw/simulator/nps_sensor_mag.h b/sw/simulator/nps/nps_sensor_mag.h similarity index 100% rename from sw/simulator/nps_sensor_mag.h rename to sw/simulator/nps/nps_sensor_mag.h diff --git a/sw/simulator/nps_sensors.c b/sw/simulator/nps/nps_sensors.c similarity index 100% rename from sw/simulator/nps_sensors.c rename to sw/simulator/nps/nps_sensors.c diff --git a/sw/simulator/nps_sensors.h b/sw/simulator/nps/nps_sensors.h similarity index 100% rename from sw/simulator/nps_sensors.h rename to sw/simulator/nps/nps_sensors.h diff --git a/sw/simulator/nps_random.c b/sw/simulator/nps_random.c deleted file mode 100644 index 75994b59a3..0000000000 --- a/sw/simulator/nps_random.c +++ /dev/null @@ -1,37 +0,0 @@ -#include "nps_random.h" - - -#include - -void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev) { - vect->x += get_gaussian_noise() * std_dev->x; - vect->y += get_gaussian_noise() * std_dev->y; - vect->z += get_gaussian_noise() * std_dev->z; -} - -/* - http://www.taygeta.com/random/gaussian.html -*/ - -#include "booz_r250.h" - -double get_gaussian_noise(void) { - - double x1; - static int nb_call = 0; - static double x2, w; - if (nb_call==0) r250_init(0); - nb_call++; - if (nb_call%2) { - do { - x1 = 2.0 * dr250() - 1.0; - x2 = 2.0 * dr250() - 1.0; - w = x1 * x1 + x2 * x2; - } while ( w >= 1.0 ); - - w = sqrt( (-2.0 * log( w ) ) / w ); - return x1 * w; - } - else - return x2 * w; -} diff --git a/sw/simulator/tests/Makefile b/sw/simulator/old_booz/tests/Makefile similarity index 100% rename from sw/simulator/tests/Makefile rename to sw/simulator/old_booz/tests/Makefile diff --git a/sw/simulator/tests/test_fdm.c b/sw/simulator/old_booz/tests/test_fdm.c similarity index 100% rename from sw/simulator/tests/test_fdm.c rename to sw/simulator/old_booz/tests/test_fdm.c diff --git a/sw/simulator/tests/test_sensors.c b/sw/simulator/old_booz/tests/test_sensors.c similarity index 100% rename from sw/simulator/tests/test_sensors.c rename to sw/simulator/old_booz/tests/test_sensors.c