feat: BlueROV2 Heavy updated control (attitude and position) and model (#25052)

* rft: clean merge to PX4

* fix: formatting

* fix: extra line

* fix: moved submarine out of "is_ground_vehicle", added proper check for center-throttle

* feat: updated gazebo models to include bluerov update

* fix: use 'is_uuv_vehicle', remove FW_MM/LLC from uuv build

* fix: added saturation to thrust and torque messages via param

* doc: updated parameters documentation for uuv

* fix: formatting

* feat: matching hardware reference

* fix: thrusters kg

* rft: removed commented lines

* fix: update gz reference given hw setup

* fix: hardware references

* fix: recommendations

* fix: updated settings to match hardware

* rft: check only for fixed and rotary wing for high throttle

Co-authored-by: Daniel Agar <daniel@agar.ca>

* fix: commit oupsie

* fix: format

* rft: remove is_uuv

* fix: hw parameters, uuv build target for v6x

* feat: added support for D-pad attitude changes in stabilized position control

* fix: position setpoint update and parametrized trajectory age and att change

* fix: format

* fix: removed duplicated call to check_validity_setpoint

* fix: setpoint update on arming logic

* fix: setpoint initialization for stabilized mode

---------

Co-authored-by: Daniel Agar <daniel@agar.ca>

src/modules/commander/Commander.cpp

@@ -574,7 +574,9 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 
 			if (!_vehicle_control_mode.flag_control_climb_rate_enabled &&
 			    !_failsafe_flags.manual_control_signal_lost && !_is_throttle_low
-			    && !is_ground_vehicle(_vehicle_status)) {
+			    && ((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)
+				|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING))
+			   ) {
 
 				mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t");
 				events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info},

src/modules/commander/commander_helper.cpp

@@ -119,7 +119,8 @@ bool is_fixed_wing(const vehicle_status_s &current_status)
 
 bool is_ground_vehicle(const vehicle_status_s &current_status)
 {
-	return (current_status.system_type == VEHICLE_TYPE_BOAT || current_status.system_type == VEHICLE_TYPE_GROUND_ROVER);
+	return (current_status.system_type == VEHICLE_TYPE_BOAT ||
+		current_status.system_type == VEHICLE_TYPE_GROUND_ROVER);
 }
 
 // End time for currently blinking LED message, 0 if no blink message

src/modules/simulation/gz_bridge/server.config

@@ -15,6 +15,8 @@
     </plugin>
     <plugin entity_name="*" entity_type="world" filename="libOpticalFlowSystem.so" name="custom::OpticalFlowSystem"/>
     <plugin entity_name="*" entity_type="world" filename="libGstCameraSystem.so" name="custom::GstCameraSystem"/>
+    <plugin entity_name="*" entity_type="world" filename="libGenericMotorModelPlugin.so" name="gz::sim::systems::GenericMotorModel"/>
+    <plugin entity_name="*" entity_type="world" filename="libBuoyancySystemPlugin.so" name="gz::sim::systems::BuoyancySystem"/>
     <!-- <plugin entity_name="*" entity_type="world" filename="libTemplatePlugin.so" name="custom::TemplateSystem"/> -->
   </plugins>
 </server_config>

src/modules/simulation/gz_plugins/CMakeLists.txt

@@ -56,12 +56,14 @@ if (gz-transport_FOUND)
     add_subdirectory(template_plugin)
     add_subdirectory(gstreamer)
     add_subdirectory(moving_platform_controller)
+    add_subdirectory(generic_motor)
+    add_subdirectory(buoyancy)
     add_subdirectory(spacecraft_thruster)
 
     # Add an alias target for each plugin
     if (TARGET GstCameraSystem)
-        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController SpacecraftThrusterModelPlugin TemplatePlugin GstCameraSystem)
+        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin GstCameraSystem SpacecraftThrusterModelPlugin)
     else()
-        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController SpacecraftThrusterModelPlugin TemplatePlugin)
+        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin SpacecraftThrusterModelPlugin)
     endif()
 endif()

src/modules/simulation/gz_plugins/buoyancy/BuoyancySystem.cpp

