vmount: refactor to auto-select between all inputs

It is not convenient having to change a parameter to change a gimbal
from RC input to mavlink input mode or back. This refactor changes the
behaviour to use whatever is available, RC or mavlink commands.

Once a mavlink command is followed, control can be taken back using RC,
however, this requires a clear stick change.

src/drivers/vmount/input.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2017 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
@@ -43,7 +43,7 @@
 namespace vmount
 {
 
-int InputBase::update(unsigned int timeout_ms, ControlData **control_data)
+int InputBase::update(unsigned int timeout_ms, ControlData **control_data, bool already_active)
 {
 	if (!_initialized) {
 		int ret = initialize();
@@ -60,7 +60,7 @@ int InputBase::update(unsigned int timeout_ms, ControlData **control_data)
 		return 0;
 	}
 
-	return update_impl(timeout_ms, control_data);
+	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,

src/drivers/vmount/input.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2017 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
@@ -63,15 +63,17 @@ public:
 	 *                     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);
+	virtual int update(unsigned int timeout_ms, ControlData **control_data, bool already_active);
 
 	/** report status to stdout */
 	virtual void print_status() = 0;
 
 protected:
-	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data) = 0;
+	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) = 0;
 
 	virtual int initialize() { return 0; }
 

src/drivers/vmount/input_mavlink.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2017 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
@@ -55,8 +55,7 @@ namespace vmount
 {
 
 
-InputMavlinkROI::InputMavlinkROI(InputRC *manual_override)
-	: _manual_control(manual_override)
+InputMavlinkROI::InputMavlinkROI()
 {
 }
 
@@ -85,34 +84,24 @@ int InputMavlinkROI::initialize()
 		return -errno;
 	}
 
-	if (_manual_control) {
-		int ret = _manual_control->initialize();
-
-		if (ret) {
-			PX4_ERR("failed to initialize rc input");
-			return ret;
-		}
-	}
-
 	return 0;
 }
 
 
 
-int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_data)
+int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)
 {
-	px4_pollfd_struct_t polls[3];
+	// already_active is unused, we don't care what happened previously.
+
+	// Default to no change, set if we receive anything.
+	*control_data = nullptr;
+
+	const int num_poll = 2;
+	px4_pollfd_struct_t polls[num_poll];
 	polls[0].fd = 		_vehicle_roi_sub;
 	polls[0].events = 	POLLIN;
 	polls[1].fd = 		_position_setpoint_triplet_sub;
 	polls[1].events = 	POLLIN;
-	int num_poll = 2;
-
-	if (_manual_control && _allow_manual_control) {
-		polls[num_poll].fd = 		_manual_control->_get_subscription_fd();
-		polls[num_poll].events = 	POLLIN;
-		++num_poll;
-	}
 
 	int ret = px4_poll(polls, num_poll, timeout_ms);
 
@@ -121,7 +110,7 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_
 	}
 
 	if (ret == 0) {
-		*control_data = nullptr;
+		// Timeout, _control_data is already null
 
 	} else {
 		if (polls[0].revents & POLLIN) {
@@ -129,30 +118,20 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_
 			orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi);
 
 			if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
-				_allow_manual_control = true;
-
-				if (!_manual_control) {
-					_control_data.type = ControlData::Type::Neutral;
-				}
 
 			} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
-				_allow_manual_control = false;
 				_read_control_data_from_position_setpoint_sub();
 				_control_data.type_data.lonlat.roll_angle = 0.f;
 				_control_data.type_data.lonlat.pitch_fixed_angle = -10.f;
 
 			} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
-				_allow_manual_control = false;
 				//TODO how to do this?
 
 			} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
-				_allow_manual_control = false;
 				control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt);
 
 			} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
-				_allow_manual_control = false;
 				//TODO is this even suported?
-
 			}
 
 			_cur_roi_mode = vehicle_roi.mode;
@@ -163,9 +142,6 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_
 			}
 
 			_control_data.gimbal_shutter_retract = false;
