Rename vmount to gimbal

src/modules/gimbal/CMakeLists.txt

@@ -0,0 +1,49 @@
+############################################################################
+#
+#   Copyright (c) 2016 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_module(
+	MODULE drivers__gimbal
+	MAIN gimbal
+	COMPILE_FLAGS
+	SRCS
+		input.cpp
+		input_mavlink.cpp
+		input_rc.cpp
+		input_test.cpp
+		output.cpp
+		output_mavlink.cpp
+		output_rc.cpp
+		gimbal.cpp
+	DEPENDS
+		geo
+	)
+

src/modules/gimbal/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_GIMBAL
+	bool "gimbal"
+	default n
+	---help---
+		Enable support for GIMBAL

src/modules/gimbal/common.h

@@ -0,0 +1,90 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016 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.
+*
+****************************************************************************/
+
+
+#pragma once
+
+#include <stdint.h>
+
+namespace gimbal
+{
+
+/**
+ * @struct ControlData
+ * This defines the common API between an input and an output of the gimbal driver.
+ * Each output must support the (full) set of the commands, and an input can create all
+ * or a subset of the types.
+ */
+struct ControlData {
+
+	enum class Type {
+		Neutral = 0,
+		Angle,
+		LonLat
+	};
+
+	union TypeData {
+		struct TypeAngle {
+			float q[4];
+			float angular_velocity[3];
+
+			enum class Frame : uint8_t {
+				AngleBodyFrame = 0, // Also called follow mode, angle relative to vehicle forward (usually default for yaw axis).
+				AngularRate = 1, // Angular rate set only.
+				AngleAbsoluteFrame = 2 // Also called lock mode, angle relative to horizon/world, lock mode. (usually default for roll and pitch).
+			} frames[3];
+		} angle;
+
+		struct TypeLonLat {
+			double lon; // longitude in deg
+			double lat; // latitude in deg
+			float altitude; // altitude in m
+			float roll_offset; // roll offset in rad
+			float pitch_offset; // pitch offset in rad
+			float yaw_offset;  // yaw offset in rad
+			float pitch_fixed_angle; // ignored if < -pi, otherwise use a fixed pitch angle instead of the altitude
+		} lonlat;
+	} type_data;
+
+	Type type = Type::Neutral;
+
+	bool gimbal_shutter_retract = false; // whether to lock the gimbal (only in RC output mode)
+
+	uint8_t sysid_primary_control = 0; // The MAVLink system ID selected to be in control, 0 for no one.
+	uint8_t compid_primary_control = 0; // The MAVLink component ID selected to be in control, 0 for no one.
+	// uint8_t sysid_secondary_control = 0; // The MAVLink system ID selected for additional input, not implemented yet.
+	// uint8_t compid_secondary_control = 0; // The MAVLink component ID selected for additional input, not implemented yet.
+};
+
+
+} /* namespace gimbal */

src/modules/gimbal/gimbal.cpp

