acquire gimbal control before sending neutral command

msg/GimbalManagerSetAttitude.msg

@@ -20,3 +20,5 @@ float32[4] q
 float32 angular_velocity_x
 float32 angular_velocity_y
 float32 angular_velocity_z
+
+uint8 ORB_QUEUE_LENGTH = 2

src/lib/CMakeLists.txt

@@ -52,6 +52,7 @@ add_subdirectory(dataman_client EXCLUDE_FROM_ALL)
 add_subdirectory(drivers EXCLUDE_FROM_ALL)
 add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL)
 add_subdirectory(geo EXCLUDE_FROM_ALL)
+add_subdirectory(gimbal_control EXCLUDE_FROM_ALL)
 add_subdirectory(heatshrink EXCLUDE_FROM_ALL)
 add_subdirectory(hysteresis EXCLUDE_FROM_ALL)
 add_subdirectory(l1 EXCLUDE_FROM_ALL)

src/lib/gimbal_control/CMakeLists.txt

@@ -0,0 +1,34 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(gimbal_control Gimbal.cpp)

src/lib/gimbal_control/Gimbal.cpp

@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "Gimbal.hpp"
+#include <px4_platform_common/events.h>
+
+using namespace time_literals;
+using namespace matrix;
+
+Gimbal::Gimbal(ModuleParams *parent) :
+	ModuleParams(parent)
+{}
+
+Gimbal::~Gimbal()
+{
+	releaseGimbalControlIfNeeded();
+}
+
+bool Gimbal::checkForTelemetry(const hrt_abstime now)
+{
+	if (_gimbal_device_attitude_status_sub.updated()) {
+		gimbal_device_attitude_status_s gimbal_device_attitude_status{};
+
+		if (_gimbal_device_attitude_status_sub.copy(&gimbal_device_attitude_status)) {
+			_telemtry_timestamp = gimbal_device_attitude_status.timestamp;
+			_telemetry_flags = gimbal_device_attitude_status.device_flags;
+			_telemetry_yaw = Eulerf(Quatf(gimbal_device_attitude_status.q)).psi();
+		}
+	}
+
+	return now < _telemtry_timestamp + 2_s;
+}
+
+void Gimbal::acquireGimbalControlIfNeeded()
+{
+	if (!_have_gimbal_control) {
+		_have_gimbal_control = true;
+
+		vehicle_command_s vehicle_command{};
+		vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
+		vehicle_command.param1 = _param_mav_sys_id.get();
+		vehicle_command.param2 = _param_mav_comp_id.get();
+		vehicle_command.param3 = -1.0f; // Leave unchanged.
+		vehicle_command.param4 = -1.0f; // Leave unchanged.
+		vehicle_command.timestamp = hrt_absolute_time();
+		vehicle_command.source_system = _param_mav_sys_id.get();
+		vehicle_command.source_component = _param_mav_comp_id.get();
+		vehicle_command.target_system = _param_mav_sys_id.get();
+		vehicle_command.target_component = _param_mav_sys_id.get();
+		vehicle_command.from_external = false;
+		_vehicle_command_pub.publish(vehicle_command);
+	}
+}
+
+void Gimbal::releaseGimbalControlIfNeeded()
+{
+	if (_have_gimbal_control) {
+		_have_gimbal_control = false;
+
+		// Restore default flags, setting rate setpoints to NAN lead to unexpected behavior
+		publishGimbalManagerSetAttitude(FLAGS_ROLL_PITCH_LOCKED,
+						Quatf(NAN, NAN, NAN, NAN),
+						Vector3f(NAN, 0.f, 0.f));
+
+		// Release gimbal
+		vehicle_command_s vehicle_command{};
+		vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
+		vehicle_command.param1 = -3.0f; // Remove control if it had it.
+		vehicle_command.param2 = -3.0f; // Remove control if it had it.
+		vehicle_command.param3 = -1.0f; // Leave unchanged.
+		vehicle_command.param4 = -1.0f; // Leave unchanged.
+		vehicle_command.timestamp = hrt_absolute_time();
+		vehicle_command.from_external = false;
+		vehicle_command.source_system = _param_mav_sys_id.get();
+		vehicle_command.source_component = _param_mav_comp_id.get();
+		vehicle_command.target_system = _param_mav_sys_id.get();
+		vehicle_command.target_component = _param_mav_comp_id.get();
+		_vehicle_command_pub.publish(vehicle_command);
+	}
+}
+
+void Gimbal::publishGimbalManagerSetAttitude(const uint16_t gimbal_flags,
+		const matrix::Quatf &q_gimbal_setpoint,
+		const matrix::Vector3f &gimbal_rates)
+{
+	gimbal_manager_set_attitude_s gimbal_setpoint{};
+	gimbal_setpoint.origin_sysid = _param_mav_sys_id.get();
+	gimbal_setpoint.origin_compid =  _param_mav_comp_id.get();
+	gimbal_setpoint.flags = gimbal_flags;
+	q_gimbal_setpoint.copyTo(gimbal_setpoint.q);
+	gimbal_setpoint.angular_velocity_x =  gimbal_rates(0);
+	gimbal_setpoint.angular_velocity_y = gimbal_rates(1);
+	gimbal_setpoint.angular_velocity_z = gimbal_rates(2);
+	gimbal_setpoint.timestamp = hrt_absolute_time();
+	_gimbal_manager_set_attitude_pub.publish(gimbal_setpoint);
+}
+
+void Gimbal::setNeutral()
+{
+	vehicle_command_s vehicle_command = {};
+	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
+	vehicle_command.param1 = NAN;
+	vehicle_command.param2 = NAN;
+	vehicle_command.param3 = NAN;
+	vehicle_command.param4 = NAN;
+	vehicle_command.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL;
+	_vehicle_command_pub.publish(vehicle_command);
+}

