vmount: refactor for v2 auto input, test command

This is a bigger refactor of vmount to clean it up and plug some holes
in functionality.

The changes include:
- Fixing/simplifying the test command.
- Changing the dependencies of the input and output classes to just the
  parameter list.
- Adding a separate topic to publish gimbal v1 commands to avoid
  polluting the vehicle_command topic.
- Removing outdated comments and author lists.
- Extracting the gimbal v2 "in control" notion outside into control_data
  rather than only the v2 input.

msg/vehicle_command.msg

@@ -175,3 +175,5 @@ uint8 source_system		# System sending the command
 uint8 source_component		# Component sending the command
 uint8 confirmation		# 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
 bool from_external
+
+# TOPICS vehicle_command gimbal_v1_command

src/modules/mavlink/mavlink_main.cpp

@@ -2394,6 +2394,30 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 		}
 
+		// For legacy gimbals using the mavlink gimbal v1 protocol, we need to send out commands.
+		// We don't care about acks for these though.
+		if (_gimbal_v1_command_sub.updated()) {
+			vehicle_command_s cmd;
+			_gimbal_v1_command_sub.copy(&cmd);
+
+			// FIXME: filter for target system/component
+
+			mavlink_command_long_t msg{};
+			msg.param1 = cmd.param1;
+			msg.param2 = cmd.param2;
+			msg.param3 = cmd.param3;
+			msg.param4 = cmd.param4;
+			msg.param5 = cmd.param5;
+			msg.param6 = cmd.param6;
+			msg.param7 = cmd.param7;
+			msg.command = cmd.command;
+			msg.target_system = cmd.target_system;
+			msg.target_component = cmd.target_component;
+			msg.confirmation = 0;
+
+			mavlink_msg_command_long_send_struct(get_channel(), &msg);
+		}
+
 		/* check for shell output */
 		if (_mavlink_shell && _mavlink_shell->available() > 0) {
 			if (get_free_tx_buf() >= MAVLINK_MSG_ID_SERIAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {

src/modules/mavlink/mavlink_main.h

@@ -544,6 +544,7 @@ private:
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_command_ack_sub{ORB_ID(vehicle_command_ack)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _gimbal_v1_command_sub{ORB_ID(gimbal_v1_command)};
 
 	static bool		_boot_complete;
 

src/modules/vmount/common.h

@@ -31,11 +31,6 @@
 *
 ****************************************************************************/
 
-/**
- * @file common.h
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
 
 #pragma once
 
@@ -52,44 +47,44 @@ namespace vmount
  */
 struct ControlData {
 
-	enum class Type : uint8_t {
+	enum class Type {
-		Neutral = 0,      /**< move to neutral position */
+		Neutral = 0,
-		Angle,            /**< control the roll, pitch & yaw angle directly */
+		Angle,
-		LonLat            /**< control via lon, lat */
+		LonLat
 	};
 
-
-	Type type = Type::Neutral;
-
 	union TypeData {
 		struct TypeAngle {
-			float q[4]; /**< attitude quaternion */
+			float q[4];
-			float angular_velocity[3]; // angular velocity
+			float angular_velocity[3];
 
-			// according to DO_MOUNT_CONFIGURE
 			enum class Frame : uint8_t {
-				AngleBodyFrame = 0, /**< Follow mode, angle relative to vehicle (usually default for yaw axis). */
+				AngleBodyFrame = 0, // Also called follow mode, angle relative to vehicle forward (usually default for yaw axis).
-				AngularRate = 1, /**< Angular rate set only, for compatibility with MAVLink v1 protocol. */
+				AngularRate = 1, // Angular rate set only.
-				AngleAbsoluteFrame = 2/**< Lock mode, angle relative to horizon/world, lock mode. (usually default for roll and pitch). */
+				AngleAbsoluteFrame = 2 // Also called lock mode, angle relative to horizon/world, lock mode. (usually default for roll and pitch).
-			} frames[3]; /**< Mode. */
+			} frames[3];
 		} angle;
 
 		struct TypeLonLat {
-			double lon;              /**< longitude in [deg] */
+			double lon; // longitude in deg
-			double lat;              /**< latitude in [deg] */
+			double lat; // latitude in deg
-			float altitude;          /**< altitude in [m] */
+			float altitude; // altitude in m
-			float roll_angle;        /**< roll is set to a fixed angle. Set to 0 for level horizon [rad] */
+			float roll_offset; // roll offset in rad
-			float pitch_angle_offset; /**< angular offset for pitch [rad] */
+			float pitch_offset; // pitch offset in rad
-			float yaw_angle_offset;  /**< angular offset for yaw [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 */
+			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 stabilize_axis[3] = { false, false, false }; /**< whether the vmount driver should stabilize an axis
+	bool gimbal_shutter_retract = false; // whether to lock the gimbal (only in RC output mode)
-	 	 	 	 	 	 	 	 	 	 (if the output supports it, this can also be done externally) */
-	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 vmount */

src/modules/vmount/input.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
+*   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
@@ -31,56 +31,29 @@
 *
 ****************************************************************************/
 
-/**
- * @file input.cpp
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
-
 #include "input.h"
 
 
 namespace vmount
 {
 
-int InputBase::update(unsigned int timeout_ms, ControlData **control_data, bool already_active)
+InputBase::InputBase(Parameters &parameters) :
-{
+	_parameters(parameters)
-	if (!_initialized) {
+{}
-		int ret = initialize();
 
-		if (ret) {
+void InputBase::control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude,
-			return ret;
+		float roll_angle,
-		}
-
-		//on startup, set the mount to a neutral position
-		_control_data.type = ControlData::Type::Neutral;
-		_control_data.gimbal_shutter_retract = true;
-		*control_data = &_control_data;
-		_initialized = true;
-		return 0;
-	}
-
-	return update_impl(timeout_ms, control_data, already_active);
-}
-
-void InputBase::control_data_set_lon_lat(double lon, double lat, float altitude, float roll_angle,
 		float pitch_fixed_angle)
 {
-	_control_data.type = ControlData::Type::LonLat;
+	control_data.type = ControlData::Type::LonLat;
-	_control_data.type_data.lonlat.lon = lon;
+	control_data.type_data.lonlat.lon = lon;
-	_control_data.type_data.lonlat.lat = lat;
+	control_data.type_data.lonlat.lat = lat;
-	_control_data.type_data.lonlat.altitude = altitude;
+	control_data.type_data.lonlat.altitude = altitude;
-	_control_data.type_data.lonlat.roll_angle = roll_angle;
+	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_fixed_angle = pitch_fixed_angle;
-	_control_data.type_data.lonlat.pitch_angle_offset = 0.f;
+	control_data.type_data.lonlat.pitch_offset = 0.f;
-	_control_data.type_data.lonlat.yaw_angle_offset = 0.f;
+	control_data.type_data.lonlat.yaw_offset = 0.f;
 }
 
-void InputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize)
-{
-	_control_data.stabilize_axis[0] = roll_stabilize;
-	_control_data.stabilize_axis[1] = pitch_stabilize;
-	_control_data.stabilize_axis[2] = yaw_stabilize;
-}
 
 } /* namespace vmount */

src/modules/vmount/input.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
+*   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
@@ -31,62 +31,39 @@
 *
 ****************************************************************************/
 
-/**
- * @file input.h
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
-
 #pragma once
 
 #include "common.h"
+#include "math.h"
+#include "vmount_params.h"
 
 namespace vmount
 {
 
+struct Parameters;
 
-/**
- ** class InputBase
- * Base class for all driver input classes
- */
 class InputBase
 {
 public:
-	InputBase() = default;
+	enum class UpdateResult {
+		NoUpdate,
+		UpdatedActive,
+		UpdatedActiveOnce,
+		UpdatedNotActive,
+	};
+
+	InputBase() = delete;
+	explicit InputBase(Parameters &parameters);
 	virtual ~InputBase() = default;
 
-	/**
+	virtual int initialize() = 0;
-	 * Wait for an input update, with a timeout.
+	virtual UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) = 0;
-	 * @param timeout_ms timeout in ms
+	virtual void print_status() const = 0;
-	 * @param control_data unchanged on error. On success it is nullptr if no new
-	 *                     data is available, otherwise set to an object.
-	 *                     If it is set, the returned object will not be changed for
-	 *                     subsequent calls to update() that return no new data
-	 *                     (in other words: if (some) control_data values change,
-	 *                     non-null will be returned).
-	 * @param already_active true if the mode was already active last time, false if it was not and "major"
-	 *                       change is necessary such as big stick movement for RC.
-	 * @return 0 on success, <0 otherwise
-	 */
-	virtual int update(unsigned int timeout_ms, ControlData **control_data, bool already_active);
-
-	/** report status to stdout */
-	virtual void print_status() = 0;
-
-	void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize);
-
 protected:
-	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) = 0;
+	void control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude, float roll_angle = NAN,
+				      float pitch_fixed_angle = NAN);
 
-	virtual int initialize() { return 0; }
+	Parameters &_parameters;
-
-	void control_data_set_lon_lat(double lon, double lat, float altitude, float roll_angle = 0.f,
-				      float pitch_fixed_angle = -10.f);
-
-	ControlData _control_data;
-
-private:
-	bool _initialized = false;
 };
 
 

src/modules/vmount/input_mavlink.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
+*   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
@@ -31,11 +31,6 @@
 *
 ****************************************************************************/
 
-/**
- * @file input_mavlink.h
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
 
 #pragma once
 
@@ -48,32 +43,30 @@
 #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 <uORB/topics/vehicle_global_position.h>
 #include <lib/geo/geo.h>
 
 
 namespace vmount
 {
-/**
+
- ** class InputMavlinkROI
- ** Input based on the vehicle_roi topic
- */
 class InputMavlinkROI : public InputBase
 {
 public:
-	InputMavlinkROI() = default;
+	InputMavlinkROI() = delete;
+	explicit InputMavlinkROI(Parameters &parameters);
 	virtual ~InputMavlinkROI();
 
-	virtual void print_status();
+	void print_status() const override;
-
+	UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
-protected:
+	int initialize() override;
-	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
-	virtual int initialize();
 
 private:
-	void _read_control_data_from_position_setpoint_sub();
+	void _read_control_data_from_position_setpoint_sub(ControlData &control_data);
 
 	int _vehicle_roi_sub = -1;
 	int _position_setpoint_triplet_sub = -1;
@@ -81,50 +74,49 @@ private:
 };
 
 
-/**
- ** class InputMavlinkCmdMount
- ** Input based on the VEHICLE_CMD_DO_MOUNT_CONTROL mavlink command
- */
 class InputMavlinkCmdMount : public InputBase
 {
 public:
-	InputMavlinkCmdMount();
+	InputMavlinkCmdMount() = delete;
+	explicit InputMavlinkCmdMount(Parameters &parameters);
 	virtual ~InputMavlinkCmdMount();
 
-	virtual void print_status();
+	void print_status() const override;
-
+	UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
-protected:
+	int initialize() override;
-	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
-	virtual int initialize();
 
 private:
-	void _ack_vehicle_command(vehicle_command_s *cmd);
+	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;
-
-	int32_t _mav_sys_id{1}; ///< our mavlink system id
-	int32_t _mav_comp_id{1}; ///< our mavlink component id
 };
 
 class InputMavlinkGimbalV2 : public InputBase
 {
 public:
-	InputMavlinkGimbalV2(uint8_t mav_sys_id, uint8_t mav_comp_id, float &mnt_rate_pitch, float &mnt_rate_yaw);
+	InputMavlinkGimbalV2() = delete;
+	explicit InputMavlinkGimbalV2(Parameters &parameters);
 	virtual ~InputMavlinkGimbalV2();
 
-	virtual void print_status();
+	UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
-
+	int initialize() override;
-protected:
+	void print_status() const override;
-	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
-	virtual int initialize();
 
 private:
-	void _set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q,
+	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(vehicle_command_s *cmd, uint8_t result);
+	void _ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result);
 	void _stream_gimbal_manager_information();
-	void _stream_gimbal_manager_status();
+	void _stream_gimbal_manager_status(const ControlData &control_data);
-	void _read_control_data_from_position_setpoint_sub();
+	void _read_control_data_from_position_setpoint_sub(ControlData &control_data);
 
 	int _vehicle_roi_sub = -1;
 	int _gimbal_manager_set_attitude_sub = -1;
@@ -132,17 +124,7 @@ private:
 	int _position_setpoint_triplet_sub = -1;
 	int _vehicle_command_sub = -1;
 
-	uint8_t _mav_sys_id{1}; ///< our mavlink system id
-	uint8_t _mav_comp_id{1}; ///< our mavlink component id
-
-	uint8_t _sys_id_primary_control{0};
-	uint8_t _comp_id_primary_control{0};
-
-	float &_mnt_rate_pitch;
-	float &_mnt_rate_yaw;
-
 	uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)};
-	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
 	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;

src/modules/vmount/input_rc.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
+*   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
@@ -31,11 +31,6 @@
 *
 ****************************************************************************/
 
-/**
- * @file input_rc.cpp
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
 
 #include "input_rc.h"
 
@@ -50,17 +45,9 @@
 namespace vmount
 {
 
-
+InputRC::InputRC(Parameters &parameters) :
-InputRC::InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw, float mnt_rate_pitch,
+	InputBase(parameters)
-		 float mnt_rate_yaw, int rc_in_mode)
+{}
-{
-	_aux_channels[0] = aux_channel_roll;
-	_aux_channels[1] = aux_channel_pitch;
-	_aux_channels[2] = aux_channel_yaw;
-	_mnt_rate_pitch = mnt_rate_pitch;
-	_mnt_rate_yaw = mnt_rate_yaw;
-	_rc_in_mode = rc_in_mode;
-}
 
 InputRC::~InputRC()
 {
@@ -80,11 +67,8 @@ int InputRC::initialize()
 	return 0;
 }
 
-int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)
+InputRC::UpdateResult InputRC::update(unsigned int timeout_ms, ControlData &control_data, bool already_active)
 {
-	// Default to no change signalled by NULL.
-	*control_data = nullptr;
-
 	px4_pollfd_struct_t polls[1];
 	polls[0].fd = 		_manual_control_setpoint_sub;
 	polls[0].events = 	POLLIN;
@@ -92,24 +76,25 @@ int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data, bo
 	int ret = px4_poll(polls, 1, timeout_ms);
 
 	if (ret < 0) {
-		return -errno;
+		return UpdateResult::NoUpdate;
 	}
 
 	if (ret == 0) {
-		// Timeout, leave NULL
+		// If we have been active before, we stay active, unless someone steals
-	} else {
+		// the control away.
-		if (polls[0].revents & POLLIN) {
+		if (already_active) {
-			// Only if there was a change, we update the control data, otherwise leave it NULL.
+			return UpdateResult::UpdatedActive;
-			if (_read_control_data_from_subscription(_control_data, already_active)) {
-				*control_data = &_control_data;
-			}
 		}
 	}
 
-	return 0;
+	if (polls[0].revents & POLLIN) {
+		return _read_control_data_from_subscription(control_data, already_active);
+	}
+
+	return UpdateResult::NoUpdate;
 }
 
-bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bool already_active)
+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);
@@ -123,20 +108,23 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo
 
 	// 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).
-	bool major_movement = false;
 
 	// Detect a big stick movement
-	for (int i = 0; i < 3; ++i) {
+	const bool major_movement = [&]() {
-		if (fabsf(_last_set_aux_values[i] - new_aux_values[i]) > 0.25f) {
+		for (int i = 0; i < 3; ++i) {
-			major_movement = true;
+			if (fabsf(_last_set_aux_values[i] - new_aux_values[i]) > 0.25f) {
+				return true;
+			}
 		}
-	}
 
-	if (already_active || major_movement || _first_time) {
+		return false;
+	}();
 
-		_first_time = false;
+	if (already_active || major_movement) {
+		control_data.sysid_primary_control = _parameters.mav_sysid;
+		control_data.compid_primary_control = _parameters.mav_compid;
 
-		if (_rc_in_mode == 0) {
+		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),
@@ -160,8 +148,8 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo
 			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(_mnt_rate_pitch) * new_aux_values[1];
+			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(_mnt_rate_yaw) * new_aux_values[2];
+			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;
@@ -174,16 +162,33 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo
 		}
 
 		control_data.gimbal_shutter_retract = false;
-		return true;
+
+		return UpdateResult::UpdatedActive;
 
 	} else {
-		return false;
+		return UpdateResult::NoUpdate;
 	}
 }
 
 float InputRC::_get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx)
 {
-	switch (_aux_channels[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;
@@ -208,9 +213,10 @@ float InputRC::_get_aux_value(const manual_control_setpoint_s &manual_control_se
 	}
 }
 
-void InputRC::print_status()
+void InputRC::print_status() const
 {
-	PX4_INFO("Input: RC (channels: roll=%i, pitch=%i, yaw=%i)", _aux_channels[0], _aux_channels[1], _aux_channels[2]);
+	PX4_INFO("Input: RC (channels: roll=%" PRIi32 ", pitch=%" PRIi32 ", yaw=%" PRIi32 ")", _parameters.mnt_man_roll,
+		 _parameters.mnt_man_pitch, _parameters.mnt_man_yaw);
 }
 
 } /* namespace vmount */

src/modules/vmount/input_rc.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
+*   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
@@ -31,66 +31,37 @@
 *
 ****************************************************************************/
 
-/**
- * @file input_rc.h
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
 
 #pragma once
 
 #include "input.h"
+#include "vmount_params.h"
 #include <uORB/topics/manual_control_setpoint.h>
 
 namespace vmount
 {
 
-class InputMavlinkROI;
-class InputMavlinkCmdMount;
-
-/**
- ** class InputRC
- * RC input class using manual_control_setpoint topic
- */
 class InputRC : public InputBase
 {
 public:
+	InputRC() = delete;
+	explicit InputRC(Parameters &parameters);
 
-	/**
-	 * @param aux_channel_roll   which aux channel to use for roll (set to 0 to use a fixed angle of 0)
-	 * @param aux_channel_pitch
-	 * @param aux_channel_yaw
-	 */
-	InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw, float mnt_rate_pitch, float mnt_rate_yaw,
-		int rc_in_mode);
 	virtual ~InputRC();
 
-	virtual void print_status();
+	virtual void print_status() const;
+
+	virtual UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active);
 
-protected:
-	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
 	virtual int initialize();
 
-	/**
-	 * @return true if there was a change in control data
-	 */
-	virtual bool _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);
-
 private:
-	int _aux_channels[3] {};
+	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);
-	float _mnt_rate_pitch{0.f};
-	float _mnt_rate_yaw{0.f};
-
-	int _rc_in_mode{0};
 
 	int _manual_control_setpoint_sub{-1};
 
-	bool _first_time{true};
 	float _last_set_aux_values[3] {};
 };
 
-
 } /* namespace vmount */

src/modules/vmount/input_test.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
+*   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
@@ -31,16 +31,8 @@
 *
 ****************************************************************************/
 
-/**
- * @file input_test.cpp
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
-
 #include "input_test.h"
 
-#include <math.h>
-
 #include <px4_platform_common/posix.h>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/mathlib.h>
@@ -49,39 +41,42 @@
 namespace vmount
 {
 
-InputTest::InputTest(float roll_deg, float pitch_deg, float yaw_deg)
+InputTest::InputTest(Parameters &parameters) :
+	InputBase(parameters)
+{}
+
+InputTest::UpdateResult InputTest::update(unsigned int timeout_ms, ControlData &control_data, bool already_active)
 {
-	_angles[0] = roll_deg;
+	if (!_has_been_set.load()) {
-	_angles[1] = pitch_deg;
+		return UpdateResult::NoUpdate;
-	_angles[2] = yaw_deg;
+	}
-}
 
-bool InputTest::finished()
+	control_data.type = ControlData::Type::Angle;
-{
-	return true; /* only a single-shot test (for now) */
-}
 