@@ -0,0 +1,540 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2022 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Gimbal/mount driver.
+ * Supported inputs:
+ * - RC
+ * - MAVLink gimbal protocol v1
+ * - MAVLink gimbal protocol v2
+ * - Test CLI commands
+ * Supported outputs:
+ * - PWM
+ * - MAVLink gimbal protocol v1
+ * - MAVLink gimbal protocol v2
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/tasks.h>
+
+#include "gimbal_params.h"
+#include "input_mavlink.h"
+#include "input_rc.h"
+#include "input_test.h"
+#include "output_rc.h"
+#include "output_mavlink.h"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/atomic.h>
+
+using namespace time_literals;
+using namespace gimbal;
+
+static px4::atomic<bool> thread_should_exit {false};
+static px4::atomic<bool> thread_running {false};
+
+static constexpr int input_objs_len_max = 3;
+
+struct ThreadData {
+	InputBase *input_objs[input_objs_len_max] = {nullptr, nullptr, nullptr};
+	int input_objs_len = 0;
+	int last_input_active = -1;
+	OutputBase *output_obj = nullptr;
+	InputTest *test_input = nullptr;
+};
+static ThreadData *g_thread_data = nullptr;
+
+
+static void usage();
+static void update_params(ParameterHandles &param_handles, Parameters &params);
+static bool initialize_params(ParameterHandles &param_handles, Parameters &params);
+
+static int gimbal_thread_main(int argc, char *argv[]);
+extern "C" __EXPORT int gimbal_main(int argc, char *argv[]);
+
+static int gimbal_thread_main(int argc, char *argv[])
+{
+	ParameterHandles param_handles;
+	Parameters params {};
+	ThreadData thread_data;
+
+	if (!initialize_params(param_handles, params)) {
+		PX4_ERR("could not get mount parameters!");
+		delete g_thread_data->test_input;
+		return -1;
+	}
+
+	uORB::SubscriptionInterval parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	thread_running.store(true);
+	ControlData control_data {};
+	g_thread_data = &thread_data;
+
+	thread_data.test_input = new InputTest(params);
+
+	bool alloc_failed = false;
+
+	thread_data.input_objs[thread_data.input_objs_len++] = thread_data.test_input;
+
+	switch (params.mnt_mode_in) {
+	case 0:
+		// Automatic
+		// MAVLINK_V2 as well as RC input are supported together.
+		// Whichever signal is updated last, gets control, for RC there is a deadzone
+		// to avoid accidental activation.
+		thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkGimbalV2(params);
+
+		thread_data.input_objs[thread_data.input_objs_len++] = new InputRC(params);
+		break;
+
+	case 1: // RC only
+		thread_data.input_objs[thread_data.input_objs_len++] = new InputRC(params);
+		break;
+
+	case 2: // MAVLINK_ROI commands only (to be deprecated)
+		thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkROI(params);
+		break;
+
+	case 3: // MAVLINK_DO_MOUNT commands only (to be deprecated)
+		thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkCmdMount(params);
+		break;
+
+	case 4: //MAVLINK_V2
+		thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkGimbalV2(params);
+		break;
+
+	default:
+		PX4_ERR("invalid input mode %" PRId32, params.mnt_mode_in);
+		break;
+	}
+
+	for (int i = 0; i < thread_data.input_objs_len; ++i) {
+		if (!thread_data.input_objs[i]) {
+			alloc_failed = true;
+		}
+	}
+
+	if (alloc_failed) {
+		PX4_ERR("input objs memory allocation failed");
+		thread_should_exit.store(true);
+	}
+
+	if (!alloc_failed) {
+		for (int i = 0; i < thread_data.input_objs_len; ++i) {
+			if (thread_data.input_objs[i]->initialize() != 0) {
+				PX4_ERR("Input %d failed", i);
+				thread_should_exit.store(true);
+			}
+		}
+	}
+
+	switch (params.mnt_mode_out) {
+	case 0: //AUX
+		thread_data.output_obj = new OutputRC(params);
+
+		if (!thread_data.output_obj) { alloc_failed = true; }
+
+		break;
+
+	case 1: //MAVLink gimbal v1 protocol
+		thread_data.output_obj = new OutputMavlinkV1(params);
+
+		if (!thread_data.output_obj) { alloc_failed = true; }
+
+		break;
+
+	case 2: //MAVLink gimbal v2 protocol
+		thread_data.output_obj = new OutputMavlinkV2(params);
+
+		if (!thread_data.output_obj) { alloc_failed = true; }
+
+		break;
+
+	default:
+		PX4_ERR("invalid output mode %" PRId32, params.mnt_mode_out);
+		thread_should_exit.store(true);
+		break;
+	}
+
+	if (alloc_failed) {
+		PX4_ERR("output memory allocation failed");
+		thread_should_exit.store(true);
+	}
+
+	while (!thread_should_exit.load()) {
+
+		const bool updated = parameter_update_sub.updated();
+
+		if (updated) {
+			parameter_update_s pupdate;
+			parameter_update_sub.copy(&pupdate);
+			update_params(param_handles, params);
+		}
+
+		if (thread_data.last_input_active == -1) {
+			// Reset control as no one is active anymore, or yet.
+			control_data.sysid_primary_control = 0;
+			control_data.compid_primary_control = 0;
+		}
+
+		InputBase::UpdateResult update_result = InputBase::UpdateResult::NoUpdate;
+
+		if (thread_data.input_objs_len > 0) {
+
+			// get input: we cannot make the timeout too large, because the output needs to update
+			// periodically for stabilization and angle updates.
+
+			for (int i = 0; i < thread_data.input_objs_len; ++i) {
+
+				const bool already_active = (thread_data.last_input_active == i);
+				// poll only on active input to reduce latency, or on all if none is active
+				const unsigned int poll_timeout =
+					(already_active || thread_data.last_input_active == -1) ? 20 : 0;
+
+				update_result = thread_data.input_objs[i]->update(poll_timeout, control_data, already_active);
+
+				switch (update_result) {
+				case InputBase::UpdateResult::NoUpdate:
+					if (already_active) {
+						// No longer active.
+						thread_data.last_input_active = -1;
+					}
+
+					break;
+
+				case InputBase::UpdateResult::UpdatedActive:
+					thread_data.last_input_active = i;
+					break;
+
+				case InputBase::UpdateResult::UpdatedActiveOnce:
+					thread_data.last_input_active = -1;
+					break;
+
+				case InputBase::UpdateResult::UpdatedNotActive:
+					if (already_active) {
+						// No longer active
+						thread_data.last_input_active = -1;
+					}
+
+					break;
+				}
+			}
+
+			if (params.mnt_do_stab == 1) {
+				thread_data.output_obj->set_stabilize(true, true, true);
+
+			} else if (params.mnt_do_stab == 2) {
+				thread_data.output_obj->set_stabilize(false, false, true);
+
+			} else {
+				thread_data.output_obj->set_stabilize(false, false, false);
+			}
+
+			// Update output
+			thread_data.output_obj->update(
+				control_data,
+				update_result != InputBase::UpdateResult::NoUpdate);
+
+			// Only publish the mount orientation if the mode is not mavlink v1 or v2
+			// If the gimbal speaks mavlink it publishes its own orientation.
+			if (params.mnt_mode_out != 1 && params.mnt_mode_out != 2) { // 1 = MAVLink v1, 2 = MAVLink v2
+				thread_data.output_obj->publish();
+			}
+
+		} else {
+			// We still need to wake up regularly to check for thread exit requests
+			px4_usleep(1e6);
+		}
+	}
+
+	g_thread_data = nullptr;
+
+	for (int i = 0; i < input_objs_len_max; ++i) {
+		if (thread_data.input_objs[i]) {
+			delete (thread_data.input_objs[i]);
+			thread_data.input_objs[i] = nullptr;
+		}
+	}
+
+	thread_data.input_objs_len = 0;
+
+	if (thread_data.output_obj) {
+		delete (thread_data.output_obj);
+		thread_data.output_obj = nullptr;
+	}
+
+	thread_running.store(false);
+	return 0;
+}
+
+int gimbal_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		PX4_ERR("missing command");
+		usage();
+		return -1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running.load()) {
+			PX4_WARN("mount driver already running");
+			return 1;
+		}
+
+		thread_should_exit.store(false);
+
+		int gimbal_task = px4_task_spawn_cmd("gimbal",
+						     SCHED_DEFAULT,
+						     SCHED_PRIORITY_DEFAULT,
+						     2100,
+						     gimbal_thread_main,
+						     nullptr);
+
+		int counter = 0;
+
+		while (!thread_running.load() && gimbal_task >= 0) {
+			px4_usleep(5000);
+
+			if (++counter >= 100) {
+				break;
+			}
+		}
+
+		if (gimbal_task < 0) {
+			PX4_ERR("failed to start");
+			return -1;
+		}
+
+		return counter < 100 || thread_should_exit.load() ? 0 : -1;
+	}
+
+	else if (!strcmp(argv[1], "stop")) {
+
+		if (!thread_running.load()) {
+			PX4_WARN("mount driver not running");
+			return 0;
+		}
+
+		thread_should_exit.store(true);
+
+		while (thread_running.load()) {
+			px4_usleep(100000);
+		}
+
+		return 0;
+	}
+
+	else if (!strcmp(argv[1], "test")) {
+
+		if (thread_running.load() && g_thread_data && g_thread_data->test_input) {
+
+			if (argc >= 4) {
+				bool found_axis = false;
+				const char *axis_names[3] = {"roll", "pitch", "yaw"};
+				int angles[3] = { 0, 0, 0 };
+
+				for (int arg_i = 2 ; arg_i < (argc - 1); ++arg_i) {
+					for (int axis_i = 0; axis_i < 3; ++axis_i) {
+						if (!strcmp(argv[arg_i], axis_names[axis_i])) {
+							int angle_deg = (int)strtol(argv[arg_i + 1], nullptr, 0);
+							angles[axis_i] = angle_deg;
+							found_axis = true;
+						}
+					}
+				}
+
+				if (!found_axis) {
+					usage();
+					return -1;
+				}
+
+				g_thread_data->test_input->set_test_input(angles[0], angles[1], angles[2]);
+				return 0;
+			}
+
+		} else {
+			PX4_WARN("not running");
+			usage();
+			return 1;
+		}
+	}
+
+	else if (!strcmp(argv[1], "status")) {
+		if (thread_running.load() && g_thread_data && g_thread_data->test_input) {
+
+			if (g_thread_data->input_objs_len == 0) {
+				PX4_INFO("Input: None");
+
+			} else {
+				PX4_INFO("Input Selected");
+
+				for (int i = 0; i < g_thread_data->input_objs_len; ++i) {
+					if (i == g_thread_data->last_input_active) {
+						g_thread_data->input_objs[i]->print_status();
+					}
+				}
+
+				PX4_INFO("Input not selected");
+
+				for (int i = 0; i < g_thread_data->input_objs_len; ++i) {
+					if (i != g_thread_data->last_input_active) {
+						g_thread_data->input_objs[i]->print_status();
+					}
+				}
+
+			}
+
+			if (g_thread_data->output_obj) {
+				g_thread_data->output_obj->print_status();
+
+			} else {
+				PX4_INFO("Output: None");
+			}
+
+		} else {
+			PX4_INFO("not running");
+		}
+
+		return 0;
+	}
+
+	PX4_ERR("unrecognized command");
+	usage();
+	return -1;
+}
+
+void update_params(ParameterHandles &param_handles, Parameters &params)
+{
+	param_get(param_handles.mnt_mode_in, &params.mnt_mode_in);
+	param_get(param_handles.mnt_mode_out, &params.mnt_mode_out);
+	param_get(param_handles.mnt_mav_sysid_v1, &params.mnt_mav_sysid_v1);
+	param_get(param_handles.mnt_mav_compid_v1, &params.mnt_mav_compid_v1);
+	param_get(param_handles.mnt_ob_lock_mode, &params.mnt_ob_lock_mode);
+	param_get(param_handles.mnt_ob_norm_mode, &params.mnt_ob_norm_mode);
+	param_get(param_handles.mnt_man_pitch, &params.mnt_man_pitch);
+	param_get(param_handles.mnt_man_roll, &params.mnt_man_roll);
+	param_get(param_handles.mnt_man_yaw, &params.mnt_man_yaw);
+	param_get(param_handles.mnt_do_stab, &params.mnt_do_stab);
+	param_get(param_handles.mnt_range_pitch, &params.mnt_range_pitch);
+	param_get(param_handles.mnt_range_roll, &params.mnt_range_roll);
+	param_get(param_handles.mnt_range_yaw, &params.mnt_range_yaw);
+	param_get(param_handles.mnt_off_pitch, &params.mnt_off_pitch);
+	param_get(param_handles.mnt_off_roll, &params.mnt_off_roll);
+	param_get(param_handles.mnt_off_yaw, &params.mnt_off_yaw);
+	param_get(param_handles.mav_sysid, &params.mav_sysid);
+	param_get(param_handles.mav_compid, &params.mav_compid);
+	param_get(param_handles.mnt_rate_pitch, &params.mnt_rate_pitch);
+	param_get(param_handles.mnt_rate_yaw, &params.mnt_rate_yaw);
+	param_get(param_handles.mnt_rc_in_mode, &params.mnt_rc_in_mode);
+}
+
+bool initialize_params(ParameterHandles &param_handles, Parameters &params)
+{
+	param_handles.mnt_mode_in = param_find("MNT_MODE_IN");
+	param_handles.mnt_mode_out = param_find("MNT_MODE_OUT");
+	param_handles.mnt_mav_sysid_v1 = param_find("MNT_MAV_SYSID");
+	param_handles.mnt_mav_compid_v1 = param_find("MNT_MAV_COMPID");
+	param_handles.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE");
+	param_handles.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE");
+	param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH");
+	param_handles.mnt_man_roll = param_find("MNT_MAN_ROLL");
+	param_handles.mnt_man_yaw = param_find("MNT_MAN_YAW");
+	param_handles.mnt_do_stab = param_find("MNT_DO_STAB");
+	param_handles.mnt_range_pitch = param_find("MNT_RANGE_PITCH");
+	param_handles.mnt_range_roll = param_find("MNT_RANGE_ROLL");
+	param_handles.mnt_range_yaw = param_find("MNT_RANGE_YAW");
+	param_handles.mnt_off_pitch = param_find("MNT_OFF_PITCH");
+	param_handles.mnt_off_roll = param_find("MNT_OFF_ROLL");
+	param_handles.mnt_off_yaw = param_find("MNT_OFF_YAW");
+	param_handles.mav_sysid = param_find("MAV_SYS_ID");
+	param_handles.mav_compid = param_find("MAV_COMP_ID");
+	param_handles.mnt_rate_pitch = param_find("MNT_RATE_PITCH");
+	param_handles.mnt_rate_yaw = param_find("MNT_RATE_YAW");
+	param_handles.mnt_rc_in_mode = param_find("MNT_RC_IN_MODE");
+
+	if (param_handles.mnt_mode_in == PARAM_INVALID ||
+	    param_handles.mnt_mode_out == PARAM_INVALID ||
+	    param_handles.mnt_mav_sysid_v1 == PARAM_INVALID ||
+	    param_handles.mnt_mav_compid_v1 == PARAM_INVALID ||
+	    param_handles.mnt_ob_lock_mode == PARAM_INVALID ||
+	    param_handles.mnt_ob_norm_mode == PARAM_INVALID ||
+	    param_handles.mnt_man_pitch == PARAM_INVALID ||
+	    param_handles.mnt_man_roll == PARAM_INVALID ||
+	    param_handles.mnt_man_yaw == PARAM_INVALID ||
+	    param_handles.mnt_do_stab == PARAM_INVALID ||
+	    param_handles.mnt_range_pitch == PARAM_INVALID ||
+	    param_handles.mnt_range_roll == PARAM_INVALID ||
+	    param_handles.mnt_range_yaw == PARAM_INVALID ||
+	    param_handles.mnt_off_pitch == PARAM_INVALID ||
+	    param_handles.mnt_off_roll == PARAM_INVALID ||
+	    param_handles.mnt_off_yaw == PARAM_INVALID ||
+	    param_handles.mav_sysid == PARAM_INVALID ||
+	    param_handles.mav_compid == PARAM_INVALID ||
+	    param_handles.mnt_rate_pitch == PARAM_INVALID ||
+	    param_handles.mnt_rate_yaw == PARAM_INVALID ||
+	    param_handles.mnt_rc_in_mode == PARAM_INVALID
+	   ) {
+		return false;
+	}
+
+	update_params(param_handles, params);
+	return true;
+}
+
+static void usage()
+{
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Mount/gimbal Gimbal control driver. It maps several different input methods (eg. RC or MAVLink) to a configured
+output (eg. AUX channels or MAVLink).
+
+Documentation how to use it is on the [gimbal_control](https://docs.px4.io/master/en/advanced/gimbal_control.html) page.
+
+### Examples
+Test the output by setting a angles (all omitted axes are set to 0):
+$ gimbal test pitch -45 yaw 30
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("gimbal", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test the output: set a fixed angle for one or multiple axes (gimbal must be running)");
+	PRINT_MODULE_USAGE_ARG("roll|pitch|yaw <angle>", "Specify an axis and an angle in degrees", false);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}

src/modules/gimbal/gimbal_params.c

@@ -0,0 +1,270 @@
+/****************************************************************************
+*
+*   Copyright (c) 2013-2020 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.
+*
+****************************************************************************/
+
+
+/**
+* Mount input mode
+*
+* This is the protocol used between the ground station and the autopilot.
+*
+* Recommended is Auto, RC only or MAVLink gimbal protocol v2.
+* The rest will be deprecated.
+*
+* @value -1 DISABLED
+* @value 0 Auto (RC and MAVLink gimbal protocol v2)
+* @value 1 RC
+* @value 2 MAVLINK_ROI (protocol v1, to be deprecated)
+* @value 3 MAVLINK_DO_MOUNT (protocol v1, to be deprecated)
+* @value 4 MAVlink gimbal protocol v2
+* @min -1
+* @max 4
+* @group Mount
+* @reboot_required true
+*/
+PARAM_DEFINE_INT32(MNT_MODE_IN, -1);
+
+/**
+* Mount output mode
+*
+* This is the protocol used between the autopilot and a connected gimbal.
+*
+* Recommended is the MAVLink gimbal protocol v2 if the gimbal supports it.
+*
+* @value 0 AUX
+* @value 1 MAVLink gimbal protocol v1
+* @value 2 MAVLink gimbal protocol v2
+* @min 0
+* @max 2
+* @group Mount
+* @reboot_required true
+*/
+PARAM_DEFINE_INT32(MNT_MODE_OUT, 0);
+
+/**
+* Mavlink System ID of the mount
+*
+* If MNT_MODE_OUT is MAVLink gimbal protocol v1, mount configure/control commands will be sent with this target ID.
+*
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_MAV_SYSID, 1);
+
+/**
+* Mavlink Component ID of the mount
+*
+* If MNT_MODE_OUT is MAVLink protocol v2, mount configure/control commands will be sent with this component ID.
+*
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_MAV_COMPID, 154);
+
+/**
+* Mixer value for selecting normal mode
+*
+* if required by the gimbal (only in AUX output mode)
+*
+* @min -1.0
+* @max 1.0
+* @decimal 3
+* @group Mount
+*/
+PARAM_DEFINE_FLOAT(MNT_OB_NORM_MODE, -1.0f);
+
+/**
+* Mixer value for selecting a locking mode
+*
+* if required for the gimbal (only in AUX output mode)
+*
+* @min -1.0
+* @max 1.0
+* @decimal 3
+* @group Mount
+*/
+PARAM_DEFINE_FLOAT(MNT_OB_LOCK_MODE, 0.0f);
+
+/**
+* Auxiliary channel to control roll (in AUX input or manual mode).
+*
+* @value 0 Disable
+* @value 1 AUX1
+* @value 2 AUX2
+* @value 3 AUX3
+* @value 4 AUX4
+* @value 5 AUX5
+* @value 6 AUX6
+* @min 0
+* @max 6
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_MAN_ROLL, 0);
+
+/**
+* Auxiliary channel to control pitch (in AUX input or manual mode).
+*
+* @value 0 Disable
+* @value 1 AUX1
+* @value 2 AUX2
+* @value 3 AUX3
+* @value 4 AUX4
+* @value 5 AUX5
+* @value 6 AUX6
+* @min 0
+* @max 6
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_MAN_PITCH, 0);
+
+/**
+* Auxiliary channel to control yaw (in AUX input or manual mode).
+*
+* @value 0 Disable
+* @value 1 AUX1
+* @value 2 AUX2
+* @value 3 AUX3
+* @value 4 AUX4
+* @value 5 AUX5
+* @value 6 AUX6
+* @min 0
+* @max 6
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_MAN_YAW, 0);
+
+/**
+* Stabilize the mount
+*
+* Set to true for servo gimbal, false for passthrough.
+* This is required for a gimbal which is not capable of stabilizing itself
+* and relies on the IMU's attitude estimation.
+*
+* @value 0 Disable
+* @value 1 Stabilize all axis
+* @value 2 Stabilize yaw for absolute/lock mode.
+* @min 0
+* @max 2
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_DO_STAB, 0);
+
+/**
+* Range of pitch channel output in degrees (only in AUX output mode).
+*
+* @min 1.0
+* @max 720.0
+* @decimal 1
+* @group Mount
+*/
+PARAM_DEFINE_FLOAT(MNT_RANGE_PITCH, 90.0f);
+
+/**
+* Range of roll channel output in degrees (only in AUX output mode).
+*
+* @min 1.0
+* @max 720.0
+* @decimal 1
+* @group Mount
+*/
+PARAM_DEFINE_FLOAT(MNT_RANGE_ROLL, 90.0f);
+
+/**
+* Range of yaw channel output in degrees (only in AUX output mode).
+*
+* @min 1.0
+* @max 720.0
+* @decimal 1
+* @group Mount
+*/
+PARAM_DEFINE_FLOAT(MNT_RANGE_YAW, 360.0f);
+
+/**
+* Offset for pitch channel output in degrees.
+*
+* @min -360.0
+* @max 360.0
+* @decimal 1
+* @group Mount
+*/
+PARAM_DEFINE_FLOAT(MNT_OFF_PITCH, 0.0f);
+
+/**
+* Offset for roll channel output in degrees.
+*
+* @min -360.0
+* @max 360.0
+* @decimal 1
+* @group Mount
+*/
+PARAM_DEFINE_FLOAT(MNT_OFF_ROLL, 0.0f);
+
+/**
+* Offset for yaw channel output in degrees.
+*
+* @min -360.0
+* @max 360.0
+* @decimal 1
+* @group Mount
+*/
+PARAM_DEFINE_FLOAT(MNT_OFF_YAW, 0.0f);
+
+/**
+ * Angular pitch rate for manual input in degrees/second.
+ *
+ * Full stick input [-1..1] translats to [-pitch rate..pitch rate].
+ *
+ * @min 1.0
+ * @max 90.0
+ * @group Mount
+ */
+PARAM_DEFINE_FLOAT(MNT_RATE_PITCH, 30.0f);
+
+/**
+ * Angular yaw rate for manual input in degrees/second.
+ *
+ * Full stick input [-1..1] translats to [-yaw rate..yaw rate].
+ *
+ * @min 1.0
+ * @max 90.0
+ * @group Mount
+ */
+PARAM_DEFINE_FLOAT(MNT_RATE_YAW, 30.0f);
+
+/**
+ * Input mode for RC gimbal input
+ *
+ * @value 0 Angle
+ * @value 1 Angular rate
+ * @min 0
+ * @max 1
+ * @group Mount
+ */
+PARAM_DEFINE_INT32(MNT_RC_IN_MODE, 1);

src/modules/gimbal/gimbal_params.h

@@ -0,0 +1,91 @@
+/****************************************************************************
+*
+*   Copyright (c) 2022 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.
+*
+****************************************************************************/
+
+
+#pragma once
+
+#include <stdint.h>
+#include <lib/parameters/param.h>
+
+namespace gimbal
+{
+
+struct Parameters {
+	int32_t mnt_mode_in;
+	int32_t mnt_mode_out;
+	int32_t mnt_mav_sysid_v1;
+	int32_t mnt_mav_compid_v1;
+	float mnt_ob_lock_mode;
+	float mnt_ob_norm_mode;
+	int32_t mnt_man_pitch;
+	int32_t mnt_man_roll;
+	int32_t mnt_man_yaw;
+	int32_t mnt_do_stab;
+	float mnt_range_pitch;
+	float mnt_range_roll;
+	float mnt_range_yaw;
+	float mnt_off_pitch;
+	float mnt_off_roll;
+	float mnt_off_yaw;
+	int32_t mav_sysid;
+	int32_t mav_compid;
+	float mnt_rate_pitch;
+	float mnt_rate_yaw;
+	int32_t mnt_rc_in_mode;
+};
+
+struct ParameterHandles {
+	param_t mnt_mode_in;
+	param_t mnt_mode_out;
+	param_t mnt_mav_sysid_v1;
+	param_t mnt_mav_compid_v1;
+	param_t mnt_ob_lock_mode;
+	param_t mnt_ob_norm_mode;
+	param_t mnt_man_pitch;
+	param_t mnt_man_roll;
+	param_t mnt_man_yaw;
+	param_t mnt_do_stab;
+	param_t mnt_range_pitch;
+	param_t mnt_range_roll;
+	param_t mnt_range_yaw;
+	param_t mnt_off_pitch;
+	param_t mnt_off_roll;
+	param_t mnt_off_yaw;
+	param_t mav_sysid;
+	param_t mav_compid;
+	param_t mnt_rate_pitch;
+	param_t mnt_rate_yaw;
+	param_t mnt_rc_in_mode;
+};
+
+} /* namespace gimbal */

src/modules/gimbal/input.cpp

@@ -0,0 +1,59 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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 "input.h"
+
+
+namespace gimbal
+{
+
+InputBase::InputBase(Parameters &parameters) :
+	_parameters(parameters)
+{}
+
+void InputBase::control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude,
+		float roll_angle,
+		float pitch_fixed_angle)
+{
+	control_data.type = ControlData::Type::LonLat;
+	control_data.type_data.lonlat.lon = lon;
+	control_data.type_data.lonlat.lat = lat;
+	control_data.type_data.lonlat.altitude = altitude;
+	control_data.type_data.lonlat.roll_offset = roll_angle;
+	control_data.type_data.lonlat.pitch_fixed_angle = pitch_fixed_angle;
+	control_data.type_data.lonlat.pitch_offset = 0.f;
+	control_data.type_data.lonlat.yaw_offset = 0.f;
+}
+
+
+} /* namespace gimbal */

src/modules/gimbal/input.h

@@ -0,0 +1,70 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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.
+*
+****************************************************************************/
+
+#pragma once
+
+#include "common.h"
+#include "math.h"
+#include "gimbal_params.h"
+
+namespace gimbal
+{
+
+struct Parameters;
+
+class InputBase
+{
+public:
+	enum class UpdateResult {
+		NoUpdate,
+		UpdatedActive,
+		UpdatedActiveOnce,
+		UpdatedNotActive,
+	};
+
+	InputBase() = delete;
+	explicit InputBase(Parameters &parameters);
+	virtual ~InputBase() = default;
+
+	virtual int initialize() = 0;
+	virtual UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) = 0;
+	virtual void print_status() const = 0;
+protected:
+	void control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude, float roll_angle = NAN,
+				      float pitch_fixed_angle = NAN);
+
+	Parameters &_parameters;
+};
+
+
+} /* namespace gimbal */