src/lib/gimbal_control/Gimbal.hpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Gimbal.hpp
+ * @brief Gimbal library to interface with other units
+ * @author Pernilla Wikström <pernilla@auterion.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/module_params.h>
+#include <systemlib/mavlink_log.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/gimbal_manager_set_attitude.h>
+#include <uORB/topics/gimbal_device_attitude_status.h>
+#include <uORB/topics/vehicle_command.h>
+
+/**
+* Fore more information on gimbal v2, see https://mavlink.io/en/services/gimbal_v2.html
+ **/
+class Gimbal : public ModuleParams
+{
+public:
+	Gimbal(ModuleParams *parent);
+	~Gimbal();
+
+	bool checkForTelemetry(const hrt_abstime now);
+	void publishGimbalManagerSetAttitude(const uint16_t gimbal_flags,
+					     const matrix::Quatf &q_gimbal_setpoint,
+					     const matrix::Vector3f &gimbal_rates);
+	/**
+	 * Acquire Gimbal Control Authority to restrict other modules / Ground station
+	 * to control the gimbal until the gimbal is released.
+	 */
+	void acquireGimbalControlIfNeeded();
+
+	/**
+	 * Releases Gimbal Control Authority, to allow other modules / Ground station
+	 * to control the gimbal.
+	 */
+	void releaseGimbalControlIfNeeded();
+
+	void setNeutral();
+	float getTelemetryYaw() { return _telemetry_yaw; }
+	bool allAxesLockedConfirmed() { return _telemetry_flags == FLAGS_ALL_AXES_LOCKED; }
+
+	static constexpr uint16_t FLAGS_ALL_AXES_LOCKED = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK
+			| gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK
+			| gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK;
+	static constexpr uint16_t FLAGS_ROLL_PITCH_LOCKED =
+		gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK
+		| gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK;
+
+private:
+	bool _have_gimbal_control{false};
+
+	uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)};
+	hrt_abstime _telemtry_timestamp{};
+	uint16_t _telemetry_flags{};
+	float _telemetry_yaw{};
+
+	uORB::Publication<gimbal_manager_set_attitude_s> _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
+	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
+		(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id
+	)
+};

src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt

@@ -37,5 +37,5 @@ px4_add_library(FlightTaskAutoFollowTarget
 	FlightTaskAutoFollowTarget.cpp
 )
 
