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 {
-		Neutral = 0,      /**< move to neutral position */
-		Angle,            /**< control the roll, pitch & yaw angle directly */
-		LonLat            /**< control via lon, lat */
+	enum class Type {
+		Neutral = 0,
+		Angle,
+		LonLat
 	};
 
-
-	Type type = Type::Neutral;
-
 	union TypeData {
 		struct TypeAngle {
-			float q[4]; /**< attitude quaternion */
-			float angular_velocity[3]; // angular velocity
+			float q[4];
+			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). */
-				AngularRate = 1, /**< Angular rate set only, for compatibility with MAVLink v1 protocol. */
-				AngleAbsoluteFrame = 2/**< Lock mode, angle relative to horizon/world, lock mode. (usually default for roll and pitch). */
-			} frames[3]; /**< Mode. */
+				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_angle;        /**< roll is set to a fixed angle. Set to 0 for level horizon [rad] */
-			float pitch_angle_offset; /**< angular offset for pitch [rad] */
-			float yaw_angle_offset;  /**< angular offset for yaw [rad] */
-			float pitch_fixed_angle; /**< ignored if < -pi, otherwise use a fixed pitch angle instead of the altitude */
+			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 stabilize_axis[3] = { false, false, false }; /**< whether the vmount driver should stabilize an axis
-	 	 	 	 	 	 	 	 	 	 (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) */
+	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)
-{
-	if (!_initialized) {
-		int ret = initialize();
+InputBase::InputBase(Parameters &parameters) :
+	_parameters(parameters)
+{}
 
-		if (ret) {
-			return ret;
-		}
-
-		//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,
+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_angle = roll_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.yaw_angle_offset = 0.f;
+	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;
 }
 
-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;
 
-	/**
-	 * Wait for an input update, with a timeout.
-	 * @param timeout_ms timeout in ms
-	 * @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);
-
+	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:
-	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; }
-
-	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;
+	Parameters &_parameters;
 };
 
 

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();
-
-protected:
-	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
-	virtual int initialize();
+	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();
+	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();
-
-protected:
-	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
-	virtual int initialize();
+	void print_status() const override;
+	UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
+	int initialize() override;
 
 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();
-
-protected:
-	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
-	virtual int initialize();
+	UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
+	int initialize() override;
+	void print_status() const override;
 
 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 _read_control_data_from_position_setpoint_sub();
+	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;
@@ -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(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw, float mnt_rate_pitch,
-		 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(Parameters &parameters) :
+	InputBase(parameters)
+{}
 
 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
-	} else {
-		if (polls[0].revents & POLLIN) {
-			// Only if there was a change, we update the control data, otherwise leave it NULL.
-			if (_read_control_data_from_subscription(_control_data, already_active)) {
-				*control_data = &_control_data;
-			}
+		// If we have been active before, we stay active, unless someone steals
+		// the control away.
+		if (already_active) {
+			return UpdateResult::UpdatedActive;
 		}
 	}
 
-	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
+	const bool major_movement = [&]() {
 		for (int i = 0; i < 3; ++i) {
 			if (fabsf(_last_set_aux_values[i] - new_aux_values[i]) > 0.25f) {
-			major_movement = true;
+				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[2] = math::radians(_mnt_rate_yaw) * new_aux_values[2];
+			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;
@@ -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] {};
-
-	float _mnt_rate_pitch{0.f};
-	float _mnt_rate_yaw{0.f};
-
-	int _rc_in_mode{0};
+	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};
 
-	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;
-	_angles[1] = pitch_deg;
-	_angles[2] = yaw_deg;
-}
+	if (!_has_been_set.load()) {
+		return UpdateResult::NoUpdate;
+	}
 
-bool InputTest::finished()
-{
-	return true; /* only a single-shot test (for now) */
-}
+	control_data.type = ControlData::Type::Angle;
 
-int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool already_active)
-{
-	//we directly override the update() here, since we don't need the initialization from the base class
-
-	_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;
+	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(_angles[1]),
-		math::radians(_angles[2]));
+		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);
+	q.copyTo(control_data.type_data.angle.q);
 