src/modules/gimbal/input_mavlink.cpp

@@ -0,0 +1,973 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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 "input_mavlink.h"
+#include <uORB/Publication.hpp>
+#include <uORB/topics/gimbal_manager_information.h>
+#include <uORB/topics/vehicle_command_ack.h>
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/posix.h>
+#include <errno.h>
+#include <math.h>
+#include <matrix/matrix/math.hpp>
+
+namespace gimbal
+{
+
+InputMavlinkROI::InputMavlinkROI(Parameters &parameters) :
+	InputBase(parameters) {}
+
+InputMavlinkROI::~InputMavlinkROI()
+{
+	if (_vehicle_roi_sub >= 0) {
+		orb_unsubscribe(_vehicle_roi_sub);
+	}
+
+	if (_position_setpoint_triplet_sub >= 0) {
+		orb_unsubscribe(_position_setpoint_triplet_sub);
+	}
+}
+
+int InputMavlinkROI::initialize()
+{
+	_vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
+
+	if (_vehicle_roi_sub < 0) {
+		return -errno;
+	}
+
+	_position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+
+	if (_position_setpoint_triplet_sub < 0) {
+		return -errno;
+	}
+
+	return 0;
+}
+
+InputMavlinkROI::UpdateResult
+InputMavlinkROI::update(unsigned int timeout_ms, ControlData &control_data, bool already_active)
+{
+	const int num_poll = 2;
+	px4_pollfd_struct_t polls[num_poll];
+	polls[0].fd = _vehicle_roi_sub;
+	polls[0].events = POLLIN;
+	polls[1].fd = _position_setpoint_triplet_sub;
+	polls[1].events = POLLIN;
+
+	int ret = px4_poll(polls, num_poll, timeout_ms);
+
+	if (ret <= 0) {
+		return UpdateResult::NoUpdate;
+	}
+
+	if (polls[0].revents & POLLIN) {
+		vehicle_roi_s vehicle_roi;
+		orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi);
+
+		if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) {
+
+			control_data.type = ControlData::Type::Neutral;
+			_cur_roi_mode = vehicle_roi.mode;
+			control_data.sysid_primary_control = _parameters.mav_sysid;
+			control_data.compid_primary_control = _parameters.mav_compid;
+
+			return UpdateResult::UpdatedActiveOnce;
+
+		} else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) {
+			control_data.type = ControlData::Type::LonLat;
+			_read_control_data_from_position_setpoint_sub(control_data);
+			control_data.type_data.lonlat.pitch_fixed_angle = -10.f;
+
+			control_data.type_data.lonlat.roll_offset = vehicle_roi.roll_offset;
+			control_data.type_data.lonlat.pitch_offset = vehicle_roi.pitch_offset;
+			control_data.type_data.lonlat.yaw_offset = vehicle_roi.yaw_offset;
+
+			_cur_roi_mode = vehicle_roi.mode;
+
+			return UpdateResult::UpdatedActive;
+
+		} else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) {
+			control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt);
+
+			_cur_roi_mode = vehicle_roi.mode;
+
+			return UpdateResult::UpdatedActive;
+
+		} else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) {
+			PX4_WARN("ROI_TARGET not supported yet");
+			return UpdateResult::NoUpdate;
+		}
+
+		return UpdateResult::NoUpdate;
+	}
+
+	// check whether the position setpoint has been updated
+	if (polls[1].revents & POLLIN) {
+		if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) {
+			_read_control_data_from_position_setpoint_sub(control_data);
+
+			return UpdateResult::UpdatedActive;
+
+		} else { // must do an orb_copy() in *every* case
+			position_setpoint_triplet_s position_setpoint_triplet;
+			orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub,
+				 &position_setpoint_triplet);
+		}
+	}
+
+	return UpdateResult::NoUpdate;
+}
+
+void InputMavlinkROI::_read_control_data_from_position_setpoint_sub(ControlData &control_data)
+{
+	position_setpoint_triplet_s position_setpoint_triplet;
+	orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet);
+	control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon;
+	control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat;
+	control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt;
+}
+
+void InputMavlinkROI::print_status() const
+{
+	PX4_INFO("Input: Mavlink (ROI)");
+}
+
+InputMavlinkCmdMount::InputMavlinkCmdMount(Parameters &parameters) :
+	InputBase(parameters)
+{}
+
+InputMavlinkCmdMount::~InputMavlinkCmdMount()
+{
+	if (_vehicle_command_sub >= 0) {
+		orb_unsubscribe(_vehicle_command_sub);
+	}
+}
+
+int InputMavlinkCmdMount::initialize()
+{
+	if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) {
+		return -errno;
+	}
+
+	// rate-limit inputs to 100Hz. If we don't do this and the output is configured to mavlink mode,
+	// it will publish vehicle_command's as well, causing the input poll() in here to return
+	// immediately, which in turn will cause an output update and thus a busy loop.
+	orb_set_interval(_vehicle_command_sub, 10);
+
+	return 0;
+}
+
+
+InputMavlinkCmdMount::UpdateResult
+InputMavlinkCmdMount::update(unsigned int timeout_ms, ControlData &control_data, bool already_active)
+{
+	const int num_poll = 1;
+	px4_pollfd_struct_t polls[num_poll];
+	polls[0].fd = _vehicle_command_sub;
+	polls[0].events = POLLIN;
+
+	int poll_timeout = (int) timeout_ms;
+
+	const hrt_abstime poll_start = hrt_absolute_time();
+
+	// If we get a command that we need to handle we exit the loop, otherwise we poll until we
+	// reach the timeout.
+	// We can't return early instead because we need to copy all topics that triggered poll.
+
+	bool exit_loop = false;
+	UpdateResult update_result = UpdateResult::NoUpdate;
+
+	while (!exit_loop && poll_timeout >= 0) {
+
+		const int ret = px4_poll(polls, num_poll, poll_timeout);
+
+		if (ret <= 0) {
+			// Error, or timeout, give up.
+			exit_loop = true;
+		}
+
+		if (polls[0].revents & POLLIN) {
+
+			vehicle_command_s vehicle_command;
+			orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command);
+
+			update_result = _process_command(control_data, vehicle_command);
+
+		}
+
+		if (update_result != UpdateResult::NoUpdate) {
+			exit_loop = true;
+		}
+
+		// Keep going reading commands until timeout is up.
+		poll_timeout = timeout_ms - (hrt_absolute_time() - poll_start) / 1000;
+	}
+
+	return update_result;
+}
+
+InputMavlinkCmdMount::UpdateResult
+InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_command_s &vehicle_command)
+{
+	// Process only if the command is for us or for anyone (component id 0).
+	const bool sysid_correct = (vehicle_command.target_system == _parameters.mav_sysid);
+	const bool compid_correct = ((vehicle_command.target_component == _parameters.mav_compid) ||
+				     (vehicle_command.target_component == 0));
+
+	if (!sysid_correct || !compid_correct) {
+		return UpdateResult::NoUpdate;
+	}
+
+	if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) {
+
+		UpdateResult update_result = UpdateResult::NoUpdate;
+
+		switch ((int) vehicle_command.param7) {
+		case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
+			control_data.gimbal_shutter_retract = true;
+
+		// fallthrough
+		case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
+			control_data.type = ControlData::Type::Neutral;
+			control_data.sysid_primary_control = vehicle_command.source_system;
+			control_data.compid_primary_control = vehicle_command.source_component;
+			update_result = UpdateResult::UpdatedActiveOnce;
+			break;
+
+		case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: {
+				control_data.type = ControlData::Type::Angle;
+				control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+				control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+				control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+
+				// gimbal spec has roll on channel 0, MAVLink spec has pitch on channel 0
+				const float roll = math::radians(vehicle_command.param2);
+				// gimbal spec has pitch on channel 1, MAVLink spec has roll on channel 1
+				const float pitch = math::radians(vehicle_command.param1);
+				// both specs have yaw on channel 2
+				float yaw = math::radians(vehicle_command.param3);
+
+				matrix::Eulerf euler(roll, pitch, yaw);
+
+				matrix::Quatf q(euler);
+				q.copyTo(control_data.type_data.angle.q);
+
+				control_data.type_data.angle.angular_velocity[0] = NAN;
+				control_data.type_data.angle.angular_velocity[1] = NAN;
+				control_data.type_data.angle.angular_velocity[2] = NAN;
+				control_data.sysid_primary_control = vehicle_command.source_system;
+				control_data.compid_primary_control = vehicle_command.source_component;
+				update_result = UpdateResult::UpdatedActive;
+			}
+			break;
+
+		case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
+			// Take over control ourselves, that's supposedly what RC means.
+			control_data.sysid_primary_control = _parameters.mav_sysid;
+			control_data.compid_primary_control = _parameters.mav_compid;
+			update_result = UpdateResult::NoUpdate;
+			break;
+
+		case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
+			control_data_set_lon_lat(control_data, (double)vehicle_command.param6, (double)vehicle_command.param5,
+						 vehicle_command.param4);
+			control_data.sysid_primary_control = vehicle_command.source_system;
+			control_data.compid_primary_control = vehicle_command.source_component;
+			update_result = UpdateResult::UpdatedActive;
+			break;
+		}
+
+		_ack_vehicle_command(vehicle_command);
+		return update_result;
+
+	} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
+
+		// Stabilization params are ignored. Use MNT_DO_STAB param instead.
+
+		const int params[] = {
+			(int)((float) vehicle_command.param5 + 0.5f),
+			(int)((float) vehicle_command.param6 + 0.5f),
+			(int)(vehicle_command.param7 + 0.5f)
+		};
+
+		for (int i = 0; i < 3; ++i) {
+
+			if (params[i] == 0) {
+				control_data.type_data.angle.frames[i] =
+					ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+
+			} else if (params[i] == 1) {
+				control_data.type_data.angle.frames[i] =
+					ControlData::TypeData::TypeAngle::Frame::AngularRate;
+
+			} else if (params[i] == 2) {
+				control_data.type_data.angle.frames[i] =
+					ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+
+			} else {
+				// Not supported, fallback to body angle.
+				control_data.type_data.angle.frames[i] =
+					ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+			}
+		}
+
+		control_data.sysid_primary_control = vehicle_command.source_system;
+		control_data.compid_primary_control = vehicle_command.source_component;
+
+		_ack_vehicle_command(vehicle_command);
+		return UpdateResult::UpdatedActive;
+	}
+
+	return UpdateResult::NoUpdate;
+}
+
+void InputMavlinkCmdMount::_ack_vehicle_command(const vehicle_command_s &cmd)
+{
+	vehicle_command_ack_s vehicle_command_ack{};
+
+	vehicle_command_ack.timestamp = hrt_absolute_time();
+	vehicle_command_ack.command = cmd.command;
+	vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+	vehicle_command_ack.target_system = cmd.source_system;
+	vehicle_command_ack.target_component = cmd.source_component;
+
+	uORB::Publication<vehicle_command_ack_s> cmd_ack_pub{ORB_ID(vehicle_command_ack)};
+	cmd_ack_pub.publish(vehicle_command_ack);
+}
+
+void InputMavlinkCmdMount::print_status() const
+{
+	PX4_INFO("Input: Mavlink (CMD_MOUNT)");
+}
+
+InputMavlinkGimbalV2::InputMavlinkGimbalV2(Parameters &parameters) :
+	InputBase(parameters)
+{
+	_stream_gimbal_manager_information();
+}
+
+InputMavlinkGimbalV2::~InputMavlinkGimbalV2()
+{
+	if (_vehicle_roi_sub >= 0) {
+		orb_unsubscribe(_vehicle_roi_sub);
+	}
+
+	if (_position_setpoint_triplet_sub >= 0) {
+		orb_unsubscribe(_position_setpoint_triplet_sub);
+	}
+
+	if (_gimbal_manager_set_attitude_sub >= 0) {
+		orb_unsubscribe(_gimbal_manager_set_attitude_sub);
+	}
+
+	if (_vehicle_command_sub >= 0) {
+		orb_unsubscribe(_vehicle_command_sub);
+	}
+
+	if (_gimbal_manager_set_manual_control_sub >= 0) {
+		orb_unsubscribe(_gimbal_manager_set_manual_control_sub);
+	}
+}
+
+
+void InputMavlinkGimbalV2::print_status() const
+{
+	PX4_INFO("Input: Mavlink (Gimbal V2)");
+}
+
+int InputMavlinkGimbalV2::initialize()
+{
+	_vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
+
+	if (_vehicle_roi_sub < 0) {
+		return -errno;
+	}
+
+	_position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+
+	if (_position_setpoint_triplet_sub < 0) {
+		return -errno;
+	}
+
+	_gimbal_manager_set_attitude_sub = orb_subscribe(ORB_ID(gimbal_manager_set_attitude));
+
+	if (_gimbal_manager_set_attitude_sub < 0) {
+		return -errno;
+	}
+
+	if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) {
+		return -errno;
+	}
+
+	if ((_gimbal_manager_set_manual_control_sub = orb_subscribe(ORB_ID(gimbal_manager_set_manual_control))) < 0) {
+		return -errno;
+	}
+
+	// rate-limit inputs to 100Hz. If we don't do this and the output is configured to mavlink mode,
+	// it will publish vehicle_command's as well, causing the input poll() in here to return
+	// immediately, which in turn will cause an output update and thus a busy loop.
+	orb_set_interval(_vehicle_command_sub, 10);
+
+	return 0;
+}
+
+void InputMavlinkGimbalV2::_stream_gimbal_manager_status(const ControlData &control_data)
+{
+	gimbal_device_attitude_status_s gimbal_device_attitude_status{};
+
+	if (_gimbal_device_attitude_status_sub.updated()) {
+		_gimbal_device_attitude_status_sub.copy(&gimbal_device_attitude_status);
+
+		gimbal_manager_status_s gimbal_manager_status{};
+		gimbal_manager_status.timestamp = hrt_absolute_time();
+		gimbal_manager_status.flags = gimbal_device_attitude_status.device_flags;
+		gimbal_manager_status.gimbal_device_id = 0;
+		gimbal_manager_status.primary_control_sysid = control_data.sysid_primary_control;
+		gimbal_manager_status.primary_control_compid = control_data.compid_primary_control;
+		gimbal_manager_status.secondary_control_sysid = 0; // TODO: support secondary control
+		gimbal_manager_status.secondary_control_compid = 0; // TODO: support secondary control
+		_gimbal_manager_status_pub.publish(gimbal_manager_status);
+	}
+}
+
+void InputMavlinkGimbalV2::_stream_gimbal_manager_information()
+{
+	// TODO: Take gimbal_device_information into account.
+
+	gimbal_manager_information_s gimbal_manager_info;
+	gimbal_manager_info.timestamp = hrt_absolute_time();
+
+	gimbal_manager_info.cap_flags =
+		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL |
+		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK |
+		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS |
+		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK |
+		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS |
+		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK |
+		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL;
+
+	gimbal_manager_info.pitch_max = M_PI_F / 2;
+	gimbal_manager_info.pitch_min = -M_PI_F / 2;
+	gimbal_manager_info.yaw_max = M_PI_F;
+	gimbal_manager_info.yaw_min = -M_PI_F;
+
+	_gimbal_manager_info_pub.publish(gimbal_manager_info);
+}
+
+InputMavlinkGimbalV2::UpdateResult
+InputMavlinkGimbalV2::update(unsigned int timeout_ms, ControlData &control_data, bool already_active)
+{
+
+	const int num_poll = 5;
+	px4_pollfd_struct_t polls[num_poll];
+	polls[0].fd = _gimbal_manager_set_attitude_sub;
+	polls[0].events = POLLIN;
+	polls[1].fd = _vehicle_roi_sub;
+	polls[1].events = POLLIN;
+	polls[2].fd = _position_setpoint_triplet_sub;
+	polls[2].events = POLLIN;
+	polls[3].fd = _vehicle_command_sub;
+	polls[3].events = POLLIN;
+	polls[4].fd = _gimbal_manager_set_manual_control_sub;
+	polls[4].events = POLLIN;
+
+	int poll_timeout = (int) timeout_ms;
+
+	hrt_abstime poll_start = hrt_absolute_time();
+
+	// If we get a command that we need to handle we exit the loop, otherwise we poll until we
+	// reach the timeout.
+	// We can't return early instead because we need to copy all topics that triggered poll.
+
+	bool exit_loop = false;
+	UpdateResult update_result = already_active ? UpdateResult::UpdatedActive : UpdateResult::NoUpdate;
+
+	while (!exit_loop && poll_timeout >= 0) {
+
+		const int ret = px4_poll(polls, num_poll, poll_timeout);
+
+		if (ret <= 0) {
+			// Error, or timeout, give up.
+			exit_loop = true;
+		}
+
+		if (polls[0].revents & POLLIN) {
+			gimbal_manager_set_attitude_s set_attitude;
+			orb_copy(ORB_ID(gimbal_manager_set_attitude), _gimbal_manager_set_attitude_sub, &set_attitude);
+
+			update_result = _process_set_attitude(control_data, set_attitude);
+		}
+
+		if (polls[1].revents & POLLIN) {
+			vehicle_roi_s vehicle_roi;
+			orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi);
+
+			UpdateResult new_result = _process_vehicle_roi(control_data, vehicle_roi);
+
+			if (new_result != UpdateResult::NoUpdate) {
+				update_result = new_result;
+			}
+		}
+
+		// check whether the position setpoint got updated
+		if (polls[2].revents & POLLIN) {
+			position_setpoint_triplet_s position_setpoint_triplet;
+			orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub,
+				 &position_setpoint_triplet);
+
+			UpdateResult new_result = _process_position_setpoint_triplet(control_data, position_setpoint_triplet);
+
+			if (new_result != UpdateResult::NoUpdate) {
+				update_result = new_result;
+			}
+		}
+
+		if (polls[3].revents & POLLIN) {
+			vehicle_command_s vehicle_command;
+			orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command);
+
+			UpdateResult new_result = _process_command(control_data, vehicle_command);
+
+			if (new_result != UpdateResult::NoUpdate) {
+				update_result = new_result;
+			}
+		}
+
+		if (polls[4].revents & POLLIN) {
+			gimbal_manager_set_manual_control_s set_manual_control;
+			orb_copy(ORB_ID(gimbal_manager_set_manual_control), _gimbal_manager_set_manual_control_sub,
+				 &set_manual_control);
+
+			update_result = _process_set_manual_control(control_data, set_manual_control);
+		}
+
+		poll_timeout = timeout_ms - (hrt_absolute_time() - poll_start) / 1000;
+	}
+
+	_stream_gimbal_manager_status(control_data);
+
+	return update_result;
+}
+
+InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_attitude(ControlData &control_data,
+		const gimbal_manager_set_attitude_s &set_attitude)
+{
+
+	if (set_attitude.origin_sysid == control_data.sysid_primary_control &&
+	    set_attitude.origin_compid == control_data.compid_primary_control) {
+		const matrix::Quatf q(set_attitude.q);
+		const matrix::Vector3f angular_velocity(set_attitude.angular_velocity_x,
+							set_attitude.angular_velocity_y,
+							set_attitude.angular_velocity_z);
+
+		_set_control_data_from_set_attitude(control_data, set_attitude.flags, q, angular_velocity);
+		return UpdateResult::UpdatedActive;
+
+	} else {
+		PX4_DEBUG("Ignoring gimbal_manager_set_attitude from %d/%d",
+			  set_attitude.origin_sysid, set_attitude.origin_compid);
+		return UpdateResult::UpdatedNotActive;
+	}
+}
+
+InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_vehicle_roi(ControlData &control_data,
+		const vehicle_roi_s &vehicle_roi)
+{
+	control_data.gimbal_shutter_retract = false;
+
+	if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) {
+
+		control_data.type = ControlData::Type::Neutral;
+		_cur_roi_mode = vehicle_roi.mode;
+		return UpdateResult::UpdatedActiveOnce;
+
+	} else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) {
+		control_data.type = ControlData::Type::LonLat;
+		_read_control_data_from_position_setpoint_sub(control_data);
+		control_data.type_data.lonlat.pitch_fixed_angle = -10.f;
+
+		control_data.type_data.lonlat.roll_offset = vehicle_roi.roll_offset;
+		control_data.type_data.lonlat.pitch_offset = vehicle_roi.pitch_offset;
+		control_data.type_data.lonlat.yaw_offset = vehicle_roi.yaw_offset;
+
+		_cur_roi_mode = vehicle_roi.mode;
+
+		return UpdateResult::UpdatedActive;
+
+	} else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) {
+		control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt);
+
+		_cur_roi_mode = vehicle_roi.mode;
+
+		return UpdateResult::UpdatedActive;
+
+	} else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) {
+		//TODO is this even supported?
+
+		return UpdateResult::NoUpdate;
+
+	}
+
+	return UpdateResult::NoUpdate;
+}
+
+InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_position_setpoint_triplet(ControlData &control_data,
+		const position_setpoint_triplet_s &position_setpoint_triplet)
+{
+	if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) {
+		control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon;
+		control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat;
+		control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt;
+		return UpdateResult::UpdatedActive;
+
+	} else {
+		return UpdateResult::NoUpdate;
+	}
+}
+
+InputMavlinkGimbalV2::UpdateResult
+InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_command_s &vehicle_command)
+{
+	// Process only if the command is for us or for anyone (component id 0).
+	const bool sysid_correct =
+		(vehicle_command.target_system == _parameters.mav_sysid) || (vehicle_command.target_system == 0);
+	const bool compid_correct = ((vehicle_command.target_component == _parameters.mav_compid) ||
+				     (vehicle_command.target_component == 0));
+
+	if (!sysid_correct || !compid_correct) {
+		return UpdateResult::NoUpdate;
+	}
+
+	if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) {
+		// FIXME: Remove me later. For now, we support this for legacy missions
+		//        using gimbal v1 protocol.
+
+		UpdateResult update_result = UpdateResult::NoUpdate;
+
+		switch ((int) vehicle_command.param7) {
+		case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
+			control_data.gimbal_shutter_retract = true;
+
+		// fallthrough
+
+		case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
+			control_data.type = ControlData::Type::Neutral;
+			update_result = InputBase::UpdateResult::UpdatedActiveOnce;
+			break;
+
+		case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: {
+				control_data.type = ControlData::Type::Angle;
+				control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+				control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+				control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+
+				// gimbal spec has roll on channel 0, MAVLink spec has pitch on channel 0
+				const float roll = math::radians(vehicle_command.param2);
+				// gimbal spec has pitch on channel 1, MAVLink spec has roll on channel 1
+				const float pitch = math::radians(vehicle_command.param1);
+				// both specs have yaw on channel 2
+				float yaw = math::radians(vehicle_command.param3);
+
+				// We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that.
+				if (yaw > M_PI_F) {
+					yaw -= 2 * M_PI_F;
+				}
+
+				matrix::Eulerf euler(roll, pitch, yaw);
+
+				matrix::Quatf q(euler);
+				q.copyTo(control_data.type_data.angle.q);
+
+				control_data.type_data.angle.angular_velocity[0] = NAN;
+				control_data.type_data.angle.angular_velocity[1] = NAN;
+				control_data.type_data.angle.angular_velocity[2] = NAN;
+				update_result = InputBase::UpdateResult::UpdatedActive;
+			}
+			break;
+
+		case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
+			update_result = UpdateResult::NoUpdate;
+			break;
+
+		case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
+			control_data_set_lon_lat(control_data, (double)vehicle_command.param6, (double)vehicle_command.param5,
+						 vehicle_command.param4);
+			update_result = UpdateResult::UpdatedActive;
+			break;
+		}
+
+		_ack_vehicle_command(vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+		return update_result;
+
+	} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
+
+		// Stabilization params are ignored. Use MNT_DO_STAB param instead.
+
+		const int params[] = {
+			(int)((float) vehicle_command.param5 + 0.5f),
+			(int)((float) vehicle_command.param6 + 0.5f),
+			(int)(vehicle_command.param7 + 0.5f)
+		};
+
+		for (int i = 0; i < 3; ++i) {
+
+			if (params[i] == 0) {
+				control_data.type_data.angle.frames[i] =
+					ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+
+			} else if (params[i] == 1) {
+				control_data.type_data.angle.frames[i] =
+					ControlData::TypeData::TypeAngle::Frame::AngularRate;
+
+			} else if (params[i] == 2) {
+				control_data.type_data.angle.frames[i] =
+					ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+
+			} else {
+				// Not supported, fallback to body angle.
+				control_data.type_data.angle.frames[i] =
+					ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+			}
+		}
+
+		_ack_vehicle_command(vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+		return UpdateResult::UpdatedActive;
+
+	} else if (vehicle_command.command ==
+		   vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE) {
+
+		const int param_sysid = roundf(vehicle_command.param1);
+		const int param_compid = roundf(vehicle_command.param2);
+
+		uint8_t new_sysid_primary_control = [&]() {
+			if (param_sysid >= 0 && param_sysid < 256) {
+				// Valid new sysid.
+				return (uint8_t) param_sysid;
+
+			} else if (param_sysid == -1) {
+				// leave unchanged
+				return control_data.sysid_primary_control;
+
+			} else if (param_sysid == -2) {
+				// set itself
+				return (uint8_t) _parameters.mav_sysid;
+
+			} else if (param_sysid == -3) {
+				// release control if in control
+				if (control_data.sysid_primary_control == vehicle_command.source_system) {
+					return (uint8_t) 0;
+
+				} else {
+					return control_data.sysid_primary_control;
+				}
+
+			} else {
+				PX4_WARN("Unknown param1 value for DO_GIMBAL_MANAGER_CONFIGURE");
+				return control_data.sysid_primary_control;
+			}
+		}();
+
+		uint8_t new_compid_primary_control = [&]() {
+			if (param_compid >= 0 && param_compid < 256) {
+				// Valid new compid.
+				return (uint8_t) param_compid;
+
+			} else if (param_compid == -1) {
+				// leave unchanged
+				return control_data.compid_primary_control;
+
+			} else if (param_compid == -2) {
+				// set itself
+				return (uint8_t) _parameters.mav_compid;
+
+			} else if (param_compid == -3) {
+				// release control if in control
+				if (control_data.compid_primary_control == vehicle_command.source_component) {
+					return (uint8_t) 0;
+
+				} else {
+					return control_data.compid_primary_control;
+				}
+
+			} else {
+				PX4_WARN("Unknown param2 value for DO_GIMBAL_MANAGER_CONFIGURE");
+				return control_data.compid_primary_control;
+			}
+		}();
+
+		_ack_vehicle_command(vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+		if (new_sysid_primary_control != control_data.sysid_primary_control ||
+		    new_compid_primary_control != control_data.compid_primary_control) {
+			PX4_INFO("Configured primary gimbal control sysid/compid from %d/%d to %d/%d",
+				 control_data.sysid_primary_control, control_data.compid_primary_control,
+				 new_sysid_primary_control, new_compid_primary_control);
+			control_data.sysid_primary_control = new_sysid_primary_control;
+			control_data.compid_primary_control = new_compid_primary_control;
+		}
+
+		return UpdateResult::UpdatedActive;
+
+		// TODO: support secondary control
+		// TODO: support gimbal device id for multiple gimbals
+
+	} else if (vehicle_command.command ==
+		   vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW) {
+
+		if (vehicle_command.source_system == control_data.sysid_primary_control &&
+		    vehicle_command.source_component == control_data.compid_primary_control) {
+
+			const matrix::Eulerf euler(0.0f, math::radians(vehicle_command.param1),
+						   math::radians(vehicle_command.param2));
+			const matrix::Quatf q(euler);
+			const matrix::Vector3f angular_velocity(0.0f, vehicle_command.param3,
+								vehicle_command.param4);
+			const uint32_t flags = vehicle_command.param5;
+
+			// TODO: support gimbal device id for multiple gimbals
+
+			_set_control_data_from_set_attitude(control_data, flags, q, angular_velocity);
+			_ack_vehicle_command(vehicle_command,
+					     vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+			return UpdateResult::UpdatedActive;
+
+		} else {
+			PX4_INFO("GIMBAL_MANAGER_PITCHYAW from %d/%d denied, in control: %d/%d",
+				 vehicle_command.source_system,
+				 vehicle_command.source_component,
+				 control_data.sysid_primary_control, control_data.compid_primary_control);
+			_ack_vehicle_command(vehicle_command,
+					     vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
+
+			return UpdateResult::UpdatedNotActive;
+		}
+	}
+
+	return UpdateResult::NoUpdate;
+}
+
+InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_control(ControlData &control_data,
+		const gimbal_manager_set_manual_control_s &set_manual_control)
+{
+	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);
+
+		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);
+
+		_set_control_data_from_set_attitude(control_data, set_manual_control.flags, q,
+						    angular_velocity);
+
+		return UpdateResult::UpdatedActive;
+
+	} else {
+		PX4_DEBUG("Ignoring gimbal_manager_set_manual_control from %d/%d",
+			  set_manual_control.origin_sysid, set_manual_control.origin_compid);
+		return UpdateResult::UpdatedNotActive;
+	}
+}
+
+void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags,
+		const matrix::Quatf &q,
+		const matrix::Vector3f &angular_velocity)
+{
+	if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_RETRACT) != 0) {
+		// not implemented in ControlData
+	} else if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL) != 0) {
+		control_data.type = ControlData::Type::Neutral;
+
+	} else {
+		control_data.type = ControlData::Type::Angle;
+
+		q.copyTo(control_data.type_data.angle.q);
+
+		control_data.type_data.angle.angular_velocity[0] = angular_velocity(0);
+		control_data.type_data.angle.angular_velocity[1] = angular_velocity(1);
+		control_data.type_data.angle.angular_velocity[2] = angular_velocity(2);
+
+		control_data.type_data.angle.frames[0] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK)
+				? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame
+				: ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+
+		control_data.type_data.angle.frames[1] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK)
+				? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame
+				: ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+
+		control_data.type_data.angle.frames[2] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK)
+				? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame
+				: ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+	}
+}
+
+//TODO move this one to input.cpp such that it can be shared across functions
+void InputMavlinkGimbalV2::_ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result)
+{
+	vehicle_command_ack_s vehicle_command_ack{};
+
+	vehicle_command_ack.timestamp = hrt_absolute_time();
+	vehicle_command_ack.command = cmd.command;
+	vehicle_command_ack.result = result;
+	vehicle_command_ack.target_system = cmd.source_system;
+	vehicle_command_ack.target_component = cmd.source_component;
+
+	uORB::Publication<vehicle_command_ack_s> cmd_ack_pub{ORB_ID(vehicle_command_ack)};
+	cmd_ack_pub.publish(vehicle_command_ack);
+}
+
+void InputMavlinkGimbalV2::_read_control_data_from_position_setpoint_sub(ControlData &control_data)
+{
+	position_setpoint_triplet_s position_setpoint_triplet;
+	orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet);
+	control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon;
+	control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat;
+	control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt;
+}
+
+} /* namespace gimbal */