-target_link_libraries(FlightTaskAutoFollowTarget PUBLIC FlightTaskAuto follow_target_estimator)
+target_link_libraries(FlightTaskAutoFollowTarget PUBLIC FlightTaskAuto follow_target_estimator gimbal_control)
 target_include_directories(FlightTaskAutoFollowTarget PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp

@@ -59,7 +59,7 @@ FlightTaskAutoFollowTarget::FlightTaskAutoFollowTarget() : _sticks(this)
 
 FlightTaskAutoFollowTarget::~FlightTaskAutoFollowTarget()
 {
-	releaseGimbalControl();
+	_gimbal_control.releaseGimbalControlIfNeeded();
 	_target_estimator.Stop();
 }
 
@@ -428,33 +428,8 @@ bool FlightTaskAutoFollowTarget::update()
 	return true;
 }
 
-void FlightTaskAutoFollowTarget::releaseGimbalControl()
-{
-	// NOTE: If other flight tasks start using gimbal control as well
-	// it might be worth moving this release mechanism to a common base
-	// class for gimbal-control flight tasks
-
-	vehicle_command_s vehicle_command = {};
-	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
-	vehicle_command.param1 = -3.0f; // Remove control if it had it.
-	vehicle_command.param2 = -3.0f; // Remove control if it had it.
-	vehicle_command.param3 = -1.0f; // Leave unchanged.
-	vehicle_command.param4 = -1.0f; // Leave unchanged.
-
-	vehicle_command.timestamp = hrt_absolute_time();
-	vehicle_command.source_system = _param_mav_sys_id.get();
-	vehicle_command.source_component = _param_mav_comp_id.get();
-	vehicle_command.target_system = _param_mav_sys_id.get();
-	vehicle_command.target_component = _param_mav_comp_id.get();
-	vehicle_command.confirmation = false;
-	vehicle_command.from_external = false;
-
-	_vehicle_command_pub.publish(vehicle_command);
-}
-
 float FlightTaskAutoFollowTarget::pointGimbalAt(const float xy_distance, const float z_distance)
 {
-	gimbal_manager_set_attitude_s msg{};
 	float pitch_down_angle = 0.0f;
 
 	if (PX4_ISFINITE(z_distance)) {
@@ -465,11 +440,10 @@ float FlightTaskAutoFollowTarget::pointGimbalAt(const float xy_distance, const f
 		pitch_down_angle = 0.0;
 	}
 
-	const Quatf q_gimbal = Quatf(Eulerf(0, -pitch_down_angle, 0));
-	q_gimbal.copyTo(msg.q);
-
-	msg.timestamp = hrt_absolute_time();
-	_gimbal_manager_set_attitude_pub.publish(msg);
+	_gimbal_control.acquireGimbalControlIfNeeded();
+	_gimbal_control.publishGimbalManagerSetAttitude(Gimbal::FLAGS_ROLL_PITCH_LOCKED,
+			Quatf(Eulerf(0, -pitch_down_angle, 0)),
+			Vector3f(NAN, NAN, NAN));
 
 	return pitch_down_angle;
 }

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

@@ -53,11 +53,10 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/follow_target_status.h>
 #include <uORB/topics/follow_target_estimator.h>
-#include <uORB/topics/gimbal_manager_set_attitude.h>
-#include <uORB/topics/vehicle_command.h>
 
 #include <lib/mathlib/math/filter/second_order_reference_model.hpp>
 #include <motion_planning/VelocitySmoothing.hpp>
+#include <lib/gimbal_control/Gimbal.hpp>
 
 // << Follow Target Behavior related constants >>
 
@@ -252,14 +251,7 @@ protected:
 	 */
 	float pointGimbalAt(const float xy_distance, const float z_distance);
 
-	/**
-	 * Release Gimbal Control
-	 *
-	 * Releases Gimbal Control Authority of Follow-Target Flight Task, to allow other modules / Ground station
-	 * to control the gimbal when the task exits.
-	 * Fore more information on gimbal v2, see https://mavlink.io/en/services/gimbal_v2.html
-	 */
-	void releaseGimbalControl();
+	Gimbal _gimbal_control{this};
 
 	// Sticks object to read in stick commands from the user
 	Sticks _sticks;
@@ -305,6 +297,4 @@ protected:
 	uORB::Subscription _follow_target_estimator_sub{ORB_ID(follow_target_estimator)};
 
 	uORB::Publication<follow_target_status_s> _follow_target_status_pub{ORB_ID(follow_target_status)};
-	uORB::Publication<gimbal_manager_set_attitude_s> _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
-	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
 };