-	_control_data.gimbal_shutter_retract = false;
-	*control_data = &_control_data;
-	return 0;
+	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()
@@ -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;
 
-	/**
-	 * set to a fixed angle
-	 */
-	InputTest(float roll_deg, float pitch_deg, float yaw_deg);
-	virtual ~InputTest() {}
+	UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
+	int initialize() override;
+	void print_status() const override;
 
-	/** check whether the test finished, and thus the main thread can quit */
-	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();
+	void set_test_input(int roll_deg, int pitch_deg, int yaw_deg);
 
 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)
-	: _config(output_config)
+OutputBase::OutputBase(const Parameters &parameters)
+	: _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 &lon = _cur_control_data->type_data.lonlat.lon;
-	const float &alt = _cur_control_data->type_data.lonlat.altitude;
+	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 = _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) ?
-		      _cur_control_data->type_data.lonlat.pitch_fixed_angle :
+	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);
@@ -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;
-	yaw += _cur_control_data->type_data.lonlat.yaw_angle_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);
 
@@ -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;
 
-	/**
-	 * Update the output.
-	 * @param data new command if non null
-	 * @return 0 on success, <0 otherwise
-	 */
-	virtual int update(const ControlData *control_data) = 0;
+	virtual void print_status() const = 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(bool force_update = false);
-
-	const ControlData *_cur_control_data = nullptr;
+	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
@@ -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)
-	: OutputBase(output_config)
-{
-}
+OutputMavlinkV1::OutputMavlinkV1(const Parameters &parameters)
+	: 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_component = (uint8_t)_config.mavlink_comp_id_v1;
+	vehicle_command.target_system = (uint8_t)_parameters.mnt_mav_sysid_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);
-		_previous_control_data_type = control_data->type;
+			(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) {
+			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.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.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;
 
-			_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.param2 = math::degrees(_angle_outputs[0] + _config.roll_offset);
-	vehicle_command.param3 = math::degrees(_angle_outputs[2] + _config.yaw_offset);
+	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;
 
-	_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)
-	: OutputBase(output_config),
-	  _mav_sys_id(mav_sys_id),
-	  _mav_comp_id(mav_comp_id)
+OutputMavlinkV2::OutputMavlinkV2(const Parameters &parameters)
+	: OutputBase(parameters)
 {
 }
 
-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();
-		_publish_gimbal_device_set_attitude();
+			_handle_position_update(control_data);
 			_last_update = t;
 		}
 
-	return 0;
+		_publish_gimbal_device_set_attitude();
+	}
 }
 
 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_component = _mav_comp_id;
+	vehicle_cmd.source_system = _parameters.mav_sysid;
+	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)
-	: OutputBase(output_config)
+OutputRC::OutputRC(const Parameters &parameters)
+	: OutputBase(parameters)
 {
 }
 