src/modules/gimbal/input_mavlink.h

@@ -0,0 +1,133 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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.
+*
+****************************************************************************/
+
+
+#pragma once
+
+#include "input.h"
+#include "input_rc.h"
+#include <cstdint>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/gimbal_device_attitude_status.h>
+#include <uORB/topics/gimbal_manager_information.h>
+#include <uORB/topics/gimbal_manager_status.h>
+#include <uORB/topics/gimbal_manager_set_attitude.h>
+#include <uORB/topics/gimbal_manager_set_manual_control.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_roi.h>
+#include <lib/geo/geo.h>
+
+
+namespace gimbal
+{
+
+class InputMavlinkROI : public InputBase
+{
+public:
+	InputMavlinkROI() = delete;
+	explicit InputMavlinkROI(Parameters &parameters);
+	virtual ~InputMavlinkROI();
+
+	void print_status() const override;
+	UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
+	int initialize() override;
+
+private:
+	void _read_control_data_from_position_setpoint_sub(ControlData &control_data);
+
+	int _vehicle_roi_sub = -1;
+	int _position_setpoint_triplet_sub = -1;
+	uint8_t _cur_roi_mode {vehicle_roi_s::ROI_NONE};
+};
+
+
+class InputMavlinkCmdMount : public InputBase
+{
+public:
+	InputMavlinkCmdMount() = delete;
+	explicit InputMavlinkCmdMount(Parameters &parameters);
+	virtual ~InputMavlinkCmdMount();
+
+	void print_status() const override;
+	UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
+	int initialize() override;
+
+private:
+	UpdateResult _process_command(ControlData &control_data, const vehicle_command_s &vehicle_command);
+	void _ack_vehicle_command(const vehicle_command_s &cmd);
+
+	int _vehicle_command_sub = -1;
+};
+
+class InputMavlinkGimbalV2 : public InputBase
+{
+public:
+	InputMavlinkGimbalV2() = delete;
+	explicit InputMavlinkGimbalV2(Parameters &parameters);
+	virtual ~InputMavlinkGimbalV2();
+
+	UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
+	int initialize() override;
+	void print_status() const override;
+
+private:
+	UpdateResult _process_set_attitude(ControlData &control_data, const gimbal_manager_set_attitude_s &set_attitude);
+	UpdateResult _process_vehicle_roi(ControlData &control_data, const vehicle_roi_s &vehicle_roi);
+	UpdateResult _process_position_setpoint_triplet(ControlData &control_data,
+			const position_setpoint_triplet_s &position_setpoint_triplet);
+	UpdateResult _process_command(ControlData &control_data, const vehicle_command_s &vehicle_command);
+	UpdateResult _process_set_manual_control(ControlData &control_data,
+			const gimbal_manager_set_manual_control_s &set_manual_control);
+	void _set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags, const matrix::Quatf &q,
+			const matrix::Vector3f &angular_velocity);
+	void _ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result);
+	void _stream_gimbal_manager_information();
+	void _stream_gimbal_manager_status(const ControlData &control_data);
+	void _read_control_data_from_position_setpoint_sub(ControlData &control_data);
+
+	int _vehicle_roi_sub = -1;
+	int _gimbal_manager_set_attitude_sub = -1;
+	int _gimbal_manager_set_manual_control_sub = -1;
+	int _position_setpoint_triplet_sub = -1;
+	int _vehicle_command_sub = -1;
+
+	uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)};
+	uORB::Publication<gimbal_manager_information_s> _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)};
+	uORB::Publication<gimbal_manager_status_s> _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)};
+	uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;
+};
+
+} /* namespace gimbal */