@@ -0,0 +1,699 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#include <gz/msgs/wrench.pb.h>
+
+#include <map>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+#include <unordered_set>
+#include <utility>
+#include <vector>
+
+#include <gz/common/Mesh.hh>
+#include <gz/common/MeshManager.hh>
+#include <gz/common/Profiler.hh>
+
+#include <gz/plugin/Register.hh>
+
+#include <gz/math/Helpers.hh>
+#include <gz/math/Pose3.hh>
+#include <gz/math/Vector3.hh>
+#include <gz/math/Rand.hh>
+
+#include <gz/msgs/Utility.hh>
+
+#include <sdf/sdf.hh>
+
+#include "gz/sim/components/CenterOfVolume.hh"
+#include "gz/sim/components/Collision.hh"
+#include "gz/sim/components/Gravity.hh"
+#include "gz/sim/components/Inertial.hh"
+#include "gz/sim/components/Link.hh"
+#include "gz/sim/components/ParentEntity.hh"
+#include "gz/sim/components/Pose.hh"
+#include "gz/sim/components/Volume.hh"
+#include "gz/sim/components/World.hh"
+#include "gz/sim/Link.hh"
+#include "gz/sim/Model.hh"
+#include "gz/sim/Util.hh"
+
+#include "BuoyancySystem.hpp"
+
+using namespace gz;
+using namespace sim;
+using namespace systems;
+
+class gz::sim::systems::BuoyancyPrivate
+{
+public: enum BuoyancyType {
+		/// \brief Applies same buoyancy to whole world.
+		UNIFORM_BUOYANCY,
+		/// \brief Uses z-axis to determine buoyancy of the world
+		/// This is useful for worlds where we want to simulate the ocean interface.
+		/// Or for instance if we want to simulate different levels of buoyancies
+		/// at different depths.
+		GRADED_BUOYANCY
+	};
+public: BuoyancyType buoyancyType{BuoyancyType::UNIFORM_BUOYANCY};
+	/// \brief Get the fluid density based on a pose.
+	/// \param[in] _pose The pose to use when computing the fluid density. The
+	/// pose frame is left undefined because this function currently returns
+	/// a constant value, see the todo in the function implementation.
+	/// \return The fluid density at the givein pose.
+public: double UniformFluidDensity(const math::Pose3d &_pose) const;
+
+	/// \brief Get the resultant buoyant force on a shape.
+	/// \param[in] _pose World pose of the shape's origin.
+	/// \param[in] _shape The collision mesh of a shape. Currently must
+	/// be box or sphere.
+	/// \param[in] _gravity Gravity acceleration in the world frame.
+	/// Updates this->buoyancyForces containing {force, center_of_volume} to be
+	/// applied on the link.
+public:
+	template<typename T>
+	void GradedFluidDensity(
+		const math::Pose3d &_pose, const T &_shape, const math::Vector3d &_gravity);
+
+	/// \brief Check for new links to apply buoyancy forces to. Calculates the
+	/// volume and center of volume for every new link and stages them to be
+	/// committed when `CommitNewEntities` is called.
+	/// \param[in] _ecm The Entity Component Manager.
+public: void CheckForNewEntities(const EntityComponentManager &_ecm);
+
+	/// \brief Commits the new entities to the ECM.
+	/// \param[in] _ecm The Entity Component Manager.
+public: void CommitNewEntities(EntityComponentManager &_ecm);
+
+	/// \brief Check if an entity is enabled or not.
+	/// \param[in] _entity Target entity
+	/// \param[in] _ecm Entity component manager
+	/// \return True if buoyancy should be applied.
+public: bool IsEnabled(Entity _entity,
+			       const EntityComponentManager &_ecm) const;
+
+	/// \brief Model interface
+public: Entity world{kNullEntity};
+
+	/// \brief The density of the fluid in which the object is submerged in
+	/// kg/m^3. Defaults to 1000, the fluid density of water.
+public: double fluidDensity{1000};
+
+	/// \brief When using GradedBuoyancy, we provide a different buoyancy for
+	/// each layer. The key on this map is height in meters and the value is fluid
+	/// density. I.E all the fluid between $key$m and $next_key$m has the density
+	/// $value$kg/m^3. Everything below the first key is considered as having
+	/// fluidDensity.
+public: std::map<double, double> layers;
+
+	/// \brief Holds information about forces contributed by a single collision
+	/// shape.
+public: struct BuoyancyActionPoint {
+		/// \brief The force to be applied, expressed in the world frame.
+		math::Vector3d force;
+
+		/// \brief The point from which the force will be applied, expressed in
+		/// the collision's frame.
+		math::Vector3d point;
+
+		/// \brief The world pose of the collision.
+		math::Pose3d pose;
+	};
+
+	/// \brief List of points from where the forces act.
+	/// This holds values refent to the current link being processed and must be
+	/// cleared between links.
+	/// \TODO(chapulina) It's dangerous to keep link-specific values in a member
+	/// variable. We should consider reducing the scope of this variable and pass
+	/// it across functions as needed.
+public: std::vector<BuoyancyActionPoint> buoyancyForces;
+
+	/// \brief Resolve all forces as if they act as a Wrench from the give pose.
+	/// \param[in] _linkInWorld The point from which all poses are to be resolved.
+	/// This is the link's origin in the world frame.
+	/// \return A pair of {force, torque} describing the wrench to be applied
+	/// at _pose, expressed in the world frame.
+public: std::pair<math::Vector3d, math::Vector3d> ResolveForces(
+		const math::Pose3d &_linkInWorld);
+
+	/// \brief Scoped names of entities that buoyancy should apply to. If empty,
+	/// all links will receive buoyancy.
+public: std::unordered_set<std::string> enabled;
+
+	/// \brief Basic buoyancy enabler flag. This is used to set the buoyancy
+	/// equal to the gravity compensation of the base_link mass, with added noise
+	/// to simulate the effect of buoyancy. Useful for testing purposes.
+public: bool basicBuoyancy{false};
+
+	/// \brief Center of volumes to be added on the next Pre-update
+public: std::unordered_map<Entity, math::Vector3d> centerOfVolumes;
+
+	/// \brief Volumes to be added on the next.
+public: std::unordered_map<Entity, double> volumes;
+};
+
+//////////////////////////////////////////////////
+double BuoyancyPrivate::UniformFluidDensity(const math::Pose3d &/*_pose*/) const
+{
+	return this->fluidDensity;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+void BuoyancyPrivate::GradedFluidDensity(
+	const math::Pose3d &_pose, const T &_shape, const math::Vector3d &_gravity)
+{
+	auto prevLayerFluidDensity = this->fluidDensity;
+	auto prevLayerVol = 0.0;
+	auto centerOfBuoyancy = math::Vector3d{0, 0, 0};
+
+	for (const auto &[height, currFluidDensity] : this->layers) {
+		// TODO(arjo): Transform plane and slice the shape
+		math::Planed plane{math::Vector3d{0, 0, 1}, height - _pose.Pos().Z()};
+		auto vol = _shape.VolumeBelow(plane);
+
+		// Short circuit.
+		if (vol <= 0) {
+			prevLayerFluidDensity = currFluidDensity;
+			continue;
+		}
+
+		// Calculate point from which force is applied
+		auto cov = _shape.CenterOfVolumeBelow(plane);
+
+		if (!cov.has_value()) {
+			prevLayerFluidDensity = currFluidDensity;
+			continue;
+		}
+
+		// Archimedes principle for this layer
+		auto forceMag =  - (vol - prevLayerVol) * _gravity * prevLayerFluidDensity;
+
+		// Accumulate layers.
+		prevLayerFluidDensity = currFluidDensity;
+
+		auto cob = (cov.value() * vol - centerOfBuoyancy * prevLayerVol)
+			   / (vol - prevLayerVol);
+		centerOfBuoyancy = cov.value();
+		auto buoyancyAction = BuoyancyActionPoint {
+			forceMag,
+			cob,
+			_pose
+		};
+		this->buoyancyForces.push_back(buoyancyAction);
+
+		prevLayerVol = vol;
+	}
+
+	// For the rest of the layers.
+	auto vol = _shape.Volume();
+
+	// No force contributed by this layer.
+	if (std::abs(vol - prevLayerVol) < 1e-10) {
+		return;
+	}
+
+	// Archimedes principle for this layer
+	auto forceMag = - (vol - prevLayerVol) * _gravity * prevLayerFluidDensity;
+
+	// Calculate centre of buoyancy
+	auto cov = math::Vector3d{0, 0, 0};
+	auto cob =
+		(cov * vol - centerOfBuoyancy * prevLayerVol) / (vol - prevLayerVol);
+	centerOfBuoyancy = cov;
+	auto buoyancyAction = BuoyancyActionPoint {
+		forceMag,
+		cob,
+		_pose
+	};
+	this->buoyancyForces.push_back(buoyancyAction);
+}
+
+//////////////////////////////////////////////////
+std::pair<math::Vector3d, math::Vector3d> BuoyancyPrivate::ResolveForces(
+	const math::Pose3d &_linkInWorld)
+{
+	auto force = math::Vector3d{0, 0, 0};
+	auto torque = math::Vector3d{0, 0, 0};
+
+	for (const auto &b : this->buoyancyForces) {
+		force += b.force;
+
+		// Pose offset from application point (COV) to collision origin, expressed
+		// in the collision frame
+		math::Pose3d pointInCol{b.point, math::Quaterniond::Identity};
+
+		// Application point in the world frame
+		auto pointInWorld = b.pose * pointInCol;
+
+		// Offset between the link origin and the force application point
+		auto offset = _linkInWorld.Pos() - pointInWorld.Pos();
+
+		torque += b.force.Cross(offset);
+	}
+
+	return {force, torque};
+}
+
+//////////////////////////////////////////////////
+void BuoyancyPrivate::CheckForNewEntities(const EntityComponentManager &_ecm)
+{
+	// Compute the volume and center of volume for each new link
+	_ecm.EachNew<components::Link, components::Inertial>(
+		[&](const Entity & _entity,
+		    const components::Link *,
+	const components::Inertial *) -> bool {
+		if (this->basicBuoyancy == true) return true;
+		// Skip if the entity already has a volume and center of volume
+		if (_ecm.EntityHasComponentType(_entity,
+						components::CenterOfVolume().TypeId()) &&
+		    _ecm.EntityHasComponentType(_entity,
+						components::Volume().TypeId()))
+		{
+			return true;
+		}
+
+		if (!this->IsEnabled(_entity, _ecm))
+		{
+			return true;
+		}
+
+		Link link(_entity);
+
+		std::vector<Entity> collisions = _ecm.ChildrenByComponents(
+			_entity, components::Collision());
+
+		double volumeSum = 0;
+		gz::math::Vector3d weightedPosInLinkSum =
+		gz::math::Vector3d::Zero;
+
+		// Compute the volume of the link by iterating over all the collision
+		// elements and storing each geometry's volume.
+		for (const Entity &collision : collisions)
+		{
+			double volume = 0;
+			const components::CollisionElement *coll =
+			_ecm.Component<components::CollisionElement>(collision);
+
+			if (!coll) {
+				gzerr << "Invalid collision pointer. This shouldn't happen\n";
+				continue;
+			}
+
+			switch (coll->Data().Geom()->Type()) {
+			case sdf::GeometryType::BOX:
+				volume = coll->Data().Geom()->BoxShape()->Shape().Volume();
+				break;
+
+			case sdf::GeometryType::SPHERE:
+				volume = coll->Data().Geom()->SphereShape()->Shape().Volume();
+				break;
+
+			case sdf::GeometryType::CYLINDER:
+				volume = coll->Data().Geom()->CylinderShape()->Shape().Volume();
+				break;
+
+			case sdf::GeometryType::PLANE:
+				// Ignore plane shapes. They have no volume and are not expected
+				// to be buoyant.
+				break;
+
+			case sdf::GeometryType::MESH: {
+					std::string file = asFullPath(
+								   coll->Data().Geom()->MeshShape()->Uri(),
+								   coll->Data().Geom()->MeshShape()->FilePath());
+
+					if (common::MeshManager::Instance()->IsValidFilename(file)) {
+						const common::Mesh *mesh =
+							common::MeshManager::Instance()->Load(file);
+
+						if (mesh) {
+							volume = mesh->Volume();
+
+						} else {
+							gzerr << "Unable to load mesh[" << file << "]\n";
+						}
+
+					} else {
+						gzerr << "Invalid mesh filename[" << file << "]\n";
+					}
+
+					break;
+				}
+
+			default:
+				gzerr << "Unsupported collision geometry["
+				      << static_cast<int>(coll->Data().Geom()->Type()) << "]\n";
+				break;
+			}
+
+			volumeSum += volume;
+			auto poseInLink = _ecm.Component<components::Pose>(collision)->Data();
+			weightedPosInLinkSum += volume * poseInLink.Pos();
+		}
+
+		if (volumeSum > 0)
+		{
+			// Stage calculation results for future commit. We do this because
+			// during PostUpdate the ECM is const, so we can't modify it,
+			this->centerOfVolumes[_entity] = weightedPosInLinkSum / volumeSum;
+			this->volumes[_entity] = volumeSum;
+		}
+
+		return true;
+	});
+}
+
+//////////////////////////////////////////////////
+void BuoyancyPrivate::CommitNewEntities(EntityComponentManager &_ecm)
+{
+	for (const auto [_entity, _cov] : this->centerOfVolumes) {
+		if (_ecm.HasEntity(_entity)) {
+			_ecm.CreateComponent(_entity, components::CenterOfVolume(_cov));
+		}
+	}
+
+	for (const auto [_entity, _vol] : this->volumes) {
+		if (_ecm.HasEntity(_entity)) {
+			_ecm.CreateComponent(_entity, components::Volume(_vol));
+		}
+	}
+
+	this->centerOfVolumes.clear();
+	this->volumes.clear();
+}
+
+//////////////////////////////////////////////////
+bool BuoyancyPrivate::IsEnabled(Entity _entity,
+				const EntityComponentManager &_ecm) const
+{
+	// If there's nothing enabled, all entities are enabled
+	if (this->enabled.empty()) {
+		return true;
+	}
+
+	auto entity = _entity;
+
+	while (entity != kNullEntity) {
+		// Fully scoped name
+		auto name = scopedName(entity, _ecm, "::", false);
+
+		// Remove world name
+		name = removeParentScope(name, "::");
+
+		if (this->enabled.find(name) != this->enabled.end()) {
+			return true;
+		}
+
+		// Check parent
+		auto parentComp = _ecm.Component<components::ParentEntity>(entity);
+
+		if (nullptr == parentComp) {
+			return false;
+		}
+
+		entity = parentComp->Data();
+	}
+
+	return false;
+}
+
+//////////////////////////////////////////////////
+Buoyancy::Buoyancy()
+	: dataPtr(std::make_unique<BuoyancyPrivate>())
+{
+}
+
+//////////////////////////////////////////////////
+void Buoyancy::Configure(const Entity &_entity,
+			 const std::shared_ptr<const sdf::Element> &_sdf,
+			 EntityComponentManager &_ecm,
+			 EventManager &/*_eventMgr*/)
+{
+	// Store the world.
+	this->dataPtr->world = _entity;
+
+	// Get the gravity (defined in world frame)
+	const components::Gravity *gravity = _ecm.Component<components::Gravity>(
+			this->dataPtr->world);
+
+	if (!gravity) {
+		gzerr << "Unable to get the gravity vector. Make sure this plugin is "
+		      << "attached to a <world>, not a <model>." << std::endl;
+		return;
+	}
+
+	if (_sdf->HasElement("uniform_fluid_density")) {
+		this->dataPtr->fluidDensity = _sdf->Get<double>("uniform_fluid_density");
+
+	} else if (_sdf->HasElement("graded_buoyancy")) {
+		this->dataPtr->buoyancyType =
+			BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY;
+
+		auto gradedElement = _sdf->GetFirstElement();
+
+		if (gradedElement == nullptr) {
+			gzerr << "Unable to get element description" << std::endl;
+			return;
+		}
+
+		auto argument = gradedElement->GetFirstElement();
+
+		while (argument != nullptr) {
+			if (argument->GetName() == "default_density") {
+				argument->GetValue()->Get<double>(this->dataPtr->fluidDensity);
+				gzdbg << "Default density set to "
+				      << this->dataPtr->fluidDensity << std::endl;
+			}
+
+			if (argument->GetName() == "density_change") {
+				auto depth = argument->Get<double>("above_depth", 0.0);
+				auto density = argument->Get<double>("density", 0.0);
+
+				if (!depth.second) {
+					gzwarn << "No <above_depth> tag was found as a "
+					       << "child of <density_change>" << std::endl;
+				}
+
+				if (!density.second) {
+					gzwarn << "No <density> tag was found as a "
+					       << "child of <density_change>" << std::endl;
+				}
+
+				this->dataPtr->layers[depth.first] = density.first;
+				gzdbg << "Added layer at " << depth.first << ", "
+				      <<  density.first << std::endl;
+			}
+
+			argument = argument->GetNextElement();
+		}
+
+	} else if (_sdf->HasElement("basic_buoyancy")) {
+		// This is a legacy tag, which is equivalent to
+		this->dataPtr->basicBuoyancy = _sdf->Get<bool>("basic_buoyancy", false).first;
+
+	} else {
+		gzwarn <<
+		       "Neither <graded_buoyancy>, <uniform_fluid_density>, or <basic_buyancy> were specified"
+		       << std::endl
+		       << "\tDefaulting to <basic_buyancy>"
+		       << std::endl;
+		this->dataPtr->basicBuoyancy = true;
+	}
+
+	if (_sdf->HasElement("enable")) {
+		for (auto enableElem = _sdf->FindElement("enable");
+		     enableElem != nullptr;
+		     enableElem = enableElem->GetNextElement("enable")) {
+			this->dataPtr->enabled.insert(enableElem->Get<std::string>());
+		}
+	}
+}
+
+//////////////////////////////////////////////////
+void Buoyancy::PreUpdate(const UpdateInfo &_info,
+			 EntityComponentManager &_ecm)
+{
+	GZ_PROFILE("Buoyancy::PreUpdate");
+	this->dataPtr->CheckForNewEntities(_ecm);
+	this->dataPtr->CommitNewEntities(_ecm);
+
+	// Only update if not paused.
+	if (_info.paused) {
+		return;
+	}
+
+	const components::Gravity *gravity = _ecm.Component<components::Gravity>(
+			this->dataPtr->world);
+
+	if (!gravity) {
+		gzerr << "Unable to get the gravity vector. Has gravity been defined?"
+		      << std::endl;
+		return;
+	}
+
+	if (!this->dataPtr->basicBuoyancy) {
+		// Iterate over all links and apply buoyancy forces.
+		_ecm.Each<components::Link,
+			  components::Volume,
+			  components::CenterOfVolume>(
+				  [&](const Entity & _entity,
+				      const components::Link *,
+				      const components::Volume * _volume,
+		const components::CenterOfVolume * _centerOfVolume) -> bool {
+			// World pose of the link.
+			math::Pose3d linkWorldPose = worldPose(_entity, _ecm);
+
+			Link link(_entity);
+
+			math::Vector3d buoyancy;
+			// By Archimedes' principle,
+			// buoyancy = -(mass*gravity)*fluid_density/object_density
+			// object_density = mass/volume, so the mass term cancels.
+			if (this->dataPtr->buoyancyType
+			    == BuoyancyPrivate::BuoyancyType::UNIFORM_BUOYANCY)
+			{
+				buoyancy =
+				-this->dataPtr->UniformFluidDensity(linkWorldPose) *
+				_volume->Data() * gravity->Data();
+
+				// Convert the center of volume to the world frame
+				math::Vector3d offsetWorld = linkWorldPose.Rot().RotateVector(
+					_centerOfVolume->Data());
+				// Compute the torque that should be applied due to buoyancy and
+				// the center of volume.
+				math::Vector3d torque = offsetWorld.Cross(buoyancy);
+
+				// Apply the wrench to the link. This wrench is applied in the
+				// Physics System.
+				link.AddWorldWrench(_ecm, buoyancy, torque);
+
+			} else if (this->dataPtr->buoyancyType
+				   == BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY)
+			{
+				std::vector<Entity> collisions = _ecm.ChildrenByComponents(
+					_entity, components::Collision());
+				this->dataPtr->buoyancyForces.clear();
+
+				for (auto e : collisions) {
+					const components::CollisionElement *coll =
+					_ecm.Component<components::CollisionElement>(e);
+
+					auto pose = worldPose(e, _ecm);
+
+					if (!coll) {
+						gzerr << "Invalid collision pointer. This shouldn't happen\n";
+						continue;
+					}
+
+					switch (coll->Data().Geom()->Type()) {
+					case sdf::GeometryType::BOX:
+						this->dataPtr->GradedFluidDensity<math::Boxd>(
+							pose,
+							coll->Data().Geom()->BoxShape()->Shape(),
+							gravity->Data());
+						break;
+
+					case sdf::GeometryType::SPHERE:
+						this->dataPtr->GradedFluidDensity<math::Sphered>(
+							pose,
+							coll->Data().Geom()->SphereShape()->Shape(),
+							gravity->Data());
+						break;
+
+					default: {
+							static bool warned{false};
+
+							if (!warned) {
+								gzwarn << "Only <box> and <sphere> collisions are supported "
+								       << "by the graded buoyancy option." << std::endl;
+								warned = true;
+							}
+
+							break;
+						}
+					}
+				}
+
+				auto [force, torque] = this->dataPtr->ResolveForces(linkWorldPose);
+				// Apply the wrench to the link. This wrench is applied in the
+				// Physics System.
+				link.AddWorldWrench(_ecm, force, torque);
+			}
+
+			return true;
+		});
+
+	} else {
+		// Apply simple buyancy to baselink with some white noise.
+
+		// Collect all masses in the model
+		double mass = 0.0;
+
+		_ecm.Each<components::Link,
+			  components::Inertial>(
+				  [&](const Entity & _entity,
+				      const components::Link *,
+		const components::Inertial * _inertial) -> bool {
+			// Get all masses and add them up
+			Link link(_entity);
+
+			if (_ecm.EntityHasComponentType(_entity,
+							components::Inertial().TypeId()))
+			{
+				const auto *inertialComp =
+				_ecm.Component<components::Inertial>(_entity);
+
+				if (inertialComp) {
+					mass = inertialComp->Data().MassMatrix().Mass();
+					// Apply the buoyancy force
+					math::Vector3d buoyancy = -gravity->Data() * mass;
+					math::Vector3d randVec(0.0, 0.0, -gravity->Data().Z() * mass * math::Rand::DblUniform(-0.01, 0.01));
+					math::Vector3d force = buoyancy + randVec;
+
+					link.AddWorldWrench(_ecm, force, math::Vector3d{0, 0, 0});
+				}
+			}
+
+			return true;
+		});
+	}
+}
+
+//////////////////////////////////////////////////
+void Buoyancy::PostUpdate(
+	const UpdateInfo &/*_info*/,
+	const EntityComponentManager &_ecm)
+{
+	this->dataPtr->CheckForNewEntities(_ecm);
+}
+
+//////////////////////////////////////////////////
+bool Buoyancy::IsEnabled(Entity _entity,
+			 const EntityComponentManager &_ecm) const
+{
+	return this->dataPtr->IsEnabled(_entity, _ecm);
+}
+
+GZ_ADD_PLUGIN(Buoyancy,
+	      System,
+	      Buoyancy::ISystemConfigure,
+	      Buoyancy::ISystemPreUpdate,
+	      Buoyancy::ISystemPostUpdate)
+
+GZ_ADD_PLUGIN_ALIAS(Buoyancy,
+		    "gz::sim::systems::BuoyancySystem")

src/modules/simulation/gz_plugins/buoyancy/BuoyancySystem.hpp

@@ -0,0 +1,158 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef GZ_SIM_SYSTEMS_BUOYANCY_SYSTEM_HPP_
+#define GZ_SIM_SYSTEMS_BUOYANCY_SYSTEM_HPP_
+
+#include <gz/sim/System.hh>
+#include <memory>
+
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE
+{
+namespace systems
+{
+// Forward declaration
+class BuoyancyPrivate;
+
+/// \brief A system that simulates buoyancy of objects immersed in fluid.
+/// All SDF parameters are optional. This system must be attached to the
+/// world and this system will apply buoyancy to all links that have collision
+/// shapes.
+///
+/// The volume and center of volume will be computed for each link, and
+/// stored as components. During each iteration, Archimedes' principle is
+/// applied to each link with a volume and center of volume component.
+///
+/// Plane shapes are not handled by this plugin, and will not be affected
+/// by buoyancy.
+///
+/// ## System Parameters
+///
+/// * `<uniform_fluid_density>` sets the density of the fluid that surrounds
+/// the buoyant object. [Units: kgm^-3]
+/// * `<graded_buoyancy>` allows you to define a world where the buoyancy
+/// changes along the Z axis. An example of such a world could be if we are
+/// simulating an open ocean with its surface and under water behaviour. This
+/// mode slices the volume of each collision mesh according to where the water
+/// line is set. When defining a `<graded_buoyancy>` tag, one must also define
+/// `<default_density>` and `<density_change>` tags.
+/// * `<default_density>` is the default fluid which the world should be
+/// filled with. [Units: kgm^-3]
+/// * `<density_change>` allows you to define a new layer.
+/// * `<above_depth>` a child property of `<density_change>`. This determines
+/// the height at which the next fluid layer should start. [Units: m]
+/// * `<density>` the density of the fluid in this layer. [Units: kgm^-3]
+/// * `<enable>` used to indicate which models will have buoyancy.
+/// Add one enable element per model or link. This element accepts names
+/// scoped from the top level model (i.e. `<model>::<nested_model>::<link>`).
+/// If there are no enabled entities, all models in simulation will be
+/// affected by buoyancy.
+///
+/// ## Examples
+///
+/// ### uniform_fluid_density world
+///
+/// The `buoyancy.sdf` SDF file contains three submarines. The first
+/// submarine is neutrally buoyant, the second sinks, and the third
+/// floats. To run:
+///
+/// ```
+/// gz sim -v 4 buoyancy.sdf
+/// ```
+///
+/// ### graded_buoyancy world
+///
+/// Often when simulating a maritime environment one may need to simulate both
+/// surface and underwater vessels. This means the buoyancy plugin needs to
+/// take into account two different fluids. One being water with a density of
+/// 1000kgm^-3 and another being air with a very light density of say 1kgm^-3.
+/// An example for such a configuration may be found in the
+/// `graded_buoyancy.sdf` world.
+///
+/// ```
+/// gz sim -v 4 graded_buoyancy.sdf
+/// ```
+///
+/// You should be able to see a sphere bobbing up and down undergoing simple
+/// harmonic motion on the surface of the fluid (this is expected behaviour
+/// as the SHM is usually damped by the hydrodynamic forces. See the hydro-
+/// dynamics plugin for an example of how to use it). The key part of this is
+///
+/// ```
+/// <graded_buoyancy>
+///   <default_density>1000</default_density>
+///   <density_change>
+///     <above_depth>0</above_depth>
+///     <density>1</density>
+///   </density_change>
+/// </graded_buoyancy>
+/// ```
+/// The default density tag says that by default the world has a fluid density
+/// of 1000kgm^-3. This essentially states that by default the world is filled
+/// with dihydrogen monoxide (aka water). The `<density_change>` tag
+/// essentially establishes the fact that there is a nother fluid. The
+/// `<above_depth>` tag says that above z=0 there is another fluid with a
+/// different density. The density of that fluid is defined by the `<density>`
+/// tag. We will be simulating air with a fluid density of 1kgm^-3.
+class Buoyancy
+	: public System,
+	  public ISystemConfigure,
+	  public ISystemPreUpdate,
+	  public ISystemPostUpdate
+{
+	/// \brief Constructor
+public: Buoyancy();
+
+	/// \brief Destructor
+public: ~Buoyancy() override = default;
+
+	// Documentation inherited
+public: void Configure(const Entity &_entity,
+			       const std::shared_ptr<const sdf::Element> &_sdf,
+			       EntityComponentManager &_ecm,
+			       EventManager &_eventMgr) override;
+
+	// Documentation inherited
+public: void PreUpdate(
+		const UpdateInfo &_info,
+		EntityComponentManager &_ecm) override;
+
+	// Documentation inherited
+public: void PostUpdate(
+		const UpdateInfo &_info,
+		const EntityComponentManager &_ecm) override;
+
+	/// \brief Check if an entity is enabled or not.
+	/// \param[in] _entity Target entity
+	/// \param[in] _ecm Entity component manager
+	/// \return True if buoyancy should be applied.
+public: bool IsEnabled(Entity _entity,
+			       const EntityComponentManager &_ecm) const;
+
+	/// \brief Private data pointer
+private: std::unique_ptr<BuoyancyPrivate> dataPtr;
+};
+}
+}
+}
+}
+
+#endif