-int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool already_active)
+	control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
-{
+	control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
-	//we directly override the update() here, since we don't need the initialization from the base class
+	control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
-
-	_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(_angles[0]),
+		math::radians((float)_roll_deg),
-		math::radians(_angles[1]),
+		math::radians((float)_pitch_deg),
-		math::radians(_angles[2]));
+		math::radians((float)_yaw_deg));
 	matrix::Quatf q(euler);
 
-	q.copyTo(_control_data.type_data.angle.q);
+	q.copyTo(control_data.type_data.angle.q);
 
-	_control_data.gimbal_shutter_retract = false;
+	control_data.gimbal_shutter_retract = false;
-	*control_data = &_control_data;
+
-	return 0;
+	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()
@@ -89,9 +84,21 @@ int InputTest::initialize()
 	return 0;
 }
 
-void InputTest::print_status()
+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 vmount */

src/modules/vmount/input_test.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
+*   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
@@ -31,47 +31,33 @@
 *
 ****************************************************************************/
 
-/**
- * @file input_test.h
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
-
 #pragma once
 
 #include "input.h"
+#include <px4_platform_common/atomic.h>
 
 namespace vmount
 {
 
-/**
- ** class InputTest
- * Send a single control command, configured via constructor arguments
- */
 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;
-	 * set to a fixed angle
+	int initialize() override;
-	 */
+	void print_status() const override;
-	InputTest(float roll_deg, float pitch_deg, float yaw_deg);
-	virtual ~InputTest() {}
 
