conf/airframes/booz2_a1.xml

@@ -154,8 +154,10 @@
  </section>
 
 
- <section name="SIMULATOR">
-   <define name="ACTUATOR_COMMAND_NAMES"  value="[&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;]"/>
+ <section name="SIMULATOR" prefix="NPS_">
+   <define name="ACTUATOR_NAMES"  value="[&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;]"/>
+   <define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
+   <define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_booz2_a1.h&quot;"/>
  </section>
 
 
@@ -171,7 +173,7 @@ include $(PAPARAZZI_SRC)/conf/autopilot/booz2_test_progs.makefile
 sim.CFLAGS += -DBSM_PARAMS=\"booz_sensors_model_params_booz2_a1.h\"
 #include $(PAPARAZZI_SRC)/conf/autopilot/booz2_simulator.makefile
 #include $(PAPARAZZI_SRC)/conf/autopilot/booz2_simulator_jsbsim.makefile
-sim.CFLAGS += -DNPS_SENSORS_PARAMS=\"booz_sensors_model_params_booz2_a1.h\"
+
 include $(PAPARAZZI_SRC)/conf/autopilot/booz2_simulator_nps.makefile
 
 

conf/autopilot/booz2_simulator_nps.makefile

@@ -21,7 +21,7 @@ sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lmeschach -lpcre -lglibivy
 sim.CFLAGS  += -I$(SIMDIR) -I/usr/local/include -I$(MY_JSBSIM_LIB)/include/JSBSim
 sim.LDFLAGS += -L$(MY_JSBSIM_LIB)/lib -lJSBSim
 
-sim.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ_SIM) -I../simulator
+sim.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ_SIM) -I../simulator -I$(PAPARAZZI_HOME)/conf/simulator
 sim.CFLAGS += -DJSBSIM_ROOT_DIR=\"/home/violato/enac/programs/JSBSim/\"
 
 sim.srcs = $(SIMDIR)/nps_main.c                      \

conf/simulator/nps_sensors_params_booz2_a1.h

