Fix airspeed calculation under wind influence by wind triangle

src/systems/air_speed/AirSpeed.cc

@@ -39,6 +39,9 @@
 #include "gz/sim/components/Name.hh"
 #include "gz/sim/components/ParentEntity.hh"
 #include "gz/sim/components/Pose.hh"
+#include "gz/sim/components/Wind.hh"
+#include "gz/sim/components/WindMode.hh"
+#include "gz/sim/components/LinearVelocitySeed.hh"
 #include "gz/sim/components/Sensor.hh"
 #include "gz/sim/EntityComponentManager.hh"
 #include "gz/sim/Util.hh"
@@ -67,6 +70,12 @@ class gz::sim::systems::AirSpeedPrivate
 
   public: Entity entity;
 
+  /// \brief Wind entity on which this sytem operates.
+  public: Entity windEntity;
+
+  /// \param[in] _ecm Mutable reference to the EntityComponentManager.
+  public: void Load(EntityComponentManager &_ecm);
+
   /// \brief Create sensor
   /// \param[in] _ecm Immutable reference to ECM.
   /// \param[in] _entity Entity of the IMU
@@ -101,12 +110,20 @@ AirSpeed::AirSpeed() :
 //////////////////////////////////////////////////
 AirSpeed::~AirSpeed() = default;
 
+//////////////////////////////////////////////////
+void AirSpeed::Configure(const Entity &_entity,
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    EntityComponentManager &_ecm,
+    EventManager &)
+{
+  this->dataPtr->Load(_ecm);
+}
+
 //////////////////////////////////////////////////
 void AirSpeed::PreUpdate(const UpdateInfo &/*_info*/,
     EntityComponentManager &_ecm)
 {
   GZ_PROFILE("AirSpeed::PreUpdate");
-
   // Create components
   for (auto entity : this->dataPtr->newSensors)
   {
@@ -137,7 +154,6 @@ void AirSpeed::PostUpdate(const UpdateInfo &_info,
            << std::chrono::duration<double>(_info.dt).count()
            << "s]. System may not work properly." << std::endl;
   }
-
   this->dataPtr->CreateSensors(_ecm);
 
   if (!_info.paused)
@@ -172,6 +188,13 @@ void AirSpeed::PostUpdate(const UpdateInfo &_info,
   this->dataPtr->RemoveAirSpeedEntities(_ecm);
 }
 
+//////////////////////////////////////////////////
+void AirSpeedPrivate::Load(
+    EntityComponentManager &_ecm)
+{
+  this->windEntity = _ecm.EntityByComponents(components::Wind());
+}
+
 //////////////////////////////////////////////////
 void AirSpeedPrivate::AddAirSpeed(
   const EntityComponentManager &_ecm,
@@ -265,6 +288,14 @@ void AirSpeedPrivate::UpdateAirSpeeds(const EntityComponentManager &_ecm)
 
           math::Vector3d sensorRelativeVel = relativeVel(_entity, _ecm);
           it->second->SetVelocity(sensorRelativeVel);
+
+          // update wind velocity
+          auto windVelSeed =
+          _ecm.Component<components::WorldLinearVelocitySeed>(this->windEntity);
+          math::Vector3d windVel{windVelSeed->Data().X(),
+                                 windVelSeed->Data().Y(),
+                                 windVelSeed->Data().Z()};
+          it->second->SetWindVelocity(windVel);
         }
         else
         {
@@ -300,6 +331,7 @@ void AirSpeedPrivate::RemoveAirSpeedEntities(
 }
 
 GZ_ADD_PLUGIN(AirSpeed, System,
+  AirSpeed::ISystemConfigure,
   AirSpeed::ISystemPreUpdate,
   AirSpeed::ISystemPostUpdate
 )

src/systems/air_speed/AirSpeed.hh

@@ -37,6 +37,7 @@ namespace systems
   /// readings over gz transport
   class AirSpeed:
     public System,
+    public ISystemConfigure,
     public ISystemPreUpdate,
     public ISystemPostUpdate
   {
@@ -46,6 +47,12 @@ namespace systems
     /// \brief Destructor
     public: ~AirSpeed() override;
 
+    // Documentation inherited
+    public: void Configure(const Entity &_entity,
+                           const std::shared_ptr<const sdf::Element> &_sdf,
+                           EntityComponentManager &_ecm,
+                           EventManager &_eventMgr) final;
+
     /// Documentation inherited
     public: void PreUpdate(const UpdateInfo &_info,
                            EntityComponentManager &_ecm) final;