src/modules/simulation/gz_plugins/buoyancy/CMakeLists.txt

@@ -0,0 +1,68 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+# Template for a new plugin project
+# Replace TemplatePlugin with your plugin name
+project(BuoyancySystemPlugin)
+
+# Add external dependencies if needed
+# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake)
+
+# Find required packages
+# find_package(PackageName REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED
+    # Add your source files here
+    BuoyancySystem.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+    PUBLIC px4_gz_msgs
+    PUBLIC gz-sensors${gz-sensors_VERSION_MAJOR}::gz-sensors${gz-sensors_VERSION_MAJOR}
+    PUBLIC gz-plugin${gz-plugin_VERSION_MAJOR}::gz-plugin${gz-plugin_VERSION_MAJOR}
+    PUBLIC gz-sim${gz-sim_VERSION_MAJOR}::gz-sim${gz-sim_VERSION_MAJOR}
+    PUBLIC gz-transport${gz-transport_VERSION_MAJOR}::gz-transport${gz-transport_VERSION_MAJOR}
+    # Add other dependencies as needed
+    # PUBLIC ${OtherLib_LIBS}
+)
+
+target_include_directories(${PROJECT_NAME}
+    PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
+    PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
+    PUBLIC px4_gz_msgs
+    # Add other include directories as needed
+    # PUBLIC ${OtherLib_INCLUDE_DIRS}
+)
+
+# Add dependencies if needed
+# add_dependencies(${PROJECT_NAME} ExternalDependency)

src/modules/simulation/gz_plugins/generic_motor/CMakeLists.txt

@@ -0,0 +1,68 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+# Template for a new plugin project
+# Replace TemplatePlugin with your plugin name
+project(GenericMotorModelPlugin)
+
+# Add external dependencies if needed
+# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake)
+
+# Find required packages
+# find_package(PackageName REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED
+    # Add your source files here
+    GenericMotorModel.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+    PUBLIC px4_gz_msgs
+    PUBLIC gz-sensors${gz-sensors_VERSION_MAJOR}::gz-sensors${gz-sensors_VERSION_MAJOR}
+    PUBLIC gz-plugin${gz-plugin_VERSION_MAJOR}::gz-plugin${gz-plugin_VERSION_MAJOR}
+    PUBLIC gz-sim${gz-sim_VERSION_MAJOR}::gz-sim${gz-sim_VERSION_MAJOR}
+    PUBLIC gz-transport${gz-transport_VERSION_MAJOR}::gz-transport${gz-transport_VERSION_MAJOR}
+    # Add other dependencies as needed
+    # PUBLIC ${OtherLib_LIBS}
+)
+
+target_include_directories(${PROJECT_NAME}
+    PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
+    PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
+    PUBLIC px4_gz_msgs
+    # Add other include directories as needed
+    # PUBLIC ${OtherLib_INCLUDE_DIRS}
+)
+
+# Add dependencies if needed
+# add_dependencies(${PROJECT_NAME} ExternalDependency)

src/modules/simulation/gz_plugins/generic_motor/GenericMotorModel.cpp

