fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor(sensor_airspeed_sim): remove previous airspeed failure implementation

Browse Source
This commit is contained in:
Balduin 2026-04-08 16:02:00 +02:00
parent 44c128aade
commit 06556f4828
3 changed files with 31 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp
View File

@ -108,49 +108,47 @@ void SensorAirspeedSim::Run()
updateParams();
}
if (_sim_failure.get() == 0) {
if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated()
&& _vehicle_attitude_sub.updated()) {
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 + ABSOLUTE_ZERO_C; // K to C
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 + ABSOLUTE_ZERO_C; // K to C
differential_pressure.timestamp = hrt_absolute_time();
_differential_pressure_pub.publish(differential_pressure);
}
}
perf_end(_loop_perf);

4
src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp
View File

@ -91,8 +91,4 @@ private:
uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
DEFINE_PARAMETERS(
(ParamInt<px4::params::SIM_ARSPD_FAIL>) _sim_failure
)
};

11
src/modules/simulation/sensor_airspeed_sim/parameters.yaml
View File

@ -13,14 +13,3 @@ parameters:
reboot_required: true
min: 0
max: 1
SIM_ARSPD_FAIL:
description:
short: Dynamically simulate failure of airspeed sensor instance
type: enum
values:
0: Disabled
1: Enabled
default: 0
reboot_required: true
min: 0
max: 1