src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/CMakeLists.txt

@@ -36,4 +36,4 @@ px4_add_library(FlightTaskManualAccelerationSlow
 )
 target_include_directories(FlightTaskManualAccelerationSlow PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
-target_link_libraries(FlightTaskManualAccelerationSlow PUBLIC FlightTaskManualAcceleration FlightTaskUtility)
+target_link_libraries(FlightTaskManualAccelerationSlow PUBLIC FlightTaskManualAcceleration FlightTaskUtility gimbal_control)

src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp

@@ -39,6 +39,7 @@
 #include <px4_platform_common/events.h>
 
 using namespace time_literals;
+using namespace matrix;
 
 bool FlightTaskManualAccelerationSlow::update()
 {
@@ -128,7 +129,29 @@ bool FlightTaskManualAccelerationSlow::update()
 	FlightTaskManualAltitude::_velocity_constraint_down = velocity_down;
 	FlightTaskManualAcceleration::_stick_yaw.setYawspeedConstraint(yaw_rate);
 
-	return FlightTaskManualAcceleration::update();
+	bool ret = FlightTaskManualAcceleration::update();
+
+	// Optimize input-to-video latency gimbal control
+	if (_gimbal.checkForTelemetry(_time_stamp_current) && haveTakenOff()) {
+		_gimbal.acquireGimbalControlIfNeeded();
+
+		// the exact same _yawspeed_setpoint is setpoint for the gimbal and vehicle feed-forward
+		const float pitchrate_setpoint = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_pitch.get()) * yaw_rate;
+		_yawspeed_setpoint = _sticks.getYaw() * yaw_rate;
+
+		_gimbal.publishGimbalManagerSetAttitude(Gimbal::FLAGS_ALL_AXES_LOCKED, Quatf(NAN, NAN, NAN, NAN),
+							Vector3f(NAN, pitchrate_setpoint, _yawspeed_setpoint));
+
+		if (_gimbal.allAxesLockedConfirmed()) {
+			// but the vehicle makes sure it stays alligned with the gimbal absolute yaw
+			_yaw_setpoint = _gimbal.getTelemetryYaw();
+		}
+
+	} else {
+		_gimbal.releaseGimbalControlIfNeeded();
+	}
+
+	return ret;
 }
 
 float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(int parameter_value)
@@ -136,3 +159,11 @@ float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(i
 	const int sanitized_index = math::constrain(parameter_value - 1, 0, 5);
 	return _sticks.getAux()(sanitized_index);
 }
+
+bool FlightTaskManualAccelerationSlow::haveTakenOff()
+{
+	takeoff_status_s takeoff_status{};
+	_takeoff_status_sub.copy(&takeoff_status);
+
+	return takeoff_status.takeoff_state == takeoff_status_s::TAKEOFF_STATE_FLIGHT;
+}

src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp

@@ -42,6 +42,7 @@
 
 #include "FlightTaskManualAcceleration.hpp"
 #include "StickAccelerationXY.hpp"
+#include "lib/gimbal_control/Gimbal.hpp"
 
 #include <lib/weather_vane/WeatherVane.hpp>
 #include <uORB/Subscription.hpp>
@@ -67,9 +68,15 @@ private:
 
 	bool _velocity_limits_received_before{false};
 
+	// Gimbal control
+	Gimbal _gimbal{this};
+
 	uORB::Subscription _velocity_limits_sub{ORB_ID(velocity_limits)};
 	velocity_limits_s _velocity_limits{};
 
+	uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};
+	bool haveTakenOff();
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAcceleration,
 					(ParamInt<px4::params::MC_SLOW_MAP_HVEL>) _param_mc_slow_map_hvel,
 					(ParamInt<px4::params::MC_SLOW_MAP_VVEL>) _param_mc_slow_map_vvel,
@@ -83,6 +90,7 @@ private:
 					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
 					(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
 					(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
-					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max
+					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max,
+					(ParamInt<px4::params::MC_SLOW_MAP_PTCH>) _param_mc_slow_map_pitch
 				       )
 };