src/modules/gimbal/input_rc.cpp

@@ -0,0 +1,222 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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 "input_rc.h"
+
+#include <math.h>
+#include <errno.h>
+#include <mathlib/mathlib.h>
+#include <matrix/matrix/math.hpp>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/defines.h>
+
+
+namespace gimbal
+{
+
+InputRC::InputRC(Parameters &parameters) :
+	InputBase(parameters)
+{}
+
+InputRC::~InputRC()
+{
+	if (_manual_control_setpoint_sub >= 0) {
+		orb_unsubscribe(_manual_control_setpoint_sub);
+	}
+}
+
+int InputRC::initialize()
+{
+	_manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+
+	if (_manual_control_setpoint_sub < 0) {
+		return -errno;
+	}
+
+	return 0;
+}
+
+InputRC::UpdateResult InputRC::update(unsigned int timeout_ms, ControlData &control_data, bool already_active)
+{
+	px4_pollfd_struct_t polls[1];
+	polls[0].fd = 		_manual_control_setpoint_sub;
+	polls[0].events = 	POLLIN;
+
+	int ret = px4_poll(polls, 1, timeout_ms);
+
+	if (ret < 0) {
+		return UpdateResult::NoUpdate;
+	}
+
+	if (ret == 0) {
+		// If we have been active before, we stay active, unless someone steals
+		// the control away.
+		if (already_active) {
+			return UpdateResult::UpdatedActive;
+		}
+	}
+
+	if (polls[0].revents & POLLIN) {
+		return _read_control_data_from_subscription(control_data, already_active);
+	}
+
+	return UpdateResult::NoUpdate;
+}
+
+InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData &control_data, bool already_active)
+{
+	manual_control_setpoint_s manual_control_setpoint{};
+	orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &manual_control_setpoint);
+	control_data.type = ControlData::Type::Angle;
+
+	float new_aux_values[3];
+
+	for (int i = 0; i < 3; ++i) {
+		new_aux_values[i] = _get_aux_value(manual_control_setpoint, i);
+	}
+
+	// If we were already active previously, we just update normally. Otherwise, there needs to be
+	// a major stick movement to re-activate manual (or it's running for the very first time).
+
+	// Detect a big stick movement
+	const bool major_movement = [&]() {
+		for (int i = 0; i < 3; ++i) {
+			if (fabsf(_last_set_aux_values[i] - new_aux_values[i]) > 0.25f) {
+				return true;
+			}
+		}
+
+		return false;
+	}();
+
+	if (already_active || major_movement) {
+		control_data.sysid_primary_control = _parameters.mav_sysid;
+		control_data.compid_primary_control = _parameters.mav_compid;
+
+		if (_parameters.mnt_rc_in_mode == 0) {
+			// We scale manual input from roll -180..180, pitch -90..90, yaw, -180..180 degrees.
+			matrix::Eulerf euler(new_aux_values[0] * math::radians(180.f),
+					     new_aux_values[1] * math::radians(90.f),
+					     new_aux_values[2] * math::radians(180.f));
+
+			matrix::Quatf q(euler);
+			q.copyTo(control_data.type_data.angle.q);
+
+			control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+			control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+			control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+
+			control_data.type_data.angle.angular_velocity[0] = NAN;
+			control_data.type_data.angle.angular_velocity[1] = NAN;
+			control_data.type_data.angle.angular_velocity[2] = NAN;
+
+		} else {
+			control_data.type_data.angle.q[0] = NAN;
+			control_data.type_data.angle.q[1] = NAN;
+			control_data.type_data.angle.q[2] = NAN;
+			control_data.type_data.angle.q[3] = NAN;
+
+			control_data.type_data.angle.angular_velocity[0] = 0.f;
+			control_data.type_data.angle.angular_velocity[1] = math::radians(_parameters.mnt_rate_pitch) * new_aux_values[1];
+			control_data.type_data.angle.angular_velocity[2] = math::radians(_parameters.mnt_rate_yaw) * new_aux_values[2];
+
+			control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
+			control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
+			control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
+		}
+
+		for (int i = 0; i < 3; ++i) {
+			// We always use follow mode with RC input for now.
+			_last_set_aux_values[i] = new_aux_values[i];
+		}
+
+		control_data.gimbal_shutter_retract = false;
+
+		return UpdateResult::UpdatedActive;
+
+	} else {
+		return UpdateResult::NoUpdate;
+	}
+}
+
+float InputRC::_get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx)
+{
+	int32_t aux_channel = [&]() {
+		switch (channel_idx) {
+		case 0:
+			return _parameters.mnt_man_roll;
+
+		case 1:
+			return _parameters.mnt_man_pitch;
+
+		case 2:
+			return _parameters.mnt_man_yaw;
+
+		default:
+			return int32_t(0);
+		}
+	}();
+
+	switch (aux_channel) {
+
+	case 1:
+		return manual_control_setpoint.aux1;
+
+	case 2:
+		return manual_control_setpoint.aux2;
+
+	case 3:
+		return manual_control_setpoint.aux3;
+
+	case 4:
+		return manual_control_setpoint.aux4;
+
+	case 5:
+		return manual_control_setpoint.aux5;
+
+	case 6:
+		return manual_control_setpoint.aux6;
+
+	default:
+		return 0.0f;
+	}
+}
+
+void InputRC::print_status() const
+{
+	PX4_INFO("Input: RC (channels: roll=%" PRIi32 ", pitch=%" PRIi32 ", yaw=%" PRIi32 ")", _parameters.mnt_man_roll,
+		 _parameters.mnt_man_pitch, _parameters.mnt_man_yaw);
+}
+
+} /* namespace gimbal */