-	/** check whether the test finished, and thus the main thread can quit */
+	void set_test_input(int roll_deg, int pitch_deg, int yaw_deg);
-	bool finished();
-
-	virtual int update(unsigned int timeout_ms, ControlData **control_data, bool already_active);
-
-protected:
-	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) { return 0; } //not needed
-
-	virtual int initialize();
-
-	virtual void print_status();
 
 private:
-	float _angles[3]; /**< desired angles in [deg] */
+	int _roll_deg {0};
+	int _pitch_deg {0};
+	int _yaw_deg {0};
+
+	px4::atomic<bool> _has_been_set {false};
 };
 
 

src/modules/vmount/output.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
+*   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
@@ -31,15 +31,8 @@
 *
 ****************************************************************************/
 
-/**
- * @file output.cpp
- * @author Leon Müller (thedevleon)
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
 
 #include "output.h"
-#include <errno.h>
 
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -53,8 +46,8 @@
 namespace vmount
 {
 
-OutputBase::OutputBase(const OutputConfig &output_config)
+OutputBase::OutputBase(const Parameters &parameters)
-	: _config(output_config)
+	: _parameters(parameters)
 {
 	_last_update = hrt_absolute_time();
 }
@@ -88,16 +81,14 @@ float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
 	return atan2f(z, target_distance);
 }
 
-void OutputBase::_set_angle_setpoints(const ControlData *control_data)
+void OutputBase::_set_angle_setpoints(const ControlData &control_data)
 {
-	_cur_control_data = control_data;
+	switch (control_data.type) {
-
-	switch (control_data->type) {
 	case ControlData::Type::Angle:
 
 		{
 			for (int i = 0; i < 3; ++i) {
-				switch (control_data->type_data.angle.frames[i]) {
+				switch (control_data.type_data.angle.frames[i]) {
 				case ControlData::TypeData::TypeAngle::Frame::AngularRate:
 					break;
 
@@ -110,18 +101,18 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data)
 					break;
 				}
 
-				_angle_velocity[i] = control_data->type_data.angle.angular_velocity[i];
+				_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];
+				_q_setpoint[i] = control_data.type_data.angle.q[i];
 			}
 		}
 
 		break;
 
 	case ControlData::Type::LonLat:
-		_handle_position_update(true);
+		_handle_position_update(control_data, true);
 		break;
 
 	case ControlData::Type::Neutral:
@@ -134,47 +125,39 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data)
 		_angle_velocity[2] = NAN;
 		break;
 	}
-
-	for (int i = 0; i < 3; ++i) {
-		_stabilize[i] = control_data->stabilize_axis[i];
-	}
 }
 
-void OutputBase::_handle_position_update(bool force_update)
+void OutputBase::_handle_position_update(const ControlData &control_data, bool force_update)
 {
-	if (!_cur_control_data || _cur_control_data->type != ControlData::Type::LonLat) {
+	if (control_data.type != ControlData::Type::LonLat) {
 		return;
 	}
 
 	vehicle_global_position_s vehicle_global_position{};
-	vehicle_local_position_s vehicle_local_position{};
 
 	if (force_update) {
 		_vehicle_global_position_sub.copy(&vehicle_global_position);
-		_vehicle_local_position_sub.copy(&vehicle_local_position);
 
 	} else {
 		if (!_vehicle_global_position_sub.update(&vehicle_global_position)) {
 			return;
 		}
-
-		if (!_vehicle_local_position_sub.update(&vehicle_local_position)) {
-			return;
-		}
 	}
 
 	const double &vlat = vehicle_global_position.lat;
 	const double &vlon = vehicle_global_position.lon;
 
-	const double &lat = _cur_control_data->type_data.lonlat.lat;
+	const double &lat = control_data.type_data.lonlat.lat;
-	const double &lon = _cur_control_data->type_data.lonlat.lon;
+	const double &lon = control_data.type_data.lonlat.lon;
-	const float &alt = _cur_control_data->type_data.lonlat.altitude;
+	const float &alt = control_data.type_data.lonlat.altitude;
 
-	float roll = _cur_control_data->type_data.lonlat.roll_angle;
+	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 = (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) ?
+	float pitch = (control_data.type_data.lonlat.pitch_fixed_angle >= -M_PI_F) ?
-		      _cur_control_data->type_data.lonlat.pitch_fixed_angle :
+		      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);
@@ -182,8 +165,13 @@ void OutputBase::_handle_position_update(bool force_update)
 	_absolute_angle[2] = true;
 
 	// add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
-	pitch += _cur_control_data->type_data.lonlat.pitch_angle_offset;
+	if (PX4_ISFINITE(control_data.type_data.lonlat.pitch_offset)) {
-	yaw += _cur_control_data->type_data.lonlat.yaw_angle_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);
 
@@ -246,5 +234,12 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t)
 	}
 }
 
+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 vmount */
 