@@ -0,0 +1,931 @@
+/*
+ * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
+ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+ * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
+ * Copyright (C) 2019 Open Source Robotics Foundation
+ * Copyright (C) 2022 Benjamin Perseghetti, Rudis Laboratories
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include "GenericMotorModel.hpp"
+
+#include <mutex>
+
+#include <gz/msgs/actuators.pb.h>
+
+#include <gz/common/Profiler.hh>
+
+#include <gz/plugin/Register.hh>
+#include <gz/transport/Node.hh>
+
+#include <gz/math/Helpers.hh>
+#include <gz/math/Pose3.hh>
+#include <gz/math/Vector3.hh>
+#include <gz/msgs/Utility.hh>
+
+#include <sdf/sdf.hh>
+
+#include "gz/sim/components/Actuators.hh"
+#include "gz/sim/components/ExternalWorldWrenchCmd.hh"
+#include "gz/sim/components/JointAxis.hh"
+#include "gz/sim/components/JointVelocity.hh"
+#include "gz/sim/components/JointVelocityCmd.hh"
+#include "gz/sim/components/LinearVelocity.hh"
+#include "gz/sim/components/ParentLinkName.hh"
+#include "gz/sim/components/Pose.hh"
+#include "gz/sim/components/Wind.hh"
+#include "gz/sim/Link.hh"
+#include "gz/sim/Model.hh"
+#include "gz/sim/Util.hh"
+
+// from rotors_gazebo_plugins/include/rotors_gazebo_plugins/common.h
+/// \brief    This class can be used to apply a first order filter on a signal.
+///           It allows different acceleration and deceleration time constants.
+/// \details
+///           Short reveiw of discrete time implementation of first order system
+///           Laplace:
+///             X(s)/U(s) = 1/(tau*s + 1)
+///           continous time system:
+///             dx(t) = (-1/tau)*x(t) + (1/tau)*u(t)
+///           discretized system (ZoH):
+///             x(k+1) = exp(samplingTime*(-1/tau))*x(k) + (1 - exp(samplingTime*(-1/tau))) * u(k) // NOLINT
+template <typename T>
+class FirstOrderFilter
+{
+public:
+	FirstOrderFilter(double _timeConstantUp, double _timeConstantDown, T _initialState): // NOLINT
+		timeConstantUp(_timeConstantUp),
+		timeConstantDown(_timeConstantDown),
+		previousState(_initialState) {}
+
+	/// \brief    This method will apply a first order filter on the _inputState.
+	T UpdateFilter(T _inputState, double _samplingTime)
+	{
+		T outputState;
+
+		if (_inputState > previousState) {
+			// Calcuate the outputState if accelerating.
+			double alphaUp = exp(-_samplingTime / timeConstantUp);
+			// x(k+1) = Ad*x(k) + Bd*u(k)
+			outputState = alphaUp * previousState + (1 - alphaUp) * _inputState;
+
+		} else {
+			// Calculate the outputState if decelerating.
+			double alphaDown = exp(-_samplingTime / timeConstantDown);
+			outputState = alphaDown * previousState + (1 - alphaDown) * _inputState;
+		}
+
+		previousState = outputState;
+		return outputState;
+	}
+
+	~FirstOrderFilter() = default;
+
+protected:
+	double timeConstantUp;
+	double timeConstantDown;
+	T previousState;
+};
+
+using namespace gz;
+using namespace sim;
+using namespace systems;
+
+/// \brief Constants for specifying clockwise (kCw) and counter-clockwise (kCcw)
+/// directions of rotation.
+namespace turning_direction
+{
+static const int kCcw = 1;
+static const int kCw = -1;
+}  // namespace turning_direction
+
+/// \brief Type of input command to motor.
+enum class MotorType {
+	kVelocity,
+	kPosition,
+	kForce,
+	kForcePolynomial
+};
+
+/// \brief Type of control methology
+enum class ControlMethod {
+	kRPM,
+	kDutyCycle
+};
+
+class gz::sim::systems::GenericMotorModelPrivate
+{
+	/// \brief Callback for actuator commands.
+public: void OnActuatorMsg(const msgs::Actuators &_msg);
+
+	/// \brief Apply link forces and moments based on propeller state.
+public: void UpdateForcesAndMoments(EntityComponentManager &_ecm);
+
+	/// \brief Joint Entity
+public: Entity jointEntity;
+
+	/// \brief Joint name
+public: std::string jointName;
+
+	/// \brief Link Entity
+public: Entity linkEntity;
+
+	/// \brief Link name
+public: std::string linkName;
+
+	/// \brief Parent link Entity
+public: Entity parentLinkEntity;
+
+	/// \brief Parent link name
+public: std::string parentLinkName;
+
+	/// \brief Model interface
+public: Model model{kNullEntity};
+
+	/// \brief Topic for actuator commands.
+public: std::string commandSubTopic;
+
+	/// \brief Topic namespace.
+public: std::string robotNamespace;
+
+	/// \brief Sampling time (from motor_model.hpp).
+public: double samplingTime = 0.01;
+
+	/// \brief Index of motor in Actuators msg on multirotor_base.
+public: int actuatorNumber = 0;
+
+	/// \brief Turning direction of the motor.
+public: int turningDirection = turning_direction::kCw;
+
+	/// \brief Type of input command to motor.
+public: MotorType motorType = MotorType::kVelocity;
+
+	/// \brief Type of input command to motor.
+public: ControlMethod controlMethod = ControlMethod::kRPM;
+
+	/// \brief Maximum rotational velocity command with units of rad/s.
+	/// The default value is taken from gazebo_motor_model.h
+	/// and is approximately 8000 revolutions / minute (rpm).
+public: double maxRotVelocity = 838.0;
+
+	/// \brief Minimum rotor velocity, below which the rotor is considered
+	/// to be stopped. Simulates minimum velocity achieved by the motor
+public: double minCommand = 0.0;
+
+	/// \brief Maximum duty cycle value
+public: double maxDutyCycle = 0.0;
+
+	/// \brief Minimum duty cycle value
+public: double minDutyCycle = 0.0;
+
+	/// \brief Bidirectional-capable motor. Set to true if it mimics an ESC
+	/// with bidirectional control. Zero input will be middle of max and min
+	/// duty cycles.
+public: int bidirectionalMotor = 0;
+
+	/// \brief Moment constant for computing drag torque based on thrust
+	/// with units of length (m).
+	/// The default value is taken from gazebo_motor_model.h
+public: double momentConstant = 0.016;
+
+	/// \brief Thrust coefficient for propeller with units of N / (rad/s)^2.
+	/// The default value is taken from gazebo_motor_model.h
+public: double motorConstant = 8.54858e-06;
+
+	/// \brief Reference input to motor. For MotorType kVelocity, this
+	/// is the reference angular velocity in rad/s.
+public: double refMotorInput = 0.0;
+
+	/// \brief Rolling moment coefficient with units of N*m / (m/s^2).
+	/// The default value is taken from gazebo_motor_model.h
+public: double rollingMomentCoefficient = 1.0e-6;
+
+	/// \brief Rotor drag coefficient for propeller with units of N / (m/s^2).
+	/// The default value is taken from gazebo_motor_model.h
+public: double rotorDragCoefficient = 1.0e-4;
+
+	/// \brief Large joint velocities can cause problems with aliasing,
+	/// so the joint velocity used by the physics engine is reduced
+	/// this factor, while the larger value is used for computing
+	/// propeller thrust.
+	/// The default value is taken from gazebo_motor_model.h
+public: double rotorVelocitySlowdownSim = 10.0;
+
+	/// \brief Time constant for rotor deceleration.
+	/// The default value is taken from gazebo_motor_model.h
+public: double timeConstantDown = 1.0 / 40.0;
+
+	/// \brief Time constant for rotor acceleration.
+	/// The default value is taken from gazebo_motor_model.h
+public: double timeConstantUp = 1.0 / 80.0;
+
+	/// \brief Filter on rotor velocity that has different time constants
+	/// for increasing and decreasing values.
+public: std::unique_ptr<FirstOrderFilter<double>> rotorVelocityFilter;
+
+	/// \brief Received Actuators message. This is nullopt if no message has been
+	/// received.
+public: std::optional<msgs::Actuators> recvdActuatorsMsg;
+
+	/// \brief Mutex to protect recvdActuatorsMsg.
+public: std::mutex recvdActuatorsMsgMutex;
+
+	/// \brief Polynomial for RPM to Thrust conversion - admits 5 degrees with asymmetric behavior
+public: std::vector<double> positiveTorquePolynomial = {};
+public: std::vector<double> positiveThrustPolynomial = {};
+public: std::vector<double> negativeTorquePolynomial = {};
+public: std::vector<double> negativeThrustPolynomial = {};
+
+	/// \brief Gazebo communication node.
+public: transport::Node node;
+};
+
+//////////////////////////////////////////////////
+GenericMotorModel::GenericMotorModel()
+	: dataPtr(std::make_unique<GenericMotorModelPrivate>())
+{
+}
+
+//////////////////////////////////////////////////
+void GenericMotorModel::Configure(const Entity &_entity,
+				  const std::shared_ptr<const sdf::Element> &_sdf,
+				  EntityComponentManager &_ecm,
+				  EventManager &/*_eventMgr*/)
+{
+	this->dataPtr->model = Model(_entity);
+
+	if (!this->dataPtr->model.Valid(_ecm)) {
+		gzerr << "GenericMotorModel plugin should be attached to a model "
+		      << "entity. Failed to initialize." << std::endl;
+		return;
+	}
+
+	auto sdfClone = _sdf->Clone();
+
+	this->dataPtr->robotNamespace.clear();
+
+	if (sdfClone->HasElement("robotNamespace")) {
+		this->dataPtr->robotNamespace =
+			sdfClone->Get<std::string>("robotNamespace");
+
+	} else {
+		gzwarn << "No robotNamespace set using entity name.\n";
+		this->dataPtr->robotNamespace = this->dataPtr->model.Name(_ecm);
+	}
+
+	// Get params from SDF
+	if (sdfClone->HasElement("jointName")) {
+		this->dataPtr->jointName = sdfClone->Get<std::string>("jointName");
+	}
+
+	if (this->dataPtr->jointName.empty()) {
+		gzerr << "GenericMotorModel found an empty jointName parameter. "
+		      << "Failed to initialize.";
+		return;
+	}
+
+	if (sdfClone->HasElement("linkName")) {
+		this->dataPtr->linkName = sdfClone->Get<std::string>("linkName");
+	}
+
+	if (this->dataPtr->linkName.empty()) {
+		gzerr << "GenericMotorModel found an empty linkName parameter. "
+		      << "Failed to initialize.";
+		return;
+	}
+
+	if (sdfClone->HasElement("actuator_number")) {
+		this->dataPtr->actuatorNumber =
+			sdfClone->GetElement("actuator_number")->Get<int>();
+
+	} else if (sdfClone->HasElement("motorNumber")) {
+		this->dataPtr->actuatorNumber =
+			sdfClone->GetElement("motorNumber")->Get<int>();
+		gzwarn << "<motorNumber> is depricated, "
+		       << "please use <actuator_number>.\n";
+
+	} else {
+		gzerr << "Please specify a actuator_number.\n";
+	}
+
+	if (sdfClone->HasElement("turningDirection")) {
+		auto turningDirection =
+			sdfClone->GetElement("turningDirection")->Get<std::string>();
+
+		if (turningDirection == "cw") {
+			this->dataPtr->turningDirection = turning_direction::kCw;
+
+		} else if (turningDirection == "ccw") {
+			this->dataPtr->turningDirection = turning_direction::kCcw;
+
+		} else {
+			gzerr << "Please only use 'cw' or 'ccw' as turningDirection.\n";
+		}
+
+	} else {
+		gzerr << "Please specify a turning direction ('cw' or 'ccw').\n";
+	}
+
+	if (sdfClone->HasElement("controlMethod")) {
+		auto controlMethod =
+			sdfClone->GetElement("controlMethod")->Get<std::string>();
+
+		if (controlMethod == "rpm") {
+			this->dataPtr->controlMethod = ControlMethod::kRPM;
+
+		} else if (controlMethod == "duty_cycle") {
+			this->dataPtr->controlMethod = ControlMethod::kDutyCycle;
+
+		} else {
+			gzerr << "Please only use 'rpm' or 'duty_cycle' as controlMethod.\n";
+		}
+
+	} else {
+		this->dataPtr->controlMethod = ControlMethod::kRPM;
+	}
+
+	// Check required parameters for each control method type
+	if (this->dataPtr->controlMethod == ControlMethod::kDutyCycle) {
+		if (sdfClone->HasElement("minCommand")) {
+			this->dataPtr->minCommand =
+				sdfClone->GetElement("minCommand")->Get<double>();
+
+		} else {
+			gzerr << "Please specify a minCommand.\n";
+		}
+
+		if (sdfClone->HasElement("maxDutyCycle")) {
+			this->dataPtr->maxDutyCycle =
+				sdfClone->GetElement("maxDutyCycle")->Get<double>();
+
+		} else {
+			gzerr << "Please specify a maxDutyCycle.\n";
+		}
+
+		if (sdfClone->HasElement("minDutyCycle")) {
+			this->dataPtr->minDutyCycle =
+				sdfClone->GetElement("minDutyCycle")->Get<double>();
+
+		} else {
+			gzerr << "Please specify a minDutyCycle.\n";
+		}
+
+	}
+
+	if (sdfClone->HasElement("motorType")) {
+		auto motorType = sdfClone->GetElement("motorType")->Get<std::string>();
+
+		if (motorType == "velocity") {
+			this->dataPtr->motorType = MotorType::kVelocity;
+
+		} else if (motorType == "position") {
+			this->dataPtr->motorType = MotorType::kPosition;
+			gzerr << "motorType 'position' not implemented" << std::endl;
+
+		} else if (motorType == "force") {
+			this->dataPtr->motorType = MotorType::kForce;
+			gzerr << "motorType 'force' not implemented" << std::endl;
+
+		} else if (motorType == "force_polynomial") {
+			this->dataPtr->motorType = MotorType::kForcePolynomial;
+
+			// Check validity of required parameters
+			if (sdfClone->HasElement("positiveThrustPolynomial")) {
+				std::string raw_coeffs = sdfClone->GetElement("positiveThrustPolynomial")->Get<std::string>();
+				std::vector<double> coeffs = this->ParsePolynomial(raw_coeffs);
+
+				if (coeffs.size() < 4) {
+					gzerr << "positiveThrustPolynomial must have at least 4 elements, but got "
+					      << coeffs.size() << std::endl;
+				}
+
+				for (const auto &coeff : coeffs) {
+					this->dataPtr->positiveThrustPolynomial.push_back(coeff);
+				}
+
+			} else {
+				gzerr << "Please specify a positiveThrustPolynomial.\n";
+			}
+
+			if (sdfClone->HasElement("negativeThrustPolynomial")) {
+				std::string raw_coeffs = sdfClone->GetElement("negativeThrustPolynomial")->Get<std::string>();
+				std::vector<double> coeffs = this->ParsePolynomial(raw_coeffs);
+
+				if (coeffs.size() < 4) {
+					gzerr << "negativeThrustPolynomial must have at least 4 elements, but got "
+					      << coeffs.size() << std::endl;
+				}
+
+				for (const auto &coeff : coeffs) {
+					this->dataPtr->negativeThrustPolynomial.push_back(coeff);
+				}
+
+			} else {
+				this->dataPtr->negativeThrustPolynomial = this->dataPtr->positiveThrustPolynomial;
+				gzwarn << "Using positive thrust polynomial as negative thrust polynomial (symmetric performance)." << std::endl;
+			}
+
+			if (sdfClone->HasElement("positiveTorquePolynomial")) {
+				std::string raw_coeffs = sdfClone->GetElement("positiveTorquePolynomial")->Get<std::string>();
+				std::vector<double> coeffs = this->ParsePolynomial(raw_coeffs);
+
+				if (coeffs.size() < 4 || coeffs.size() > 6) {
+					gzerr << "positiveTorquePolynomial must have between 4 and 6 elements, but got "
+					      << coeffs.size() << std::endl;
+				}
+
+				for (const auto &coeff : coeffs) {
+					this->dataPtr->positiveTorquePolynomial.push_back(coeff);
+				}
+
+			} else {
+				gzdbg << "No positive torque polynomial. Using zero.\n" << std::endl;
+				this->dataPtr->positiveTorquePolynomial = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+			}
+
+			if (sdfClone->HasElement("negativeTorquePolynomial")) {
+				std::string raw_coeffs = sdfClone->GetElement("negativeTorquePolynomial")->Get<std::string>();
+				std::vector<double> coeffs = this->ParsePolynomial(raw_coeffs);
+
+				if (coeffs.size() < 4 || coeffs.size() > 6) {
+					gzerr << "negativeTorquePolynomial must have between 4 and 6 elements, but got "
+					      << coeffs.size() << std::endl;
+				}
+
+				for (const auto &coeff : coeffs) {
+					this->dataPtr->negativeTorquePolynomial.push_back(coeff);
+				}
+
+			} else {
+				this->dataPtr->negativeTorquePolynomial = this->dataPtr->positiveTorquePolynomial;
+				gzwarn << "Using positive torque polynomial as negative torque polynomial (symmetric performance)." << std::endl;
+			}
+
+			// print final polynomial values
+			gzdbg << "Added motor " << this->dataPtr->actuatorNumber << ". Polynomials:\n - "
+			      << "Positive thrust polynomial: ";
+
+			for (const auto &val : this->dataPtr->positiveThrustPolynomial) {
+				gzdbg << val << " ";
+			}
+
+			gzdbg << "\n - Negative thrust polynomial: ";
+
+			for (const auto &val : this->dataPtr->negativeThrustPolynomial) {
+				gzdbg << val << " ";
+			}
+
+			gzdbg << "\n - Positive torque polynomial: ";
+
+			for (const auto &val : this->dataPtr->positiveTorquePolynomial) {
+				gzdbg << val << " ";
+			}
+
+			gzdbg << "\n - Negative torque polynomial: ";
+
+			for (const auto &val : this->dataPtr->negativeTorquePolynomial) {
+				gzdbg << val << " ";
+			}
+
+			gzdbg << std::endl;
+
+		} else {
+			gzerr << "Please only use 'velocity', 'position', "
+			      "'force' or 'force_polynomial' as motorType.\n"
+			      << std::endl;
+		}
+
+	} else {
+		gzwarn << "motorType not specified, using velocity.\n";
+		this->dataPtr->motorType = MotorType::kVelocity;
+	}
+
+	sdfClone->Get<std::string>("commandSubTopic",
+				   this->dataPtr->commandSubTopic, this->dataPtr->commandSubTopic);
+
+	sdfClone->Get<double>("rotorDragCoefficient",
+			      this->dataPtr->rotorDragCoefficient,
+			      this->dataPtr->rotorDragCoefficient);
+	sdfClone->Get<double>("rollingMomentCoefficient",
+			      this->dataPtr->rollingMomentCoefficient,
+			      this->dataPtr->rollingMomentCoefficient);
+	sdfClone->Get<double>("maxRotVelocity",
+			      this->dataPtr->maxRotVelocity, this->dataPtr->maxRotVelocity);
+	sdfClone->Get<double>("motorConstant",
+			      this->dataPtr->motorConstant, this->dataPtr->motorConstant);
+	sdfClone->Get<double>("momentConstant",
+			      this->dataPtr->momentConstant, this->dataPtr->momentConstant);
+	sdfClone->Get<int>("bidirectionalMotor",
+			   this->dataPtr->bidirectionalMotor, this->dataPtr->bidirectionalMotor);
+
+	sdfClone->Get<double>("timeConstantUp",
+			      this->dataPtr->timeConstantUp, this->dataPtr->timeConstantUp);
+	sdfClone->Get<double>("timeConstantDown",
+			      this->dataPtr->timeConstantDown, this->dataPtr->timeConstantDown);
+	sdfClone->Get<double>("rotorVelocitySlowdownSim",
+			      this->dataPtr->rotorVelocitySlowdownSim, 10);
+
+	// Create the first order filter.
+	this->dataPtr->rotorVelocityFilter =
+		std::make_unique<FirstOrderFilter<double>>(
+			this->dataPtr->timeConstantUp, this->dataPtr->timeConstantDown,
+			this->dataPtr->refMotorInput);
+
+	// Subscribe to actuator command messages
+	std::string topic = transport::TopicUtils::AsValidTopic(
+				    this->dataPtr->robotNamespace + "/" + this->dataPtr->commandSubTopic);
+
+	if (topic.empty()) {
+		gzerr << "Failed to create topic for [" << this->dataPtr->robotNamespace
+		      << "]" << std::endl;
+		return;
+
+	} else {
+		gzdbg << "Listening to topic: " << topic << std::endl;
+	}
+
+	this->dataPtr->node.Subscribe(topic,
+				      &GenericMotorModelPrivate::OnActuatorMsg, this->dataPtr.get());
+}
+
+//////////////////////////////////////////////////
+void GenericMotorModel::PreUpdate(const UpdateInfo &_info,
+				  EntityComponentManager &_ecm)
+{
+	GZ_PROFILE("GenericMotorModel::PreUpdate");
+
+	// \TODO(anyone) Support rewind
+	if (_info.dt < std::chrono::steady_clock::duration::zero()) {
+		gzwarn << "Detected jump back in time ["
+		       << std::chrono::duration<double>(_info.dt).count()
+		       << "s]. System may not work properly." << std::endl;
+	}
+
+	// If the joint or links haven't been identified yet, look for them
+	if (this->dataPtr->jointEntity == kNullEntity) {
+		this->dataPtr->jointEntity =
+			this->dataPtr->model.JointByName(_ecm, this->dataPtr->jointName);
+
+		const auto parentLinkName = _ecm.Component<components::ParentLinkName>(
+						    this->dataPtr->jointEntity);
+
+		if (parentLinkName) {
+			this->dataPtr->parentLinkName = parentLinkName->Data();
+		}
+	}
+
+	if (this->dataPtr->linkEntity == kNullEntity) {
+		this->dataPtr->linkEntity =
+			this->dataPtr->model.LinkByName(_ecm, this->dataPtr->linkName);
+	}
+
+	if (this->dataPtr->parentLinkEntity == kNullEntity) {
+		this->dataPtr->parentLinkEntity =
+			this->dataPtr->model.LinkByName(_ecm, this->dataPtr->parentLinkName);
+	}
+
+	if (this->dataPtr->jointEntity == kNullEntity ||
+	    this->dataPtr->linkEntity == kNullEntity ||
+	    this->dataPtr->parentLinkEntity == kNullEntity) {
+		return;
+	}
+
+	// skip UpdateForcesAndMoments if needed components are missing
+	bool doUpdateForcesAndMoments = true;
+
+	const auto jointVelocity = _ecm.Component<components::JointVelocity>(
+					   this->dataPtr->jointEntity);
+
+	if (!jointVelocity) {
+		_ecm.CreateComponent(this->dataPtr->jointEntity,
+				     components::JointVelocity());
+		doUpdateForcesAndMoments = false;
+
+	} else if (jointVelocity->Data().empty()) {
+		doUpdateForcesAndMoments = false;
+	}
+
+	if (!_ecm.Component<components::JointVelocityCmd>(
+		    this->dataPtr->jointEntity)) {
+		_ecm.CreateComponent(this->dataPtr->jointEntity,
+				     components::JointVelocityCmd({0}));
+		doUpdateForcesAndMoments = false;
+	}
+
+	if (!_ecm.Component<components::WorldPose>(this->dataPtr->linkEntity)) {
+		_ecm.CreateComponent(this->dataPtr->linkEntity, components::WorldPose());
+		doUpdateForcesAndMoments = false;
+	}
+
+	if (!_ecm.Component<components::WorldLinearVelocity>(
+		    this->dataPtr->linkEntity)) {
+		_ecm.CreateComponent(this->dataPtr->linkEntity,
+				     components::WorldLinearVelocity());
+		doUpdateForcesAndMoments = false;
+	}
+
+	if (!_ecm.Component<components::WorldPose>(this->dataPtr->parentLinkEntity)) {
+		_ecm.CreateComponent(this->dataPtr->parentLinkEntity,
+				     components::WorldPose());
+		doUpdateForcesAndMoments = false;
+	}
+
+	// Nothing left to do if paused.
+	if (_info.paused) {
+		return;
+	}
+
+	this->dataPtr->samplingTime =
+		std::chrono::duration<double>(_info.dt).count();
+
+	if (doUpdateForcesAndMoments) {
+		this->dataPtr->UpdateForcesAndMoments(_ecm);
+	}
+}
+
+//////////////////////////////////////////////////
+void GenericMotorModelPrivate::OnActuatorMsg(
+	const msgs::Actuators &_msg)
+{
+	std::lock_guard<std::mutex> lock(this->recvdActuatorsMsgMutex);
+	this->recvdActuatorsMsg = _msg;
+}
+
+//////////////////////////////////////////////////
+void GenericMotorModelPrivate::UpdateForcesAndMoments(
+	EntityComponentManager &_ecm)
+{
+	GZ_PROFILE("GenericMotorModelPrivate::UpdateForcesAndMoments");
+
+	std::optional<msgs::Actuators> msg;
+	auto actuatorMsgComp =
+		_ecm.Component<components::Actuators>(this->model.Entity());
+
+	// Actuators messages can come in from transport or via a component. If a
+	// component is available, it takes precedence.
+	if (actuatorMsgComp) {
+		msg = actuatorMsgComp->Data();
+
+	} else {
+		std::lock_guard<std::mutex> lock(this->recvdActuatorsMsgMutex);
+
+		if (this->recvdActuatorsMsg.has_value()) {
+			msg = *this->recvdActuatorsMsg;
+			this->recvdActuatorsMsg.reset();
+		}
+	}
+
+	if (msg.has_value()) {
+		if (this->actuatorNumber > msg->velocity_size() - 1) {
+			gzerr << "You tried to access index " << this->actuatorNumber
+			      << " of the Actuator array which is of size "
+			      << std::max(msg->velocity_size(), msg->normalized_size()) << std::endl;
+			return;
+		}
+
+		if (this->motorType == MotorType::kVelocity) {
+			this->refMotorInput = std::min(
+						      static_cast<double>(msg->velocity(this->actuatorNumber)),
+						      static_cast<double>(this->maxRotVelocity));
+
+		} else if (this->motorType == MotorType::kForcePolynomial &&
+			   this->controlMethod == ControlMethod::kRPM) {
+			// Uses RPM as reference
+			this->refMotorInput = std::min(
+						      static_cast<double>(msg->velocity(this->actuatorNumber)),
+						      static_cast<double>(this->maxRotVelocity));
+
+		} else if (this->motorType == MotorType::kForcePolynomial &&
+			   this->controlMethod == ControlMethod::kDutyCycle &&
+			   msg->velocity(this->actuatorNumber) >= this->minDutyCycle) {
+			// Create normalized reference based on the duty cycle
+			this->refMotorInput = ((msg->velocity(this->actuatorNumber) - (this->minDutyCycle +
+						(this->maxDutyCycle - this->minDutyCycle) * this->bidirectionalMotor / 2.0)) / ((this->maxDutyCycle -
+								this->minDutyCycle) - (this->maxDutyCycle - this->minDutyCycle) * this->bidirectionalMotor / 2.0));
+
+		} else {
+			this->refMotorInput = msg->velocity(this->actuatorNumber);
+		}
+	}
+
+	switch (this->motorType) {
+	case (MotorType::kPosition): {
+			// TODO: Integrate
+			// double err = joint_->GetAngle(0).Radian() - this->refMotorInput;
+			// double force = pids_.Update(err, this->samplingTime);
+			// joint_->SetForce(0, force);
+			break;
+		}
+
+	case (MotorType::kForce): {
+			// TODO: Integrate
+			// joint_->SetForce(0, this->refMotorInput);
+			break;
+		}
+
+	case (MotorType::kForcePolynomial): {
+			sim::Link link(this->linkEntity);
+			const auto worldPose = link.WorldPose(_ecm);
+			using Vector3 = math::Vector3d;
+
+			// Compute thrust
+			double thrust = 0.0;
+
+			if (this->refMotorInput >= this->minCommand) {
+				for (unsigned int i = 0; i < this->positiveThrustPolynomial.size(); i++) {
+					thrust += this->positiveThrustPolynomial[i] * std::pow(this->refMotorInput, i);
+				}
+
+			} else if (this->refMotorInput <= -this->minCommand) {
+				for (unsigned int i = 0; i < this->negativeThrustPolynomial.size(); i++) {
+					thrust -= this->negativeThrustPolynomial[i] * std::pow(std::abs(this->refMotorInput), i);
+				}
+
+			} else {
+				thrust = 0.0;
+			}
+
+			link.AddWorldForce(_ecm, worldPose->Rot().RotateVector(Vector3(0, 0, thrust)));
+
+			// Compute torques
+			double torque = 0.0;
+
+			if (this->refMotorInput >= this->minCommand) {
+				for (unsigned int i = 0; i < this->positiveTorquePolynomial.size(); i++) {
+					torque += this->positiveTorquePolynomial[i] * std::pow(this->refMotorInput, i);
+				}
+
+			} else if (this->refMotorInput <= -this->minCommand) {
+				for (unsigned int i = 0; i < this->negativeTorquePolynomial.size(); i++) {
+					torque -= this->negativeTorquePolynomial[i] * std::pow(std::abs(this->refMotorInput), i);
+				}
+
+			} else {
+				torque = 0.0;
+			}
+
+			link.AddWorldForce(_ecm, worldPose->Rot().RotateVector(Vector3(0, 0, torque)));
+
+			// Set joint velocities
+			const auto jointVelCmd = _ecm.Component<components::JointVelocityCmd>(
+							 this->jointEntity);
+			*jointVelCmd = components::JointVelocityCmd({this->refMotorInput / this->rotorVelocitySlowdownSim});
+			break;
+		}
+
+	default: { // MotorType::kVelocity
+			const auto jointVelocity = _ecm.Component<components::JointVelocity>(
+							   this->jointEntity);
+			double motorRotVel = jointVelocity->Data()[0];
+
+			if (motorRotVel / (2 * GZ_PI) > 1 / (2 * this->samplingTime)) {
+				gzerr << "Aliasing on motor [" << this->actuatorNumber
+				      << "] might occur. Consider making smaller simulation time "
+				      "steps or raising the rotorVelocitySlowdownSim param.\n";
+			}
+
+			double realMotorVelocity =
+				motorRotVel * this->rotorVelocitySlowdownSim;
+			// Get the direction of the rotor rotation.
+			int realMotorVelocitySign =
+				(realMotorVelocity > 0) - (realMotorVelocity < 0);
+			// Assuming symmetric propellers (or rotors) for the thrust calculation.
+			double thrust = this->turningDirection * realMotorVelocitySign *
+					realMotorVelocity * realMotorVelocity *
+					this->motorConstant;
+
+			using Pose = math::Pose3d;
+			using Vector3 = math::Vector3d;
+
+			Link link(this->linkEntity);
+			const auto worldPose = link.WorldPose(_ecm);
+
+			// Apply a force to the link.
+			link.AddWorldForce(_ecm,
+					   worldPose->Rot().RotateVector(Vector3(0, 0, thrust)));
+
+			const auto jointPose = _ecm.Component<components::Pose>(
+						       this->jointEntity);
+
+			if (!jointPose) {
+				gzerr << "joint " << this->jointName << " has no Pose"
+				      << "component" << std::endl;
+				return;
+			}
+
+			// computer joint world pose by multiplying child link WorldPose
+			// with joint Pose
+			Pose jointWorldPose = *worldPose * jointPose->Data();
+
+			const auto jointAxisComp = _ecm.Component<components::JointAxis>(
+							   this->jointEntity);
+
+			if (!jointAxisComp) {
+				gzerr << "joint " << this->jointName << " has no JointAxis"
+				      << "component" << std::endl;
+				return;
+			}
+
+			const auto worldLinearVel = link.WorldLinearVelocity(_ecm);
+
+			Entity windEntity = _ecm.EntityByComponents(components::Wind());
+			auto windLinearVel =
+				_ecm.Component<components::WorldLinearVelocity>(windEntity);
+			Vector3 windSpeedWorld = windLinearVel->Data();
+
+			// Forces from Philppe Martin's and Erwan Salaun's
+			// 2010 IEEE Conference on Robotics and Automation paper
+			// The True Role of Accelerometer Feedback in Quadrotor Control
+			// - \omega * \lambda_1 * V_A^{\perp}
+			Vector3 jointAxis =
+				jointWorldPose.Rot().RotateVector(jointAxisComp->Data().Xyz());
+			Vector3 bodyVelocityWorld = *worldLinearVel;
+			Vector3 relativeWindVelocityWorld = bodyVelocityWorld - windSpeedWorld;
+			Vector3 bodyVelocityPerpendicular =
+				relativeWindVelocityWorld -
+				(relativeWindVelocityWorld.Dot(jointAxis) * jointAxis);
+			Vector3 airDrag = -std::abs(realMotorVelocity) *
+					  this->rotorDragCoefficient *
+					  bodyVelocityPerpendicular;
+
+			// Apply air drag to link.
+			link.AddWorldForce(_ecm, airDrag);
+			// Moments get the parent link, such that the resulting torques can be
+			// applied.
+			Vector3 parentWorldTorque;
+			auto parentWrenchComp =
+				_ecm.Component<components::ExternalWorldWrenchCmd>(
+					this->parentLinkEntity);
+			// gazebo_motor_model.cpp subtracts the GetWorldCoGPose() of the
+			// child link from the parent but only uses the rotation component.
+			// Since GetWorldCoGPose() uses the link frame orientation, it
+			// is equivalent to use WorldPose().Rot().
+			Link parentLink(this->parentLinkEntity);
+			const auto parentWorldPose = parentLink.WorldPose(_ecm);
+			// The transformation from the parent_link to the link_.
+			// Pose poseDifference =
+			//  parent_links.at(0)->GetWorldCoGPose().Inverse()
+			//  * link_->GetWorldCoGPose()
+			Pose poseDifference = (*parentWorldPose).Inverse() * (*worldPose);
+			Vector3 dragTorque(
+				0, 0, -this->turningDirection * thrust * this->momentConstant);
+			// Transforming the drag torque into the parent frame to handle
+			// arbitrary rotor orientations.
+			Vector3 dragTorqueParentFrame =
+				poseDifference.Rot().RotateVector(dragTorque);
+			parentWorldTorque =
+				parentWorldPose->Rot().RotateVector(dragTorqueParentFrame);
+
+			Vector3 rollingMoment;
+			// - \omega * \mu_1 * V_A^{\perp}
+			rollingMoment = -std::abs(realMotorVelocity) *
+					this->rollingMomentCoefficient *
+					bodyVelocityPerpendicular;
+			parentWorldTorque += rollingMoment;
+
+			if (!parentWrenchComp) {
+				components::ExternalWorldWrenchCmd wrench;
+				msgs::Set(wrench.Data().mutable_torque(), parentWorldTorque);
+				_ecm.CreateComponent(this->parentLinkEntity, wrench);
+
+			} else {
+				msgs::Set(parentWrenchComp->Data().mutable_torque(),
+					  msgs::Convert(parentWrenchComp->Data().torque()) + parentWorldTorque);
+			}
+
+			// Apply the filter on the motor's velocity.
+			double refMotorRotVel;
+			refMotorRotVel = this->rotorVelocityFilter->UpdateFilter(
+						 this->refMotorInput, this->samplingTime);
+
+			const auto jointVelCmd = _ecm.Component<components::JointVelocityCmd>(
+							 this->jointEntity);
+			*jointVelCmd = components::JointVelocityCmd({
+				this->turningDirection *refMotorRotVel
+				/ this->rotorVelocitySlowdownSim});
+		}
+	}
+}
+
+GZ_ADD_PLUGIN(GenericMotorModel,
+	      System,
+	      GenericMotorModel::ISystemConfigure,
+	      GenericMotorModel::ISystemPreUpdate)
+
+GZ_ADD_PLUGIN_ALIAS(GenericMotorModel,
+		    "gz::sim::systems::GenericMotorModel")

