From 52a98229b18bbc6639884ccaf1a1543768f00086 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 18 Sep 2025 12:16:38 +0200 Subject: [PATCH] * add AGP also for SimulatorMavlink * add AGP failure injection * add distance sensor failure injection --- ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim | 18 +-- .../init.d-posix/px4-rc.mavlinksim | 9 ++ msg/versioned/VehicleCommand.msg | 1 + .../failure_injection/failureInjection.cpp | 57 ++++++++ .../failure_injection/failureInjection.hpp | 14 ++ .../sensor_sim_manager/SensorSimManager.cpp | 138 +++++++++++------- .../sensor_sim_manager/SensorSimManager.hpp | 16 +- .../sensor_sim_manager/parameters.c | 33 +++++ .../simulator_mavlink/SimulatorMavlink.cpp | 23 ++- .../simulator_mavlink/SimulatorMavlink.hpp | 1 + .../simulation/simulator_sih/sih_params.c | 24 --- src/systemcmds/failure/failure.cpp | 2 + 12 files changed, 238 insertions(+), 98 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim index ba32f6284d..4b21b462d5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim @@ -193,16 +193,8 @@ else exit 1 fi -# TODO: - -# # NOTE: Only for rover_mecanum and spacecraft_2d. All other models have -# # the magnetometer sensor in the model.sdf. -# if param compare -s SENS_EN_MAGSIM 1 -# then -# sensor_mag_sim start -# fi -# # NOTE: new gz has airspeed sensor, remove once added -# if param compare -s SENS_EN_ARSPDSIM 1 -# then -# sensor_airspeed_sim start -# fi +# Start sensor simulation manager for additional sensors not handled by gz +if param compare -s SENS_EN_AGPSIM 1 || param compare -s SENS_EN_MAGSIM 1 || param compare -s SENS_EN_ARSPDSIM 1 +then + sensor_sim_manager start +fi diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim index f2d571062c..44a6b77d9c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim @@ -5,6 +5,7 @@ param set-default EKF2_GPS_DELAY 10 param set-default EKF2_MULTI_IMU 3 param set-default SENS_IMU_MODE 0 +param set-default SIM_GZ_EN 1 simulator_tcp_port=$((4560+px4_instance)) @@ -25,3 +26,11 @@ else echo "INFO [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOSTNAME}" simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}" fi + +echo "mvlinksim" + +# Start sensor simulation manager for additional sensors not handled by gz +if param compare -s SENS_EN_AGPSIM 1 || param compare -s SENS_EN_MAGSIM 1 || param compare -s SENS_EN_ARSPDSIM 1 +then + sensor_sim_manager start +fi diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 789ecd6cf4..df6a53f3c1 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -141,6 +141,7 @@ uint8 FAILURE_UNIT_SENSOR_VIO = 6 uint8 FAILURE_UNIT_SENSOR_DISTANCE_SENSOR = 7 uint8 FAILURE_UNIT_SENSOR_AIRSPEED = 8 uint8 FAILURE_UNIT_SENSOR_GPS_ALT = 9 +uint8 FAILURE_UNIT_SENSOR_AGP = 10 uint8 FAILURE_UNIT_SYSTEM_BATTERY = 100 uint8 FAILURE_UNIT_SYSTEM_MOTOR = 101 uint8 FAILURE_UNIT_SYSTEM_SERVO = 102 diff --git a/src/modules/simulation/failure_injection/failureInjection.cpp b/src/modules/simulation/failure_injection/failureInjection.cpp index 4923fbe71b..9b413fa47b 100644 --- a/src/modules/simulation/failure_injection/failureInjection.cpp +++ b/src/modules/simulation/failure_injection/failureInjection.cpp @@ -423,6 +423,63 @@ void FailureInjection::check_failure_injections() supported = true; _vio_blocked = false; } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AGP) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, agp off"); + supported = true; + _agp_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, agp ok"); + supported = true; + _agp_blocked = false; + _agp_stuck = false; + _agp_drift = false; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + PX4_WARN("CMD_INJECT_FAILURE, agp stuck"); + supported = true; + _agp_stuck = true; + _agp_blocked = false; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_DRIFT) { + PX4_WARN("CMD_INJECT_FAILURE, agp drift"); + supported = true; + _agp_drift = true; + } + + } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR) { + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, distance sensor off"); + supported = true; + _distance_sensor_blocked = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, distance sensor ok"); + supported = true; + _distance_sensor_blocked = false; + _distance_sensor_stuck = false; + _distance_sensor_wrong = false; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + PX4_WARN("CMD_INJECT_FAILURE, distance sensor stuck"); + supported = true; + _distance_sensor_stuck = true; + _distance_sensor_blocked = false; + _distance_sensor_wrong = false; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { + PX4_WARN("CMD_INJECT_FAILURE, distance sensor wrong (blocked at 0.5m)"); + supported = true; + _distance_sensor_wrong = true; + _distance_sensor_blocked = false; + _distance_sensor_stuck = false; + } } if (handled) { diff --git a/src/modules/simulation/failure_injection/failureInjection.hpp b/src/modules/simulation/failure_injection/failureInjection.hpp index 402ecc21e2..e287ceeb6e 100644 --- a/src/modules/simulation/failure_injection/failureInjection.hpp +++ b/src/modules/simulation/failure_injection/failureInjection.hpp @@ -53,6 +53,14 @@ public: bool is_vio_blocked() { return _vio_blocked; } + bool is_agp_blocked() { return _agp_blocked; } + bool is_agp_stuck() { return _agp_stuck; } + bool is_agp_drift() { return _agp_drift; } + + bool is_distance_sensor_blocked() { return _distance_sensor_blocked; } + bool is_distance_sensor_stuck() { return _distance_sensor_stuck; } + bool is_distance_sensor_wrong() { return _distance_sensor_wrong; } + private: uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; @@ -89,5 +97,11 @@ private: bool _airspeed_disconnected{false}; hrt_abstime _airspeed_blocked_timestamp{0}; bool _vio_blocked{false}; + bool _agp_blocked{false}; + bool _agp_stuck{false}; + bool _agp_drift{false}; + bool _distance_sensor_blocked{false}; + bool _distance_sensor_stuck{false}; + bool _distance_sensor_wrong{false}; }; diff --git a/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp b/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp index f19951aa19..5f087008a8 100644 --- a/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp +++ b/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp @@ -97,20 +97,28 @@ SensorSimManager::SensorSimManager() : .enabled = false, }; - // IMU: 250 Hz - _imu_timing = { - .interval_us = 4000, - .next_update_time = now, - .offset_us = static_cast(_uniform_dist(_gen) * 4000), - .enabled = true, // Always enabled - }; - // Distance sensor: 50 Hz _distance_sensor_timing = { .interval_us = 20000, .next_update_time = now, .offset_us = static_cast(_uniform_dist(_gen) * 20000), - .enabled = true, // Always enabled + .enabled = false, + }; + + // IMU: 250 Hz + _imu_timing = { + .interval_us = 4000, + .next_update_time = now, + .offset_us = static_cast(_uniform_dist(_gen) * 4000), + .enabled = true, + }; + + // Failure Injection Poll: 10 Hz + _failure_update = { + .interval_us = 100000, + .next_update_time = now, + .offset_us = 0, + .enabled = true, }; _gps_timing.next_update_time += _gps_timing.offset_us; @@ -121,6 +129,8 @@ SensorSimManager::SensorSimManager() : _imu_timing.next_update_time += _imu_timing.offset_us; _distance_sensor_timing.next_update_time += _distance_sensor_timing.offset_us; + _param_sim_gz_en_handle = param_find("SIM_GZ_EN"); + } SensorSimManager::~SensorSimManager() @@ -137,6 +147,10 @@ SensorSimManager::~SensorSimManager() bool SensorSimManager::init() { + if (_param_sim_gz_en_handle != PARAM_INVALID) { + param_get(_param_sim_gz_en_handle, &_sim_gz_en_value); + } + ScheduleOnInterval(2_ms); return true; } @@ -183,23 +197,28 @@ void SensorSimManager::Run() perf_begin(_loop_perf); - if (_parameter_update_sub.updated()) { - parameter_update_s parameter_update; - _parameter_update_sub.copy(¶meter_update); - updateParams(); - - _gps_timing.enabled = (_param_sens_en_gpssim.get() != 0); - _baro_timing.enabled = (_param_sens_en_barosim.get() != 0); - _mag_timing.enabled = (_param_sens_en_magsim.get() != 0); - _airspeed_timing.enabled = (_param_sens_en_arspdsim.get() != 0); - _agp_timing.enabled = (_param_sens_en_agpsim.get() != 0); - } - const hrt_abstime now = hrt_absolute_time(); - if (_gps_timing.enabled && now >= _gps_timing.next_update_time) { - _failure_injection.check_failure_injections(); // TODO, just here for now... + if (now >= _failure_update.next_update_time) { + _failure_injection.check_failure_injections(); + _failure_update.next_update_time = now + _failure_update.interval_us; + if (_parameter_update_sub.updated()) { + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + updateParams(); + + _gps_timing.enabled = (_param_sens_en_gpssim.get() != 0); + _baro_timing.enabled = (_param_sens_en_barosim.get() != 0); + _mag_timing.enabled = (_param_sens_en_magsim.get() != 0); + _airspeed_timing.enabled = (_param_sens_en_arspdsim.get() != 0); + _agp_timing.enabled = (_param_sens_en_agpsim.get() != 0); + _distance_sensor_timing.enabled = (_param_sens_en_distsim.get() != 0); + _imu_timing.enabled = (_sim_gz_en_value == 0); + } + } + + if (_gps_timing.enabled && now >= _gps_timing.next_update_time) { updateGPS(); _gps_timing.next_update_time = now + _gps_timing.interval_us; } @@ -247,11 +266,10 @@ void SensorSimManager::updateGPS() vehicle_global_position_s gpos{}; _vehicle_global_position_sub.copy(&gpos); - // Check if data is recent (within 20ms), TODO const hrt_abstime now = hrt_absolute_time(); if (lpos.timestamp > 0 && gpos.timestamp > 0 && - (now - lpos.timestamp) < 20000 && (now - gpos.timestamp) < 20000) { + (now - lpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) { double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); @@ -333,10 +351,9 @@ void SensorSimManager::updateBarometer() vehicle_global_position_s gpos; if (_vehicle_global_position_sub.copy(&gpos)) { - // Check if data is recent (within 20ms), TODO const hrt_abstime now = hrt_absolute_time(); - if (gpos.timestamp > 0 && (now - gpos.timestamp) < 20000) { + if (gpos.timestamp > 0 && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) { const float dt = math::constrain((gpos.timestamp - _last_baro_update_time) * 1e-6f, 0.001f, 0.1f); const float alt_msl = gpos.alt; @@ -406,10 +423,9 @@ void SensorSimManager::updateMagnetometer() vehicle_global_position_s gpos; if (_vehicle_global_position_sub.copy(&gpos)) { - // Check if data is recent (within 20ms), TODO const hrt_abstime now = hrt_absolute_time(); - if (gpos.timestamp > 0 && (now - gpos.timestamp) < 20000 && gpos.eph < 1000) { + if (gpos.timestamp > 0 && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && gpos.eph < 1000) { // magnetic field data returned by the geo library using the current GPS position const float declination_rad = math::radians(get_mag_declination_degrees(gpos.lat, gpos.lon)); const float inclination_rad = math::radians(get_mag_inclination_degrees(gpos.lat, gpos.lon)); @@ -466,11 +482,12 @@ void SensorSimManager::updateAirspeed() vehicle_attitude_s attitude{}; _vehicle_attitude_sub.copy(&attitude); - // Check if data is recent (within 20ms), TODO + // Check if groundtruth data is recent const hrt_abstime now = hrt_absolute_time(); if (lpos.timestamp > 0 && gpos.timestamp > 0 && attitude.timestamp > 0 && - (now - lpos.timestamp) < 20000 && (now - gpos.timestamp) < 20000 && (now - attitude.timestamp) < 20000) { + (now - lpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US + && (now - attitude.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) { Vector3f velocity_E{lpos.vx, lpos.vy, lpos.vz}; // Velocity in NED frame (similar to Earth frame) @@ -514,6 +531,7 @@ void SensorSimManager::updateAirspeed() const float airspeed_blockage_rampup_time = 1e6f; // 1 second in microseconds float airspeed_blockage_scale = 1.f; + if (blocked_timestamp > 0) { airspeed_blockage_scale = math::constrain(1.f - (now - blocked_timestamp) / airspeed_blockage_rampup_time, 1.f - blockage_fraction, 1.f); @@ -521,7 +539,8 @@ void SensorSimManager::updateAirspeed() differential_pressure_s differential_pressure{}; differential_pressure.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION - differential_pressure.differential_pressure_pa = diff_pressure * 100.f * airspeed_blockage_scale; // hPa to Pa with blockage scaling + differential_pressure.differential_pressure_pa = diff_pressure * 100.f * + airspeed_blockage_scale; // hPa to Pa with blockage scaling differential_pressure.temperature = temperature_local; differential_pressure.timestamp = hrt_absolute_time(); _differential_pressure_pub.publish(differential_pressure); @@ -537,26 +556,24 @@ void SensorSimManager::updateAGP() vehicle_global_position_s gpos{}; _vehicle_global_position_sub.copy(&gpos); - // Check if data is recent (within 20ms), TODO const hrt_abstime now = hrt_absolute_time(); - if (gpos.timestamp > 0 && (now - gpos.timestamp) < 20000) { + if (gpos.timestamp > 0 && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) { + + if (_failure_injection.is_agp_blocked()) { + perf_end(_agp_perf); + return; + } const uint64_t current_time = gpos.timestamp; const float dt = (current_time - _agp_time_last_update) * 1e-6f; _agp_time_last_update = current_time; - // Handle failure modes - enum class FailureMode : int32_t { - Stuck = (1 << 0), - Drift = (1 << 1) - }; - - if (!(_sim_agp_fail.get() & static_cast(FailureMode::Stuck))) { + if (!_failure_injection.is_agp_stuck()) { _agp_measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid); } - if (_sim_agp_fail.get() & static_cast(FailureMode::Drift)) { + if (_failure_injection.is_agp_drift()) { _agp_position_bias += matrix::Vector3f(1.5f, -5.f, 0.f) * dt; _agp_measured_lla += _agp_position_bias; @@ -605,12 +622,12 @@ void SensorSimManager::updateIMU() vehicle_angular_velocity_s angular_velocity{}; _vehicle_angular_velocity_sub.copy(&angular_velocity); - // Check if data is recent (within 20ms) const hrt_abstime now = hrt_absolute_time(); if (gpos.timestamp > 0 && lpos.timestamp > 0 && attitude.timestamp > 0 && angular_velocity.timestamp > 0 && - (now - gpos.timestamp) < 20000 && (now - lpos.timestamp) < 20000 && - (now - attitude.timestamp) < 20000 && (now - angular_velocity.timestamp) < 20000) { + (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && (now - lpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && + (now - attitude.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US + && (now - angular_velocity.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) { // The sensor signals reconstruction and noise levels are from [1] // [1] Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight." @@ -679,12 +696,15 @@ void SensorSimManager::updateDistanceSensor() vehicle_attitude_s attitude{}; _vehicle_attitude_sub.copy(&attitude); - // Check if data is recent (within 20ms), TODO const hrt_abstime now = hrt_absolute_time(); if (lpos.timestamp > 0 && attitude.timestamp > 0 && - (now - lpos.timestamp) < 20000 && (now - attitude.timestamp) < 20000 && - fabsf(_distance_snsr_override) < 10000) { + (now - lpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && (now - attitude.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) { + + if (_failure_injection.is_distance_sensor_blocked()) { + perf_end(_distance_sensor_perf); + return; + } device::Device::DeviceId device_id; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; @@ -696,22 +716,30 @@ void SensorSimManager::updateDistanceSensor() distance_sensor.device_id = device_id.devid; distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - distance_sensor.min_distance = _distance_snsr_min; - distance_sensor.max_distance = _distance_snsr_max; + distance_sensor.min_distance = _distance_snsr_min.get(); + distance_sensor.max_distance = _distance_snsr_max.get(); distance_sensor.signal_quality = -1; - if (_distance_snsr_override >= 0.f) { - distance_sensor.current_distance = _distance_snsr_override; + float current_distance; + + if (_failure_injection.is_distance_sensor_stuck()) { + current_distance = _last_distance_sensor_value; + + } else if (_failure_injection.is_distance_sensor_wrong()) { + current_distance = 0.5f; } else { Quatf q{attitude.q}; - distance_sensor.current_distance = -lpos.z / q.dcm_z()(2); + current_distance = -lpos.z / q.dcm_z()(2); - if (distance_sensor.current_distance > _distance_snsr_max) { - distance_sensor.current_distance = UINT16_MAX / 100.f; + if (current_distance > _distance_snsr_max.get()) { + current_distance = UINT16_MAX / 100.f; } + + _last_distance_sensor_value = current_distance; } + distance_sensor.current_distance = current_distance; distance_sensor.timestamp = hrt_absolute_time(); _distance_sensor_pub.publish(distance_sensor); } diff --git a/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp b/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp index ddec12dbd0..b6b364696a 100644 --- a/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp +++ b/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp @@ -63,6 +63,7 @@ #include #include #include +#include #include using namespace time_literals; @@ -116,6 +117,7 @@ private: SensorTiming _agp_timing; SensorTiming _imu_timing; SensorTiming _distance_sensor_timing; + SensorTiming _failure_update; // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -164,13 +166,15 @@ private: (ParamInt) _param_sens_en_magsim, (ParamInt) _param_sens_en_arspdsim, (ParamInt) _param_sens_en_agpsim, + (ParamInt) _param_sens_en_distsim, (ParamInt) _sim_gps_used, (ParamFloat) _sim_baro_off_p, (ParamFloat) _sim_baro_off_t, (ParamFloat) _sim_mag_offset_x, (ParamFloat) _sim_mag_offset_y, (ParamFloat) _sim_mag_offset_z, - (ParamInt) _sim_agp_fail + (ParamFloat) _distance_snsr_min, + (ParamFloat) _distance_snsr_max ) hrt_abstime _last_baro_update_time{0}; @@ -190,9 +194,7 @@ private: matrix::Vector3f _last_gyro{}; matrix::Vector3f _specific_force_E{}; - float _distance_snsr_min{0.3f}; - float _distance_snsr_max{50.0f}; - float _distance_snsr_override{-1.0f}; + float _last_distance_sensor_value{0.0f}; // Air constants static constexpr float TEMPERATURE_MSL = 288.0f; // [K] @@ -204,4 +206,10 @@ private: // IMU constants static constexpr float T1_C = 15.0f; // Temperature constant + static constexpr hrt_abstime GROUNDTRUTH_DATA_MAX_AGE_US = 12000; + + // Parameter handles for cross-module access + param_t _param_sim_gz_en_handle{PARAM_INVALID}; + int32_t _sim_gz_en_value{0}; + }; diff --git a/src/modules/simulation/sensor_sim_manager/parameters.c b/src/modules/simulation/sensor_sim_manager/parameters.c index bccceafa8a..076ffc90fa 100644 --- a/src/modules/simulation/sensor_sim_manager/parameters.c +++ b/src/modules/simulation/sensor_sim_manager/parameters.c @@ -75,6 +75,15 @@ PARAM_DEFINE_INT32(SENS_EN_ARSPDSIM, 0); */ PARAM_DEFINE_INT32(SENS_EN_AGPSIM, 0); +/** + * Enable simulated distance sensor + * + * @group Simulation + * @value 0 Disabled + * @value 1 Enabled + */ +PARAM_DEFINE_INT32(SENS_EN_DISTSIM, 0); + /** * Number of GPS satellites used in simulation * @@ -136,3 +145,27 @@ PARAM_DEFINE_FLOAT(SIM_MAG_OFFSET_Z, 0.0f); * @bit 1 Drift */ PARAM_DEFINE_INT32(SIM_AGP_FAIL, 0); + +/** + * distance sensor minimum range + * + * @unit m + * @min 0.0 + * @max 10.0 + * @decimal 4 + * @increment 0.01 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MIN, 0.0f); + +/** + * distance sensor maximum range + * + * @unit m + * @min 0.0 + * @max 1000.0 + * @decimal 4 + * @increment 0.01 + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MAX, 100.0f); diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index fe2656552e..da0e0ec9b0 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -325,7 +325,8 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil } // differential pressure - if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_failure_injection.is_airspeed_disconnected()) { + if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS + && !_failure_injection.is_airspeed_disconnected()) { const float blockage_fraction = 0.7; // defines max blockage (fully ramped) const float airspeed_blockage_rampup_time = 1_s; // time it takes to go max blockage, linear ramp @@ -333,6 +334,7 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil float airspeed_blockage_scale = 1.f; hrt_abstime blocked_timestamp = _failure_injection.get_airspeed_blocked_timestamp(); + if (blocked_timestamp > 0) { airspeed_blockage_scale = math::constrain(1.f - (hrt_absolute_time() - blocked_timestamp) / airspeed_blockage_rampup_time, 1.f - blockage_fraction, 1.f); @@ -1225,11 +1227,28 @@ void SimulatorMavlink::run() int SimulatorMavlink::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavlink) { + if (_failure_injection.is_distance_sensor_blocked()) { + return PX4_OK; + } + distance_sensor_s dist{}; dist.timestamp = hrt_absolute_time(); dist.min_distance = dist_mavlink->min_distance / 100.0f; dist.max_distance = dist_mavlink->max_distance / 100.0f; - dist.current_distance = dist_mavlink->current_distance / 100.0f; + + float current_distance = dist_mavlink->current_distance / 100.0f; + + if (_failure_injection.is_distance_sensor_stuck()) { + current_distance = _last_distance_sensor_value; + + } else if (_failure_injection.is_distance_sensor_wrong()) { + current_distance = 0.5f; + + } else { + _last_distance_sensor_value = current_distance; + } + + dist.current_distance = current_distance; dist.type = dist_mavlink->type; dist.variance = dist_mavlink->covariance * 1e-4f; // cm^2 to m^2 diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index 48b6011df1..eea0fabbdf 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -203,6 +203,7 @@ private: uORB::PublicationMulti *_dist_pubs[ORB_MULTI_MAX_INSTANCES] {}; uint32_t _dist_sensor_ids[ORB_MULTI_MAX_INSTANCES] {}; + float _last_distance_sensor_value{0.0f}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c index 25c9498e63..6314823105 100644 --- a/src/modules/simulation/simulator_sih/sih_params.c +++ b/src/modules/simulation/simulator_sih/sih_params.c @@ -282,30 +282,6 @@ PARAM_DEFINE_FLOAT(SIH_LOC_LON0, 8.545594f); */ PARAM_DEFINE_FLOAT(SIH_LOC_H0, 489.4f); -/** - * distance sensor minimum range - * - * @unit m - * @min 0.0 - * @max 10.0 - * @decimal 4 - * @increment 0.01 - * @group Simulation In Hardware - */ -PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MIN, 0.0f); - -/** - * distance sensor maximum range - * - * @unit m - * @min 0.0 - * @max 1000.0 - * @decimal 4 - * @increment 0.01 - * @group Simulation In Hardware - */ -PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MAX, 100.0f); - /** * if >= 0 the distance sensor measures will be overridden by this value * diff --git a/src/systemcmds/failure/failure.cpp b/src/systemcmds/failure/failure.cpp index 6679afc362..2c57b2a2df 100644 --- a/src/systemcmds/failure/failure.cpp +++ b/src/systemcmds/failure/failure.cpp @@ -58,6 +58,7 @@ static constexpr FailureUnit failure_units[] = { { "baro", vehicle_command_s::FAILURE_UNIT_SENSOR_BARO}, { "gps", vehicle_command_s::FAILURE_UNIT_SENSOR_GPS}, { "gps_alt", vehicle_command_s::FAILURE_UNIT_SENSOR_GPS_ALT}, + { "agp", vehicle_command_s::FAILURE_UNIT_SENSOR_AGP}, { "optical_flow", vehicle_command_s::FAILURE_UNIT_SENSOR_OPTICAL_FLOW}, { "vio", vehicle_command_s::FAILURE_UNIT_SENSOR_VIO}, { "distance_sensor", vehicle_command_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR}, @@ -135,6 +136,7 @@ int inject_failure(const FailureUnit& unit, const FailureType& type, uint8_t ins command.param3 = static_cast(instance); command.timestamp = hrt_absolute_time(); command_pub.publish(command); + printf("pub\n"); vehicle_command_ack_s ack;