[nps] simple wind simulation

No vertical wind component for now, only magnitude and direction.
Also no gusts so far...
Can be changed via settings.

Towards solving issue #540

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    \

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    \

conf/settings/nps.xml

@@ -7,6 +7,8 @@
        <dl_setting var="nps_bypass_ahrs" min="0" step="1" max="1" module="nps/nps_autopilot_rotorcraft" shortname="bypass_ahrs" values="No|Yes"/>
        <dl_setting var="nps_bypass_ins" min="0" step="1" max="1" module="nps/nps_autopilot_rotorcraft" shortname="bypass_ins" values="No|Yes"/>
        <dl_setting var="nps_electrical.supply_voltage" min="0" step="0.1" max="24" module="nps/nps_electrical" shortname="bat_voltage" unit="V"/>
+       <dl_setting var="nps_atmosphere.wind_speed" min="0" step="0.1" max="25" module="nps/nps_atmosphere" shortname="wind_speed" unit="m/s"/>
+       <dl_setting var="nps_atmosphere.wind_dir" min="0" step="1" max="360" module="nps/nps_atmosphere" shortname="wind_dir" unit="rad" alt_unit="deg"/>
     </dl_settings>
 
   </dl_settings>

sw/simulator/nps/nps_atmosphere.c

@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
+ *
+ * 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);
+}
+

sw/simulator/nps/nps_atmosphere.h

@@ -1,6 +1,45 @@
+/*
+ * Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
+ *
+ * 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 */
 
 

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"

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
 

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 */

sw/simulator/nps/nps_fdm_jsbsim.c

@@ -35,6 +35,7 @@
 #include <models/FGPropulsion.h>
 #include <models/FGGroundReactions.h>
 #include <models/FGAccelerations.h>
+#include <models/atmosphere/FGWinds.h>
 
 #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);
 }
 
 /**

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);
 }

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);