diff --git a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile
index ed7ce66951..3587d244df 100644
--- a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile
+++ b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile
@@ -53,6 +53,7 @@ nps.srcs += $(NPSDIR)/nps_main.c \
$(NPSDIR)/nps_sensor_baro.c \
$(NPSDIR)/nps_sensor_gps.c \
$(NPSDIR)/nps_electrical.c \
+ $(NPSDIR)/nps_atmosphere.c \
$(NPSDIR)/nps_radio_control.c \
$(NPSDIR)/nps_radio_control_joystick.c \
$(NPSDIR)/nps_radio_control_spektrum.c \
diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
index 45b618f08d..8db11612fd 100644
--- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
@@ -49,6 +49,7 @@ nps.srcs += $(NPSDIR)/nps_main.c \
$(NPSDIR)/nps_sensor_baro.c \
$(NPSDIR)/nps_sensor_gps.c \
$(NPSDIR)/nps_electrical.c \
+ $(NPSDIR)/nps_atmosphere.c \
$(NPSDIR)/nps_radio_control.c \
$(NPSDIR)/nps_radio_control_joystick.c \
$(NPSDIR)/nps_radio_control_spektrum.c \
diff --git a/conf/settings/nps.xml b/conf/settings/nps.xml
index 279caca01c..60dfaa5c98 100644
--- a/conf/settings/nps.xml
+++ b/conf/settings/nps.xml
@@ -7,6 +7,8 @@
+
+
diff --git a/sw/simulator/nps/nps_atmosphere.c b/sw/simulator/nps/nps_atmosphere.c
new file mode 100644
index 0000000000..e676b089cb
--- /dev/null
+++ b/sw/simulator/nps/nps_atmosphere.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * 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.
+ */
+
+/**
+ * @file nps_atmosphere.c
+ * Atmosphere model (pressure, wind) for NPS.
+ */
+
+#include "nps_atmosphere.h"
+#include "nps_fdm.h"
+
+#ifndef NPS_QNH
+#define NPS_QNH 101325.0
+#endif
+
+#ifndef NPS_WIND_SPEED
+#define NPS_WIND_SPEED 0.0
+#endif
+
+#ifndef NPS_WIND_DIR
+#define NPS_WIND_DIR 0
+#endif
+
+struct NpsAtmosphere nps_atmosphere;
+
+void nps_atmosphere_init(void) {
+
+ nps_atmosphere.qnh = NPS_QNH;
+ nps_atmosphere.wind_speed = NPS_WIND_SPEED;
+ nps_atmosphere.wind_dir = NPS_WIND_DIR;
+
+}
+
+void nps_atmosphere_update(double dt __attribute__((unused))) {
+ nps_fdm_set_wind(nps_atmosphere.wind_speed, nps_atmosphere.wind_dir);
+}
+
diff --git a/sw/simulator/nps/nps_atmosphere.h b/sw/simulator/nps/nps_atmosphere.h
index d167c77dff..88cc1cf820 100644
--- a/sw/simulator/nps/nps_atmosphere.h
+++ b/sw/simulator/nps/nps_atmosphere.h
@@ -1,6 +1,45 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * 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.
+ */
+
+/**
+ * @file nps_atmosphere.h
+ * Atmosphere model (pressure, wind) for NPS.
+ */
+
#ifndef NPS_ATMOSPHERE_H
#define NPS_ATMOSPHERE_H
+#include "math/pprz_algebra_double.h"
+
+struct NpsAtmosphere {
+ double qnh; ///< barometric pressure at sea level in Pascal
+ double wind_speed; ///< wind magnitude in m/s
+ double wind_dir; ///< wind direction in radians north=0, increasing CCW
+};
+
+extern struct NpsAtmosphere nps_atmosphere;
+
+extern void nps_atmosphere_init(void);
+extern void nps_atmosphere_update(double dt);
+
#endif /* NPS_ATMOSPHERE_H */
diff --git a/sw/simulator/nps/nps_electrical.c b/sw/simulator/nps/nps_electrical.c
index bc4c3e520a..97e66eb908 100644
--- a/sw/simulator/nps/nps_electrical.c
+++ b/sw/simulator/nps/nps_electrical.c
@@ -19,6 +19,10 @@
* Boston, MA 02111-1307, USA.
*/
+/**
+ * @file nps_electrical.c
+ * Electrical status (bat voltage) for NPS.
+ */
#include "nps_electrical.h"
#include "generated/airframe.h"
diff --git a/sw/simulator/nps/nps_electrical.h b/sw/simulator/nps/nps_electrical.h
index a8f6054814..d1b15bcfdb 100644
--- a/sw/simulator/nps/nps_electrical.h
+++ b/sw/simulator/nps/nps_electrical.h
@@ -19,6 +19,11 @@
* Boston, MA 02111-1307, USA.
*/
+/**
+ * @file nps_electrical.h
+ * Electrical status (bat voltage) for NPS.
+ */
+
#ifndef NPS_ELECTRICAL_H
#define NPS_ELECTRICAL_H
diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h
index 4332fe0104..c223c9fda6 100644
--- a/sw/simulator/nps/nps_fdm.h
+++ b/sw/simulator/nps/nps_fdm.h
@@ -86,11 +86,14 @@ struct NpsFdm {
struct DoubleVect3 ltp_g;
struct DoubleVect3 ltp_h;
+ struct DoubleVect3 wind; ///< velocity in m/s in NED
+
};
extern struct NpsFdm fdm;
extern void nps_fdm_init(double dt);
extern void nps_fdm_run_step(double* commands);
+extern void nps_fdm_set_wind(double speed, double dir);
#endif /* NPS_FDM */
diff --git a/sw/simulator/nps/nps_fdm_jsbsim.c b/sw/simulator/nps/nps_fdm_jsbsim.c
index 4487ae8c14..64dd9dc813 100644
--- a/sw/simulator/nps/nps_fdm_jsbsim.c
+++ b/sw/simulator/nps/nps_fdm_jsbsim.c
@@ -35,6 +35,7 @@
#include
#include
#include
+#include
#include "nps_fdm.h"
#include "math/pprz_geodetic.h"
@@ -179,6 +180,12 @@ void nps_fdm_run_step(double* commands) {
}
+void nps_fdm_set_wind(double speed, double dir) {
+ FGWinds* Winds = FDMExec->GetWinds();
+ Winds->SetWindspeed(FeetOfMeters(speed));
+ Winds->SetWindPsi(dir);
+}
+
/**
* Feed JSBSim with the latest actuator commands.
*
@@ -199,6 +206,7 @@ static void feed_jsbsim(double* commands) {
}
+
/**
* Populates the NPS fdm struct after a simulation step.
*/
@@ -295,9 +303,15 @@ static void fetch_state(void) {
/*
* rotational speed and accelerations
*/
- jsbsimvec_to_rate(&fdm.body_ecef_rotvel,&propagate->GetPQR());
- jsbsimvec_to_rate(&fdm.body_ecef_rotaccel,&accelerations->GetPQRdot());
+ jsbsimvec_to_rate(&fdm.body_ecef_rotvel, &propagate->GetPQR());
+ jsbsimvec_to_rate(&fdm.body_ecef_rotaccel, &accelerations->GetPQRdot());
+
+ /*
+ * wind
+ */
+ const FGColumnVector3& fg_wind_ned = FDMExec->GetWinds()->GetTotalWindNED();
+ jsbsimvec_to_vec(&fdm.wind, &fg_wind_ned);
}
/**
diff --git a/sw/simulator/nps/nps_ivy_common.c b/sw/simulator/nps/nps_ivy_common.c
index c93bd7d4eb..4c869a22ac 100644
--- a/sw/simulator/nps/nps_ivy_common.c
+++ b/sw/simulator/nps/nps_ivy_common.c
@@ -9,6 +9,7 @@
#include "nps_autopilot.h"
#include "nps_fdm.h"
#include "nps_sensors.h"
+#include "nps_atmosphere.h"
#include "subsystems/ins.h"
#include "subsystems/navigation/common_flight_plan.h"
@@ -186,4 +187,10 @@ void nps_ivy_display(void) {
h_body.x,
h_body.y,
h_body.z);
+
+ IvySendMsg("%d NPS_WIND %f %f %f",
+ AC_ID,
+ fdm.wind.x,
+ fdm.wind.y,
+ fdm.wind.z);
}
diff --git a/sw/simulator/nps/nps_main.c b/sw/simulator/nps/nps_main.c
index a740eb15e0..6686beb324 100644
--- a/sw/simulator/nps/nps_main.c
+++ b/sw/simulator/nps/nps_main.c
@@ -84,7 +84,7 @@ double time_to_double(struct timeval *t) {
return ((double)t->tv_sec + (double)(t->tv_usec * 1e-6));
}
-int main ( int argc, char** argv) {
+int main (int argc, char** argv) {
if (!nps_main_parse_options(argc, argv)) return 1;
@@ -121,6 +121,7 @@ static void nps_main_init(void) {
nps_ivy_init(nps_main.ivy_bus);
nps_fdm_init(SIM_DT);
+ nps_atmosphere_init();
nps_sensors_init(nps_main.sim_time);
printf("Simulating with dt of %f\n", SIM_DT);
@@ -153,6 +154,8 @@ static void nps_main_init(void) {
static void nps_main_run_sim_step(void) {
// printf("sim at %f\n", nps_main.sim_time);
+ nps_atmosphere_update(SIM_DT);
+
nps_autopilot_run_systime_step();
nps_fdm_run_step(autopilot.commands);