wind addition to advanced_lift_drag plugin (#2226)

* wind addition to advanced_lift_drag plugin Signed-off-by: frederik <[email protected]> * added note on wind Signed-off-by: frederik <[email protected]> --------- Signed-off-by: frederik <[email protected]> Co-authored-by: frederik <[email protected]>
gazebosim · Nov 9, 2023 · e88a46f · e88a46f
1 parent 82fbdba
commit e88a46f
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 1 deletion.
diff --git a/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc b/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);

diff --git a/src/systems/advanced_lift_drag/AdvancedLiftDrag.hh b/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
   ///