-int OutputRC::update(const ControlData *control_data)
+void OutputRC::update(const ControlData &control_data, bool new_setpoints)
 {
-	if (control_data) {
-		//got new command
-		_retract_gimbal = control_data->gimbal_shutter_retract;
+	if (new_setpoints) {
+		_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[1] = constrain((_angle_outputs[1] + _config.pitch_offset) * _config.pitch_scale, -1.f, 1.f);
-	actuator_controls.control[2] = constrain((_angle_outputs[2] + _config.yaw_offset) * _config.yaw_scale, -1.f, 1.f);
-	actuator_controls.control[3] = constrain(_retract_gimbal ? _config.gimbal_retracted_mode_value :
-				       _config.gimbal_normal_mode_value, -1.f, 1.f);
+	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;
-
-	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 print_status();
+	virtual void update(const ControlData &control_data, bool new_setpoints);
+	virtual void print_status() const;
 
 private:
 	void _stream_device_attitude_status();
@@ -72,5 +62,4 @@ private:
 	bool _retract_gimbal = true;
 };
 
-
 } /* namespace vmount */

src/modules/vmount/vmount.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2020, 2021 PX4 Development Team. All rights reserved.
+ *   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
@@ -32,30 +32,24 @@
  ****************************************************************************/
 
 /**
- * @file vmount.cpp
- * @author Leon Müller (thedevleon)
- * @author Beat Küng <beat-kueng@gmx.net>
- * @author Julian Oes <julian@oes.ch>
- * @author Matthew Edwards (mje-nz)
- *
- * Driver for to control mounts such as gimbals or servos.
- * Inputs for the mounts can RC and/or mavlink commands.
- * Outputs to the mounts can be RC (PWM) output or mavlink.
+ * 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 <math.h>
 #include <stdlib.h>
-#include <stdio.h>
-#include <stdbool.h>
 #include <string.h>
-#include <sys/types.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <systemlib/err.h>
-#include <lib/parameters/param.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/tasks.h>
 
+#include "vmount_params.h"
 #include "input_mavlink.h"
 #include "input_rc.h"
 #include "input_test.h"
@@ -66,105 +60,30 @@
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
 
-#include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/module.h>
+#include <px4_platform_common/atomic.h>
 
 using namespace time_literals;
 using namespace vmount;
 
-/* thread state */
-static volatile bool thread_should_exit = false;
-static volatile bool thread_running = false;
+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 volatile ThreadData *g_thread_data = nullptr;
-
-struct Parameters {
-	int32_t mnt_mode_in;
-	int32_t mnt_mode_out;
-	int32_t mnt_mav_sys_id_v1;
-	int32_t mnt_mav_comp_id_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_sys_id;
-	int32_t mav_comp_id;
-	float mnt_rate_pitch;
-	float mnt_rate_yaw;
-	int32_t mnt_rc_in_mode;
-
-	bool operator!=(const Parameters &p)
-	{
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wfloat-equal"
-		return mnt_mode_in != p.mnt_mode_in ||
-		       mnt_mode_out != p.mnt_mode_out ||
-		       mnt_mav_sys_id_v1 != p.mnt_mav_sys_id_v1 ||
-		       mnt_mav_comp_id_v1 != p.mnt_mav_comp_id_v1 ||
-		       fabsf(mnt_ob_lock_mode - p.mnt_ob_lock_mode) > 1e-6f ||
-		       fabsf(mnt_ob_norm_mode - p.mnt_ob_norm_mode) > 1e-6f ||
-		       mnt_man_pitch != p.mnt_man_pitch ||
-		       mnt_man_roll != p.mnt_man_roll ||
-		       mnt_man_yaw != p.mnt_man_yaw ||
-		       mnt_do_stab != p.mnt_do_stab ||
-		       mnt_range_pitch != p.mnt_range_pitch ||
-		       mnt_range_roll != p.mnt_range_roll ||
-		       mnt_range_yaw != p.mnt_range_yaw ||
-		       mnt_off_pitch != p.mnt_off_pitch ||
-		       mnt_off_roll != p.mnt_off_roll ||
-		       mnt_off_yaw != p.mnt_off_yaw ||
-		       mav_sys_id != p.mav_sys_id ||
-		       mav_comp_id != p.mav_comp_id ||
-		       mnt_rc_in_mode != p.mnt_rc_in_mode;
-#pragma GCC diagnostic pop
-
-	}
-};
-
-struct ParameterHandles {
-	param_t mnt_mode_in;
-	param_t mnt_mode_out;
-	param_t mnt_mav_sys_id_v1;
-	param_t mnt_mav_comp_id_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_sys_id;
-	param_t mav_comp_id;
-	param_t mnt_rate_pitch;
-	param_t mnt_rate_yaw;
-	param_t mnt_rc_in_mode;
-};
+static ThreadData *g_thread_data = nullptr;
 
 
-/* functions */
 static void usage();
-static void update_params(ParameterHandles &param_handles, Parameters &params, bool &got_changes);
-static bool get_params(ParameterHandles &param_handles, Parameters &params);
+static void update_params(ParameterHandles &param_handles, Parameters &params);
+static bool initialize_params(ParameterHandles &param_handles, Parameters &params);
 
 static int vmount_thread_main(int argc, char *argv[]);
 extern "C" __EXPORT int vmount_main(int argc, char *argv[]);
@@ -173,135 +92,56 @@ static int vmount_thread_main(int argc, char *argv[])
 {
 	ParameterHandles param_handles;
 	Parameters params {};
-	OutputConfig output_config;
 	ThreadData thread_data;
 
-	InputTest *test_input = nullptr;
-
-	// We don't need the task name.
-	argc -= 1;
-	argv += 1;
-
-	if (argc > 0 && !strcmp(argv[0], "test")) {
-		PX4_INFO("Starting in test mode");
-
-		const char *axis_names[3] = {"roll", "pitch", "yaw"};
-		float angles[3] = { 0.f, 0.f, 0.f };
-
-		if (argc == 3) {
-			bool found_axis = false;
-
-			for (int i = 0 ; i < 3; ++i) {
-				if (!strcmp(argv[1], axis_names[i])) {
-					long angle_deg = strtol(argv[2], nullptr, 0);
-					angles[i] = (float)angle_deg;
-					found_axis = true;
-				}
-			}
-
-			if (!found_axis) {
-				usage();
-				return -1;
-			}
-
-			test_input = new InputTest(angles[0], angles[1], angles[2]);
-
-			if (!test_input) {
-				PX4_ERR("memory allocation failed");
-				return -1;
-			}
-
-		} else {
-			usage();
-			return -1;
-		}
-	}
-
-	if (!get_params(param_handles, params)) {
+	if (!initialize_params(param_handles, params)) {
 		PX4_ERR("could not get mount parameters!");
-		delete test_input;
+		delete g_thread_data->test_input;
 		return -1;
 	}
 
 	uORB::SubscriptionInterval parameter_update_sub{ORB_ID(parameter_update), 1_s};
-	thread_running = true;
-	ControlData *control_data = nullptr;
+	thread_running.store(true);
+	ControlData control_data {};
 	g_thread_data = &thread_data;
 
-	int last_active = -1;
-
-	while (!thread_should_exit) {
-
-		if (!thread_data.input_objs[0] && (params.mnt_mode_in >= 0 || test_input)) { //need to initialize
-
-			output_config.gimbal_normal_mode_value = params.mnt_ob_norm_mode;
-			output_config.gimbal_retracted_mode_value = params.mnt_ob_lock_mode;
-			output_config.pitch_scale = 1.0f / (math::radians(params.mnt_range_pitch / 2.0f));
-			output_config.roll_scale = 1.0f / (math::radians(params.mnt_range_roll / 2.0f));
-			output_config.yaw_scale = 1.0f / (math::radians(params.mnt_range_yaw / 2.0f));
-			output_config.pitch_offset = math::radians(params.mnt_off_pitch);
-			output_config.roll_offset = math::radians(params.mnt_off_roll);
-			output_config.yaw_offset = math::radians(params.mnt_off_yaw);
-			output_config.mavlink_sys_id_v1 = params.mnt_mav_sys_id_v1;
-			output_config.mavlink_comp_id_v1 = params.mnt_mav_comp_id_v1;
+	thread_data.test_input = new InputTest(params);
 
 	bool alloc_failed = false;
-			thread_data.input_objs_len = 1;
 
-			if (test_input) {
-				thread_data.input_objs[0] = test_input;
+	thread_data.input_objs[thread_data.input_objs_len++] = thread_data.test_input;
 
-			} else {
 	switch (params.mnt_mode_in) {
 	case 0:
-
 		// Automatic
-					thread_data.input_objs[0] = new InputMavlinkCmdMount();
-					thread_data.input_objs[1] = new InputMavlinkROI();
-
-					// RC is on purpose last here so that if there are any mavlink
-					// messages, they will take precedence over RC.
-					// This logic is done further below while update() is called.
-					thread_data.input_objs[2] = new InputRC(params.mnt_man_roll,
-										params.mnt_man_pitch,
-										params.mnt_man_yaw,
-										params.mnt_rate_pitch,
-										params.mnt_rate_yaw,
-										params.mnt_rc_in_mode);
-					thread_data.input_objs_len = 3;
+		// 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
-					thread_data.input_objs[0] = new InputRC(params.mnt_man_roll,
-										params.mnt_man_pitch,
-										params.mnt_man_yaw,
-										params.mnt_rate_pitch,
-										params.mnt_rate_yaw,
-										params.mnt_rc_in_mode);
+	case 1: // RC only
+		thread_data.input_objs[thread_data.input_objs_len++] = new InputRC(params);
 		break;
 
-				case 2: //MAVLINK_ROI
-					thread_data.input_objs[0] = new InputMavlinkROI();
+	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
-					thread_data.input_objs[0] = new InputMavlinkCmdMount();
+	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[0] = new InputMavlinkGimbalV2(
-						params.mav_sys_id,
-						params.mav_comp_id,
-						params.mnt_rate_pitch,
-						params.mnt_rate_yaw);
+		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]) {
@@ -309,23 +149,37 @@ static int vmount_thread_main(int argc, char *argv[])
 		}
 	}
 
+	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(output_config);
+		thread_data.output_obj = new OutputRC(params);
 
 		if (!thread_data.output_obj) { alloc_failed = true; }
 
 		break;
 
-			case 1: //MAVLink v1 gimbal protocol
-				thread_data.output_obj = new OutputMavlinkV1(output_config);
+	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 v2 gimbal protocol
-				thread_data.output_obj = new OutputMavlinkV2(params.mav_sys_id, params.mav_comp_id, output_config);
+	case 2: //MAVLink gimbal v2 protocol
+		thread_data.output_obj = new OutputMavlinkV2(params);
 
 		if (!thread_data.output_obj) { alloc_failed = true; }
 
@@ -333,71 +187,86 @@ static int vmount_thread_main(int argc, char *argv[])
 
 	default:
 		PX4_ERR("invalid output mode %" PRId32, params.mnt_mode_out);
-				thread_should_exit = true;
+		thread_should_exit.store(true);
 		break;
 	}
 
 	if (alloc_failed) {
-				thread_data.input_objs_len = 0;
-				PX4_ERR("memory allocation failed");
-				thread_should_exit = true;
+		PX4_ERR("output memory allocation failed");
+		thread_should_exit.store(true);
 	}
 
-			if (thread_should_exit) {
-				break;
+	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);
 		}
 
-			int ret = thread_data.output_obj->initialize();
+		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;
+		}
 
-			if (ret) {
-				PX4_ERR("failed to initialize output mode (%i)", ret);
-				thread_should_exit = true;
-				break;
-			}
-		}
+		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.
+			// 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) {
 
