[nps] add temp sensor, baro uses pressure from fdm

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

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;

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

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

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

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;
 

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

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