src/modules/vmount/output.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
+*   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
@@ -31,15 +31,11 @@
 *
 ****************************************************************************/
 
-/**
- * @file output.h
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
 
 #pragma once
 
 #include "common.h"
+#include "vmount_params.h"
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
 #include <uORB/Publication.hpp>
@@ -47,71 +43,37 @@
 #include <uORB/topics/mount_orientation.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
 
 namespace vmount
 {
 
-struct OutputConfig {
-	float gimbal_retracted_mode_value;	/**< Mixer output value for selecting gimbal retracted mode */
-	float gimbal_normal_mode_value;		/**< Mixer output value for selecting gimbal normal mode */
-
-	/** Scale factor for pitch channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
-	float pitch_scale;
-	/** Scale factor for roll channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
-	float roll_scale;
-	/** Scale factor for yaw channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
-	float yaw_scale;
-
-	float pitch_offset;	/**< Offset for pitch channel in radians */
-	float roll_offset;	/**< Offset for roll channel in radians */
-	float yaw_offset;	/**< Offset for yaw channel in radians */
-
-	uint32_t mavlink_sys_id_v1;	/**< Mavlink target system id for mavlink output only for v1 */
-	uint32_t mavlink_comp_id_v1;
-};
-
-
-/**
- ** class OutputBase
- * Base class for all driver output classes
- */
 class OutputBase
 {
 public:
-	OutputBase(const OutputConfig &output_config);
+	OutputBase() = delete;
+	explicit OutputBase(const Parameters &parameters);
 	virtual ~OutputBase() = default;
 
-	virtual int initialize() { return 0; }
+	virtual void update(const ControlData &control_data, bool new_setpoints) = 0;
 
-	/**
+	virtual void print_status() const = 0;
-	 * Update the output.
-	 * @param data new command if non null
-	 * @return 0 on success, <0 otherwise
-	 */
-	virtual int update(const ControlData *control_data) = 0;
 
-	/** report status to stdout */
-	virtual void print_status() = 0;
-
-	/** Publish _angle_outputs as a mount_orientation message. */
 	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 OutputConfig &_config;
+	const Parameters &_parameters;
 
 	/** set angle setpoints, speeds & stabilize flags */
-	void _set_angle_setpoints(const ControlData *control_data);
+	void _set_angle_setpoints(const ControlData &control_data);
 
-	/** check if vehicle position changed and update the setpoint angles if in gps mode */
+	void _handle_position_update(const ControlData &control_data, bool force_update = false);
-	void _handle_position_update(bool force_update = false);
-
-	const ControlData *_cur_control_data = nullptr;
 
 	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
@@ -131,7 +93,6 @@ protected:
 private:
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
-	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 
 	uORB::Publication<mount_orientation_s> _mount_orientation_pub{ORB_ID(mount_orientation)};
 };

