MC Slowmode: Yawstick for gimbal control, vehicle yaw follows gimbal (#24242)

* Yawsticks on gimbal, vehicle follows gimbal in slowmode, once vehicle has taken off

* Increase queue length to avoid automatically unadvertise queued publications with queue length 1

* Improve readability

---------

Co-authored-by: Pernilla <pernilla@auterion.com>

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/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 "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/flight_mode_manager/tasks/Utility/CMakeLists.txt

@@ -36,6 +36,7 @@ px4_add_library(FlightTaskUtility
 	StickAccelerationXY.cpp
 	StickTiltXY.cpp
 	StickYaw.cpp
+	Gimbal.cpp
 )
 
 target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis SlewRate motion_planning mathlib)

src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp

@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   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);
+}

src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp

@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   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 interface with flight tasks
+ * @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>
+
+#include "Sticks.hpp"
+
+
+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);
+	void acquireGimbalControlIfNeeded();
+	void releaseGimbalControlIfNeeded();
+	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/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);