-
-		} else if (num_poll > 2 && (polls[2].revents & POLLIN)) {
-			_manual_control->_read_control_data_from_subscription(_control_data);
 		}
 
 		// check whether the position setpoint got updated
@@ -190,11 +166,10 @@ void InputMavlinkROI::_read_control_data_from_position_setpoint_sub()
 
 void InputMavlinkROI::print_status()
 {
-	PX4_INFO("Input: Mavlink (ROI)%s", _manual_control ? " (with manual)" : "");
+	PX4_INFO("Input: Mavlink (ROI)");
 }
 
-InputMavlinkCmdMount::InputMavlinkCmdMount(InputRC *manual_override)
-	: _manual_control(manual_override)
+InputMavlinkCmdMount::InputMavlinkCmdMount()
 {
 }
 
@@ -211,31 +186,19 @@ int InputMavlinkCmdMount::initialize()
 		return -errno;
 	}
 
-	if (_manual_control) {
-		int ret = _manual_control->initialize();
-
-		if (ret) {
-			PX4_ERR("failed to initialize rc input");
-			return ret;
-		}
-	}
-
 	return 0;
 }
 
 
-int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **control_data)
+int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)
 {
-	px4_pollfd_struct_t polls[2];
+	// Default to notify that there was no change.
+	*control_data = nullptr;
+
+	const int num_poll = 1;
+	px4_pollfd_struct_t polls[num_poll];
 	polls[0].fd = 		_vehicle_command_sub;
 	polls[0].events = 	POLLIN;
-	int num_poll = 1;
-
-	if (_manual_control && _allow_manual_control) {
-		polls[num_poll].fd = 		_manual_control->_get_subscription_fd();
-		polls[num_poll].events = 	POLLIN;
-		++num_poll;
-	}
 
 	int ret = px4_poll(polls, num_poll, timeout_ms);
 
@@ -244,7 +207,7 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
 	}
 
 	if (ret == 0) {
-		*control_data = nullptr;
+		// Timeout control_data already null.
 
 	} else {
 		*control_data = &_control_data;
@@ -260,7 +223,6 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
 			_control_data.gimbal_shutter_retract = false;
 
 			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) {
-				_allow_manual_control = false;
 
 				switch ((int)vehicle_command.param7) {
 				case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
@@ -283,7 +245,6 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
 					break;
 
 				case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
-					_allow_manual_control = true;
 					*control_data = nullptr;
 
 					break;
@@ -304,9 +265,6 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
 
 				_ack_vehicle_command(vehicle_command.command);
 			}
-
-		} else if (num_poll > 1 && (polls[1].revents & POLLIN)) {
-			_manual_control->_read_control_data_from_subscription(_control_data);
 		}
 
 	}
@@ -333,7 +291,7 @@ void InputMavlinkCmdMount::_ack_vehicle_command(uint16_t command)
 
 void InputMavlinkCmdMount::print_status()
 {
-	PX4_INFO("Input: Mavlink (CMD_MOUNT)%s", _manual_control ? " (with manual)" : "");
+	PX4_INFO("Input: Mavlink (CMD_MOUNT)");
 }
 
 

src/drivers/vmount/input_mavlink.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2017 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
@@ -54,18 +54,13 @@ namespace vmount
 class InputMavlinkROI : public InputBase
 {
 public:
-
-	/**
-	 * @param manual_control if non-null allow manual input as long as we have not received any mavlink
-	 * command yet or ROI mode is set to NONE.
-	 */
-	InputMavlinkROI(InputRC *manual_control = nullptr);
+	InputMavlinkROI();
 	virtual ~InputMavlinkROI();
 
 	virtual void print_status();
 
 protected:
-	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data);
+	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
 	virtual int initialize();
 
 private:
@@ -73,8 +68,6 @@ private:
 
 	int _vehicle_roi_sub = -1;
 	int _position_setpoint_triplet_sub = -1;
-	bool _allow_manual_control = true;
-	InputRC *_manual_control;
 	uint8_t _cur_roi_mode = vehicle_roi_s::VEHICLE_ROI_NONE;
 };
 