src/modules/gimbal/input_rc.h

@@ -0,0 +1,67 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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.
+*
+****************************************************************************/
+
+
+#pragma once
+
+#include "input.h"
+#include "gimbal_params.h"
+#include <uORB/topics/manual_control_setpoint.h>
+
+namespace gimbal
+{
+
+class InputRC : public InputBase
+{
+public:
+	InputRC() = delete;
+	explicit InputRC(Parameters &parameters);
+
+	virtual ~InputRC();
+
+	virtual void print_status() const;
+
+	virtual UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active);
+
+	virtual int initialize();
+
+private:
+	virtual UpdateResult _read_control_data_from_subscription(ControlData &control_data, bool already_active);
+	float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx);
+
+	int _manual_control_setpoint_sub{-1};
+
+	float _last_set_aux_values[3] {};
+};
+
+} /* namespace gimbal */

src/modules/gimbal/input_test.cpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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 "input_test.h"
+
+#include <px4_platform_common/posix.h>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
+
+
+namespace gimbal
+{
+
+InputTest::InputTest(Parameters &parameters) :
+	InputBase(parameters)
+{}
+
+InputTest::UpdateResult InputTest::update(unsigned int timeout_ms, ControlData &control_data, bool already_active)
+{
+	if (!_has_been_set.load()) {
+		return UpdateResult::NoUpdate;
+	}
+
+	control_data.type = ControlData::Type::Angle;
+
+	control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+	control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+	control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+
+	matrix::Eulerf euler(
+		math::radians((float)_roll_deg),
+		math::radians((float)_pitch_deg),
+		math::radians((float)_yaw_deg));
+	matrix::Quatf q(euler);
+
+	q.copyTo(control_data.type_data.angle.q);
+
+	control_data.gimbal_shutter_retract = false;
+
+	control_data.type_data.angle.angular_velocity[0] = NAN;
+	control_data.type_data.angle.angular_velocity[1] = NAN;
+	control_data.type_data.angle.angular_velocity[2] = NAN;
+
+	// For testing we mark ourselves as in control.
+	control_data.sysid_primary_control = _parameters.mav_sysid;
+	control_data.compid_primary_control = _parameters.mav_compid;
+
+	_has_been_set.store(false);
+	return UpdateResult::UpdatedActive;
+}
+
+int InputTest::initialize()
+{
+	return 0;
+}
+
+void InputTest::print_status() const
+{
+	PX4_INFO("Input: Test");
+	PX4_INFO_RAW("  roll : % 3d deg\n", _roll_deg);
+	PX4_INFO_RAW("  pitch: % 3d deg\n", _pitch_deg);
+	PX4_INFO_RAW("  yaw  : % 3d deg\n", _yaw_deg);
+}
+
+void InputTest::set_test_input(int roll_deg, int pitch_deg, int yaw_deg)
+{
+	_roll_deg = roll_deg;
+	_pitch_deg = pitch_deg;
+	_yaw_deg = yaw_deg;
+
+	_has_been_set.store(true);
+}
+
+} /* namespace gimbal */

