moved nps to its own directory

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             \

sw/simulator/nps/nps_random.c

@@ -0,0 +1,227 @@
+#include "nps_random.h"
+
+
+#include <math.h>
+#include <limits.h>
+
+/*
+ * 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;
+}
+

sw/simulator/nps_random.c

@@ -1,37 +0,0 @@
-#include "nps_random.h"
-
-
-#include <math.h>
-
-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;
-}