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