@@ -0,0 +1,186 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2009 Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ *
+ */
+
+#ifndef NPS_SENSORS_PARAMS_H
+#define NPS_SENSORS_PARAMS_H
+
+#include "airframe.h"
+
+
+#define BSM_BODY_TO_IMU_PHI    RadOfDeg(4.)
+#define BSM_BODY_TO_IMU_THETA  RadOfDeg(3.)
+//#define BSM_BODY_TO_IMU_PHI    RadOfDeg(0.)
+//#define BSM_BODY_TO_IMU_THETA  RadOfDeg(0.)
+#define BSM_BODY_TO_IMU_PSI    RadOfDeg(0.)
+
+/* 
+ * Accelerometer 
+ */
+#define BSM_ACCEL_RESOLUTION      (65536)
+/* ms-2 */
+/* aka 2^10/ACCEL_X_SENS  */
+#define BSM_ACCEL_SENSITIVITY_XX  -408.92695
+#define BSM_ACCEL_SENSITIVITY_YY  -412.69325
+#define BSM_ACCEL_SENSITIVITY_ZZ  -407.32522
+#define BSM_ACCEL_NEUTRAL_X       32081
+#define BSM_ACCEL_NEUTRAL_Y       33738
+#define BSM_ACCEL_NEUTRAL_Z       32441
+/* m2s-4 */
+//#define BSM_ACCEL_NOISE_STD_DEV_X 0
+//#define BSM_ACCEL_NOISE_STD_DEV_Y 0
+//#define BSM_ACCEL_NOISE_STD_DEV_Z 0
+
+#define BSM_ACCEL_NOISE_STD_DEV_X 1.e-1
+#define BSM_ACCEL_NOISE_STD_DEV_Y 1.e-1
+#define BSM_ACCEL_NOISE_STD_DEV_Z 1.1e-1
+
+/* ms-2 */
+#define BSM_ACCEL_BIAS_X          0
+#define BSM_ACCEL_BIAS_Y          0
+#define BSM_ACCEL_BIAS_Z          0
+/* s */
+#define BSM_ACCEL_DT              (1./512.)
+
+
+
+/* 
+ * Gyrometer 
+ */
+#define NPS_GYRO_RESOLUTION       65536
+
+/* 2^12/GYRO_X_SENS */
+#define BSM_GYRO_SENSITIVITY_PP   ( 4055.)
+#define BSM_GYRO_SENSITIVITY_QQ   (-4055.)
+#define BSM_GYRO_SENSITIVITY_RR   (-4055.)
+
+#define BSM_GYRO_NEUTRAL_P        33924
+#define BSM_GYRO_NEUTRAL_Q        33417
+#define BSM_GYRO_NEUTRAL_R        32809
+
+//#define BSM_GYRO_NOISE_STD_DEV_P  RadOfDeg(0.)
+//#define BSM_GYRO_NOISE_STD_DEV_Q  RadOfDeg(0.)
+//#define BSM_GYRO_NOISE_STD_DEV_R  RadOfDeg(0.)
+
+#define BSM_GYRO_NOISE_STD_DEV_P  RadOfDeg(0.5)
+#define BSM_GYRO_NOISE_STD_DEV_Q  RadOfDeg(0.5)
+#define BSM_GYRO_NOISE_STD_DEV_R  RadOfDeg(0.5)
+
+#define BSM_GYRO_BIAS_INITIAL_P  RadOfDeg( 0.0)
+#define BSM_GYRO_BIAS_INITIAL_Q  RadOfDeg(  .0)
+#define BSM_GYRO_BIAS_INITIAL_R  RadOfDeg(  .0)
+
+#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.)
+#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.)
+#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.)
+
+#define BSM_GYRO_DT (1./512.)
+
+
+
+/*
+ *  Magnetometer
+ */
+//#define BSM_MAG_RESOLUTION 65536
+
+#define BSM_MAG_IMU_TO_SENSOR_PHI              0.
+#define BSM_MAG_IMU_TO_SENSOR_THETA            0.
+#define BSM_MAG_IMU_TO_SENSOR_PSI    RadOfDeg(45.)
+
+#define BSM_MAG_SENSITIVITY_XX   (1.*(1<<11)/-4.94075530)
+#define BSM_MAG_SENSITIVITY_YY   (1.*(1<<11)/ 5.10207664)
+#define BSM_MAG_SENSITIVITY_ZZ   (1.*(1<<11)/-4.90788848)
+
+#define BSM_MAG_NEUTRAL_X  2358
+#define BSM_MAG_NEUTRAL_Y  2362
+#define BSM_MAG_NEUTRAL_Z  2119
+
+//#define BSM_MAG_NOISE_STD_DEV_X  0
+//#define BSM_MAG_NOISE_STD_DEV_Y  0
+//#define BSM_MAG_NOISE_STD_DEV_Z  0
+
+#define BSM_MAG_NOISE_STD_DEV_X  2e-3
+#define BSM_MAG_NOISE_STD_DEV_Y  2e-3
+#define BSM_MAG_NOISE_STD_DEV_Z  2e-3
+
+#define BSM_MAG_DT (1./100.)
+
+
+/*
+ *  Range meter
+ */
+#define BSM_RANGEMETER_RESOLUTION  (1024)
+#define BSM_RANGEMETER_SENSITIVITY (1024. / 12.)
+#define BSM_RANGEMETER_MAX_RANGE   (6. * BSM_RANGEMETER_SENSITIVITY)
+#define BSM_RANGEMETER_DT          (1./20.)
+
+
+/*
+ *  Barometer
+ */
+/* m */
+/* aka 2^8/INS_BARO_SENS  */
+#define BSM_BARO_QNH             900.
+#define BSM_BARO_SENSITIVITY      17.066667
+#define BSM_BARO_DT          (1./100.)
+#define BSM_BARO_NOISE_STD_DEV     5.e-2
+
+/*
+ *  GPS
+ */
+
+#ifdef GPS_PERFECT
+
+#define BSM_GPS_SPEED_NOISE_STD_DEV            0.
+#define BSM_GPS_SPEED_LATENCY                  0.
+#define BSM_GPS_POS_NOISE_STD_DEV              0.001
+#define BSM_GPS_POS_BIAS_INITIAL_X             0.
+#define BSM_GPS_POS_BIAS_INITIAL_Y             0.
+#define BSM_GPS_POS_BIAS_INITIAL_Z             0.
+#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0. 
+#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0. 
+#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0. 
+#define BSM_GPS_POS_LATENCY                    0.
+
+#else
+
+#define BSM_GPS_SPEED_NOISE_STD_DEV            1e-1
+#define BSM_GPS_SPEED_LATENCY                  0.25
+#define BSM_GPS_POS_NOISE_STD_DEV              1e-1
+#define BSM_GPS_POS_BIAS_INITIAL_X             0e-1
+#define BSM_GPS_POS_BIAS_INITIAL_Y            -0e-1
+#define BSM_GPS_POS_BIAS_INITIAL_Z            -0e-1
+#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3 
+#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3 
+#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3 
+#define BSM_GPS_POS_LATENCY                    0.25
+
+#endif /* GPS_PERFECT */
+
+#define BSM_GPS_POS_INITIAL_UTM_EAST            37728000
+#define BSM_GPS_POS_INITIAL_UTM_NORTH          482464300
+#define BSM_GPS_POS_INITIAL_UTM_ALT                15200
+
+#define BSM_GPS_DT                           (1./4.)
+
+#endif /* NPS_SENSORS_PARAMS_H */

sw/simulator/nps_sensor_gyro.c

@@ -1,6 +1,7 @@
 //#include "nps_sensor_gyro.h"
 #include "nps_sensors.h"
 
+#include "airframe.h"
 #include NPS_SENSORS_PARAMS
 
 
@@ -14,11 +15,8 @@ bool_t nps_sensor_gyro_available() {
 
 void  nps_sensor_gyro_init(double time) {
   VECT3_ASSIGN(sensors.gyro.value, 0., 0., 0.);
+  sensors.gyro.resolution = NPS_GYRO_RESOLUTION;
 #if 0
-  bsm.gyro = v_get(AXIS_NB);
-  bsm.gyro->ve[AXIS_P] = 0.;
-  bsm.gyro->ve[AXIS_Q] = 0.;
-  bsm.gyro->ve[AXIS_R] = 0.;
   bsm.gyro_resolution = BSM_GYRO_RESOLUTION;
   
   bsm.gyro_sensitivity = m_get(AXIS_NB, AXIS_NB);