src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c

@@ -156,3 +156,17 @@ PARAM_DEFINE_FLOAT(MC_SLOW_DEF_VVEL, 1.f);
  * @group Multicopter Position Slow Mode
  */
 PARAM_DEFINE_FLOAT(MC_SLOW_DEF_YAWR, 45.f);
+
+/**
+ * RC_MAP_AUX{N} to allow for gimbal pitch rate control in position slow mode
+ *
+ * @value 0 No pitch rate input
+ * @value 1 AUX1
+ * @value 2 AUX2
+ * @value 3 AUX3
+ * @value 4 AUX4
+ * @value 5 AUX5
+ * @value 6 AUX6
+ * @group Multicopter Position Slow Mode
+ */
+PARAM_DEFINE_INT32(MC_SLOW_MAP_PTCH, 0);

src/modules/gimbal/input_mavlink.cpp

@@ -43,6 +43,8 @@
 #include <math.h>
 #include <matrix/matrix/math.hpp>
 
+using namespace matrix;
+
 namespace gimbal
 {
 
@@ -936,21 +938,19 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_con
 	if (set_manual_control.origin_sysid == control_data.sysid_primary_control &&
 	    set_manual_control.origin_compid == control_data.compid_primary_control) {
 
-		const matrix::Quatf q =
-			(PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw))
-			?
-			matrix::Quatf(
-				matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw))
-			:
-			matrix::Quatf(NAN, NAN, NAN, NAN);
+		Quatf q(NAN, NAN, NAN, NAN);
 
-		const matrix::Vector3f angular_velocity =
-			(PX4_ISFINITE(set_manual_control.pitch_rate) &&
-			 PX4_ISFINITE(set_manual_control.yaw_rate)) ?
-			matrix::Vector3f(0.0f,
-					 math::radians(_parameters.mnt_rate_pitch) * set_manual_control.pitch_rate,
-					 math::radians(_parameters.mnt_rate_yaw) * set_manual_control.yaw_rate) :
-			matrix::Vector3f(NAN, NAN, NAN);
+		if (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) {
+			q = Quatf(matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw));
+		}
+
+		Vector3f angular_velocity(NAN, NAN, NAN);
+
+		if (PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) {
+			angular_velocity = Vector3f(0.0f,
+						    set_manual_control.pitch_rate * math::radians(_parameters.mnt_rate_pitch),
+						    set_manual_control.yaw_rate * math::radians(_parameters.mnt_rate_yaw));
+		}
 
 		_set_control_data_from_set_attitude(control_data, set_manual_control.flags, q,
 						    angular_velocity, set_manual_control.timestamp);

src/modules/navigator/CMakeLists.txt

@@ -70,4 +70,5 @@ px4_add_module(
 		motion_planning
 		mission_feasibility_checker
 		rtl_time_estimator
+		gimbal_control
 	)

src/modules/navigator/land.cpp

@@ -73,9 +73,9 @@ Land::on_activation()
 	_navigator->reset_cruising_speed();
 
 	// set gimbal to neutral position (level with horizon) to reduce change of damage on landing
-	_navigator->acquire_gimbal_control();
-	_navigator->set_gimbal_neutral();
-	_navigator->release_gimbal_control();
+	_navigator->_gimbal_control.acquireGimbalControlIfNeeded();
+	_navigator->_gimbal_control.setNeutral();
+	_navigator->_gimbal_control.releaseGimbalControlIfNeeded();
 
 }
 

src/modules/navigator/mission_base.cpp

@@ -180,8 +180,9 @@ MissionBase::on_inactivation()
 	_navigator->disable_camera_trigger();
 
 	_navigator->stop_capturing_images();
