fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 15:17:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(sensor_airspeed_sim): Implement off, stuck, and wrong airspeed failure

Browse Source 
- wrong and off equal to simulator_mavlink implementation
     - but wrong is over timescale of 10s rather than 1s by default to
       make it harder for innovation check to flag (closer to icing)
 - stuck will just keep the last value before injecting the failure
This commit is contained in:
Balduin
2026-04-08 16:38:09 +02:00
parent 06556f4828
commit 1762c2db0b
2 changed files with 117 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp
+103 -29
View File
@@ -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<int>(vehicle_command.param1 + 0.5f);
const int failure_type = static_cast<int>(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();
src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp
+14 -1
View File
@@ -34,16 +34,20 @@
#pragma once
#include <lib/perf/perf_counter.h>
#include <mathlib/mathlib.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -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_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication<vehicle_command_ack_s> _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")};
};