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

Inject failure for airspeed sim

Browse Source
This commit is contained in:
Jaeyoung Lim
2023-02-14 15:45:01 +01:00
committed by JaeyoungLim
parent 3f50bd051f
commit 0b3f4dd385
3 changed files with 48 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp
+33 -31
View File
@@ -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);
src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp
+3 -3
View File
@@ -89,7 +89,7 @@ private:
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
// DEFINE_PARAMETERS(
// (ParamInt<px4::params::SIM_GPS_USED>) _sim_gps_used
// )
DEFINE_PARAMETERS(
(ParamInt<px4::params::SIM_ARSPD_FAIL>) _sim_failure
)
};
src/modules/simulation/sensor_airspeed_sim/parameters.c
+12
View File
@@ -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);