fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

* remove random lib since its not available on Nuttx

Browse Source 
* fix gps-stuck-failure to actually be stuck
This commit is contained in:
Marco Hauswirth 2025-09-18 17:22:18 +02:00
parent 47fc3643ae
commit a1c4aa6dc9
3 changed files with 12 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/simulation/failure_injection/failureInjection.cpp
View File

@ -92,12 +92,13 @@ bool FailureInjection::handle_gps_failure(sensor_gps_s &gps)
}
_gps_prev = gps;
} else {
gps = _gps_prev;
}
_last_gps_timestamp = gps.timestamp;
_gps_prev = gps;
return true;
}
@ -113,7 +114,6 @@ bool FailureInjection::handle_gps_alt_failure(sensor_gps_s &gps)
if (_gps_alt_wrong) {
gps.altitude_msl_m += 100.0;
gps.altitude_ellipsoid_m += 100.0;
}
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_prev = gps;
} else {
gps = _gps_prev;
}
_last_gps_timestamp = gps.timestamp;
_gps_prev = gps;
return true;

18
src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp
View File

@ -45,9 +45,7 @@ using namespace matrix;
SensorSimManager::SensorSimManager() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_gen(_rd()),
_uniform_dist(0.0f, 1.0f)
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
_px4_accel.set_temperature(kT1C);
_px4_gyro.set_temperature(kT1C);
@ -61,7 +59,7 @@ SensorSimManager::SensorSimManager() :
_gps_timing = {
.interval_us = 125000,
.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,
};
@ -69,7 +67,7 @@ SensorSimManager::SensorSimManager() :
_baro_timing = {
.interval_us = 50000,
.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,
};
@ -77,7 +75,7 @@ SensorSimManager::SensorSimManager() :
_mag_timing = {
.interval_us = 20000,
.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,
};
@ -85,7 +83,7 @@ SensorSimManager::SensorSimManager() :
_airspeed_timing = {
.interval_us = 125000,
.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,
};
@ -93,7 +91,7 @@ SensorSimManager::SensorSimManager() :
_agp_timing = {
.interval_us = 500000,
.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,
};
@ -101,7 +99,7 @@ SensorSimManager::SensorSimManager() :
_distance_sensor_timing = {
.interval_us = 20000,
.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,
};
@ -109,7 +107,7 @@ SensorSimManager::SensorSimManager() :
_imu_timing = {
.interval_us = 4000,
.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,
};

7
src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp
View File

@ -64,7 +64,7 @@
#include <lib/mathlib/mathlib.h>
#include <lib/lat_lon_alt/lat_lon_alt.hpp>
#include <parameters/param.h>
#include <random>
#include <px4_platform_common/px4_config.h>
using namespace time_literals;
@ -153,11 +153,6 @@ private:
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")};
// Random number generator for offsets and noise
std::random_device _rd;
std::mt19937 _gen;
std::uniform_real_distribution<float> _uniform_dist;
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::SENS_EN_GPSSIM>) _param_sens_en_gpssim,