src/modules/vmount/output_mavlink.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
+*   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
@@ -31,51 +31,42 @@
 *
 ****************************************************************************/
 
-/**
- * @file output_mavlink.cpp
- * @author Leon Müller (thedevleon)
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
 
 #include "output_mavlink.h"
 
-#include <math.h>
 #include <matrix/matrix/math.hpp>
 
-#include <uORB/topics/vehicle_command.h>
 #include <px4_platform_common/defines.h>
 
 
 namespace vmount
 {
 
-OutputMavlinkV1::OutputMavlinkV1(const OutputConfig &output_config)
+OutputMavlinkV1::OutputMavlinkV1(const Parameters &parameters)
-	: OutputBase(output_config)
+	: OutputBase(parameters)
-{
+{}
-}
 
-int OutputMavlinkV1::update(const ControlData *control_data)
+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)_config.mavlink_sys_id_v1;
+	vehicle_command.target_system = (uint8_t)_parameters.mnt_mav_sysid_v1;
-	vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id_v1;
+	vehicle_command.target_component = (uint8_t)_parameters.mnt_mav_compid_v1;
 
-	if (control_data) {
+	if (new_setpoints) {
 		//got new command
 		_set_angle_setpoints(control_data);
 
 		const bool configuration_changed =
-			(control_data->type != _previous_control_data_type);
+			(control_data.type != _previous_control_data_type);
-		_previous_control_data_type = 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) {
+			if (control_data.type == ControlData::Type::Neutral) {
 				vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
 
 				vehicle_command.param5 = 0.0;
@@ -85,20 +76,20 @@ int OutputMavlinkV1::update(const ControlData *control_data)
 			} 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.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.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.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;
 
-			_vehicle_command_pub.publish(vehicle_command);
+			_gimbal_v1_command_pub.publish(vehicle_command);
 		}
 	}
 
-	_handle_position_update();
+	_handle_position_update(control_data);
 
 	hrt_abstime t = hrt_absolute_time();
 	_calculate_angle_output(t);
@@ -108,18 +99,16 @@ int OutputMavlinkV1::update(const ControlData *control_data)
 
 	// vmount spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively
 	// vmount uses radians, MAVLink uses degrees
-	vehicle_command.param1 = math::degrees(_angle_outputs[1] + _config.pitch_offset);
+	vehicle_command.param1 = math::degrees(_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch));
-	vehicle_command.param2 = math::degrees(_angle_outputs[0] + _config.roll_offset);
+	vehicle_command.param2 = math::degrees(_angle_outputs[0] + math::radians(_parameters.mnt_off_roll));
-	vehicle_command.param3 = math::degrees(_angle_outputs[2] + _config.yaw_offset);
+	vehicle_command.param3 = math::degrees(_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw));
 	vehicle_command.param7 = 2.0f; // MAV_MOUNT_MODE_MAVLINK_TARGETING;
 
-	_vehicle_command_pub.publish(vehicle_command);
+	_gimbal_v1_command_pub.publish(vehicle_command);
 
 	_stream_device_attitude_status();
 
 	_last_update = t;
-
-	return 0;
 }
 
 void OutputMavlinkV1::_stream_device_attitude_status()
@@ -143,52 +132,51 @@ void OutputMavlinkV1::_stream_device_attitude_status()
 	_attitude_status_pub.publish(attitude_status);
 }
 
-void OutputMavlinkV1::print_status()
+void OutputMavlinkV1::print_status() const
 {
 	PX4_INFO("Output: MAVLink gimbal protocol v1");
 }
 
-OutputMavlinkV2::OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config)
+OutputMavlinkV2::OutputMavlinkV2(const Parameters &parameters)
-	: OutputBase(output_config),
+	: OutputBase(parameters)
-	  _mav_sys_id(mav_sys_id),
-	  _mav_comp_id(mav_comp_id)
 {
 }
 
-int OutputMavlinkV2::update(const ControlData *control_data)
+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 (control_data) {
+		if (new_setpoints) {
 			//got new command
 			_set_angle_setpoints(control_data);
+
+			_handle_position_update(control_data);
+			_last_update = t;
 		}
 
-		_handle_position_update();
 		_publish_gimbal_device_set_attitude();
-		_last_update = t;
 	}
-
-	return 0;
 }
 
 void OutputMavlinkV2::_request_gimbal_device_information()
 {
+	printf("request gimbal device\n");
 	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 = _mav_sys_id;
+	vehicle_cmd.source_system = _parameters.mav_sysid;
-	vehicle_cmd.source_component = _mav_comp_id;
+	vehicle_cmd.source_component = _parameters.mav_compid;
 	vehicle_cmd.confirmation = 0;
 	vehicle_cmd.from_external = false;
 
@@ -206,16 +194,26 @@ void OutputMavlinkV2::_check_for_gimbal_device_information()
 	}
 }
 
-void OutputMavlinkV2::print_status()
+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)_mav_sys_id;
+	set_attitude.target_system = (uint8_t)_parameters.mav_sysid;
 	set_attitude.target_component = _gimbal_device_compid;
 
 	set_attitude.angular_velocity_x = _angle_velocity[0];

src/modules/vmount/output_mavlink.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
+*   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
@@ -31,11 +31,6 @@
 *
 ****************************************************************************/
 