src/modules/simulation/gz_plugins/generic_motor/GenericMotorModel.hpp

@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2019 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef GZ_SIM_SYSTEMS_GENERICMOTORMODEL_HPP_
+#define GZ_SIM_SYSTEMS_GENERICMOTORMODEL_HPP_
+
+#include <gz/sim/System.hh>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE
+{
+namespace systems
+{
+// Forward declaration
+class GenericMotorModelPrivate;
+
+/// \brief This system applies a thrust force to models with spinning
+/// propellers. See examples/worlds/quadcopter.sdf for a demonstration.
+class GenericMotorModel
+	: public System,
+	  public ISystemConfigure,
+	  public ISystemPreUpdate
+{
+	/// \brief Constructor
+public: GenericMotorModel();
+
+	/// \brief Destructor
+public: ~GenericMotorModel() override = default;
+
+	// Documentation inherited
+public: void Configure(const Entity &_entity,
+			       const std::shared_ptr<const sdf::Element> &_sdf,
+			       EntityComponentManager &_ecm,
+			       EventManager &_eventMgr) override;
+
+	// Documentation inherited
+public: void PreUpdate(
+		const gz::sim::UpdateInfo &_info,
+		gz::sim::EntityComponentManager &_ecm) override;
+
+	/// \brief Private data pointer
+private: std::unique_ptr<GenericMotorModelPrivate> dataPtr;
+private:
+	std::vector<double> ParsePolynomial(const std::string &input)
+	{
+		std::vector<double> result;
+		std::string trimmed = input;
+
+		// Optional: remove brackets
+		trimmed.erase(std::remove(trimmed.begin(), trimmed.end(), '['), trimmed.end());
+		trimmed.erase(std::remove(trimmed.begin(), trimmed.end(), ']'), trimmed.end());
+
+		std::stringstream ss(trimmed);
+		std::string token;
+
+		while (std::getline(ss, token, ',')) {
+			try {
+				result.push_back(std::stod(token));
+
+			} catch (const std::invalid_argument &e) {
+				gzerr << "[YourPlugin] Invalid number: " << token << std::endl;
+			}
+		}
+
+		return result;
+	}
+
+};
+}
+}
+}
+}
+
+#endif

