diff --git a/sw/simulator/nps/nps_sensor_temperature.c b/sw/simulator/nps/nps_sensor_temperature.c new file mode 100644 index 0000000000..1dd2791913 --- /dev/null +++ b/sw/simulator/nps/nps_sensor_temperature.c @@ -0,0 +1,41 @@ +#include "nps_sensor_temperature.h" + +#include "generated/airframe.h" + +#include "std.h" +#include "nps_fdm.h" +#include "nps_random.h" +#include NPS_SENSORS_PARAMS + +/// 10Hz default +#ifndef NPS_TEMPERATURE_DT +#define NPS_TEMPERATURE_DT 0.01 +#endif + +#ifndef NPS_TEMPERATURE_NOISE_STD_DEV +#define NPS_TEMPERATURE_NOISE_STD_DEV 0.1 +#endif + +void nps_sensor_temperature_init(struct NpsSensorTemperature *temperature, double time) +{ + temperature->value = 0.; + temperature->noise_std_dev = NPS_TEMPERATURE_NOISE_STD_DEV; + temperature->next_update = time; + temperature->data_available = FALSE; +} + + +void nps_sensor_temperature_run_step(struct NpsSensorTemperature *temperature, double time) +{ + if (time < temperature->next_update) { + return; + } + + /* termperature in degrees Celcius */ + temperature->value = fdm.temperature; + /* add noise with std dev */ + temperature->value += get_gaussian_noise() * temperature->noise_std_dev; + + temperature->next_update += NPS_TEMPERATURE_DT; + temperature->data_available = TRUE; +} diff --git a/sw/simulator/nps/nps_sensor_temperature.h b/sw/simulator/nps/nps_sensor_temperature.h new file mode 100644 index 0000000000..d640c8e159 --- /dev/null +++ b/sw/simulator/nps/nps_sensor_temperature.h @@ -0,0 +1,20 @@ +#ifndef NPS_SENSOR_TEMPERATURE_H +#define NPS_SENSOR_TEMPERATURE_H + +#include "math/pprz_algebra.h" +#include "math/pprz_algebra_double.h" +#include "math/pprz_algebra_float.h" +#include "std.h" + +struct NpsSensorTemperature { + double value; ///< temperature in degrees Celcius + double noise_std_dev; ///< noise standard deviation + double next_update; + bool_t data_available; +}; + + +extern void nps_sensor_temperature_init(struct NpsSensorTemperature *temperature, double time); +extern void nps_sensor_temperature_run_step(struct NpsSensorTemperature *temperature, double time); + +#endif /* NPS_SENSOR_TEMPERATURE_H */