wind addition to advanced_lift_drag plugin (#2226)

* wind addition to advanced_lift_drag plugin

Signed-off-by: frederik <frederik@auterion.com>

* added note on wind

Signed-off-by: frederik <frederik@auterion.com>

---------

Signed-off-by: frederik <frederik@auterion.com>
Co-authored-by: frederik <frederik@auterion.com>

Author: fredmarkus

src/systems/advanced_lift_drag/AdvancedLiftDrag.cc

@@ -59,6 +59,7 @@
 #include "gz/sim/components/Name.hh"
 #include "gz/sim/components/ExternalWorldWrenchCmd.hh"
 #include "gz/sim/components/Pose.hh"
+#include "gz/sim/components/Wind.hh"
 
 using namespace gz;
 using namespace sim;
@@ -481,6 +482,13 @@ void AdvancedLiftDragPrivate::Update(EntityComponentManager &_ecm)
   const auto worldPose =
       _ecm.Component<components::WorldPose>(this->linkEntity);
 
+  // get wind as a component from the _ecm
+  components::WorldLinearVelocity *windLinearVel = nullptr;
+  if(_ecm.EntityByComponents(components::Wind()) != kNullEntity){
+    Entity windEntity = _ecm.EntityByComponents(components::Wind());
+    windLinearVel =
+        _ecm.Component<components::WorldLinearVelocity>(windEntity);
+  }
 
   std::vector<components::JointPosition*> controlJointPosition_vec(
     this->num_ctrl_surfaces);
@@ -499,8 +507,12 @@ void AdvancedLiftDragPrivate::Update(EntityComponentManager &_ecm)
 
   const auto &pose = worldPose->Data();
   const auto cpWorld = pose.Rot().RotateVector(this->cp);
-  const auto air_velocity = worldLinVel->Data() + worldAngVel->Data().Cross(
+  auto air_velocity = worldLinVel->Data() + worldAngVel->Data().Cross(
     cpWorld);
+  if (windLinearVel != nullptr){
+    air_velocity = worldLinVel->Data() + worldAngVel->Data().Cross(
+    cpWorld) - windLinearVel->Data();
+  }
 
   // Define body frame: X forward, Z downward, Y out the right wing
   gz::math::Vector3d body_x_axis = pose.Rot().RotateVector(this->forward);

src/systems/advanced_lift_drag/AdvancedLiftDrag.hh

@@ -44,6 +44,8 @@ namespace systems
   /// https://github.com/PX4/PX4-Autopilot/blob/main/Tools/simulation/
   /// gz/tools/avl_automation/
   ///
+  /// Note: Wind calculations can be enabled by setting the wind parameter
+  ///       in the world file.
   ///
   /// ## System Parameters
   ///