diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp index b9d8319c76..79eb8fbc88 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp @@ -111,49 +111,123 @@ void SensorAirspeedSim::Run() 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); + check_failure_injection(); - vehicle_global_position_s gpos{}; - _vehicle_global_position_sub.copy(&gpos); + if (!_airspeed_disconnected) { - vehicle_attitude_s attitude{}; - _vehicle_attitude_sub.copy(&attitude); + vehicle_local_position_s lpos{}; + _vehicle_local_position_sub.copy(&lpos); - Vector3f local_velocity = Vector3f{lpos.vx, lpos.vy, lpos.vz}; - Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * local_velocity; + vehicle_global_position_s gpos{}; + _vehicle_global_position_sub.copy(&gpos); - // 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; + vehicle_attitude_s attitude{}; + _vehicle_attitude_sub.copy(&attitude); - 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; + Vector3f local_velocity = Vector3f{lpos.vx, lpos.vy, lpos.vz}; + Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * local_velocity; - // 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; + // 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; - 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 + ABSOLUTE_ZERO_C; // K to C - differential_pressure.timestamp = hrt_absolute_time(); - _differential_pressure_pub.publish(differential_pressure); + if (!_airspeed_stuck) { + // 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; + // simulate pitot blockage: ramp down differential pressure over time + const float blockage_fraction = 0.7f; // max blockage (fully ramped) + const float airspeed_blockage_rampup_time = 30_s; + + float airspeed_blockage_scale = 1.f; + + if (_airspeed_blocked_timestamp > 0) { + airspeed_blockage_scale = math::constrain(1.f - (hrt_absolute_time() - _airspeed_blocked_timestamp) / + airspeed_blockage_rampup_time, 1.f - blockage_fraction, 1.f); + } + + _last_differential_pressure_pa = diff_pressure * 100.0f * airspeed_blockage_scale; // hPa to Pa + } + + 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 = _last_differential_pressure_pa; + differential_pressure.temperature = temperature_local + ABSOLUTE_ZERO_C; // K to C + differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(differential_pressure); + } } perf_end(_loop_perf); } +void SensorAirspeedSim::check_failure_injection() +{ + vehicle_command_s vehicle_command; + + while (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { + continue; + } + + bool handled = false; + bool supported = false; + + const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); + const int failure_type = static_cast(vehicle_command.param2 + 0.5f); + + if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) { + + handled = true; + + if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, airspeed off"); + supported = true; + _airspeed_disconnected = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) { + PX4_WARN("CMD_INJECT_FAILURE, airspeed stuck"); + supported = true; + _airspeed_stuck = true; + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { + PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate pitot blockage)"); + supported = true; + _airspeed_blocked_timestamp = hrt_absolute_time(); + + } else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { + PX4_INFO("CMD_INJECT_FAILURE, airspeed ok"); + supported = true; + _airspeed_disconnected = false; + _airspeed_stuck = false; + _airspeed_blocked_timestamp = 0; + } + } + + if (handled) { + vehicle_command_ack_s ack{}; + ack.command = vehicle_command.command; + ack.from_external = false; + ack.result = supported ? + vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : + vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(ack); + } + } +} + int SensorAirspeedSim::task_spawn(int argc, char *argv[]) { SensorAirspeedSim *instance = new SensorAirspeedSim(); diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp index fe39fbc013..8f584ae900 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp @@ -34,16 +34,20 @@ #pragma once #include +#include #include #include #include #include +#include #include #include #include -#include #include +#include #include +#include +#include #include #include @@ -74,6 +78,8 @@ public: bool init(); + void check_failure_injection(); + private: void Run() override; @@ -87,8 +93,15 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + + bool _airspeed_disconnected{false}; + bool _airspeed_stuck{false}; + hrt_abstime _airspeed_blocked_timestamp{0}; + float _last_differential_pressure_pa{0.f}; perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; };