@@ -86,26 +79,19 @@ private:
 class InputMavlinkCmdMount : public InputBase
 {
 public:
-
-	/**
-	 * @param manual_control if non-null allow manual input as long as we have not received any mavlink
-	 * command yet.
-	 */
-	InputMavlinkCmdMount(InputRC *manual_control = nullptr);
+	InputMavlinkCmdMount();
 	virtual ~InputMavlinkCmdMount();
 
 	virtual void print_status();
 
 protected:
-	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data);
+	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
 	virtual int initialize();
 
 private:
 	void _ack_vehicle_command(uint16_t command);
 
 	int _vehicle_command_sub = -1;
-	bool _allow_manual_control = true;
-	InputRC *_manual_control;
 	orb_advert_t _vehicle_command_ack_pub = nullptr;
 	bool _stabilize[3] = { false, false, false };
 };

src/drivers/vmount/input_rc.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2017 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
@@ -73,8 +73,11 @@ int InputRC::initialize()
 	return 0;
 }
 
-int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data)
+int InputRC::update_impl(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;
@@ -86,32 +89,58 @@ int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data)
 	}
 
 	if (ret == 0) {
-		*control_data = nullptr;
-
+		// Timeout, leave NULL
 	} else {
 		if (polls[0].revents & POLLIN) {
-			_read_control_data_from_subscription(_control_data);
-			*control_data = &_control_data;
+			// 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;
+			}
 		}
 	}
 
 	return 0;
 }
 
-void InputRC::_read_control_data_from_subscription(ControlData &control_data)
+bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bool already_active)
 {
 	manual_control_setpoint_s manual_control_setpoint;
 	orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &manual_control_setpoint);
 	control_data.type = ControlData::Type::Angle;
 
-	for (int i = 0; i < 3; ++i) {
-		control_data.type_data.angle.is_speed[i] = false;
-		control_data.type_data.angle.angles[i] = _get_aux_value(manual_control_setpoint, i) * M_PI_F;
+	float new_aux_values[3];
 
-		control_data.stabilize_axis[i] = false;
+	for (int i = 0; i < 3; ++i) {
+		new_aux_values[i] = _get_aux_value(manual_control_setpoint, i);
 	}
 
-	control_data.gimbal_shutter_retract = false;
+	// 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) {
+		if (fabs(_last_set_aux_values[i] - new_aux_values[i]) > 0.25) {
+			major_movement = true;
+		}
+	}
+
+	if (already_active || major_movement || _first_time) {
+
+		for (int i = 0; i < 3; ++i) {
+			control_data.type_data.angle.is_speed[i] = false;
+			control_data.type_data.angle.angles[i] = new_aux_values[i] * M_PI_F;
+			control_data.stabilize_axis[i] = false;
+
+			_last_set_aux_values[i] = new_aux_values[i];
+		}
+
+		control_data.gimbal_shutter_retract = false;
+		return true;
+
+	} else {
+		return false;
+	}
 }
 
 float InputRC::_get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx)

src/drivers/vmount/input_rc.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2017 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
@@ -67,10 +67,13 @@ public:
 	virtual void print_status();
 
 protected:
-	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data);
+	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
 	virtual int initialize();
 
-	virtual void _read_control_data_from_subscription(ControlData &control_data);
+	/**
+	 * @return true if there was a change in control data
+	 */
+	virtual bool _read_control_data_from_subscription(ControlData &control_data, bool already_active);
 
 	int _get_subscription_fd() const { return _manual_control_setpoint_sub; }
 
@@ -79,9 +82,9 @@ private:
 
 	int _aux_channels[3];
 	int _manual_control_setpoint_sub = -1;
+	bool _first_time = true;
 
-	friend class InputMavlinkROI;
-	friend class InputMavlinkCmdMount;
+	float _last_set_aux_values[3] = {};
 };
 
 

src/drivers/vmount/input_test.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2017 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
@@ -57,7 +57,7 @@ bool InputTest::finished()
 	return true; /* only a single-shot test (for now) */
 }
 
-int InputTest::update(unsigned int timeout_ms, ControlData **control_data)
+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
 