src/modules/gimbal/input_test.h

@@ -0,0 +1,64 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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.
+*
+****************************************************************************/
+
+#pragma once
+
+#include "input.h"
+#include <px4_platform_common/atomic.h>
+
+namespace gimbal
+{
+
+class InputTest : public InputBase
+{
+public:
+	InputTest() = delete;
+	explicit InputTest(Parameters &parameters);
+	virtual ~InputTest() = default;
+
+	UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
+	int initialize() override;
+	void print_status() const override;
+
+	void set_test_input(int roll_deg, int pitch_deg, int yaw_deg);
+
+private:
+	int _roll_deg {0};
+	int _pitch_deg {0};
+	int _yaw_deg {0};
+
+	px4::atomic<bool> _has_been_set {false};
+};
+
+
+} /* namespace gimbal */

src/modules/gimbal/output.cpp

@@ -0,0 +1,245 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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 "output.h"
+
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/mount_orientation.h>
+#include <px4_platform_common/defines.h>
+#include <lib/geo/geo.h>
+#include <math.h>
+#include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
+
+namespace gimbal
+{
+
+OutputBase::OutputBase(const Parameters &parameters)
+	: _parameters(parameters)
+{
+	_last_update = hrt_absolute_time();
+}
+
+void OutputBase::publish()
+{
+	mount_orientation_s mount_orientation{};
+
+	for (unsigned i = 0; i < 3; ++i) {
+		mount_orientation.attitude_euler_angle[i] = _angle_outputs[i];
+	}
+
+	mount_orientation.timestamp = hrt_absolute_time();
+	_mount_orientation_pub.publish(mount_orientation);
+}
+
+float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
+				   const vehicle_global_position_s &global_position)
+{
+	if (!_projection_reference.isInitialized()) {
+		_projection_reference.initReference(global_position.lat, global_position.lon);
+	}
+
+	float x1, y1, x2, y2;
+	_projection_reference.project(lat, lon, x1, y1);
+	_projection_reference.project(global_position.lat, global_position.lon, x2, y2);
+	float dx = x1 - x2, dy = y1 - y2;
+	float target_distance = sqrtf(dx * dx + dy * dy);
+	float z = altitude - global_position.alt;
+
+	return atan2f(z, target_distance);
+}
+
+void OutputBase::_set_angle_setpoints(const ControlData &control_data)
+{
+	switch (control_data.type) {
+	case ControlData::Type::Angle:
+
+		{
+			for (int i = 0; i < 3; ++i) {
+				switch (control_data.type_data.angle.frames[i]) {
+				case ControlData::TypeData::TypeAngle::Frame::AngularRate:
+					break;
+
+				case ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame:
+					_absolute_angle[i] = false;
+					break;
+
+				case ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame:
+					_absolute_angle[i] = true;
+					break;
+				}
+
+				_angle_velocity[i] = control_data.type_data.angle.angular_velocity[i];
+			}
+
+			for (int i = 0; i < 4; ++i) {
+				_q_setpoint[i] = control_data.type_data.angle.q[i];
+			}
+		}
+
+		break;
+
+	case ControlData::Type::LonLat:
+		_handle_position_update(control_data, true);
+		break;
+
+	case ControlData::Type::Neutral:
+		_q_setpoint[0] = 1.f;
+		_q_setpoint[1] = 0.f;
+		_q_setpoint[2] = 0.f;
+		_q_setpoint[3] = 0.f;
+		_angle_velocity[0] = NAN;
+		_angle_velocity[1] = NAN;
+		_angle_velocity[2] = NAN;
+		break;
+	}
+}
+
+void OutputBase::_handle_position_update(const ControlData &control_data, bool force_update)
+{
+	if (control_data.type != ControlData::Type::LonLat) {
+		return;
+	}
+
+	vehicle_global_position_s vehicle_global_position{};
+
+	if (force_update) {
+		_vehicle_global_position_sub.copy(&vehicle_global_position);
+
+	} else {
+		if (!_vehicle_global_position_sub.update(&vehicle_global_position)) {
+			return;
+		}
+	}
+
+	const double &vlat = vehicle_global_position.lat;
+	const double &vlon = vehicle_global_position.lon;
+
+	const double &lat = control_data.type_data.lonlat.lat;
+	const double &lon = control_data.type_data.lonlat.lon;
+	const float &alt = control_data.type_data.lonlat.altitude;
+
+	float roll = PX4_ISFINITE(control_data.type_data.lonlat.roll_offset)
+		     ? control_data.type_data.lonlat.roll_offset
+		     : 0.0f;
+
+	// interface: use fixed pitch value > -pi otherwise consider ROI altitude
+	float pitch = (control_data.type_data.lonlat.pitch_fixed_angle >= -M_PI_F) ?
+		      control_data.type_data.lonlat.pitch_fixed_angle :
+		      _calculate_pitch(lon, lat, alt, vehicle_global_position);
+
+	float yaw = get_bearing_to_next_waypoint(vlat, vlon, lat, lon);
+	// We set the yaw angle in the absolute frame in this case.
+	_absolute_angle[2] = true;
+
+	// add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
+	if (PX4_ISFINITE(control_data.type_data.lonlat.pitch_offset)) {
+		pitch += control_data.type_data.lonlat.pitch_offset;
+	}
+
+	if (PX4_ISFINITE(control_data.type_data.lonlat.yaw_offset)) {
+		yaw += control_data.type_data.lonlat.yaw_offset;
+	}
+
+	matrix::Quatf(matrix::Eulerf(roll, pitch, yaw)).copyTo(_q_setpoint);
+
+	_angle_velocity[0] = NAN;
+	_angle_velocity[1] = NAN;
+	_angle_velocity[2] = NAN;
+}
+
+void OutputBase::_calculate_angle_output(const hrt_abstime &t)
+{
+	// We only need to apply additional compensation if the required angle is
+	// absolute (world frame) as well as the gimbal is not capable of doing that
+	// calculation. (Most gimbals stabilize at least roll and pitch
+	// and only need compensation for yaw, if at all.)
+	bool compensate[3];
+
+	for (int i = 0; i < 3; ++i) {
+		compensate[i] = _stabilize[i] && _absolute_angle[i];
+	}
+
+	// get the output angles and stabilize if necessary
+	matrix::Eulerf euler_vehicle{};
+
+	if (compensate[0] || compensate[1] || compensate[2]) {
+		vehicle_attitude_s vehicle_attitude{};
+
+		if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
+			euler_vehicle = matrix::Quatf(vehicle_attitude.q);
+		}
+	}
+
+	float dt = math::constrain((t - _last_update) * 1.e-6f, 0.001f, 1.f);
+
+	const bool q_setpoint_valid = PX4_ISFINITE(_q_setpoint[0]) && PX4_ISFINITE(_q_setpoint[1])
+				      && PX4_ISFINITE(_q_setpoint[2]) && PX4_ISFINITE(_q_setpoint[3]);
+	matrix::Eulerf euler_gimbal{};
+
+	if (q_setpoint_valid) {
+		euler_gimbal = matrix::Quatf{_q_setpoint};
+	}
+
+	for (int i = 0; i < 3; ++i) {
+
+		if (q_setpoint_valid && PX4_ISFINITE(euler_gimbal(i))) {
+			_angle_outputs[i] = euler_gimbal(i);
+		}
+
+		if (PX4_ISFINITE(_angle_velocity[i])) {
+			_angle_outputs[i] += dt * _angle_velocity[i];
+		}
+
+		if (compensate[i] && PX4_ISFINITE(euler_vehicle(i))) {
+			_angle_outputs[i] -= euler_vehicle(i);
+		}
+
+		if (PX4_ISFINITE(_angle_outputs[i])) {
+			// bring angles into proper range [-pi, pi]
+			_angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]);
+		}
+	}
+}
+
+void OutputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize)
+{
+	_stabilize[0] = roll_stabilize;
+	_stabilize[1] = pitch_stabilize;
+	_stabilize[2] = yaw_stabilize;
+}
+
+} /* namespace gimbal */
+

src/modules/gimbal/output.h

@@ -0,0 +1,101 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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.
+*
+****************************************************************************/
+
+
+#pragma once
+
+#include "common.h"
+#include "gimbal_params.h"
+#include <drivers/drv_hrt.h>
+#include <lib/geo/geo.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/mount_orientation.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+namespace gimbal
+{
+
+class OutputBase
+{
+public:
+	OutputBase() = delete;
+	explicit OutputBase(const Parameters &parameters);
+	virtual ~OutputBase() = default;
+
+	virtual void update(const ControlData &control_data, bool new_setpoints) = 0;
+
+	virtual void print_status() const = 0;
+
+	void publish();
+
+	void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize);
+
+protected:
+	float _calculate_pitch(double lon, double lat, float altitude,
+			       const vehicle_global_position_s &global_position);
+
+	MapProjection _projection_reference{}; ///< class to convert (lon, lat) to local [m]
+
+	const Parameters &_parameters;
+
+	/** set angle setpoints, speeds & stabilize flags */
+	void _set_angle_setpoints(const ControlData &control_data);
+
+	void _handle_position_update(const ControlData &control_data, bool force_update = false);
+
+	float _q_setpoint[4] = { NAN, NAN, NAN, NAN }; ///< can be NAN if not specifically set
+	float _angle_velocity[3] = { NAN, NAN, NAN }; //< [rad/s], can be NAN if not specifically set
+
+	bool _stabilize[3] = { false, false, false };
+
+	// Pitch and role are by default aligned with the horizon.
+	// Yaw follows the vehicle (not lock/absolute mode).
+	bool _absolute_angle[3] = {true, true, false };
+
+	/** calculate the _angle_outputs (with speed) and stabilize if needed */
+	void _calculate_angle_output(const hrt_abstime &t);
+
+	float _angle_outputs[3] = { 0.f, 0.f, 0.f }; ///< calculated output angles (roll, pitch, yaw) [rad]
+	hrt_abstime _last_update;
+
+private:
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
+
+	uORB::Publication<mount_orientation_s> _mount_orientation_pub{ORB_ID(mount_orientation)};
+};
+
+
+} /* namespace gimbal */

src/modules/gimbal/output_mavlink.cpp

