From 778ae042e4213f2199db293cb667ec2047192593 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Wed, 17 Jun 2009 16:00:53 +0000 Subject: [PATCH] --- Makefile | 2 -- conf/airframes/booz2_a1.xml | 1 + conf/autopilot/booz2_simulator_nps.makefile | 33 +++++++++++++++++++++ sw/airborne/pprz_algebra_double.h | 32 ++++++++++---------- sw/simulator/nps_autopilot.h | 1 + sw/simulator/nps_fdm_jsbsim.c | 14 +++++---- sw/simulator/nps_main.c | 26 ++++++++++------ sw/simulator/nps_sensor_gyro.h | 26 ++++++++-------- sw/simulator/nps_sensors.h | 15 ++++++++-- 9 files changed, 101 insertions(+), 49 deletions(-) create mode 100644 conf/autopilot/booz2_simulator_nps.makefile diff --git a/Makefile b/Makefile index 5e80b63303..827707594f 100644 --- a/Makefile +++ b/Makefile @@ -123,8 +123,6 @@ include Makefile.ac sim : sim_static -jsim: - cd $(SIMULATOR); $(MAKE) PAPARAZZI_SRC=$(PAPARAZZI_SRC) ac_h ac1 ac2 ac3 ac fbw ap: static conf diff --git a/conf/airframes/booz2_a1.xml b/conf/airframes/booz2_a1.xml index 873735e4bf..1e928bbf69 100644 --- a/conf/airframes/booz2_a1.xml +++ b/conf/airframes/booz2_a1.xml @@ -171,6 +171,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 diff --git a/conf/autopilot/booz2_simulator_nps.makefile b/conf/autopilot/booz2_simulator_nps.makefile new file mode 100644 index 0000000000..d5f28423e7 --- /dev/null +++ b/conf/autopilot/booz2_simulator_nps.makefile @@ -0,0 +1,33 @@ +# +# SITL Simulator +# + +SIM_TYPE = JSBSIM + +MY_JSBSIM_ROOT = /home/violato/enac/programs/JSBSim +MY_JSBSIM_LIB = /home/violato/enac/programs/install_jsbsim +SRC_BOOZ=booz +SRC_BOOZ_SIM = $(SRC_BOOZ)/sim +#BOOZ_PRIV_ARCH = $(BOOZ_PRIV_SIM) + + +sim.ARCHDIR = $(ARCHI) +sim.ARCH = sitl +sim.TARGET = sim +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$(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 += -DJSBSIM_ROOT_DIR=\"/home/violato/enac/programs/JSBSim/\" + +sim.srcs = $(SIMDIR)/nps_main.c \ + $(SIMDIR)/nps_fdm_jsbsim.c \ + $(SIMDIR)/nps_sensors.c \ + $(SIMDIR)/nps_sensor_gyro.c \ + $(SIMDIR)/nps_autopilot.c \ + diff --git a/sw/airborne/pprz_algebra_double.h b/sw/airborne/pprz_algebra_double.h index aafdda4584..188e61ea8c 100644 --- a/sw/airborne/pprz_algebra_double.h +++ b/sw/airborne/pprz_algebra_double.h @@ -5,37 +5,37 @@ struct DoubleVect2 { - FLOAT_T x; - FLOAT_T y; + double x; + double y; }; struct DoubleVect3 { - FLOAT_T x; - FLOAT_T y; - FLOAT_T z; + double x; + double y; + double z; }; struct DoubleQuat { - FLOAT_T qi; - FLOAT_T qx; - FLOAT_T qy; - FLOAT_T qz; + double qi; + double qx; + double qy; + double qz; }; struct DoubleMat33 { - FLOAT_T m[3*3]; + double m[3*3]; }; struct DoubleEulers { - FLOAT_T phi; - FLOAT_T theta; - FLOAT_T psi; + double phi; + double theta; + double psi; }; struct DoubleRates { - FLOAT_T p; - FLOAT_T q; - FLOAT_T r; + double p; + double q; + double r; }; diff --git a/sw/simulator/nps_autopilot.h b/sw/simulator/nps_autopilot.h index 5f40c2b3db..b8d3c1be86 100644 --- a/sw/simulator/nps_autopilot.h +++ b/sw/simulator/nps_autopilot.h @@ -9,6 +9,7 @@ struct NpsAutopilot { extern struct NpsAutopilot autopilot; +extern void nps_autopilot_init(void); extern void nps_autopilot_run_step(void); #endif /* NPS_AUTOPILOT_H */ diff --git a/sw/simulator/nps_fdm_jsbsim.c b/sw/simulator/nps_fdm_jsbsim.c index 4ff95894a3..60620d92b5 100644 --- a/sw/simulator/nps_fdm_jsbsim.c +++ b/sw/simulator/nps_fdm_jsbsim.c @@ -9,8 +9,8 @@ using namespace JSBSim; -void nps_fdm_feed_jsbsim(void); -void nps_fdm_fetch_state(void); +static void feed_jsbsim(double* commands); +static void fetch_state(void); FGFDMExec* FDMExec; @@ -22,15 +22,15 @@ void nps_fdm_init(double dt) { void nps_fdm_run_step(double* commands) { - nps_fdm_feed_jsbsim(double* commands); + feed_jsbsim(commands); /* run JSBSim */ - nps_fdm_fetch_state(); + fetch_state(); } -void nps_fdm_feed_jsbsim(double* commands) { +static void feed_jsbsim(double* commands) { #if 0 printf("Actuators Names:\n"); @@ -39,7 +39,9 @@ void nps_fdm_feed_jsbsim(double* commands) { } #endif -void nps_fdm_fetch_state(void) { +} + +static void fetch_state(void) { /* JSBSim to fdm */ diff --git a/sw/simulator/nps_main.c b/sw/simulator/nps_main.c index d2478bd616..b52c649be4 100644 --- a/sw/simulator/nps_main.c +++ b/sw/simulator/nps_main.c @@ -1,23 +1,30 @@ +#include #include "nps_fdm.h" #include "nps_sensors.h" -#include "nps_athmosphere.h" -#include "nps_autopilot" +#include "nps_atmosphere.h" +#include "nps_autopilot.h" #define SIM_DT (1./512.) #define DISPLAY_DT (1./25.) -struct { +static struct { double sim_time; double display_time; } nps_main; +static void nps_main_init(void); +static void nps_main_display(void); +static void nps_main_run_sim_step(void); +static gboolean nps_main_periodic(gpointer data __attribute__ ((unused))); + int main ( int argc, char** argv) { + nps_main_init(); return 0; } @@ -26,7 +33,7 @@ int main ( int argc, char** argv) { -void nps_main_init(void) { +static void nps_main_init(void) { nps_main.sim_time = 0.; nps_main.display_time = 0.; @@ -35,23 +42,24 @@ void nps_main_init(void) { -void nps_main_run_sim_step(void) { +static void nps_main_run_sim_step(void) { nps_fdm_run_step(autopilot.commands); - nps_sensors_run_step(); + // nps_sensors_run_step(); - nps_autopilot_run_step(); + // nps_autopilot_run_step(); } -void nps_main_display(void) { + +static void nps_main_display(void) { } -gboolean nps_main_periodic(gpointer data __attribute__ ((unused))) { +static gboolean nps_main_periodic(gpointer data __attribute__ ((unused))) { /* FIXME */ #if 0 diff --git a/sw/simulator/nps_sensor_gyro.h b/sw/simulator/nps_sensor_gyro.h index 93f2a3923d..123c55591d 100644 --- a/sw/simulator/nps_sensor_gyro.h +++ b/sw/simulator/nps_sensor_gyro.h @@ -1,20 +1,20 @@ #ifndef NPS_SENSORS_GYRO_H #define NPS_SENSORS_GYRO_H -#include +#include "pprz_algebra_double.h" -struct NpsSensorsGyro { - VEC* gyro; - unsigned int gyro_resolution; - MAT* gyro_sensitivity; - VEC* gyro_neutral; - VEC* gyro_noise_std_dev; - VEC* gyro_bias_initial; - VEC* gyro_bias_random_walk_std_dev; - VEC* gyro_bias_random_walk_value; - double gyro_next_update; - int gyro_available; -} +struct NpsSensorGyro { + struct DoubleVect3 value; + unsigned int resolution; + struct DoubleMat33 sensitivity; + struct DoubleVect3 neutral; + struct DoubleVect3 noise_std_dev; + struct DoubleVect3 bias_initial; + struct DoubleVect3 bias_random_walk_std_dev; + struct DoubleVect3 bias_random_walk_value; + double next_update; + int available; +}; #endif /* NPS_SENSORS_GYRO_H */ diff --git a/sw/simulator/nps_sensors.h b/sw/simulator/nps_sensors.h index 8ba0480992..ebf3b75aa9 100644 --- a/sw/simulator/nps_sensors.h +++ b/sw/simulator/nps_sensors.h @@ -1,14 +1,23 @@ #ifndef NPS_SENSORS_H #define NPS_SENSORS_H -#include "nps_sensors_gyro.h" +#include "nps_sensor_gyro.h" +//nclude "nps_sensor_accel.h" +//nclude "nps_sensor_mag.h" +//nclude "nps_sensor_baro.h" +//#include "nps_sensor_gps.h" + struct NpsSensors { - struct NpsSensorsGyro gyro; - struct NpsSensorsGyro accel; + struct NpsSensorGyro gyro; + // struct NpsSensorAccel accel; + // struct NpsSensorMag mag; + // struct NpsSensorBaro baro; + // struct NpsSensorGps gps; }; +extern struct NpsSensors sensors; #endif /* NPS_SENSORS_H */