diff --git a/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc b/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc index 42d6efc189..33b772dae4 100644 --- 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(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(windEntity); + } std::vector 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 index 0a5be89e8f..31b420d745 100644 --- 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 ///