src/modules/uuv_att_control/uuv_att_control.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,9 +38,12 @@
  *
  * All the acknowledgments and credits for the fw wing/rover app are reported in those files.
  *
+ * 2025: refactoring of the mode settings: attitude, rate and manual control now working.
+ *
  * @author Daniel Duecker <daniel.duecker@tuhh.de>
  * @author Philipp Hastedt <philipp.hastedt@tuhh.de>
  * @author Tim Hansen <t.hansen@tuhh.de>
+ * @author Pedro Roque <padr@kth.se>
  */
 
 #include "uuv_att_control.hpp"
@@ -94,7 +97,7 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
 		float thrust_x, float thrust_y, float thrust_z)
 {
 	if (PX4_ISFINITE(roll_u)) {
-		roll_u = math::constrain(roll_u, -1.0f, 1.0f);
+		roll_u = math::constrain(roll_u, -_param_torque_sat.get(), _param_torque_sat.get());
 		_vehicle_torque_setpoint.xyz[0] = roll_u;
 
 	} else {
@@ -102,7 +105,7 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
 	}
 
 	if (PX4_ISFINITE(pitch_u)) {
-		pitch_u = math::constrain(pitch_u, -1.0f, 1.0f);
+		pitch_u = math::constrain(pitch_u, -_param_torque_sat.get(), _param_torque_sat.get());
 		_vehicle_torque_setpoint.xyz[1] = pitch_u;
 
 	} else {
@@ -110,7 +113,7 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
 	}
 
 	if (PX4_ISFINITE(yaw_u)) {
-		yaw_u = math::constrain(yaw_u, -1.0f, 1.0f);
+		yaw_u = math::constrain(yaw_u, -_param_torque_sat.get(), _param_torque_sat.get());
 		_vehicle_torque_setpoint.xyz[2] = yaw_u;
 
 	} else {
@@ -118,17 +121,33 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
 	}
 
 	if (PX4_ISFINITE(thrust_x)) {
-		thrust_x = math::constrain(thrust_x, -1.0f, 1.0f);
+		thrust_x = math::constrain(thrust_x, -_param_thrust_sat.get(), _param_thrust_sat.get());
 		_vehicle_thrust_setpoint.xyz[0] = thrust_x;
 
 	} else {
 		_vehicle_thrust_setpoint.xyz[0] = 0.0f;
 	}
+
+	if (PX4_ISFINITE(thrust_y)) {
+		thrust_y = math::constrain(thrust_y, -_param_thrust_sat.get(), _param_thrust_sat.get());
+		_vehicle_thrust_setpoint.xyz[1] = thrust_y;
+
+	} else {
+		_vehicle_thrust_setpoint.xyz[1] = 0.0f;
+	}
+
+	if (PX4_ISFINITE(thrust_z)) {
+		thrust_z = math::constrain(thrust_z, -_param_thrust_sat.get(), _param_thrust_sat.get());
+		_vehicle_thrust_setpoint.xyz[2] = thrust_z;
+
+	} else {
+		_vehicle_thrust_setpoint.xyz[2] = 0.0f;
+	}
 }
 
 void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude,
 		const vehicle_attitude_setpoint_s &attitude_setpoint, const vehicle_angular_velocity_s &angular_velocity,
-		const vehicle_rates_setpoint_s &rates_setpoint)
+		const vehicle_rates_setpoint_s &rates_setpoint, bool attitude_control_enabled)
 {
 	/** Geometric Controller
 	 *
@@ -156,6 +175,7 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude
 
 	Vector3f e_R_vec;
 	Vector3f torques;
+	torques.setZero();
 
 	/* Compute matrix: attitude error */
 	Matrix3f e_R = (rot_des.transpose() * rot_att - rot_att.transpose() * rot_des) * 0.5;
@@ -171,29 +191,109 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude
 	omega(2) -= yaw_rate_desired;
 
 	/**< P-Control */
-	torques(0) = - e_R_vec(0) * _param_roll_p.get();	/**< Roll  */
-	torques(1) = - e_R_vec(1) * _param_pitch_p.get();	/**< Pitch */
-	torques(2) = - e_R_vec(2) * _param_yaw_p.get();		/**< Yaw   */
+	if (attitude_control_enabled) {
+		torques(0) = - e_R_vec(0) * _param_roll_p.get();	/**< Roll  */
+		torques(1) = - e_R_vec(1) * _param_pitch_p.get();	/**< Pitch */
+		torques(2) = - e_R_vec(2) * _param_yaw_p.get();		/**< Yaw   */
 
-	/**< PD-Control */
-	torques(0) = torques(0) - omega(0) * _param_roll_d.get();  /**< Roll  */
-	torques(1) = torques(1) - omega(1) * _param_pitch_d.get(); /**< Pitch */
-	torques(2) = torques(2) - omega(2) * _param_yaw_d.get();   /**< Yaw   */
+		// take thrust from attitude message
+		float thrust_x = attitude_setpoint.thrust_body[0];
+		float thrust_y = attitude_setpoint.thrust_body[1];
+		float thrust_z = attitude_setpoint.thrust_body[2];
 
-	float roll_u = torques(0);
-	float pitch_u = torques(1);
-	float yaw_u = torques(2);
+		/**< PD-Control */
+		torques(0) -= omega(0) * _param_roll_d.get();  /**< Roll  */
+		torques(1) -= omega(1) * _param_pitch_d.get(); /**< Pitch */
+		torques(2) -= omega(2) * _param_yaw_d.get();   /**< Yaw   */
 
-	// take thrust as
-	float thrust_x = attitude_setpoint.thrust_body[0];
-	float thrust_y = attitude_setpoint.thrust_body[1];
-	float thrust_z = attitude_setpoint.thrust_body[2];
+		float roll_u = torques(0);
+		float pitch_u = torques(1);
+		float yaw_u = torques(2);
 
 
-	constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z);
+		constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z);
+
+	} else {
+		// take thrust from rates message
+		float thrust_x = _rates_setpoint.thrust_body[0];
+		float thrust_y = _rates_setpoint.thrust_body[1];
+		float thrust_z = _rates_setpoint.thrust_body[2];
+
+		/**< PD-Control */
+		torques(0) -= omega(0) * _param_roll_d.get();  /**< Roll  */
+		torques(1) -= omega(1) * _param_pitch_d.get(); /**< Pitch */
+		torques(2) -= omega(2) * _param_yaw_d.get();   /**< Yaw   */
+
+		float roll_u = torques(0);
+		float pitch_u = torques(1);
+		float yaw_u = torques(2);
+
+
+		constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z);
+	}
+
 	/* Geometric Controller END*/
 }
 
+void UUVAttitudeControl::generate_attitude_setpoint(float dt)
+{
+	// Avoid accumulating absolute yaw error with arming stick gesture
+	float roll = Eulerf(matrix::Quatf(_attitude_setpoint.q_d)).phi();
+	float pitch = Eulerf(matrix::Quatf(_attitude_setpoint.q_d)).theta();
+	float yaw = Eulerf(matrix::Quatf(_attitude_setpoint.q_d)).psi();
+
+	// Integrate manual control inputs
+	float yaw_setpoint = yaw + _manual_control_setpoint.yaw * dt * _param_sgm_yaw.get();
+	float roll_setpoint = roll + _manual_control_setpoint.roll * dt * _param_sgm_roll.get();
+	float pitch_setpoint = pitch + -_manual_control_setpoint.pitch * dt * _param_sgm_pitch.get();
+
+	// Generate target quaternion
+	Eulerf euler_sp(roll_setpoint, pitch_setpoint, yaw_setpoint);
+	Quatf q_sp = euler_sp;
+
+	// Normalize the quaternion to avoid numerical issues
+	q_sp.normalize();
+
+	q_sp.copyTo(_attitude_setpoint.q_d);
+
+	_attitude_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * _param_sgm_thrtl.get();
+
+	_attitude_setpoint.timestamp = hrt_absolute_time();
+}
+
+void UUVAttitudeControl::generate_rates_setpoint(float dt)
+{
+	// Integrate manual control inputs
+	_rates_setpoint.roll = _manual_control_setpoint.roll * dt * _param_rgm_roll.get();
+	_rates_setpoint.pitch = -_manual_control_setpoint.pitch * dt * _param_rgm_pitch.get();
+	_rates_setpoint.yaw = _manual_control_setpoint.yaw * dt * _param_rgm_yaw.get();
+
+	_rates_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * _param_rgm_thrtl.get();
+	_rates_setpoint.timestamp = hrt_absolute_time();
+
+}
+
+void UUVAttitudeControl::check_setpoint_validity(vehicle_attitude_s &v_att)
+{
+	const float _setpoint_age = (hrt_absolute_time() - _attitude_setpoint.timestamp) * 1e-6f;
+
+	if (_setpoint_age < 0.0f || _setpoint_age > _param_setpoint_max_age.get()) {
+		reset_attitude_setpoint(v_att);
+	}
+}
+
+void UUVAttitudeControl::reset_attitude_setpoint(vehicle_attitude_s &v_att)
+{
+	_attitude_setpoint.timestamp = hrt_absolute_time();
+	_attitude_setpoint.q_d[0] = v_att.q[0];
+	_attitude_setpoint.q_d[1] = v_att.q[1];
+	_attitude_setpoint.q_d[2] = v_att.q[2];
+	_attitude_setpoint.q_d[3] = v_att.q[3];
+	_attitude_setpoint.thrust_body[0] = 0.f;
+	_attitude_setpoint.thrust_body[1] = 0.f;
+	_attitude_setpoint.thrust_body[2] = 0.f;
+}
+
 void UUVAttitudeControl::Run()
 {
 	if (should_exit()) {
@@ -214,65 +314,82 @@ void UUVAttitudeControl::Run()
 
 	/* only run controller if attitude changed */
 	if (_vehicle_attitude_sub.update(&attitude)) {
+		const float dt = math::constrain(((attitude.timestamp_sample - _last_run) * 1e-6f), 0.0002f, 0.02f);
+		_last_run = attitude.timestamp_sample;
+
 		vehicle_angular_velocity_s angular_velocity {};
 		_angular_velocity_sub.copy(&angular_velocity);
 
-		/* Run geometric attitude controllers if NOT manual mode*/
-		if (!_vcontrol_mode.flag_control_manual_enabled
-		    && _vcontrol_mode.flag_control_attitude_enabled
-		    && _vcontrol_mode.flag_control_rates_enabled) {
+		/* Check that we are not in position / velocity / altitude modes
+		   and that we are using manual inputs */
+		if (_vcontrol_mode.flag_control_manual_enabled
+		    && !_vcontrol_mode.flag_control_position_enabled
+		    && !_vcontrol_mode.flag_control_velocity_enabled
+		    && !_vcontrol_mode.flag_control_altitude_enabled) {
 
-			int input_mode = _param_input_mode.get();
+			/* Update manual setpoints */
+			_manual_control_setpoint_sub.update(&_manual_control_setpoint);
 
-			_vehicle_attitude_setpoint_sub.update(&_attitude_setpoint);
-			_vehicle_rates_setpoint_sub.update(&_rates_setpoint);
+			if (_vcontrol_mode.flag_control_attitude_enabled
+			    && _vcontrol_mode.flag_control_rates_enabled) {
+				/* Run stabilized mode */
+				_vehicle_rates_setpoint_sub.update(&_rates_setpoint);
 
-			if (input_mode == 1) { // process manual data
-				Quatf attitude_setpoint(Eulerf(_param_direct_roll.get(), _param_direct_pitch.get(), _param_direct_yaw.get()));
-				attitude_setpoint.copyTo(_attitude_setpoint.q_d);
-				_attitude_setpoint.thrust_body[0] = _param_direct_thrust.get();
-				_attitude_setpoint.thrust_body[1] = 0.f;
-				_attitude_setpoint.thrust_body[2] = 0.f;
+				// Check setpoint validty
+				check_setpoint_validity(attitude);
+
+				/* Generate atttiude setpoint from sticks */
+				generate_attitude_setpoint(dt);
+
+				control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint, true);
+
+			} else if (!_vcontrol_mode.flag_control_attitude_enabled
+				   && _vcontrol_mode.flag_control_rates_enabled) {
+				/* Run Rate mode */
+				generate_rates_setpoint(dt);
+
+				control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint, false);
+
+			} else if (!_vcontrol_mode.flag_control_attitude_enabled
+				   && !_vcontrol_mode.flag_control_rates_enabled) {
+				/* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */
+				constrain_actuator_commands(_manual_control_setpoint.roll * _param_mgm_roll.get(),
+							    -_manual_control_setpoint.pitch * _param_mgm_pitch.get(),
+							    _manual_control_setpoint.yaw * _param_mgm_yaw.get(),
+							    _manual_control_setpoint.throttle * _param_mgm_thrtl.get(),
+							    0.f,
+							    0.f);
 			}
 
-			/* Geometric Control*/
-			int skip_controller = _param_skip_ctrl.get();
-
-			if (skip_controller) {
-				constrain_actuator_commands(_rates_setpoint.roll, _rates_setpoint.pitch, _rates_setpoint.yaw,
-							    _rates_setpoint.thrust_body[0], _rates_setpoint.thrust_body[1], _rates_setpoint.thrust_body[2]);
+		} else {
+			if (_vcontrol_mode.flag_control_attitude_enabled) {
+				/* Get attitude and rate setpoints and control system */
+				_vehicle_attitude_setpoint_sub.update(&_attitude_setpoint);
+				_vehicle_rates_setpoint_sub.update(&_rates_setpoint);
+				control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint, true);
 
 			} else {
-				control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint);
+				/* Get rate setpoints and control system */
+				_vehicle_rates_setpoint_sub.update(&_rates_setpoint);
+				control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint, false);
 			}
 		}
-	}
 