src/drivers/vmount/input_test.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2017 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
@@ -61,10 +61,10 @@ public:
 	/** check whether the test finished, and thus the main thread can quit */
 	bool finished();
 
-	virtual int update(unsigned int timeout_ms, ControlData **control_data);
+	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) { return 0; } //not needed
+	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) { return 0; } //not needed
 
 	virtual int initialize();
 

src/drivers/vmount/vmount.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2017 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
@@ -35,9 +35,11 @@
  * @file vmount.cpp
  * @author Leon Müller (thedevleon)
  * @author Beat Küng <beat-kueng@gmx.net>
- * MAV_MOUNT driver for controlling mavlink gimbals, rc gimbals/servors and
- * future kinds of mounts.
+ * @author Julian Oes <julian@oes.ch>
  *
+ * 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.
  */
 
 #include <stdlib.h>
@@ -67,8 +69,11 @@ using namespace vmount;
 /* thread state */
 static volatile bool thread_should_exit = false;
 static volatile bool thread_running = false;
+
+static constexpr unsigned input_objs_len = 3;
+
 struct ThreadData {
-	InputBase *input_obj = nullptr;
+	InputBase *input_objs[input_objs_len] = {nullptr, nullptr, nullptr};
 	OutputBase *output_obj = nullptr;
 };
 static volatile ThreadData *g_thread_data = nullptr;
