mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-25 00:03:37 +08:00
* remove random lib since its not available on Nuttx
* fix gps-stuck-failure to actually be stuck
This commit is contained in:
@@ -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,
|
||||||
|
|||||||
Reference in New Issue
Block a user