-	/* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */
-	if (_manual_control_setpoint_sub.update(&_manual_control_setpoint)) {
-		// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
-		// returning immediately and this loop will eat up resources.
-		if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) {
-			/* manual/direct control */
-			constrain_actuator_commands(_manual_control_setpoint.roll, -_manual_control_setpoint.pitch,
-						    _manual_control_setpoint.yaw,
-						    _manual_control_setpoint.throttle, 0.f, 0.f);
+		/* Only publish if any of the proper modes are enabled */
+		if (_vcontrol_mode.flag_control_manual_enabled ||
+		    _vcontrol_mode.flag_control_rates_enabled ||
+		    _vcontrol_mode.flag_control_attitude_enabled) {
+
+			_vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
+			_vehicle_thrust_setpoint.timestamp_sample = 0.f;
+			_vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint);
+
+			_vehicle_torque_setpoint.timestamp = hrt_absolute_time();
+			_vehicle_torque_setpoint.timestamp_sample = 0.f;
+			_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
 		}
 	}
 
-	/* Only publish if any of the proper modes are enabled */
-	if (_vcontrol_mode.flag_control_manual_enabled ||
-	    _vcontrol_mode.flag_control_attitude_enabled) {
-
-		_vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
-		_vehicle_thrust_setpoint.timestamp_sample = 0.f;
-		_vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint);
-
-		_vehicle_torque_setpoint.timestamp = hrt_absolute_time();
-		_vehicle_torque_setpoint.timestamp_sample = 0.f;
-		_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
-	}
-
 	perf_end(_loop_perf);
 }
 

src/modules/uuv_att_control/uuv_att_control.hpp

@@ -75,7 +75,10 @@ using matrix::Eulerf;
 using matrix::Quatf;
 using matrix::Matrix3f;
 using matrix::Vector3f;
+using matrix::Vector2f;
 using matrix::Dcmf;
+using matrix::AxisAnglef;
+using matrix::AxisAngle;
 
 using uORB::SubscriptionData;
 
@@ -122,6 +125,7 @@ private:
 	vehicle_control_mode_s _vcontrol_mode{};
 
 	perf_counter_t	_loop_perf;
+	hrt_abstime _last_run{0};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::UUV_ROLL_P>) _param_roll_p,
@@ -130,14 +134,23 @@ private:
 		(ParamFloat<px4::params::UUV_PITCH_D>) _param_pitch_d,
 		(ParamFloat<px4::params::UUV_YAW_P>) _param_yaw_p,
 		(ParamFloat<px4::params::UUV_YAW_D>) _param_yaw_d,
-		// control/input modes
-		(ParamInt<px4::params::UUV_INPUT_MODE>) _param_input_mode,
-		(ParamInt<px4::params::UUV_SKIP_CTRL>) _param_skip_ctrl,
-		// direct access to inputs
-		(ParamFloat<px4::params::UUV_DIRCT_ROLL>) _param_direct_roll,
-		(ParamFloat<px4::params::UUV_DIRCT_PITCH>) _param_direct_pitch,
-		(ParamFloat<px4::params::UUV_DIRCT_YAW>) _param_direct_yaw,
-		(ParamFloat<px4::params::UUV_DIRCT_THRUST>) _param_direct_thrust
+		// gains for the different modes
+		(ParamFloat<px4::params::UUV_SGM_ROLL>) _param_sgm_roll,
+		(ParamFloat<px4::params::UUV_SGM_PITCH>) _param_sgm_pitch,
+		(ParamFloat<px4::params::UUV_SGM_YAW>) _param_sgm_yaw,
+		(ParamFloat<px4::params::UUV_SGM_THRTL>) _param_sgm_thrtl,
+		(ParamFloat<px4::params::UUV_RGM_ROLL>) _param_rgm_roll,
+		(ParamFloat<px4::params::UUV_RGM_PITCH>) _param_rgm_pitch,
+		(ParamFloat<px4::params::UUV_RGM_YAW>) _param_rgm_yaw,
+		(ParamFloat<px4::params::UUV_RGM_THRTL>) _param_rgm_thrtl,
+		(ParamFloat<px4::params::UUV_MGM_ROLL>) _param_mgm_roll,
+		(ParamFloat<px4::params::UUV_MGM_PITCH>) _param_mgm_pitch,
+		(ParamFloat<px4::params::UUV_MGM_YAW>) _param_mgm_yaw,
+		(ParamFloat<px4::params::UUV_MGM_THRTL>) _param_mgm_thrtl,
+		(ParamFloat<px4::params::UUV_TORQUE_SAT>) _param_torque_sat,
+		(ParamFloat<px4::params::UUV_THRUST_SAT>) _param_thrust_sat,
+		(ParamFloat<px4::params::UUV_SP_MAX_AGE>) _param_setpoint_max_age
+
 	)
 
 	void Run() override;
@@ -150,7 +163,12 @@ private:
 	 * Control Attitude
 	 */
 	void control_attitude_geo(const vehicle_attitude_s &attitude, const vehicle_attitude_setpoint_s &attitude_setpoint,
-				  const vehicle_angular_velocity_s &angular_velocity, const vehicle_rates_setpoint_s &rates_setpoint);
+				  const vehicle_angular_velocity_s &angular_velocity, const vehicle_rates_setpoint_s &rates_setpoint,
+				  bool attitude_control_enabled);
 	void constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u,
 					 float thrust_x, float thrust_y, float thrust_z);
+	void generate_attitude_setpoint(float dt);
+	void generate_rates_setpoint(float dt);
+	void reset_attitude_setpoint(vehicle_attitude_s &v_att);
+	void check_setpoint_validity(vehicle_attitude_s &v_att);
 };

src/modules/uuv_att_control/uuv_att_control_params.c

@@ -42,6 +42,7 @@
  * All the ackowledgments and credits for the fw wing/rover app are reported in those files.
  *
  * @author Daniel Duecker <daniel.duecker@tuhh.de>
+ * @author Pedro Roque <padr@kth.se>
  */
 
 /*
@@ -103,48 +104,138 @@ PARAM_DEFINE_FLOAT(UUV_YAW_P, 4.0f);
 PARAM_DEFINE_FLOAT(UUV_YAW_D, 2.0f);
 
 
-// Input Modes
+// Gains for Manual Inputs in different Modes
 /**
- * Select Input Mode
+ * Roll gain for manual inputs in attitude control mode
  *
- * @value 0 use Attitude Setpoints
- * @value 1 Direct Feedthrough
  * @group UUV Attitude Control
+ * @min 0.0
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(UUV_INPUT_MODE, 0);
+PARAM_DEFINE_FLOAT(UUV_SGM_ROLL, 0.5f);
 
 /**
- * Skip the controller
+ * Pitch gain for manual inputs in attitude control mode
  *
- * @value 0 use the module's controller
- * @value 1 skip the controller and feedthrough the setpoints
+ * @group UUV Attitude Control
+ * @min 0.0
+ * @decimal 2
  */
-PARAM_DEFINE_INT32(UUV_SKIP_CTRL, 0);
+PARAM_DEFINE_FLOAT(UUV_SGM_PITCH, 0.5f);
 
 /**
- * Direct roll input
+ * Yaw gain for manual inputs in attitude control mode
+ *
+ * @group UUV Attitude Control
+ * @min 0.0
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_SGM_YAW, 0.5f);
+
+/**
+ * Throttle gain for manual inputs in attitude control mode
+ *
+ * @group UUV Attitude Control
+ * @min 0.0
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_SGM_THRTL, 0.1f);
+
+/**
+ * Roll gain for manual inputs in rate control mode
+ *
+ * @group UUV Attitude Control
+ * @min 0.0
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_RGM_ROLL, 100.0f);
+
+/**
+ * Pitch gain for manual inputs in rate control mode
+ *
+ * @group UUV Attitude Control
+ * @min 0.0
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_RGM_PITCH, 100.0f);
+
+/**
+ * Yaw gain for manual inputs in rate control mode
+ *
+ * @group UUV Attitude Control
+ * @min 0.0
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_RGM_YAW, 100.0f);
+
+/**
+ * Throttle gain for manual inputs in rate control mode
+ *
+ * @group UUV Attitude Control
+ * @min 0.0
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_RGM_THRTL, 10.0f);
+
+/**
+ * Roll gain for manual inputs in manual control mode
+ *
+ * @group UUV Attitude Control
+ * @min 0.0
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_MGM_ROLL, 0.05f);
+
+/**
+ * Pitch gain for manual inputs in manual control mode
+ *
+ * @group UUV Attitude Control
+ * @min 0.0
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_MGM_PITCH, 0.05f);
+
+/**
+ * Yaw gain for manual inputs in manual control mode
+ *
+ * @group UUV Attitude Control
+ * @min 0.0
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_MGM_YAW, 0.05f);
+
+/**
+ * Throttle gain for manual inputs in manual control mode
+ *
+ * @group UUV Attitude Control
+ * @min 0.0
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_MGM_THRTL, 0.1f);
+
+/**
+ * UUV Torque setpoint Saturation
+ *
+ * @group UUV Attitude Control
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_TORQUE_SAT, 0.3f);
+
+/**
+ * UUV Thrust setpoint Saturation
+ *
+ * @group UUV Attitude Control
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_THRUST_SAT, 0.1f);
+
+/**
+ * Maximum time (in seconds) before resetting setpoint
  *
  * @group UUV Attitude Control
  */
-PARAM_DEFINE_FLOAT(UUV_DIRCT_ROLL, 0.0f);
-
-/**
- * Direct pitch input
- *
- * @group UUV Attitude Control
- */
-PARAM_DEFINE_FLOAT(UUV_DIRCT_PITCH, 0.0f);
-
-/**
- * Direct yaw input
- *
- * @group UUV Attitude Control
- */
-PARAM_DEFINE_FLOAT(UUV_DIRCT_YAW, 0.0f);
-
-/**
- * Direct thrust input
- *
- * @group UUV Attitude Control
- */
-PARAM_DEFINE_FLOAT(UUV_DIRCT_THRUST, 0.0f);
+PARAM_DEFINE_FLOAT(UUV_SP_MAX_AGE, 2.0f);

src/modules/uuv_pos_control/uuv_pos_control.cpp

@@ -90,61 +90,172 @@ void UUVPOSControl::parameters_update(bool force)
 	}
 }
 
-void UUVPOSControl::publish_attitude_setpoint(const float thrust_x, const float thrust_y, const float thrust_z,
-		const float roll_des, const float pitch_des, const float yaw_des)
-{
-	//watch if inputs are not to high
-	vehicle_attitude_setpoint_s vehicle_attitude_setpoint = {};
-	vehicle_attitude_setpoint.timestamp = hrt_absolute_time();
-
-	const Quatf attitude_setpoint(Eulerf(roll_des, pitch_des, yaw_des));
-	attitude_setpoint.copyTo(vehicle_attitude_setpoint.q_d);
-
-	vehicle_attitude_setpoint.thrust_body[0] = thrust_x;
-	vehicle_attitude_setpoint.thrust_body[1] = thrust_y;
-	vehicle_attitude_setpoint.thrust_body[2] = thrust_z;
-
-
-	_att_sp_pub.publish(vehicle_attitude_setpoint);
-}
-
-void UUVPOSControl::pose_controller_6dof(const Vector3f &pos_des,
-		const float roll_des, const float pitch_des, const float yaw_des,
-		vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos)
+void UUVPOSControl::pose_controller_6dof(const Vector3f &pos_des, vehicle_attitude_s &vehicle_attitude,
+		vehicle_local_position_s &vlocal_pos, bool altitude_mode)
 {
 	//get current rotation of vehicle
 	Quatf q_att(vehicle_attitude.q);
 
+	// Assumes target 0 velocity
 	Vector3f p_control_output = Vector3f(_param_pose_gain_x.get() * (pos_des(0) - vlocal_pos.x) - _param_pose_gain_d_x.get()
 					     * vlocal_pos.vx,
 					     _param_pose_gain_y.get() * (pos_des(1) - vlocal_pos.y) - _param_pose_gain_d_y.get() * vlocal_pos.vy,
 					     _param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z) - _param_pose_gain_d_z.get() * vlocal_pos.vz);
 
-	Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output);//rotate the coord.sys (from global to body)
+	if (altitude_mode) {
+		// In altitude mode, we only control the z-axis
+		p_control_output(0) = 0.0f;
+		p_control_output(1) = 0.0f;
+	}
 
-	publish_attitude_setpoint(rotated_input(0),
-				  rotated_input(1),
-				  rotated_input(2),
-				  roll_des, pitch_des, yaw_des);
+	Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output); //rotate the coord.sys (from global to body)
 
+	_attitude_setpoint.timestamp = hrt_absolute_time();
+	_attitude_setpoint.q_d[0] = _trajectory_setpoint.quaternion[0];
+	_attitude_setpoint.q_d[1] = _trajectory_setpoint.quaternion[1];
+	_attitude_setpoint.q_d[2] = _trajectory_setpoint.quaternion[2];
+	_attitude_setpoint.q_d[3] = _trajectory_setpoint.quaternion[3];
+	_attitude_setpoint.thrust_body[0] = rotated_input(0);
+	_attitude_setpoint.thrust_body[1] = rotated_input(1);
+	_attitude_setpoint.thrust_body[2] = rotated_input(2);
 }
 
