diff --git a/src/modules/simulation/gz_bridge/server.config b/src/modules/simulation/gz_bridge/server.config index 0001356a3a8..b1f88198f63 100644 --- a/src/modules/simulation/gz_bridge/server.config +++ b/src/modules/simulation/gz_bridge/server.config @@ -8,6 +8,7 @@ + diff --git a/src/modules/simulation/gz_plugins/airspeed/AirSpeed.cpp b/src/modules/simulation/gz_plugins/airspeed/AirSpeed.cpp index 8d0c74dab70..103533de1ac 100644 --- a/src/modules/simulation/gz_plugins/airspeed/AirSpeed.cpp +++ b/src/modules/simulation/gz_plugins/airspeed/AirSpeed.cpp @@ -35,6 +35,8 @@ #include #include +#include +#include using namespace px4; @@ -120,6 +122,19 @@ void AirSpeed::PreUpdate(const gz::sim::UpdateInfo &_info, // Calculate differential pressure + noise in hPa const float diff_pressure_noise = standard_normal_distribution_(random_generator_) * diff_pressure_stddev_; + + // Read the wind from the ECM: the wind_info topic is only published on wind changes, + // which the subscription misses when the model spawns after the world was loaded + const gz::sim::Entity wind_entity = _ecm.EntityByComponents(gz::sim::components::Wind()); + + if (wind_entity != gz::sim::kNullEntity) { + const auto wind_linear_velocity = _ecm.Component(wind_entity); + + if (wind_linear_velocity) { + _wind_velocity = wind_linear_velocity->Data(); + } + } + // Body-relateive air velocity gz::math::Vector3d air_relative_velocity = _vehicle_velocity - _wind_velocity; gz::math::Vector3d body_velocity = _vehicle_attitude.RotateVectorReverse(air_relative_velocity);