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

scrap it all and replace by simple sine wave

Browse Source
This commit is contained in:
Balduin
2026-01-23 11:55:07 +01:00
parent f104bbefe9
commit 0e7d186613
2 changed files with 4 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/gz_bridge/GZBridge.cpp
+4 -38
View File
@@ -410,43 +410,6 @@ void GZBridge::airPressureCallback(const gz::msgs::FluidPressure &msg)
{
const uint64_t timestamp = hrt_absolute_time();
// To simulate atmospheric pressure drift, we use a time-discretised
// Ornstein-Uhlenbeck process (= integral of gaussian white noise with a
// linear stabilising feedback term). This has the advantage that we
// retain the statistical properties (stationary distribution, mixing
// time) regardless of dt.
// Ref: Gillespie, Daniel T.. (1996). Exact numerical simulation of the
// Ornstein-Uhlenbeck process and its integral. Physical Review E,
// 54(2), 2084–2091. doi:10.1103/PhysRevE.54.2084
const float dt = (float)(timestamp - _last_baro_pressure_drift_update) / 1_s;
const bool init = dt > 10;
// Parameterisation used in Gillespie 1996, but instead of c we use
// sigma_stationary - equivalent with c = 2 * sigma_stationary^2 / tau
// Relaxation time [sec]
const float tau = 3600.f * 24;
// Standard deviation of stationary distribution [Pa]
// 1000 Pa corresponds to about 80m of altitude error
const float sigma_stationary = 200;
const float mu = exp(-dt / tau); // (3.3)
const float sigma_update = sigma_stationary * sqrtf(1 - mu * mu); // (3.4a)
if (init) {
// Sample from the stationary distribution to match long time behaviour
_baro_pressure_drift = sigma_stationary * generate_wgn();
} else {
// Update equation (3.5a)
_baro_pressure_drift = mu * _baro_pressure_drift + sigma_update * generate_wgn();
}
_last_baro_pressure_drift_update = timestamp;
device::Device::DeviceId id{};
id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
id.devid_s.devtype = DRV_BARO_DEVTYPE_BAROSIM;
@@ -457,7 +420,10 @@ void GZBridge::airPressureCallback(const gz::msgs::FluidPressure &msg)
report.timestamp = timestamp;
report.timestamp_sample = timestamp;
report.device_id = id.devid;
report.pressure = (float) msg.pressure() + _baro_pressure_drift;
const float sin_offset = 12.0f * sinf(((float) timestamp / 60_s) * 2.0f * (float) MATH_PI);
report.pressure = (float) msg.pressure() + sin_offset;
report.temperature = this->_temperature;
_sensor_baro_pub.publish(report);
}
src/modules/simulation/gz_bridge/GZBridge.hpp
-3
View File
@@ -193,9 +193,6 @@ private:
const float _vel_noise_density = 0.2f; // Velocity noise process density
const float _vel_markov_time = 0.85f; // Velocity Markov process coefficient
float _baro_pressure_drift{};
hrt_abstime _last_baro_pressure_drift_update;
DEFINE_PARAMETERS(
(ParamInt<px4::params::SIM_GPS_USED>) _sim_gps_used,
(ParamInt<px4::params::SIM_GZ_EN_LIDAR>) _sim_gz_en_lidar,