-void UUVPOSControl::stabilization_controller_6dof(const Vector3f &pos_des,
-		const float roll_des, const float pitch_des, const float yaw_des,
-		vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos)
+void UUVPOSControl::check_setpoint_validity(vehicle_local_position_s &vlocal_pos)
 {
-	//get current rotation of vehicle
+	const float _setpoint_age = (hrt_absolute_time() - _trajectory_setpoint.timestamp) * 1e-6f;
+
+	if (_setpoint_age < 0.0f || _setpoint_age > _param_setpoint_max_age.get()) {
+		reset_trajectory_setpoint(vlocal_pos);
+	}
+
+	if (!PX4_ISFINITE(_trajectory_setpoint.position[0]) ||
+	    !PX4_ISFINITE(_trajectory_setpoint.position[1]) ||
+	    !PX4_ISFINITE(_trajectory_setpoint.position[2]) ||
+	    !PX4_ISFINITE(_trajectory_setpoint.quaternion[0]) ||
+	    !PX4_ISFINITE(_trajectory_setpoint.quaternion[1]) ||
+	    !PX4_ISFINITE(_trajectory_setpoint.quaternion[2]) ||
+	    !PX4_ISFINITE(_trajectory_setpoint.quaternion[3])) {
+		reset_trajectory_setpoint(vlocal_pos);
+	}
+}
+
+void UUVPOSControl::generate_trajectory_setpoint(vehicle_local_position_s &vlocal_pos,
+		vehicle_attitude_s &vehicle_attitude,
+		float dt)
+{
+	float roll = Eulerf(matrix::Quatf(_trajectory_setpoint.quaternion)).phi();
+	float pitch = Eulerf(matrix::Quatf(_trajectory_setpoint.quaternion)).theta();
+	float yaw = Eulerf(matrix::Quatf(_trajectory_setpoint.quaternion)).psi();
+
+	// Integrate manual control inputs
+	// Info:
+	//  - throttle is Z, roll is Y, pitch is X
+	//  - if param_stab_mode == 1:
+	//	- roll = 0
+	//	- pitch = 0
+	//  - if param_stab_mode == 0:
+	//      - roll can be updated with D-pad  (joystick)
+	//      - pitch can be updated with D-pad (joystick)
+	float roll_setpoint = roll;
+	float pitch_setpoint = pitch;
+
+	if (_param_stab_mode.get()) {
+		roll_setpoint = 0.0;
+		pitch_setpoint = 0.0;
+
+	} else {
+		// Update target roll and pitch setpoint with D-pad
+		switch (_manual_control_setpoint.buttons) {
+		case 2048:
+			pitch_setpoint -= dt * _param_sgm_pitch.get();
+			break;
+
+		case 4096:
+			pitch_setpoint += dt * _param_sgm_pitch.get();
+			break;
+
+		case 8192:
+			roll_setpoint -= dt * _param_sgm_roll.get();
+			break;
+
+		case 16384:
+			roll_setpoint += dt * _param_sgm_roll.get();
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	float yaw_setpoint = yaw + _manual_control_setpoint.yaw * dt * _param_sgm_yaw.get();
+
+	// Update position setpoints based on manual control inputs
+	float vx_sp = 0.0;
+
+	if (_manual_control_setpoint.pitch > _param_pos_stick_db.get()
+	    || _manual_control_setpoint.pitch < -_param_pos_stick_db.get()) {
+		// If pitch is not zero, we use it to set the roll setpoint
+		vx_sp = _manual_control_setpoint.pitch * _param_pgm_vel.get();
+	}
+
+	float vy_sp = 0.0;
+
+	if (_manual_control_setpoint.roll > _param_pos_stick_db.get()
+	    || _manual_control_setpoint.roll < -_param_pos_stick_db.get()) {
+		// If roll is not zero, we use it to set the pitch setpoint
+		vy_sp = _manual_control_setpoint.roll * _param_pgm_vel.get();
+	}
+
+	float vz_sp = 0.0;
+
+	if (_manual_control_setpoint.throttle > _param_pos_stick_db.get()
+	    || _manual_control_setpoint.throttle < -_param_pos_stick_db.get()) {
+		// If throttle is not zero, we use it to set the vertical velocity
+		vz_sp = -_manual_control_setpoint.throttle * _param_pgm_vel.get();
+	}
+
+	// rotate velocity setpoint in body frame to global frame
+	Vector3f velocity_setpoint(vx_sp, vy_sp, vz_sp);
 	Quatf q_att(vehicle_attitude.q);
+	Vector3f rotated_velocity_setpoint = q_att.rotateVector(velocity_setpoint);
 
-	Vector3f p_control_output = Vector3f(0,
-					     0,
-					     _param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z));
-	//potential d controller missing
-	Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output);//rotate the coord.sys (from global to body)
+	// Generate target quaternion
+	Eulerf euler_sp(roll_setpoint, pitch_setpoint, yaw_setpoint);
+	Quatf q_sp = euler_sp;
 
-	publish_attitude_setpoint(rotated_input(0) + pos_des(0), rotated_input(1) + pos_des(1), rotated_input(2),
-				  roll_des, pitch_des, yaw_des);
+	// Normalize the quaternion to avoid numerical issues
+	q_sp.normalize();
 
+	q_sp.copyTo(_trajectory_setpoint.quaternion);
+
+	_trajectory_setpoint.timestamp = hrt_absolute_time();
+
+	if (!_param_pos_mode.get()) {
+		_trajectory_setpoint.position[0] = _trajectory_setpoint.position[0] + vx_sp * dt; // X in world frame
+		_trajectory_setpoint.position[1] = _trajectory_setpoint.position[1] + vy_sp * dt; // Y in world frame
+		_trajectory_setpoint.position[2] = _trajectory_setpoint.position[2] + vz_sp * dt; // Z in world frame
+
+	} else {
+		_trajectory_setpoint.position[0] = _trajectory_setpoint.position[0] + rotated_velocity_setpoint(
+				0) * dt; // X in body frame
+		_trajectory_setpoint.position[1] = _trajectory_setpoint.position[1] + rotated_velocity_setpoint(
+				1) * dt; // Y in body frame
+		_trajectory_setpoint.position[2] = _trajectory_setpoint.position[2] + rotated_velocity_setpoint(
+				2) * dt; // Z in body frame
+	}
+}
+
+void UUVPOSControl::reset_trajectory_setpoint(vehicle_local_position_s &vlocal_pos)
+{
+	// Reset trajectory setpoint to current position and attitude
+	_trajectory_setpoint.timestamp = hrt_absolute_time();
+	_trajectory_setpoint.position[0] = vlocal_pos.x;
+	_trajectory_setpoint.position[1] = vlocal_pos.y;
+	_trajectory_setpoint.position[2] = vlocal_pos.z;
+	_trajectory_setpoint.quaternion[0] = _vehicle_attitude.q[0];
+	_trajectory_setpoint.quaternion[1] = _vehicle_attitude.q[1];
+	_trajectory_setpoint.quaternion[2] = _vehicle_attitude.q[2];
+	_trajectory_setpoint.quaternion[3] = _vehicle_attitude.q[3];
 }
 
 void UUVPOSControl::Run()
@@ -160,7 +271,6 @@ void UUVPOSControl::Run()
 	/* check vehicle control mode for changes to publication state */
 	_vcontrol_mode_sub.update(&_vcontrol_mode);
 
-
 	/* update parameters from storage */
 	parameters_update();
 
@@ -169,34 +279,58 @@ void UUVPOSControl::Run()
 
 	/* only run controller if local_pos changed */
 	if (_vehicle_local_position_sub.update(&vlocal_pos)) {
+		const float dt = math::constrain(((vlocal_pos.timestamp_sample - _last_run) * 1e-6f), 0.0002f, 0.02f);
+		_last_run = vlocal_pos.timestamp_sample;
 
-		/* Run geometric attitude controllers if NOT manual mode*/
-		if (!_vcontrol_mode.flag_control_manual_enabled
-		    && _vcontrol_mode.flag_control_attitude_enabled
-		    && _vcontrol_mode.flag_control_rates_enabled) {
+		// Update vehicle attitude
+		_vehicle_attitude_sub.update(&_vehicle_attitude);
 
-			_vehicle_attitude_sub.update(&_vehicle_attitude);//get current vehicle attitude
+		/* Run position or altitude mode from manual setpoints*/
+		if (_vcontrol_mode.flag_control_manual_enabled
+		    && (_vcontrol_mode.flag_control_altitude_enabled
+			|| _vcontrol_mode.flag_control_position_enabled)
+		    && _vcontrol_mode.flag_armed) {
+			/* Update manual setpoints */
+
+			const bool altitude_only_flag = _vcontrol_mode.flag_control_altitude_enabled
+							&& ! _vcontrol_mode.flag_control_position_enabled;
+
+			_manual_control_setpoint_sub.update(&_manual_control_setpoint);
+
+			// Ensure no nan and sufficiently recent setpoint
+			check_setpoint_validity(vlocal_pos);
+
+			// Generate _trajectory_setpoint -> creates _trajectory_setpoint
+			generate_trajectory_setpoint(vlocal_pos, _vehicle_attitude, dt);
+
+			pose_controller_6dof(Vector3f(_trajectory_setpoint.position), _vehicle_attitude,
+					     vlocal_pos, altitude_only_flag);
+
+		} else if (!_vcontrol_mode.flag_control_manual_enabled
+			   && (_vcontrol_mode.flag_control_altitude_enabled
+			       || _vcontrol_mode.flag_control_position_enabled)
+			   && _vcontrol_mode.flag_armed) {
+			/* Autonomous position mode - no manual inputs are used */
+			const bool altitude_only_flag = _vcontrol_mode.flag_control_altitude_enabled
+							&& ! _vcontrol_mode.flag_control_position_enabled;
+
+			// get manual control setpoint
 			_trajectory_setpoint_sub.update(&_trajectory_setpoint);
 
-			float roll_des = 0;
-			float pitch_des = 0;
-			float yaw_des = _trajectory_setpoint.yaw;
+			pose_controller_6dof(Vector3f(_trajectory_setpoint.position), _vehicle_attitude,
+					     vlocal_pos, altitude_only_flag);
 
-			//stabilization controller(keep pos and hold depth + angle) vs position controller(global + yaw)
-			if (_param_stabilization.get() == 0) {
-				pose_controller_6dof(Vector3f(_trajectory_setpoint.position),
-						     roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos);
-
-			} else {
-				stabilization_controller_6dof(Vector3f(_trajectory_setpoint.position),
-							      roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos);
-			}
+		} else {
+			// Reset if not in a valid mode (like attitude, rate, manual) to clear setpoint
+			check_setpoint_validity(vlocal_pos);
 		}
 	}
 
 	/* Only publish if any of the proper modes are enabled */
-	if (_vcontrol_mode.flag_control_manual_enabled ||
-	    _vcontrol_mode.flag_control_attitude_enabled) {
+	if (_vcontrol_mode.flag_control_position_enabled ||
+	    _vcontrol_mode.flag_control_altitude_enabled) {
+		// Print attitude setpoint
+		_att_sp_pub.publish(_attitude_setpoint);
 	}
 
 	perf_end(_loop_perf);

src/modules/uuv_pos_control/uuv_pos_control.hpp

@@ -59,8 +59,9 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/Publication.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/trajectory_setpoint.h>
+#include <uORB/topics/trajectory_setpoint6dof.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -101,16 +102,20 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
+	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint6dof)};
 	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};	/**< notification of manual control updates */
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
 
 	vehicle_attitude_s _vehicle_attitude{};
-	trajectory_setpoint_s _trajectory_setpoint{};
+	trajectory_setpoint6dof_s _trajectory_setpoint{};
+	manual_control_setpoint_s _manual_control_setpoint{};
 	vehicle_control_mode_s _vcontrol_mode{};
+	vehicle_attitude_setpoint_s _attitude_setpoint{};
 
 	perf_counter_t	_loop_perf;
+	hrt_abstime _last_run{0};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::UUV_GAIN_X_P>) _param_pose_gain_x,
@@ -119,10 +124,14 @@ private:
 		(ParamFloat<px4::params::UUV_GAIN_X_D>) _param_pose_gain_d_x,
 		(ParamFloat<px4::params::UUV_GAIN_Y_D>) _param_pose_gain_d_y,
 		(ParamFloat<px4::params::UUV_GAIN_Z_D>) _param_pose_gain_d_z,
-
-		(ParamInt<px4::params::UUV_INPUT_MODE>) _param_input_mode,
-		(ParamInt<px4::params::UUV_STAB_MODE>) _param_stabilization,
-		(ParamInt<px4::params::UUV_SKIP_CTRL>) _param_skip_ctrl
+		(ParamInt<px4::params::UUV_STAB_MODE>) _param_stab_mode,
+		(ParamFloat<px4::params::UUV_POS_STICK_DB>) _param_pos_stick_db,
+		(ParamFloat<px4::params::UUV_PGM_VEL>) _param_pgm_vel,
+		(ParamFloat<px4::params::UUV_SGM_ROLL>) _param_sgm_roll,
+		(ParamFloat<px4::params::UUV_SGM_PITCH>) _param_sgm_pitch,
+		(ParamFloat<px4::params::UUV_SGM_YAW>) _param_sgm_yaw,
+		(ParamFloat<px4::params::UUV_SP_MAX_AGE>) _param_setpoint_max_age,
+		(ParamInt<px4::params::UUV_POS_MODE>) _param_pos_mode
 	)
 
 	void Run() override;
@@ -136,10 +145,12 @@ private:
 	 */
 	void publish_attitude_setpoint(const float thrust_x, const float thrust_y, const float thrust_z,
 				       const float roll_des, const float pitch_des, const float yaw_des);
-	void pose_controller_6dof(const Vector3f &pos_des,
-				  const float roll_des, const float pitch_des, const float yaw_des,
-				  vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos);
+	void pose_controller_6dof(const Vector3f &pos_des, vehicle_attitude_s &vehicle_attitude,
+				  vehicle_local_position_s &vlocal_pos, bool altitude_mode);
 	void stabilization_controller_6dof(const Vector3f &pos_des,
 					   const float roll_des, const float pitch_des, const float yaw_des,
 					   vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos);
+	void generate_trajectory_setpoint(vehicle_local_position_s &vlocal_pos, vehicle_attitude_s &vehicle_attitude, float dt);
+	void reset_trajectory_setpoint(vehicle_local_position_s &vlocal_pos);
+	void check_setpoint_validity(vehicle_local_position_s &vlocal_pos);
 };

src/modules/uuv_pos_control/uuv_pos_control_params.c

@@ -90,8 +90,31 @@ PARAM_DEFINE_FLOAT(UUV_GAIN_Z_D, 0.2f);
 /**
  * Stabilization mode(1) or Position Control(0)
  *
- * @value 0 Position Control
- * @value 1 Stabilization Mode
+ * @value 0 Tracks previous attitude setpoint
+ * @value 1 Tracks horizontal attitude (allows yaw change)
  * @group UUV Position Control
  */
 PARAM_DEFINE_INT32(UUV_STAB_MODE, 1);
+
+/**
+ * Deadband for changing position setpoint
+ *
+ * @group UUV Position Control
+ */
+PARAM_DEFINE_FLOAT(UUV_POS_STICK_DB, 0.1f);
+
+/**
+ * Gain for position control velocity setpoint update
+ *
+ * @group UUV Position Control
+ */
+PARAM_DEFINE_FLOAT(UUV_PGM_VEL, 0.5f);
+
+/**
+ * Stabilization mode(1) or Position Control(0)
+ *
+ * @value 0 Moves position setpoint in world frame
+ * @value 1 Moves position setpoint in body frame
+ * @group UUV Position Control
+ */
+PARAM_DEFINE_INT32(UUV_POS_MODE, 1);