* remove random lib since its not available on Nuttx

* fix gps-stuck-failure to actually be stuck
This commit is contained in:
Marco Hauswirth
2025-10-03 10:41:12 +02:00
parent 47fc3643ae
commit a1c4aa6dc9
3 changed files with 12 additions and 20 deletions
@@ -92,12 +92,13 @@ bool FailureInjection::handle_gps_failure(sensor_gps_s &gps)
} }
_gps_prev = gps;
} else { } else {
gps = _gps_prev; gps = _gps_prev;
} }
_last_gps_timestamp = gps.timestamp; _last_gps_timestamp = gps.timestamp;
_gps_prev = gps;
return true; return true;
} }
@@ -113,7 +114,6 @@ bool FailureInjection::handle_gps_alt_failure(sensor_gps_s &gps)
if (_gps_alt_wrong) { if (_gps_alt_wrong) {
gps.altitude_msl_m += 100.0; gps.altitude_msl_m += 100.0;
gps.altitude_ellipsoid_m += 100.0; gps.altitude_ellipsoid_m += 100.0;
} }
if (_gps_alt_drift) { if (_gps_alt_drift) {
@@ -132,16 +132,15 @@ bool FailureInjection::handle_gps_alt_failure(sensor_gps_s &gps)
} }
gps.altitude_msl_m = _gps_alt_offset; gps.altitude_msl_m = _gps_alt_offset;
} }
_gps_prev = gps;
} else { } else {
gps = _gps_prev; gps = _gps_prev;
} }
_last_gps_timestamp = gps.timestamp; _last_gps_timestamp = gps.timestamp;
_gps_prev = gps;
return true; return true;
@@ -45,9 +45,7 @@ using namespace matrix;
SensorSimManager::SensorSimManager() : SensorSimManager::SensorSimManager() :
ModuleParams(nullptr), ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
_gen(_rd()),
_uniform_dist(0.0f, 1.0f)
{ {
_px4_accel.set_temperature(kT1C); _px4_accel.set_temperature(kT1C);
_px4_gyro.set_temperature(kT1C); _px4_gyro.set_temperature(kT1C);
@@ -61,7 +59,7 @@ SensorSimManager::SensorSimManager() :
_gps_timing = { _gps_timing = {
.interval_us = 125000, .interval_us = 125000,
.next_update_time = now, .next_update_time = now,
.offset_us = static_cast<hrt_abstime>(_uniform_dist(_gen) * 125000), .offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 125000),
.enabled = false, .enabled = false,
}; };
@@ -69,7 +67,7 @@ SensorSimManager::SensorSimManager() :
_baro_timing = { _baro_timing = {
.interval_us = 50000, .interval_us = 50000,
.next_update_time = now, .next_update_time = now,
.offset_us = static_cast<hrt_abstime>(_uniform_dist(_gen) * 50000), .offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 50000),
.enabled = false, .enabled = false,
}; };
@@ -77,7 +75,7 @@ SensorSimManager::SensorSimManager() :
_mag_timing = { _mag_timing = {
.interval_us = 20000, .interval_us = 20000,
.next_update_time = now, .next_update_time = now,
.offset_us = static_cast<hrt_abstime>(_uniform_dist(_gen) * 20000), .offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 20000),
.enabled = false, .enabled = false,
}; };
@@ -85,7 +83,7 @@ SensorSimManager::SensorSimManager() :
_airspeed_timing = { _airspeed_timing = {
.interval_us = 125000, .interval_us = 125000,
.next_update_time = now, .next_update_time = now,
.offset_us = static_cast<hrt_abstime>(_uniform_dist(_gen) * 125000), .offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 125000),
.enabled = false, .enabled = false,
}; };
@@ -93,7 +91,7 @@ SensorSimManager::SensorSimManager() :
_agp_timing = { _agp_timing = {
.interval_us = 500000, .interval_us = 500000,
.next_update_time = now, .next_update_time = now,
.offset_us = static_cast<hrt_abstime>(_uniform_dist(_gen) * 500000), .offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 500000),
.enabled = false, .enabled = false,
}; };
@@ -101,7 +99,7 @@ SensorSimManager::SensorSimManager() :
_distance_sensor_timing = { _distance_sensor_timing = {
.interval_us = 20000, .interval_us = 20000,
.next_update_time = now, .next_update_time = now,
.offset_us = static_cast<hrt_abstime>(_uniform_dist(_gen) * 20000), .offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 20000),
.enabled = false, .enabled = false,
}; };
@@ -109,7 +107,7 @@ SensorSimManager::SensorSimManager() :
_imu_timing = { _imu_timing = {
.interval_us = 4000, .interval_us = 4000,
.next_update_time = now, .next_update_time = now,
.offset_us = static_cast<hrt_abstime>(_uniform_dist(_gen) * 4000), .offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 4000),
.enabled = true, .enabled = true,
}; };
@@ -64,7 +64,7 @@
#include <lib/mathlib/mathlib.h> #include <lib/mathlib/mathlib.h>
#include <lib/lat_lon_alt/lat_lon_alt.hpp> #include <lib/lat_lon_alt/lat_lon_alt.hpp>
#include <parameters/param.h> #include <parameters/param.h>
#include <random> #include <px4_platform_common/px4_config.h>
using namespace time_literals; using namespace time_literals;
@@ -153,11 +153,6 @@ private:
perf_counter_t _imu_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": imu")}; perf_counter_t _imu_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": imu")};
perf_counter_t _distance_sensor_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": distance")}; perf_counter_t _distance_sensor_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": distance")};
// Random number generator for offsets and noise
std::random_device _rd;
std::mt19937 _gen;
std::uniform_real_distribution<float> _uniform_dist;
// Parameters // Parameters
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamInt<px4::params::SENS_EN_GPSSIM>) _param_sens_en_gpssim, (ParamInt<px4::params::SENS_EN_GPSSIM>) _param_sens_en_gpssim,