-				if (params.mnt_do_stab == 1) {
-					thread_data.input_objs[i]->set_stabilize(true, true, true);
+				const bool already_active = (thread_data.last_input_active == i);
+				const unsigned int poll_timeout = already_active ? 20 : 0; // poll only on active input to reduce latency
 
-				} else if (params.mnt_do_stab == 2) {
-					thread_data.input_objs[i]->set_stabilize(false, false, true);
+				update_result = thread_data.input_objs[i]->update(poll_timeout, control_data, already_active);
 
-				} else {
-					thread_data.input_objs[i]->set_stabilize(false, false, false);
+				switch (update_result) {
+				case InputBase::UpdateResult::NoUpdate:
+					if (already_active) {
+						// No longer active.
+						thread_data.last_input_active = -1;
 					}
 
+					break;
 
-				bool already_active = (last_active == i);
+				case InputBase::UpdateResult::UpdatedActive:
+					thread_data.last_input_active = i;
+					break;
 
-				ControlData *control_data_to_check = nullptr;
-				unsigned int poll_timeout = already_active ? 20 : 0; // poll only on active input to reduce latency
-				int ret = thread_data.input_objs[i]->update(poll_timeout, &control_data_to_check, already_active);
+				case InputBase::UpdateResult::UpdatedActiveOnce:
+					thread_data.last_input_active = -1;
+					break;
 
-				if (ret) {
-					PX4_ERR("failed to read input %i (ret: %i)", i, ret);
-					continue;
+				case InputBase::UpdateResult::UpdatedNotActive:
+					if (already_active) {
+						// No longer active
+						thread_data.last_input_active = -1;
 					}
 
-				if (control_data_to_check != nullptr || already_active) {
-					control_data = control_data_to_check;
-					last_active = i;
-				}
-			}
-
-			//update output
-			int ret = thread_data.output_obj->update(control_data);
-
-			if (ret) {
-				PX4_ERR("failed to write output (%i)", ret);
 					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.
@@ -405,50 +274,17 @@ static int vmount_thread_main(int argc, char *argv[])
 				thread_data.output_obj->publish();
 			}
 
-
 		} else {
-			//wait for parameter changes. We still need to wake up regularily to check for thread exit requests
+			// We still need to wake up regularly to check for thread exit requests
 			px4_usleep(1e6);
 		}
-
-		if (test_input && test_input->finished()) {
-			thread_should_exit = true;
-			break;
-		}
-
-		// check for parameter updates
-		if (parameter_update_sub.updated()) {
-			// clear update
-			parameter_update_s pupdate;
-			parameter_update_sub.copy(&pupdate);
-
-			// update parameters from storage
-			bool updated = false;
-			update_params(param_handles, params, updated);
-
-			if (updated) {
-				//re-init objects
-				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;
-
-				last_active = -1;
-
-				if (thread_data.output_obj) {
-					delete (thread_data.output_obj);
-					thread_data.output_obj = nullptr;
-				}
-			}
-		}
 	}
 
 	g_thread_data = nullptr;
 