@@ -80,7 +85,6 @@ struct Parameters {
 	int mnt_mav_compid;
 	int mnt_ob_lock_mode;
 	int mnt_ob_norm_mode;
-	int mnt_man_control;
 	int mnt_man_pitch;
 	int mnt_man_roll;
 	int mnt_man_yaw;
@@ -93,7 +97,6 @@ struct Parameters {
 		       mnt_mav_compid != p.mnt_mav_compid ||
 		       mnt_ob_lock_mode != p.mnt_ob_lock_mode ||
 		       mnt_ob_norm_mode != p.mnt_ob_norm_mode ||
-		       mnt_man_control != p.mnt_man_control ||
 		       mnt_man_pitch != p.mnt_man_pitch ||
 		       mnt_man_roll != p.mnt_man_roll ||
 		       mnt_man_yaw != p.mnt_man_yaw;
@@ -107,7 +110,6 @@ struct ParameterHandles {
 	param_t mnt_mav_compid;
 	param_t mnt_ob_lock_mode;
 	param_t mnt_ob_norm_mode;
-	param_t mnt_man_control;
 	param_t mnt_man_pitch;
 	param_t mnt_man_roll;
 	param_t mnt_man_yaw;
@@ -193,49 +195,77 @@ static int vmount_thread_main(int argc, char *argv[])
 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
 	thread_running = true;
 	ControlData *control_data = nullptr;
-	InputRC *manual_input = nullptr;
 	g_thread_data = &thread_data;
 
+	int last_active = -1;
+
 	while (!thread_should_exit) {
 
-		if (!thread_data.input_obj && (params.mnt_mode_in != 0 || test_input)) { //need to initialize
+		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.mavlink_sys_id = params.mnt_mav_sysid;
 			output_config.mavlink_comp_id = params.mnt_mav_compid;
 
+			bool alloc_failed = false;
+
 			if (test_input) {
-				thread_data.input_obj = test_input;
+				thread_data.input_objs[0] = test_input;
 
 			} else {
-				if (params.mnt_man_control) {
-					manual_input = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
-
-					if (!manual_input) {
-						PX4_ERR("memory allocation failed");
-						break;
-					}
-				}
-
 				switch (params.mnt_mode_in) {
-				case 1: //RC
-					if (manual_input) {
-						thread_data.input_obj = manual_input;
-						manual_input = nullptr;
+				case 0:
 
-					} else {
-						thread_data.input_obj = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
+					// Automatic
+					if (!thread_data.input_objs[0]) {
+						thread_data.input_objs[0] = new InputMavlinkCmdMount();
+
+						if (!thread_data.input_objs[0]) { alloc_failed = true; }
+					}
+
+					if (!thread_data.input_objs[1]) {
+						thread_data.input_objs[1] = new InputMavlinkROI();
+
+						if (!thread_data.input_objs[1]) { alloc_failed = true; }
+					}
+
+					// 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.
+					if (!thread_data.input_objs[2]) {
+						thread_data.input_objs[0] = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
+
+						if (!thread_data.input_objs[2]) { alloc_failed = true; }
+					}
+
+					break;
+
+				case 1: //RC
+					if (!thread_data.input_objs[0]) {
+						thread_data.input_objs[0] = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
+
+						if (!thread_data.input_objs[0]) { alloc_failed = true; }
 					}
 
 					break;
 
 				case 2: //MAVLINK_ROI
-					thread_data.input_obj = new InputMavlinkROI(manual_input);
+					if (!thread_data.input_objs[0]) {
+						thread_data.input_objs[0] = new InputMavlinkROI();
+
+						if (!thread_data.input_objs[0]) { alloc_failed = true; }
+					}
+
 					break;
 
 				case 3: //MAVLINK_DO_MOUNT
-					thread_data.input_obj = new InputMavlinkCmdMount(manual_input);
+					if (!thread_data.input_objs[0]) {
+						thread_data.input_objs[0] = new InputMavlinkCmdMount();
+
+						if (!thread_data.input_objs[0]) { alloc_failed = true; }
+					}
+
 					break;
 
 				default:
@@ -247,10 +277,16 @@ static int vmount_thread_main(int argc, char *argv[])
 			switch (params.mnt_mode_out) {
 			case 0: //AUX
 				thread_data.output_obj = new OutputRC(output_config);
+
+				if (!thread_data.output_obj) { alloc_failed = true; }
+
 				break;
 
 			case 1: //MAVLINK
 				thread_data.output_obj = new OutputMavlink(output_config);
+
+				if (!thread_data.output_obj) { alloc_failed = true; }
+
 				break;
 
 			default:
@@ -258,7 +294,7 @@ static int vmount_thread_main(int argc, char *argv[])
 				break;
 			}
 
-			if (!thread_data.input_obj || !thread_data.output_obj) {
+			if (alloc_failed) {
 				PX4_ERR("memory allocation failed");
 				thread_should_exit = true;
 				break;
@@ -273,19 +309,31 @@ static int vmount_thread_main(int argc, char *argv[])
 			}
 		}
 
-		if (thread_data.input_obj) {
+		if (thread_data.input_objs[0]) {
 
 			//get input: we cannot make the timeout too large, because the output needs to update
 			//periodically for stabilization and angle updates.
-			int ret = thread_data.input_obj->update(50, &control_data);
 
-			if (ret) {
-				PX4_ERR("failed to read input (%i)", ret);
-				break;
+			for (int i = 0; i < input_objs_len; ++i) {
+
+				bool already_active = (last_active == i);
+
+				ControlData *control_data_to_check = nullptr;
+				int ret = thread_data.input_objs[i]->update(50, &control_data, already_active);
+
+				if (ret) {
+					PX4_ERR("failed to read input %i (ret: %i)", i, ret);
+					continue;
+				}
+
+				if (control_data_to_check != nullptr) {
+					control_data = control_data_to_check;
+					last_active = i;
+				}
 			}
 
 			//update output
-			ret = thread_data.output_obj->update(control_data);
+			int ret = thread_data.output_obj->update(control_data);
 
 			if (ret) {
 				PX4_ERR("failed to write output (%i)", ret);
@@ -315,20 +363,19 @@ static int vmount_thread_main(int argc, char *argv[])
 
 			if (updated) {
 				//re-init objects
-				if (thread_data.input_obj) {
-					delete (thread_data.input_obj);
-					thread_data.input_obj = nullptr;
+				for (int i = 0; i < input_objs_len; ++i) {
+					if (thread_data.input_objs[i]) {
+						delete (thread_data.input_objs[i]);
+						thread_data.input_objs[i] = nullptr;
+					}
 				}
 
+				last_active = -1;
+
 				if (thread_data.output_obj) {
 					delete (thread_data.output_obj);
 					thread_data.output_obj = nullptr;
 				}
-
-				if (manual_input) {
-					delete (manual_input);
-					manual_input = nullptr;
-				}
 			}
 		}
 	}
@@ -337,9 +384,11 @@ static int vmount_thread_main(int argc, char *argv[])
 
 	orb_unsubscribe(parameter_update_sub);
 
-	if (thread_data.input_obj) {
-		delete (thread_data.input_obj);
-		thread_data.input_obj = nullptr;
+	for (int i = 0; i < input_objs_len; ++i) {
+		if (thread_data.input_objs[i]) {
+			delete (thread_data.input_objs[i]);
+			thread_data.input_objs[i] = nullptr;
+		}
 	}
 
 	if (thread_data.output_obj) {
@@ -347,11 +396,6 @@ static int vmount_thread_main(int argc, char *argv[])
 		thread_data.output_obj = nullptr;
 	}
 
-	if (manual_input) {
-		delete (manual_input);
-		manual_input = nullptr;
-	}
-
 	thread_running = false;
 	return 0;
 }
@@ -422,10 +466,16 @@ int vmount_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running && g_thread_data) {
 
-			if (g_thread_data->input_obj) {
-				g_thread_data->input_obj->print_status();
+			bool found_input = false;
 
-			} else {
+			for (int i = 0; i < input_objs_len; ++i) {
+				if (g_thread_data->input_objs[i]) {
+					g_thread_data->input_objs[i]->print_status();
+					found_input = true;
+				}
+			}
+
+			if (!found_input) {
 				PX4_INFO("Input: None");
 			}
 
@@ -457,7 +507,6 @@ void update_params(ParameterHandles &param_handles, Parameters &params, bool &go
 	param_get(param_handles.mnt_mav_compid, &params.mnt_mav_compid);
 	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_control, &params.mnt_man_control);
 	param_get(param_handles.mnt_man_pitch, &params.mnt_man_pitch);
 	param_get(param_handles.mnt_man_roll, &params.mnt_man_roll);
 	param_get(param_handles.mnt_man_yaw, &params.mnt_man_yaw);
@@ -473,7 +522,6 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
 	param_handles.mnt_mav_compid = 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_control = param_find("MNT_MAN_CONTROL");
 	param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH");
 	param_handles.mnt_man_roll = param_find("MNT_MAN_ROLL");
 	param_handles.mnt_man_yaw = param_find("MNT_MAN_YAW");
@@ -484,7 +532,6 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
 	    param_handles.mnt_mav_compid == PARAM_INVALID ||
 	    param_handles.mnt_ob_lock_mode == PARAM_INVALID ||
 	    param_handles.mnt_ob_norm_mode == PARAM_INVALID ||
-	    param_handles.mnt_man_control == PARAM_INVALID ||
 	    param_handles.mnt_man_pitch == PARAM_INVALID ||
 	    param_handles.mnt_man_roll == PARAM_INVALID ||
 	    param_handles.mnt_man_yaw == PARAM_INVALID) {

src/drivers/vmount/vmount_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2013-2017 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
@@ -42,7 +42,7 @@
 * 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.
-* @value 0 DISABLE
+* @value 0 AUTO
 * @value 1 RC
 * @value 2 MAVLINK_ROI
 * @value 3 MAVLINK_DO_MOUNT
@@ -99,18 +99,6 @@ PARAM_DEFINE_FLOAT(MNT_OB_NORM_MODE, -1.0f);
 */
 PARAM_DEFINE_FLOAT(MNT_OB_LOCK_MODE, 0.0f);
 
-
-/**
-* This enables the mount to be manually controlled when no ROI is set.
-*
-* If set to 1, the mount will be controlled by the AUX channels below
-* when no ROI is set.
-*
-* @boolean
-* @group Mount
-*/
-PARAM_DEFINE_INT32(MNT_MAN_CONTROL, 0);
-
 /**
 * Auxiliary channel to control roll (in AUX input or manual mode).
 *