@@ -0,0 +1,241 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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 "output_mavlink.h"
+
+#include <matrix/matrix/math.hpp>
+
+#include <px4_platform_common/defines.h>
+
+
+namespace gimbal
+{
+
+OutputMavlinkV1::OutputMavlinkV1(const Parameters &parameters)
+	: OutputBase(parameters)
+{}
+
+void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints)
+{
+	vehicle_command_s vehicle_command{};
+	vehicle_command.timestamp = hrt_absolute_time();
+	vehicle_command.target_system = (uint8_t)_parameters.mnt_mav_sysid_v1;
+	vehicle_command.target_component = (uint8_t)_parameters.mnt_mav_compid_v1;
+
+	if (new_setpoints) {
+		//got new command
+		_set_angle_setpoints(control_data);
+
+		const bool configuration_changed =
+			(control_data.type != _previous_control_data_type);
+		_previous_control_data_type = control_data.type;
+
+		if (configuration_changed) {
+
+			vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
+			vehicle_command.timestamp = hrt_absolute_time();
+
+			if (control_data.type == ControlData::Type::Neutral) {
+				vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
+
+				vehicle_command.param5 = 0.0;
+				vehicle_command.param6 = 0.0;
+				vehicle_command.param7 = 0.0f;
+
+			} else {
+				vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
+
+				vehicle_command.param5 = static_cast<double>(control_data.type_data.angle.frames[0]);
+				vehicle_command.param6 = static_cast<double>(control_data.type_data.angle.frames[1]);
+				vehicle_command.param7 = static_cast<float>(control_data.type_data.angle.frames[2]);
+			}
+
+			vehicle_command.param2 = _stabilize[0] ? 1.0f : 0.0f;
+			vehicle_command.param3 = _stabilize[1] ? 1.0f : 0.0f;
+			vehicle_command.param4 = _stabilize[2] ? 1.0f : 0.0f;
+
+			_gimbal_v1_command_pub.publish(vehicle_command);
+		}
+	}
+
+	_handle_position_update(control_data);
+
+	hrt_abstime t = hrt_absolute_time();
+	_calculate_angle_output(t);
+
+	vehicle_command.timestamp = t;
+	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
+
+	// gimbal spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively
+	// gimbal uses radians, MAVLink uses degrees
+	vehicle_command.param1 = math::degrees(_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch));
+	vehicle_command.param2 = math::degrees(_angle_outputs[0] + math::radians(_parameters.mnt_off_roll));
+	vehicle_command.param3 = math::degrees(_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw));
+	vehicle_command.param7 = 2.0f; // MAV_MOUNT_MODE_MAVLINK_TARGETING;
+
+	_gimbal_v1_command_pub.publish(vehicle_command);
+
+	_stream_device_attitude_status();
+
+	_last_update = t;
+}
+
+void OutputMavlinkV1::_stream_device_attitude_status()
+{
+	// This enables the use case where the gimbal v2 protocol is used
+	// between the ground station and the drone, and the gimbal v1 protocol is
+	// used between the drone and the gimbal.
+	gimbal_device_attitude_status_s attitude_status{};
+	attitude_status.timestamp = hrt_absolute_time();
+	attitude_status.target_system = 0;
+	attitude_status.target_component = 0;
+	attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL |
+				       gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK |
+				       gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK;
+
+	matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]);
+	matrix::Quatf q(euler);
+	q.copyTo(attitude_status.q);
+
+	attitude_status.failure_flags = 0;
+	_attitude_status_pub.publish(attitude_status);
+}
+
+void OutputMavlinkV1::print_status() const
+{
+	PX4_INFO("Output: MAVLink gimbal protocol v1");
+}
+
+OutputMavlinkV2::OutputMavlinkV2(const Parameters &parameters)
+	: OutputBase(parameters)
+{
+}
+
+void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints)
+{
+	_check_for_gimbal_device_information();
+
+	hrt_abstime t = hrt_absolute_time();
+
+
+	if (!_gimbal_device_found && t - _last_gimbal_device_checked > 1000000) {
+		_request_gimbal_device_information();
+		_last_gimbal_device_checked = t;
+
+	} else {
+		if (new_setpoints) {
+			//got new command
+			_set_angle_setpoints(control_data);
+
+			_handle_position_update(control_data);
+			_last_update = t;
+		}
+
+		_publish_gimbal_device_set_attitude();
+	}
+}
+
+void OutputMavlinkV2::_request_gimbal_device_information()
+{
+	vehicle_command_s vehicle_cmd{};
+	vehicle_cmd.timestamp = hrt_absolute_time();
+	vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE;
+	vehicle_cmd.param1 = vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION;
+	vehicle_cmd.target_system = 0;
+	vehicle_cmd.target_component = 0;
+	vehicle_cmd.source_system = _parameters.mav_sysid;
+	vehicle_cmd.source_component = _parameters.mav_compid;
+	vehicle_cmd.confirmation = 0;
+	vehicle_cmd.from_external = false;
+
+	uORB::Publication<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
+	vehicle_command_pub.publish(vehicle_cmd);
+}
+
+void OutputMavlinkV2::_check_for_gimbal_device_information()
+{
+	gimbal_device_information_s gimbal_device_information;
+
+	if (_gimbal_device_information_sub.update(&gimbal_device_information)) {
+		_gimbal_device_found = true;
+		_gimbal_device_compid = gimbal_device_information.gimbal_device_compid;
+	}
+}
+
+void OutputMavlinkV2::print_status() const
+{
+	PX4_INFO("Output: MAVLink gimbal protocol v2");
+
+	PX4_INFO_RAW("  quaternion: [%.1f %.1f %.1f %.1f]\n",
+		     (double)_q_setpoint[0],
+		     (double)_q_setpoint[1],
+		     (double)_q_setpoint[2],
+		     (double)_q_setpoint[3]);
+	PX4_INFO_RAW("  angular velocity: [%.1f %.1f %.1f]\n",
+		     (double)_angle_velocity[0],
+		     (double)_angle_velocity[1],
+		     (double)_angle_velocity[2]);
+}
+
+void OutputMavlinkV2::_publish_gimbal_device_set_attitude()
+{
+	gimbal_device_set_attitude_s set_attitude{};
+	set_attitude.timestamp = hrt_absolute_time();
+	set_attitude.target_system = (uint8_t)_parameters.mav_sysid;
+	set_attitude.target_component = _gimbal_device_compid;
+
+	set_attitude.angular_velocity_x = _angle_velocity[0];
+	set_attitude.angular_velocity_y = _angle_velocity[1];
+	set_attitude.angular_velocity_z = _angle_velocity[2];
+	set_attitude.q[0] = _q_setpoint[0];
+	set_attitude.q[1] = _q_setpoint[1];
+	set_attitude.q[2] = _q_setpoint[2];
+	set_attitude.q[3] = _q_setpoint[3];
+
+	if (_absolute_angle[0]) {
+		set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_ROLL_LOCK;
+	}
+
+	if (_absolute_angle[1]) {
+		set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_PITCH_LOCK;
+	}
+
+	if (_absolute_angle[2]) {
+		set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK;
+	}
+
+	_gimbal_device_set_attitude_pub.publish(set_attitude);
+}
+
+} /* namespace gimbal */

src/modules/gimbal/output_mavlink.h

@@ -0,0 +1,89 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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.
+*
+****************************************************************************/
+
+
+#pragma once
+
+#include "output.h"
+
+#include <uORB/Publication.hpp>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/gimbal_device_set_attitude.h>
+#include <uORB/topics/gimbal_device_information.h>
+#include <uORB/topics/gimbal_device_attitude_status.h>
+
+
+namespace gimbal
+{
+class OutputMavlinkV1 : public OutputBase
+{
+public:
+	OutputMavlinkV1(const Parameters &parameters);
+	virtual ~OutputMavlinkV1() = default;
+
+	virtual void update(const ControlData &control_data, bool new_setpoints);
+
+	virtual void print_status() const;
+
+private:
+	void _stream_device_attitude_status();
+	uORB::Publication<vehicle_command_s> _gimbal_v1_command_pub{ORB_ID(gimbal_v1_command)};
+	uORB::Publication <gimbal_device_attitude_status_s>	_attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};
+
+	ControlData::Type _previous_control_data_type {ControlData::Type::Neutral};
+};
+
+class OutputMavlinkV2 : public OutputBase
+{
+public:
+	OutputMavlinkV2(const Parameters &parameters);
+	virtual ~OutputMavlinkV2() = default;
+
+	virtual void update(const ControlData &control_data, bool new_setpoints);
+
+	virtual void print_status() const;
+
+private:
+	void _publish_gimbal_device_set_attitude();
+	void _request_gimbal_device_information();
+	void _check_for_gimbal_device_information();
+
+	uORB::Publication<gimbal_device_set_attitude_s> _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)};
+	uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)};
+
+	uint8_t _gimbal_device_compid{0};
+	hrt_abstime _last_gimbal_device_checked{0};
+	bool _gimbal_device_found {false};
+};
+
+} /* namespace gimbal */

src/modules/gimbal/output_rc.cpp

@@ -0,0 +1,112 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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 "output_rc.h"
+
+#include <uORB/topics/actuator_controls.h>
+#include <px4_platform_common/defines.h>
+#include <matrix/matrix/math.hpp>
+
+using math::constrain;
+
+namespace gimbal
+{
+
+OutputRC::OutputRC(const Parameters &parameters)
+	: OutputBase(parameters)
+{
+}
+
+void OutputRC::update(const ControlData &control_data, bool new_setpoints)
+{
+	if (new_setpoints) {
+		_retract_gimbal = control_data.gimbal_shutter_retract;
+		_set_angle_setpoints(control_data);
+	}
+
+	_handle_position_update(control_data);
+
+	hrt_abstime t = hrt_absolute_time();
+	_calculate_angle_output(t);
+
+	_stream_device_attitude_status();
+
+	// _angle_outputs are in radians, actuator_controls are in [-1, 1]
+	actuator_controls_s actuator_controls{};
+	actuator_controls.control[0] = constrain(
+					       (_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)) *
+					       (1.0f / (math::radians(_parameters.mnt_range_roll / 2.0f))),
+					       -1.f, 1.f);
+	actuator_controls.control[1] = constrain(
+					       (_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch)) *
+					       (1.0f / (math::radians(_parameters.mnt_range_pitch / 2.0f))),
+					       -1.f, 1.f);
+	actuator_controls.control[2] = constrain(
+					       (_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw)) *
+					       (1.0f / (math::radians(_parameters.mnt_range_yaw / 2.0f))),
+					       -1.f, 1.f);
+	actuator_controls.control[3] = constrain(
+					       _retract_gimbal ? _parameters.mnt_ob_lock_mode : _parameters.mnt_ob_norm_mode,
+					       -1.f, 1.f);
+	actuator_controls.timestamp = hrt_absolute_time();
+	_actuator_controls_pub.publish(actuator_controls);
+
+	_last_update = t;
+}
+
+void OutputRC::print_status() const
+{
+	PX4_INFO("Output: AUX");
+}
+
+void OutputRC::_stream_device_attitude_status()
+{
+	gimbal_device_attitude_status_s attitude_status{};
+	attitude_status.timestamp = hrt_absolute_time();
+	attitude_status.target_system = 0;
+	attitude_status.target_component = 0;
+	attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL |
+				       gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK |
+				       gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK |
+				       gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK;
+
+	matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]);
+	matrix::Quatf q(euler);
+	q.copyTo(attitude_status.q);
+
+	attitude_status.failure_flags = 0;
+	_attitude_status_pub.publish(attitude_status);
+}
+
+} /* namespace gimbal */

src/modules/gimbal/output_rc.h

@@ -0,0 +1,65 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016-2022 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.
+*
+****************************************************************************/
+
+
+#pragma once
+
+#include "output.h"
+
+#include <uORB/Publication.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/gimbal_device_attitude_status.h>
+
+namespace gimbal
+{
+
+class OutputRC : public OutputBase
+{
+public:
+	OutputRC() = delete;
+	explicit OutputRC(const Parameters &parameters);
+	virtual ~OutputRC() = default;
+
+	virtual void update(const ControlData &control_data, bool new_setpoints);
+	virtual void print_status() const;
+
+private:
+	void _stream_device_attitude_status();
+
+	uORB::Publication <actuator_controls_s>	_actuator_controls_pub{ORB_ID(actuator_controls_2)};
+	uORB::Publication <gimbal_device_attitude_status_s>	_attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};
+
+	bool _retract_gimbal = true;
+};
+
+} /* namespace gimbal */