-/**
- * @file output_mavlink.h
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
 
 #pragma once
 
@@ -50,23 +45,19 @@
 
 namespace vmount
 {
-/**
- ** class OutputMavlink
- *  Output via vehicle_command topic
- */
 class OutputMavlinkV1 : public OutputBase
 {
 public:
-	OutputMavlinkV1(const OutputConfig &output_config);
+	OutputMavlinkV1(const Parameters &parameters);
 	virtual ~OutputMavlinkV1() = default;
 
-	virtual int update(const ControlData *control_data);
+	virtual void update(const ControlData &control_data, bool new_setpoints);
 
-	virtual void print_status();
+	virtual void print_status() const;
 
 private:
 	void _stream_device_attitude_status();
-	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
+	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};
@@ -75,12 +66,12 @@ private:
 class OutputMavlinkV2 : public OutputBase
 {
 public:
-	OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config);
+	OutputMavlinkV2(const Parameters &parameters);
 	virtual ~OutputMavlinkV2() = default;
 
-	virtual int update(const ControlData *control_data);
+	virtual void update(const ControlData &control_data, bool new_setpoints);
 
-	virtual void print_status();
+	virtual void print_status() const;
 
 private:
 	void _publish_gimbal_device_set_attitude();
@@ -90,8 +81,6 @@ private:
 	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)};
 
-	int32_t _mav_sys_id{1}; ///< our mavlink system id
-	int32_t _mav_comp_id{1}; ///< our mavlink component id
 	uint8_t _gimbal_device_compid{0};
 	hrt_abstime _last_gimbal_device_checked{0};
 	bool _gimbal_device_found {false};

src/modules/vmount/output_rc.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   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
@@ -31,12 +31,6 @@
 *
 ****************************************************************************/
 
