Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 27 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
committed by Balduin
parent 46142b34e3
commit 4b34cb8834
3 changed files with 31 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp
+31 -33
View File
@@ -108,49 +108,47 @@ void SensorAirspeedSim::Run()
updateParams(); updateParams();
} }
if (_sim_failure.get() == 0) { if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated()
if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated() && _vehicle_attitude_sub.updated()) {
&& _vehicle_attitude_sub.updated()) {
vehicle_local_position_s lpos{}; vehicle_local_position_s lpos{};
_vehicle_local_position_sub.copy(&lpos); _vehicle_local_position_sub.copy(&lpos);
vehicle_global_position_s gpos{}; vehicle_global_position_s gpos{};
_vehicle_global_position_sub.copy(&gpos); _vehicle_global_position_sub.copy(&gpos);
vehicle_attitude_s attitude{}; vehicle_attitude_s attitude{};
_vehicle_attitude_sub.copy(&attitude); _vehicle_attitude_sub.copy(&attitude);
Vector3f local_velocity = Vector3f{lpos.vx, lpos.vy, lpos.vz}; Vector3f local_velocity = Vector3f{lpos.vx, lpos.vy, lpos.vz};
Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * local_velocity; Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * local_velocity;
// device id // device id
device::Device::DeviceId device_id; device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
device_id.devid_s.bus = 0; device_id.devid_s.bus = 0;
device_id.devid_s.address = 0; device_id.devid_s.address = 0;
device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM; device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM;
const float alt_amsl = gpos.alt; const float alt_amsl = gpos.alt;
const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl;
const float density_ratio = powf(TEMPERATURE_MSL / temperature_local, 4.256f); const float density_ratio = powf(TEMPERATURE_MSL / temperature_local, 4.256f);
const float air_density = AIR_DENSITY_MSL / density_ratio; const float air_density = AIR_DENSITY_MSL / density_ratio;
// calculate differential pressure + noise in hPa // calculate differential pressure + noise in hPa
const float diff_pressure_noise = (float)generate_wgn() * 0.01f; 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( float diff_pressure = sign(body_velocity(0)) * 0.005f * air_density * body_velocity(0) * body_velocity(
0) + diff_pressure_noise; 0) + diff_pressure_noise;
differential_pressure_s differential_pressure{}; differential_pressure_s differential_pressure{};
// report.timestamp_sample = time; // report.timestamp_sample = time;
differential_pressure.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION 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.differential_pressure_pa = (double)diff_pressure * 100.0; // hPa to Pa;
differential_pressure.temperature = temperature_local + ABSOLUTE_ZERO_C; // K to C differential_pressure.temperature = temperature_local + ABSOLUTE_ZERO_C; // K to C
differential_pressure.timestamp = hrt_absolute_time(); differential_pressure.timestamp = hrt_absolute_time();
_differential_pressure_pub.publish(differential_pressure); _differential_pressure_pub.publish(differential_pressure);
}
} }
perf_end(_loop_perf); perf_end(_loop_perf);
src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp
-4
View File
@@ -91,8 +91,4 @@ private:
uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
DEFINE_PARAMETERS(
(ParamInt<px4::params::SIM_ARSPD_FAIL>) _sim_failure
)
}; };
src/modules/simulation/sensor_airspeed_sim/parameters.yaml
-11
View File
@@ -13,14 +13,3 @@ parameters:
reboot_required: true reboot_required: true
min: 0 min: 0
max: 1 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