-	_navigator->set_gimbal_neutral(); // point forward
-	_navigator->release_gimbal_control();
+	_navigator->_gimbal_control.acquireGimbalControlIfNeeded();
+	_navigator->_gimbal_control.setNeutral(); // point forward
+	_navigator->_gimbal_control.releaseGimbalControlIfNeeded();
 
 	if (_navigator->get_precland()->is_activated()) {
 		_navigator->get_precland()->on_inactivation();
@@ -683,9 +684,9 @@ void MissionBase::handleLanding(WorkItemType &new_work_item_type, mission_item_s
 			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
 
 			// set gimbal to neutral position (level with horizon) to reduce change of damage on landing
-			_navigator->acquire_gimbal_control();
-			_navigator->set_gimbal_neutral();
-			_navigator->release_gimbal_control();
+			_navigator->_gimbal_control.acquireGimbalControlIfNeeded();
+			_navigator->_gimbal_control.setNeutral();
+			_navigator->_gimbal_control.releaseGimbalControlIfNeeded();
 
 		} else {
 

src/modules/navigator/mission_block.cpp

@@ -524,7 +524,7 @@ MissionBlock::issue_command(const mission_item_s &item)
 
 	// This is to support legacy DO_MOUNT_CONTROL as part of a mission.
 	if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) {
-		_navigator->acquire_gimbal_control();
+		_navigator->_gimbal_control.acquireGimbalControlIfNeeded();
 	}
 
 	// Mission item's NAV_CMD enums directly map to the according vehicle command

src/modules/navigator/navigator.h

@@ -60,6 +60,7 @@
 #if CONFIG_NAVIGATOR_ADSB
 #include <lib/adsb/AdsbConflict.h>
 #endif // CONFIG_NAVIGATOR_ADSB
+#include <lib/gimbal_control/Gimbal.hpp>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/events.h>
 #include <px4_platform_common/module.h>
@@ -277,9 +278,7 @@ public:
 
 	bool force_vtol();
 
-	void acquire_gimbal_control();
-	void release_gimbal_control();
-	void set_gimbal_neutral();
+	Gimbal _gimbal_control{this};
 
 	void preproject_stop_point(double &lat, double &lon);
 

src/modules/navigator/navigator_main.cpp

@@ -1543,29 +1543,6 @@ void Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_
 	_vehicle_cmd_ack_pub.publish(command_ack);
 }
 
-void Navigator::acquire_gimbal_control()
-{
-	vehicle_command_s vcmd = {};
-	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
-	vcmd.param1 = _vstatus.system_id;
-	vcmd.param2 = _vstatus.component_id;
-	vcmd.param3 = -1.0f; // Leave unchanged.
-	vcmd.param4 = -1.0f; // Leave unchanged.
-	publish_vehicle_cmd(&vcmd);
-}
-
-void Navigator::release_gimbal_control()
-{
-	vehicle_command_s vcmd = {};
-	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
-	vcmd.param1 = -3.0f; // Remove control if it had it.
-	vcmd.param2 = -3.0f; // Remove control if it had it.
-	vcmd.param3 = -1.0f; // Leave unchanged.
-	vcmd.param4 = -1.0f; // Leave unchanged.
-	publish_vehicle_cmd(&vcmd);
-}
-
-
 void
 Navigator::stop_capturing_images()
 {
@@ -1629,18 +1606,6 @@ void Navigator::disable_camera_trigger()
 	publish_vehicle_cmd(&cmd);
 }
 
-void Navigator::set_gimbal_neutral()
-{
-	vehicle_command_s vcmd = {};
-	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
-	vcmd.param1 = NAN;
-	vcmd.param2 = NAN;
-	vcmd.param3 = NAN;
-	vcmd.param4 = NAN;
-	vcmd.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL;
-	publish_vehicle_cmd(&vcmd);
-}
-
 void Navigator::sendWarningDescentStoppedDueToTerrain()
 {
 	mavlink_log_critical(&_mavlink_log_pub, "Terrain collision risk, descent is stopped\t");

src/modules/navigator/rtl.cpp

@@ -238,9 +238,9 @@ void RTL::on_activation()
 	}
 
 	// set gimbal to neutral position (level with horizon) to reduce change of damage on landing
-	_navigator->acquire_gimbal_control();
-	_navigator->set_gimbal_neutral();
-	_navigator->release_gimbal_control();
+	_navigator->_gimbal_control.acquireGimbalControlIfNeeded();
+	_navigator->_gimbal_control.setNeutral();
+	_navigator->_gimbal_control.releaseGimbalControlIfNeeded();
 }
 
 void RTL::on_active()