From 0b3f4dd385c07a1cd5ac2f101721c1eedd6e2f90 Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Tue, 14 Feb 2023 15:45:01 +0100 Subject: [PATCH] Inject failure for airspeed sim --- .../sensor_airspeed_sim/SensorAirspeedSim.cpp | 64 ++++++++++--------- .../sensor_airspeed_sim/SensorAirspeedSim.hpp | 6 +- .../sensor_airspeed_sim/parameters.c | 12 ++++ 3 files changed, 48 insertions(+), 34 deletions(-) diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp index a407ea8100..ec8d3ed766 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp @@ -106,47 +106,49 @@ void SensorAirspeedSim::Run() updateParams(); } - if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated() - && _vehicle_attitude_sub.updated()) { + if (_sim_failure.get() == 0) { + if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated() + && _vehicle_attitude_sub.updated()) { - vehicle_local_position_s lpos{}; - _vehicle_local_position_sub.copy(&lpos); + vehicle_local_position_s lpos{}; + _vehicle_local_position_sub.copy(&lpos); - vehicle_global_position_s gpos{}; - _vehicle_global_position_sub.copy(&gpos); + vehicle_global_position_s gpos{}; + _vehicle_global_position_sub.copy(&gpos); - vehicle_attitude_s attitude{}; - _vehicle_attitude_sub.copy(&attitude); + vehicle_attitude_s attitude{}; + _vehicle_attitude_sub.copy(&attitude); - Vector3f local_velocity = Vector3f{lpos.vx, lpos.vy, lpos.vz}; - Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * local_velocity; + Vector3f local_velocity = Vector3f{lpos.vx, lpos.vy, lpos.vz}; + Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * local_velocity; - // device id - device::Device::DeviceId device_id; - device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - device_id.devid_s.bus = 0; - device_id.devid_s.address = 0; - device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM; + // device id + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + device_id.devid_s.bus = 0; + device_id.devid_s.address = 0; + device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM; - const float alt_amsl = gpos.alt; - const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; - const float density_ratio = powf(TEMPERATURE_MSL / temperature_local, 4.256f); - const float air_density = AIR_DENSITY_MSL / density_ratio; + const float alt_amsl = gpos.alt; + const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; + const float density_ratio = powf(TEMPERATURE_MSL / temperature_local, 4.256f); + const float air_density = AIR_DENSITY_MSL / density_ratio; - // calculate differential pressure + noise in hPa - const float diff_pressure_noise = (float)generate_wgn() * 0.01f; - float diff_pressure = sign(body_velocity(0)) * 0.005f * air_density * body_velocity(0) * body_velocity( - 0) + diff_pressure_noise; + // calculate differential pressure + noise in hPa + const float diff_pressure_noise = (float)generate_wgn() * 0.01f; + float diff_pressure = sign(body_velocity(0)) * 0.005f * air_density * body_velocity(0) * body_velocity( + 0) + diff_pressure_noise; - differential_pressure_s differential_pressure{}; - // report.timestamp_sample = time; - differential_pressure.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION - differential_pressure.differential_pressure_pa = (double)diff_pressure * 100.0; // hPa to Pa; - differential_pressure.temperature = temperature_local; - differential_pressure.timestamp = hrt_absolute_time(); - _differential_pressure_pub.publish(differential_pressure); + differential_pressure_s differential_pressure{}; + // report.timestamp_sample = time; + differential_pressure.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION + differential_pressure.differential_pressure_pa = (double)diff_pressure * 100.0; // hPa to Pa; + differential_pressure.temperature = temperature_local; + differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(differential_pressure); + } } perf_end(_loop_perf); diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp index 24abd6c77b..01dd9b8e6a 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp @@ -89,7 +89,7 @@ private: perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; - // DEFINE_PARAMETERS( - // (ParamInt) _sim_gps_used - // ) + DEFINE_PARAMETERS( + (ParamInt) _sim_failure + ) }; diff --git a/src/modules/simulation/sensor_airspeed_sim/parameters.c b/src/modules/simulation/sensor_airspeed_sim/parameters.c index f462b69123..5136643cc2 100644 --- a/src/modules/simulation/sensor_airspeed_sim/parameters.c +++ b/src/modules/simulation/sensor_airspeed_sim/parameters.c @@ -42,3 +42,15 @@ * @value 1 Enabled */ PARAM_DEFINE_INT32(SENS_EN_ARSPDSIM, 0); + +/** + * Dynamically simulate failure of airspeed sensor instance + * + * @reboot_required true + * @min 0 + * @max 1 + * @group Sensors + * @value 0 Disabled + * @value 1 Enabled + */ +PARAM_DEFINE_INT32(SIM_ARSPD_FAIL, 0);