diff --git a/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile index ff84c792fd..753959f059 100644 --- a/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile +++ b/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile @@ -41,6 +41,7 @@ nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_sensor_sonar.c \ $(NPSDIR)/nps_sensor_gps.c \ $(NPSDIR)/nps_sensor_airspeed.c \ + $(NPSDIR)/nps_sensor_temperature.c \ $(NPSDIR)/nps_electrical.c \ $(NPSDIR)/nps_atmosphere.c \ $(NPSDIR)/nps_radio_control.c \ diff --git a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile index 05edd45b6a..5883e3e592 100644 --- a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile @@ -56,6 +56,7 @@ nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_sensor_sonar.c \ $(NPSDIR)/nps_sensor_gps.c \ $(NPSDIR)/nps_sensor_airspeed.c \ + $(NPSDIR)/nps_sensor_temperature.c \ $(NPSDIR)/nps_electrical.c \ $(NPSDIR)/nps_atmosphere.c \ $(NPSDIR)/nps_radio_control.c \ diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index b038e720f3..3194782fa3 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -52,6 +52,7 @@ nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_sensor_sonar.c \ $(NPSDIR)/nps_sensor_gps.c \ $(NPSDIR)/nps_sensor_airspeed.c \ + $(NPSDIR)/nps_sensor_temperature.c \ $(NPSDIR)/nps_electrical.c \ $(NPSDIR)/nps_atmosphere.c \ $(NPSDIR)/nps_radio_control.c \ diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index e1d3a0d6f8..ce874d96b3 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -127,6 +127,10 @@ void nps_autopilot_run_step(double time) Ap(event_task); } + if (nps_sensors_temperature_available()) { + AbiSendMsgTEMPERATURE(BARO_SIM_SENDER_ID, (float)sensors.temp.value); + } + #if USE_AIRSPEED if (nps_sensors_airspeed_available()) { stateSetAirspeed_f((float)sensors.airspeed.value); diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index b3563cf196..e06105e599 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -111,6 +111,16 @@ void nps_autopilot_run_step(double time) main_event(); } + if (nps_sensors_temperature_available()) { + AbiSendMsgTEMPERATURE(BARO_SIM_SENDER_ID, (float)sensors.temp.value); + } + +#if USE_AIRSPEED + if (nps_sensors_airspeed_available()) { + stateSetAirspeed_f((float)sensors.airspeed.value); + } +#endif + #if USE_SONAR if (nps_sensors_sonar_available()) { float dist = (float) sensors.sonar.value; diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h index 88ade0089b..f67b7c3236 100644 --- a/sw/simulator/nps/nps_fdm.h +++ b/sw/simulator/nps/nps_fdm.h @@ -111,6 +111,7 @@ struct NpsFdm { double total_pressure; ///< total atmospheric pressure in Pascal double dynamic_pressure; ///< dynamic pressure in Pascal double temperature; ///< current temperature in degrees Celcius + double pressure_sl; ///< pressure at sea level in Pascal // Control surface positions (normalized values) float elevator; @@ -133,6 +134,8 @@ extern void nps_fdm_run_step(bool_t launch, double *commands, int commands_nb); extern void nps_fdm_set_wind(double speed, double dir); extern void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down); extern void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity); +/** Set temperature in degrees Celcius at given height h above MSL */ +extern void nps_fdm_set_temperature(double temp, double h); #ifdef __cplusplus } /* extern "C" */ diff --git a/sw/simulator/nps/nps_fdm_crrcsim.c b/sw/simulator/nps/nps_fdm_crrcsim.c index 35af2bb807..7968e9f586 100644 --- a/sw/simulator/nps/nps_fdm_crrcsim.c +++ b/sw/simulator/nps/nps_fdm_crrcsim.c @@ -44,6 +44,7 @@ #include "math/pprz_geodetic_float.h" #include "math/pprz_algebra.h" #include "math/pprz_algebra_float.h" +#include "math/pprz_isa.h" #include "generated/airframe.h" #include "generated/flight_plan.h" @@ -121,6 +122,7 @@ void nps_fdm_init(double dt) fdm.curr_dt = dt; fdm.nan_count = 0; fdm.pressure = -1; + fdm.pressure_sl = PPRZ_ISA_SEA_LEVEL_PRESSURE; fdm.total_pressure = -1; fdm.dynamic_pressure = -1; fdm.temperature = -1; @@ -410,6 +412,8 @@ static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer) ecef_of_lla_d(&fdm->ecef_pos, &pos); fdm->hmsl = pos.alt - NAV_MSL0 / 1000.; + fdm->pressure = pprz_isa_pressure_of_altitude(fdm->hmsl); + /* gps time */ fdm->time = (double)UShortOfBuf(buffer, 27); diff --git a/sw/simulator/nps/nps_fdm_jsbsim.cpp b/sw/simulator/nps/nps_fdm_jsbsim.cpp index 46e1293fd8..3b532aae6c 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.cpp +++ b/sw/simulator/nps/nps_fdm_jsbsim.cpp @@ -278,6 +278,11 @@ void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity) Winds->SetProbabilityOfExceedence(turbulence_severity); } +void nps_fdm_set_temperature(double temp, double h) +{ + FDMExec->GetAtmosphere()->SetTemperature(temp, h, FGAtmosphere::eCelsius); +} + /** * Feed JSBSim with the latest actuator commands. * @@ -446,6 +451,7 @@ static void fetch_state(void) */ fdm.airspeed = MetersOfFeet(FDMExec->GetAuxiliary()->GetVequivalentFPS()); fdm.pressure = PascalOfPsf(FDMExec->GetAtmosphere()->GetPressure()); + fdm.pressure_sl = PascalOfPsf(FDMExec->GetAtmosphere()->GetPressureSL()); fdm.total_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->GetTotalPressure()); fdm.dynamic_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->Getqbar()); fdm.temperature = CelsiusOfRankine(FDMExec->GetAtmosphere()->GetTemperature()); diff --git a/sw/simulator/nps/nps_sensor_baro.c b/sw/simulator/nps/nps_sensor_baro.c index 5a10267614..a56f6ae3ce 100644 --- a/sw/simulator/nps/nps_sensor_baro.c +++ b/sw/simulator/nps/nps_sensor_baro.c @@ -1,7 +1,6 @@ #include "nps_sensor_baro.h" #include "generated/airframe.h" -#include "math/pprz_isa.h" #include "std.h" #include "nps_fdm.h" @@ -28,7 +27,7 @@ void nps_sensor_baro_run_step(struct NpsSensorBaro *baro, double time) } /* pressure in Pascal */ - baro->value = pprz_isa_pressure_of_altitude(fdm.hmsl); + baro->value = fdm.pressure; /* add noise with std dev Pascal */ baro->value += get_gaussian_noise() * baro->noise_std_dev; diff --git a/sw/simulator/nps/nps_sensors.c b/sw/simulator/nps/nps_sensors.c index b37ddc6ac1..f444cedee0 100644 --- a/sw/simulator/nps/nps_sensors.c +++ b/sw/simulator/nps/nps_sensors.c @@ -19,6 +19,7 @@ void nps_sensors_init(double time) nps_sensor_gps_init(&sensors.gps, time); nps_sensor_sonar_init(&sensors.sonar, time); nps_sensor_airspeed_init(&sensors.airspeed, time); + nps_sensor_temperature_init(&sensors.temp, time); } @@ -32,6 +33,7 @@ void nps_sensors_run_step(double time) nps_sensor_gps_run_step(&sensors.gps, time); nps_sensor_sonar_run_step(&sensors.sonar, time); nps_sensor_airspeed_run_step(&sensors.airspeed, time); + nps_sensor_temperature_run_step(&sensors.temp, time); } @@ -88,3 +90,12 @@ bool_t nps_sensors_airspeed_available(void) } return FALSE; } + +bool_t nps_sensors_temperature_available(void) +{ + if (sensors.temp.data_available) { + sensors.temp.data_available = FALSE; + return TRUE; + } + return FALSE; +} diff --git a/sw/simulator/nps/nps_sensors.h b/sw/simulator/nps/nps_sensors.h index f158bbe17f..decc63e907 100644 --- a/sw/simulator/nps/nps_sensors.h +++ b/sw/simulator/nps/nps_sensors.h @@ -9,6 +9,7 @@ #include "nps_sensor_gps.h" #include "nps_sensor_sonar.h" #include "nps_sensor_airspeed.h" +#include "nps_sensor_temperature.h" struct NpsSensors { struct DoubleRMat body_to_imu_rmat; @@ -19,6 +20,7 @@ struct NpsSensors { struct NpsSensorGps gps; struct NpsSensorSonar sonar; struct NpsSensorAirspeed airspeed; + struct NpsSensorTemperature temp; }; extern struct NpsSensors sensors; @@ -32,6 +34,7 @@ extern bool_t nps_sensors_baro_available(); extern bool_t nps_sensors_gps_available(); extern bool_t nps_sensors_sonar_available(); extern bool_t nps_sensors_airspeed_available(); +extern bool_t nps_sensors_temperature_available(); #endif /* NPS_SENSORS_H */