+	delete thread_data.test_input;
+	thread_data.test_input = nullptr;
+
 	for (int i = 0; i < input_objs_len_max; ++i) {
 		if (thread_data.input_objs[i]) {
 			delete (thread_data.input_objs[i]);
@@ -463,14 +299,10 @@ static int vmount_thread_main(int argc, char *argv[])
 		thread_data.output_obj = nullptr;
 	}
 
-	thread_running = false;
+	thread_running.store(false);
 	return 0;
 }
 
-/**
- * The main command function.
- * Processes command line arguments and starts the daemon.
- */
 int vmount_main(int argc, char *argv[])
 {
 	if (argc < 2) {
@@ -479,34 +311,23 @@ int vmount_main(int argc, char *argv[])
 		return -1;
 	}
 
-	const bool found_start = !strcmp(argv[1], "start");
-	const bool found_test = !strcmp(argv[1], "test");
+	if (!strcmp(argv[1], "start")) {
 
-	if (found_start || found_test) {
-
-		/* this is not an error */
-		if (thread_running) {
-			if (found_start) {
+		if (thread_running.load()) {
 			PX4_WARN("mount driver already running");
-				return 0;
-
-			} else {
-				PX4_WARN("mount driver already running, run vmount stop before 'vmount test'");
 			return 1;
 		}
-		}
 
-		thread_should_exit = false;
 		int vmount_task = px4_task_spawn_cmd("vmount",
 						     SCHED_DEFAULT,
 						     SCHED_PRIORITY_DEFAULT,
 						     2100,
 						     vmount_thread_main,
-						     (char *const *)argv + 1);
+						     nullptr);
 
 		int counter = 0;
 
-		while (!thread_running && vmount_task >= 0) {
+		while (!thread_running.load() && vmount_task >= 0) {
 			px4_usleep(5000);
 
 			if (++counter >= 100) {
@@ -519,35 +340,83 @@ int vmount_main(int argc, char *argv[])
 			return -1;
 		}
 
-		return counter < 100 || thread_should_exit ? 0 : -1;
+		return counter < 100 || thread_should_exit.load() ? 0 : -1;
 	}
 
-	if (!strcmp(argv[1], "stop")) {
+	else if (!strcmp(argv[1], "stop")) {
 
-		/* this is not an error */
-		if (!thread_running) {
+		if (!thread_running.load()) {
 			PX4_WARN("mount driver not running");
 			return 0;
 		}
 
-		thread_should_exit = true;
+		thread_should_exit.store(true);
 
-		while (thread_running) {
+		while (thread_running.load()) {
 			px4_usleep(100000);
 		}
 
 		return 0;
 	}
 
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running && g_thread_data) {
+	else if (!strcmp(argv[1], "test")) {
 
-			for (int i = 0; i < g_thread_data->input_objs_len; ++i) {
-				g_thread_data->input_objs[i]->print_status();
+		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) {
@@ -569,13 +438,12 @@ int vmount_main(int argc, char *argv[])
 	return -1;
 }
 
-void update_params(ParameterHandles &param_handles, Parameters &params, bool &got_changes)
+void update_params(ParameterHandles &param_handles, Parameters &params)
 {
-	Parameters prev_params = 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_sys_id_v1, &params.mnt_mav_sys_id_v1);
-	param_get(param_handles.mnt_mav_comp_id_v1, &params.mnt_mav_comp_id_v1);
+	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);
@@ -588,21 +456,19 @@ void update_params(ParameterHandles &param_handles, Parameters &params, bool &go
 	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_sys_id, &params.mav_sys_id);
-	param_get(param_handles.mav_comp_id, &params.mav_comp_id);
+	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);
-
-	got_changes = prev_params != params;
 }
 
-bool get_params(ParameterHandles &param_handles, Parameters &params)
+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_sys_id_v1 = param_find("MNT_MAV_SYSID");
-	param_handles.mnt_mav_comp_id_v1 = param_find("MNT_MAV_COMPID");
+	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");
@@ -615,16 +481,16 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
 	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_sys_id = param_find("MAV_SYS_ID");
-	param_handles.mav_comp_id = param_find("MAV_COMP_ID");
+	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_sys_id_v1 == PARAM_INVALID ||
-	    param_handles.mnt_mav_comp_id_v1 == 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 ||
@@ -637,8 +503,8 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
 	    param_handles.mnt_off_pitch == PARAM_INVALID ||
 	    param_handles.mnt_off_roll == PARAM_INVALID ||
 	    param_handles.mnt_off_yaw == PARAM_INVALID ||
-	    param_handles.mav_sys_id == PARAM_INVALID ||
-	    param_handles.mav_comp_id == 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
@@ -646,8 +512,7 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
 		return false;
 	}
 
-	bool dummy;
-	update_params(param_handles, params, dummy);
+	update_params(param_handles, params);
 	return true;
 }
 
@@ -656,25 +521,19 @@ static void usage()
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-Mount (Gimbal) control driver. It maps several different input methods (eg. RC or MAVLink) to a configured
+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://dev.px4.io/master/en/advanced/gimbal_control.html) page.
-
-### Implementation
-Each method is implemented in its own class, and there is a common base class for inputs and outputs.
-They are connected via an API, defined by the `ControlData` data structure. This makes sure that each input method
-can be used with each output method and new inputs/outputs can be added with minimal effort.
+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 fixed yaw angle (and the other axes to 0):
-$ vmount stop
-$ vmount test yaw 30
+Test the output by setting a angles (all omitted axes are set to 0):
+$ vmount test pitch -45 yaw 30
 )DESCR_STR");
 
 	PRINT_MODULE_USAGE_NAME("vmount", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test the output: set a fixed angle for one axis (vmount must not be running)");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test the output: set a fixed angle for one or multiple axes (vmount 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/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),
-* 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.
+* 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
+* @value 0 Auto (RC and MAVLink gimbal protocol v2)
 * @value 1 RC
-* @value 2 MAVLINK_ROI (protocol v1)
-* @value 3 MAVLINK_DO_MOUNT (protocol v1)
+* @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
@@ -61,9 +56,9 @@ PARAM_DEFINE_INT32(MNT_MODE_IN, -1);
 /**
 * Mount output mode
 *
-* AUX uses the mixer output Control Group #2.
-* 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)
+* 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
@@ -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 */