-/**
- * @file output_rc.cpp
- * @author Leon Müller (thedevleon)
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
 
 #include "output_rc.h"
 
@@ -49,20 +43,19 @@ using math::constrain;
 namespace vmount
 {
 
-OutputRC::OutputRC(const OutputConfig &output_config)
+OutputRC::OutputRC(const Parameters &parameters)
-	: OutputBase(output_config)
+	: OutputBase(parameters)
 {
 }
 
-int OutputRC::update(const ControlData *control_data)
+void OutputRC::update(const ControlData &control_data, bool new_setpoints)
 {
-	if (control_data) {
+	if (new_setpoints) {
-		//got new command
+		_retract_gimbal = control_data.gimbal_shutter_retract;
-		_retract_gimbal = control_data->gimbal_shutter_retract;
 		_set_angle_setpoints(control_data);
 	}
 
-	_handle_position_update();
+	_handle_position_update(control_data);
 
 	hrt_abstime t = hrt_absolute_time();
 	_calculate_angle_output(t);
@@ -71,20 +64,28 @@ int OutputRC::update(const ControlData *control_data)
 
 	// _angle_outputs are in radians, actuator_controls are in [-1, 1]
 	actuator_controls_s actuator_controls{};
-	actuator_controls.control[0] = constrain((_angle_outputs[0] + _config.roll_offset) * _config.roll_scale, -1.f, 1.f);
+	actuator_controls.control[0] = constrain(
-	actuator_controls.control[1] = constrain((_angle_outputs[1] + _config.pitch_offset) * _config.pitch_scale, -1.f, 1.f);
+					       (_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)) *
-	actuator_controls.control[2] = constrain((_angle_outputs[2] + _config.yaw_offset) * _config.yaw_scale, -1.f, 1.f);
+					       (1.0f / (math::radians(_parameters.mnt_range_roll / 2.0f))),
-	actuator_controls.control[3] = constrain(_retract_gimbal ? _config.gimbal_retracted_mode_value :
+					       -1.f, 1.f);
-				       _config.gimbal_normal_mode_value, -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;
-
-	return 0;
 }
 
-void OutputRC::print_status()
+void OutputRC::print_status() const
 {
 	PX4_INFO("Output: AUX");
 }

src/modules/vmount/output_rc.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   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
@@ -31,11 +31,6 @@
 *
 ****************************************************************************/
 
-/**
- * @file output_rc.h
- * @author Beat Küng <beat-kueng@gmx.net>
- *
- */
 
 #pragma once
 
@@ -48,20 +43,15 @@
 namespace vmount
 {
 
-
-/**
- ** class OutputRC
- *  Output via actuator_controls_2 topic
- */
 class OutputRC : public OutputBase
 {
 public:
-	OutputRC(const OutputConfig &output_config);
+	OutputRC() = delete;
+	explicit OutputRC(const Parameters &parameters);
 	virtual ~OutputRC() = default;
 
-	virtual int update(const ControlData *control_data);
+	virtual void update(const ControlData &control_data, bool new_setpoints);
-
+	virtual void print_status() const;
-	virtual void print_status();
 
 private:
 	void _stream_device_attitude_status();
@@ -72,5 +62,4 @@ private:
 	bool _retract_gimbal = true;
 };
 
-
 } /* namespace vmount */

src/modules/vmount/vmount_params.c

@@ -31,25 +31,20 @@
 *
 ****************************************************************************/
 
-/**
-* @file vmount_params.c
-* @author Leon Müller (thedevleon)
-* @author Matthew Edwards (mje-nz)
-*
-*/
 
 /**
 * Mount input mode
 *
-* RC uses the AUX input channels (see MNT_MAN_* parameters),
+* This is the protocol used between the ground station and the autopilot.
-* MAVLINK_ROI uses the MAV_CMD_DO_SET_ROI Mavlink message, and MAVLINK_DO_MOUNT the
+*
-* MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL messages to control a mount.
+* Recommended is Auto, RC only or MAVLink gimbal protocol v2.
+* The rest will be deprecated.
 *
 * @value -1 DISABLED
-* @value 0 AUTO
+* @value 0 Auto (RC and MAVLink gimbal protocol v2)
 * @value 1 RC
-* @value 2 MAVLINK_ROI (protocol v1)
+* @value 2 MAVLINK_ROI (protocol v1, to be deprecated)
-* @value 3 MAVLINK_DO_MOUNT (protocol v1)
+* @value 3 MAVLINK_DO_MOUNT (protocol v1, to be deprecated)
 * @value 4 MAVlink gimbal protocol v2
 * @min -1
 * @max 4
@@ -61,9 +56,9 @@ PARAM_DEFINE_INT32(MNT_MODE_IN, -1);
 /**
 * Mount output mode
 *
-* AUX uses the mixer output Control Group #2.
+* This is the protocol used between the autopilot and a connected gimbal.
-* MAVLINK uses the MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL MavLink messages
+*
-* to control a mount (set MNT_MAV_SYSID & MNT_MAV_COMPID)
+* Recommended is the MAVLink gimbal protocol v2 if the gimbal supports it.
 *
 * @value 0 AUX
 * @value 1 MAVLink gimbal protocol v1
@@ -71,6 +66,7 @@ PARAM_DEFINE_INT32(MNT_MODE_IN, -1);
 * @min 0
 * @max 2
 * @group Mount
+* @reboot_required true
 */
 PARAM_DEFINE_INT32(MNT_MODE_OUT, 0);
 

src/modules/vmount/vmount_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 vmount
+{
+
+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 vmount */