vmount: refactor architecture & use C++

This splits vmount into inputs and outputs modules with a small API in
between. It allows for greater flexibility, as any input method can be
combined with any output method. At the same time it is easy to add a new
input or output module.

src/drivers/vmount/CMakeLists.txt

@@ -37,11 +37,14 @@ px4_add_module(
 	COMPILE_FLAGS
 		-Os
 	SRCS
-        vmount.cpp
+		input.cpp
-        vmount_params.c
+		input_mavlink.cpp
-        vmount_rc.cpp
+		input_rc.cpp
-        vmount_mavlink.cpp
+		output.cpp
-        vmount_onboard.cpp
+		output_mavlink.cpp
+		output_rc.cpp
+		vmount.cpp
+		vmount_params.c
 	DEPENDS
 		platforms__common
 	)

src/drivers/vmount/common.h

@@ -0,0 +1,90 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in
+*    the documentation and/or other materials provided with the
+*    distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+*    used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file common.h
+ * @author Beat Küng <beat-kueng@gmx.net>
+ *
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+
+namespace vmount
+{
+
+
+/**
+ * @struct ControlData
+ * This defines the common API between an input and an output of the vmount driver.
+ * Each output must support the (full) set of the commands, and an input can create all
+ * or a subset of the types.
+ */
+struct ControlData {
+
+	enum class Type : uint8_t {
+		Neutral = 0,      /**< move to neutral position */
+		Angle,            /**< control the roll, pitch & yaw angle directly */
+		LonLat            /**< control via lon, lat */
+		//TODO: add more, like smooth curve, ... ?
+	};
+
+	Type type = Type::Neutral;
+
+	union TypeData {
+		struct TypeAngle {
+			float angles[3];              /**< attitude angles (roll, pitch, yaw) in rad, [-pi, +pi] if is_speed[i] == false */
+
+			bool is_speed[3];        /**< if true, the angle is the angular speed in rad/s */
+		} 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 */
+			float pitch_fixed_angle; /**< ignored if < -pi, otherwise use a fixed pitch angle instead of the altitude */
+		} lonlat;
+	} type_data;
+
+
+	bool stabilize_axis[3] = { false, false, false }; /**< whether the vmount driver should stabilize an axis */
+
+	bool gimbal_shutter_retract = false; /**< whether to lock the gimbal (only in RC output mode) */
+
+};
+
+
+} /* namespace vmount */

src/drivers/vmount/input.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -32,28 +32,47 @@
 ****************************************************************************/
 
 /**
- * @file vmount_onboard
+ * @file input.cpp
- * @author Leon Müller (thedevleon)
+ * @author Beat Küng <beat-kueng@gmx.net>
  *
  */
 
-#ifndef _VMOUNT_ONBOARD_H
+#include "input.h"
-#define _VMOUNT_ONBOARD_H
 
-#include <sys/types.h>
-#include <stdbool.h>
-#include <uORB/topics/vehicle_attitude.h>
 
-// Public functions
+namespace vmount
-bool vmount_onboard_init(void);
+{
-void vmount_onboard_deinit(void);
+
-void vmount_onboard_configure(int new_mount_mode, bool new_stab_roll, bool new_stab_pitch, bool new_stab_yaw);
+int InputBase::update(unsigned int timeout_ms, ControlData **control_data)
-void vmount_onboard_set_location(double lat, double lon, float alt);
+{
-void vmount_onboard_set_manual(double pitch_new, double roll_new, float yaw_new);
+	if (!_initialized) {
-void vmount_onboard_set_mode(int new_mount_mode);
+		int ret = initialize();
-void vmount_onboard_point(double global_lat, double global_lon, float global_alt);
+
-void vmount_onboard_point_manual(float pitch_target, float roll_target, float yaw_target);
+		if (ret) {
-void vmount_onboard_update_attitude(vehicle_attitude_s vehicle_attitude_new);
+			return ret;
-float vmount_onboard_calculate_pitch(double global_lat, double global_lon, float global_alt);
+		}
+
+		//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);
+}
+
+void InputBase::control_data_set_lon_lat(double lon, double lat, float altitude, float roll_angle,
+		float pitch_fixed_angle)
+{
+	_control_data.type = ControlData::Type::LonLat;
+	_control_data.type_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;
+}
+
+} /* namespace vmount */
 
-#endif /* _VMOUNT_ONBOARD_H */

src/drivers/vmount/input.h

@@ -0,0 +1,88 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in
+*    the documentation and/or other materials provided with the
+*    distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+*    used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file input.h
+ * @author Beat Küng <beat-kueng@gmx.net>
+ *
+ */
+
+#pragma once
+
+#include "common.h"
+
+namespace vmount
+{
+
+
+/**
+ ** class InputBase
+ * Base class for all driver input classes
+ */
+class InputBase
+{
+public:
+	virtual ~InputBase() {}
+
+	/**
+	 * 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).
+	 * @return 0 on success, <0 otherwise
+	 */
+	virtual int update(unsigned int timeout_ms, ControlData **control_data);
+
+	/** report status to stdout */
+	virtual void print_status() = 0;
+
+protected:
+	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data) = 0;
+
+	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;
+};
+
+
+} /* namespace vmount */

src/drivers/vmount/input_mavlink.cpp

@@ -0,0 +1,317 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in
+*    the documentation and/or other materials provided with the
+*    distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+*    used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file input_mavlink.cpp
+ * @author Leon Müller (thedevleon)
+ * @author Beat Küng <beat-kueng@gmx.net>
+ *
+ */
+
+#include "input_mavlink.h"
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_roi.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <drivers/drv_hrt.h>
+
+#include <px4_posix.h>
+#include <errno.h>
+
+
+namespace vmount
+{
+
+
+InputMavlinkROI::InputMavlinkROI(InputRC *manual_override)
+	: _manual_control(manual_override)
+{
+}
+
+InputMavlinkROI::~InputMavlinkROI()
+{
+	if (_vehicle_roi_sub >= 0) {
+		orb_unsubscribe(_vehicle_roi_sub);
+	}
+
+	if (_position_setpoint_triplet_sub >= 0) {
+		orb_unsubscribe(_position_setpoint_triplet_sub);
+	}
+}
+
+int InputMavlinkROI::initialize()
+{
+	_vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
+
+	if (_vehicle_roi_sub < 0) {
+		return -errno;
+	}
+
+	_position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+
+	if (_position_setpoint_triplet_sub < 0) {
+		return -errno;
+	}
+
+	return 0;
+}
+
+
+
+int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_data)
+{
+	px4_pollfd_struct_t polls[3];
+	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);
+
+	if (ret < 0) {
+		return -errno;
+	}
+
+	if (ret == 0) {
+		*control_data = nullptr;
+
+	} else {
+		if (polls[0].revents & POLLIN) {
+			vehicle_roi_s vehicle_roi;
+			orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi);
+
+			if (vehicle_roi.mode == vehicle_roi_s::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;
+
+			//set all other control data fields to defaults
+			for (int i = 0; i < 3; ++i) {
+				_control_data.stabilize_axis[i] = false;
+			}
+
+			_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
+		if ((polls[1].revents & POLLIN) && _cur_roi_mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
+			_read_control_data_from_position_setpoint_sub();
+		}
+
+		*control_data = &_control_data;
+	}
+
+	return 0;
+}
+
+void InputMavlinkROI::_read_control_data_from_position_setpoint_sub()
+{
+	position_setpoint_triplet_s position_setpoint_triplet;
+	orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet);
+	_control_data.type_data.lonlat.lon = position_setpoint_triplet.next.lon;
+	_control_data.type_data.lonlat.lat = position_setpoint_triplet.next.lat;
+	_control_data.type_data.lonlat.altitude = position_setpoint_triplet.next.alt;
+}
+
+void InputMavlinkROI::print_status()
+{
+	PX4_INFO("Input: Mavlink (ROI)%s", _manual_control ? " (with manual)" : "");
+}
+
+InputMavlinkCmdMount::InputMavlinkCmdMount(InputRC *manual_override)
+	: _manual_control(manual_override)
+{
+}
+
+InputMavlinkCmdMount::~InputMavlinkCmdMount()
+{
+	if (_vehicle_command_sub >= 0) {
+		orb_unsubscribe(_vehicle_command_sub);
+	}
+}
+
+int InputMavlinkCmdMount::initialize()
+{
+	if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) {
+		return -errno;
+	}
+
+	return 0;
+}
+
+
+int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **control_data)
+{
+	px4_pollfd_struct_t polls[2];
+	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);
+
+	if (ret < 0) {
+		return -errno;
+	}
+
+	if (ret == 0) {
+		*control_data = nullptr;
+
+	} else {
+		*control_data = &_control_data;
+
+		if (polls[0].revents & POLLIN) {
+			vehicle_command_s vehicle_command;
+			orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command);
+
+			for (int i = 0; i < 3; ++i) {
+				_control_data.stabilize_axis[i] = _stabilize[i];
+			}
+
+			_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:
+					_control_data.gimbal_shutter_retract = true;
+
+				//no break
+				case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
+					_control_data.type = ControlData::Type::Neutral;
+					break;
+
+				case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
+					_control_data.type = ControlData::Type::Angle;
+					_control_data.type_data.angle.is_speed[0] = false;
+					_control_data.type_data.angle.is_speed[1] = false;
+					_control_data.type_data.angle.is_speed[2] = false;
+					_control_data.type_data.angle.angles[0] = vehicle_command.param1;
+					_control_data.type_data.angle.angles[1] = vehicle_command.param2;
+					_control_data.type_data.angle.angles[2] = vehicle_command.param3;
+
+					break;
+
+				case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
+					_allow_manual_control = true;
+					*control_data = nullptr;
+
+					break;
+
+				case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
+					control_data_set_lon_lat(vehicle_command.param2, vehicle_command.param1, vehicle_command.param3);
+
+					break;
+				}
+
+				_ack_vehicle_command(vehicle_command.command);
+
+			} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
+				_stabilize[0] = (uint8_t) vehicle_command.param2 == 1;
+				_stabilize[1] = (uint8_t) vehicle_command.param3 == 1;
+				_stabilize[2] = (uint8_t) vehicle_command.param4 == 1;
+				_control_data.type = ControlData::Type::Neutral; //always switch to neutral position
+
+				_ack_vehicle_command(vehicle_command.command);
+			}
+
+		} else if (num_poll > 1 && (polls[1].revents & POLLIN)) {
+			_manual_control->_read_control_data_from_subscription(_control_data);
+		}
+
+	}
+
+	return 0;
+}
+
+void InputMavlinkCmdMount::_ack_vehicle_command(uint16_t command)
+{
+	vehicle_command_ack_s vehicle_command_ack;
+	vehicle_command_ack.timestamp = hrt_absolute_time();
+	vehicle_command_ack.command = command;
+	vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+	int instance;
+	orb_publish_auto(ORB_ID(vehicle_command_ack), &_vehicle_command_ack_pub, &vehicle_command_ack,
+			 &instance, ORB_PRIO_DEFAULT);
+}
+
+void InputMavlinkCmdMount::print_status()
+{
+	PX4_INFO("Input: Mavlink (CMD_MOUNT)%s", _manual_control ? " (with manual)" : "");
+}
+
+
+} /* namespace vmount */
+

src/drivers/vmount/input_mavlink.h

@@ -0,0 +1,114 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in
+*    the documentation and/or other materials provided with the
+*    distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+*    used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file input_mavlink.h
+ * @author Beat Küng <beat-kueng@gmx.net>
+ *
+ */
+
+#pragma once
+
+#include "input.h"
+#include "input_rc.h"
+#include <uORB/topics/vehicle_roi.h>
+
+namespace vmount
+{
+
+
+/**
+ ** class InputMavlinkROI
+ ** Input based on the vehicle_roi topic
+ */
+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);
+	virtual ~InputMavlinkROI();
+
+	virtual void print_status();
+
+protected:
+	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data);
+	virtual int initialize();
+
+private:
+	void _read_control_data_from_position_setpoint_sub();
+
+	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;
+};
+
+
+/**
+ ** class InputMavlinkCmdMount
+ ** Input based on the VEHICLE_CMD_DO_MOUNT_CONTROL mavlink command
+ */
+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);
+	virtual ~InputMavlinkCmdMount();
+
+	virtual void print_status();
+
+protected:
+	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data);
+	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 };
+};
+
+
+} /* namespace vmount */

src/drivers/vmount/input_rc.cpp

@@ -0,0 +1,146 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in
+*    the documentation and/or other materials provided with the
+*    distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+*    used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file input_rc.cpp
+ * @author Beat Küng <beat-kueng@gmx.net>
+ *
+ */
+
+#include "input_rc.h"
+
+#include <errno.h>
+#include <px4_posix.h>
+#include <px4_defines.h>
+
+
+namespace vmount
+{
+
+
+InputRC::InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw)
+{
+	_aux_channels[0] = aux_channel_roll;
+	_aux_channels[1] = aux_channel_pitch;
+	_aux_channels[2] = aux_channel_yaw;
+}
+
+InputRC::~InputRC()
+{
+	if (_manual_control_setpoint_sub >= 0) {
+		orb_unsubscribe(_manual_control_setpoint_sub);
+	}
+}
+
+int InputRC::initialize()
+{
+	_manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+
+	if (_manual_control_setpoint_sub < 0) {
+		return -errno;
+	}
+
+	return 0;
+}
+
+int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data)
+{
+	px4_pollfd_struct_t polls[1];
+	polls[0].fd = 		_manual_control_setpoint_sub;
+	polls[0].events = 	POLLIN;
+
+	int ret = px4_poll(polls, 1, timeout_ms);
+
+	if (ret < 0) {
+		return -errno;
+	}
+
+	if (ret == 0) {
+		*control_data = nullptr;
+
+	} else {
+		if (polls[0].revents & POLLIN) {
+			_read_control_data_from_subscription(_control_data);
+			*control_data = &_control_data;
+		}
+	}
+
+	return 0;
+}
+
+void InputRC::_read_control_data_from_subscription(ControlData &control_data)
+{
+	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;
+
+		control_data.stabilize_axis[i] = false;
+	}
+
+	control_data.gimbal_shutter_retract = false;
+}
+
+float InputRC::_get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx)
+{
+	switch (_aux_channels[channel_idx]) {
+
+	case 1:
+		return manual_control_setpoint.aux1;
+
+	case 2:
+		return manual_control_setpoint.aux2;
+
+	case 3:
+		return manual_control_setpoint.aux3;
+
+	case 4:
+		return manual_control_setpoint.aux4;
+
+	case 5:
+		return manual_control_setpoint.aux5;
+
+	default:
+		return 0.0f;
+	}
+}
+
+void InputRC::print_status()
+{
+	PX4_INFO("Input: RC (channels: roll=%i, pitch=%i, yaw=%i)", _aux_channels[0], _aux_channels[1], _aux_channels[2]);
+}
+
+} /* namespace vmount */

src/drivers/vmount/input_rc.h

@@ -0,0 +1,89 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in
+*    the documentation and/or other materials provided with the
+*    distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+*    used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file input_rc.h
+ * @author Beat Küng <beat-kueng@gmx.net>
+ *
+ */
+
+#pragma once
+
+#include "input.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:
+
+	/**
+	 * @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);
+	virtual ~InputRC();
+
+
+protected:
+	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data);
+	virtual int initialize();
+
+	virtual void print_status();
+
+private:
+	float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx);
+
+	int _get_subscription_fd() const { return _manual_control_setpoint_sub; }
+
+	void _read_control_data_from_subscription(ControlData &control_data);
+
+	int _aux_channels[3];
+	int _manual_control_setpoint_sub = -1;
+
+	friend class InputMavlinkROI;
+	friend class InputMavlinkCmdMount;
+};
+
+
+} /* namespace vmount */

src/drivers/vmount/output.cpp

@@ -0,0 +1,205 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in
+*    the documentation and/or other materials provided with the
+*    distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+*    used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @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>
+#include <px4_defines.h>
+#include <geo/geo.h>
+#include <math.h>
+
+#define LATLON_TO_M  0.01113195
+
+namespace vmount
+{
+
+OutputBase::OutputBase(const OutputConfig &output_config)
+	: _config(output_config)
+{
+	_last_update = hrt_absolute_time();
+}
+
+OutputBase::~OutputBase()
+{
+	if (_vehicle_attitude_sub >= 0) {
+		orb_unsubscribe(_vehicle_attitude_sub);
+	}
+
+	if (_vehicle_global_position_sub >= 0) {
+		orb_unsubscribe(_vehicle_global_position_sub);
+	}
+}
+
+int OutputBase::initialize()
+{
+	if ((_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude))) < 0) {
+		return -errno;
+	}
+
+	if ((_vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position))) < 0) {
+		return -errno;
+	}
+
+	return 0;
+}
+
+float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
+				   const vehicle_global_position_s &global_position)
+{
+	double scale = cos(M_DEG_TO_RAD * ((global_position.lat + lat) * 0.00000005));
+	float x = (float)((lon - global_position.lon) * scale * LATLON_TO_M);
+	float y = (float)((lat - global_position.lat) * LATLON_TO_M);
+	float z = altitude - global_position.alt;
+	float target_distance = sqrtf(x * x + y * y);
+
+	return atan2f(z, target_distance) * (float)M_RAD_TO_DEG;
+}
+
+void OutputBase::_set_angle_setpoints(const ControlData *control_data)
+{
+	_cur_control_data = control_data;
+
+	for (int i = 0; i < 3; ++i) {
+		_stabilize[i] = control_data->stabilize_axis[i];
+		_angle_speeds[i] = 0.f;
+	}
+
+	switch (control_data->type) {
+	case ControlData::Type::Angle:
+		for (int i = 0; i < 3; ++i) {
+			if (control_data->type_data.angle.is_speed[i]) {
+				_angle_speeds[i] = control_data->type_data.angle.angles[i];
+
+			} else {
+				_angle_setpoints[i] = control_data->type_data.angle.angles[i];
+			}
+		}
+
+		break;
+
+	case ControlData::Type::LonLat:
+		_handle_position_update(true);
+		break;
+
+	case ControlData::Type::Neutral:
+		_angle_setpoints[0] = 0.f;
+		_angle_setpoints[1] = 0.f;
+		_angle_setpoints[2] = 0.f;
+		break;
+	}
+}
+
+void OutputBase::_handle_position_update(bool force_update)
+{
+	bool need_update = force_update;
+
+	if (!_cur_control_data || _cur_control_data->type != ControlData::Type::LonLat) {
+		return;
+	}
+
+	if (!force_update) {
+		orb_check(_vehicle_global_position_sub, &need_update);
+	}
+
+	if (!need_update) {
+		return;
+	}
+
+	vehicle_global_position_s vehicle_global_position;
+	orb_copy(ORB_ID(vehicle_global_position), _vehicle_global_position_sub, &vehicle_global_position);
+
+	float pitch;
+	const double &lon = _cur_control_data->type_data.lonlat.lon;
+	const double &lat = _cur_control_data->type_data.lonlat.lat;
+	const float &alt = _cur_control_data->type_data.lonlat.altitude;
+
+	if (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) {
+		pitch = _cur_control_data->type_data.lonlat.pitch_fixed_angle;
+
+	} else {
+		pitch = _calculate_pitch(lon, lat, alt, vehicle_global_position);
+	}
+
+	float roll = _cur_control_data->type_data.lonlat.roll_angle;
+	float yaw = get_bearing_to_next_waypoint(vehicle_global_position.lat, vehicle_global_position.lon,
+			lat, lon) * (float)M_RAD_TO_DEG;
+
+	_angle_setpoints[0] = roll;
+	_angle_setpoints[1] = pitch;
+	_angle_setpoints[2] = yaw;
+}
+
+void OutputBase::_calculate_output_angles(const hrt_abstime &t)
+{
+	//take speed into account
+	float dt = (t - _last_update) / 1.e6f;
+
+	for (int i = 0; i < 3; ++i) {
+		_angle_setpoints[i] += dt * _angle_speeds[i];
+	}
+
+	//get the output angles and stabilize if necessary
+	vehicle_attitude_s vehicle_attitude;
+
+	if (_stabilize[0] || _stabilize[1] || _stabilize[2]) {
+		orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude);
+	}
+
+	float att[3] = { vehicle_attitude.roll, vehicle_attitude.pitch, vehicle_attitude.yaw };
+
+	for (int i = 0; i < 3; ++i) {
+		if (_stabilize[i]) {
+			_angle_outputs[i] = _angle_setpoints[i] - att[i];
+
+		} else {
+			_angle_outputs[i] = _angle_setpoints[i];
+		}
+
+		//bring angles into proper range [-pi, pi]
+		while (_angle_outputs[i] > M_PI_F) { _angle_outputs[i] -= 2.f * M_PI_F; }
+
+		while (_angle_outputs[i] < -M_PI_F) { _angle_outputs[i] += 2.f * M_PI_F; }
+	}
+}
+
+} /* namespace vmount */
+

src/drivers/vmount/output.h

@@ -0,0 +1,111 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in
+*    the documentation and/or other materials provided with the
+*    distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+*    used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file output.h
+ * @author Beat Küng <beat-kueng@gmx.net>
+ *
+ */
+
+#pragma once
+
+#include "common.h"
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/vehicle_global_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 */
+
+	uint32_t mavlink_sys_id; /**< mavlink target system id for mavlink output */
+	uint32_t mavlink_comp_id;
+};
+
+
+/**
+ ** class OutputBase
+ * Base class for all driver output classes
+ */
+class OutputBase
+{
+public:
+	OutputBase(const OutputConfig &output_config);
+	virtual ~OutputBase();
+
+	virtual int initialize();
+
+	/**
+	 * Update the output.
+	 * @param data new command if non null
+	 * @return 0 on success, <0 otherwise
+	 */
+	virtual int update(const ControlData *control_data) = 0;
+
+	/** report status to stdout */
+	virtual void print_status() = 0;
+
+protected:
+	static float _calculate_pitch(double lon, double lat, float altitude,
+				      const vehicle_global_position_s &global_position);
+
+	const OutputConfig &_config;
+
+	/** set angle setpoints, speeds & stabilize flags */
+	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;
+	float _angle_setpoints[3] = { 0.f, 0.f, 0.f };
+	float _angle_speeds[3] = { 0.f, 0.f, 0.f };
+	bool _stabilize[3] = { false, false, false };
+
+	/** calculate the _angle_outputs (with speed) and stabilize if needed */
+	void _calculate_output_angles(const hrt_abstime &t);
+
+	float _angle_outputs[3] = { 0.f, 0.f, 0.f };
+	hrt_abstime _last_update;
+
+private:
+	int _vehicle_attitude_sub = -1;
+	int _vehicle_global_position_sub = -1;
+
+};
+
+
+} /* namespace vmount */

src/drivers/vmount/output_mavlink.cpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in
+*    the documentation and/or other materials provided with the
+*    distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+*    used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file output_mavlink.cpp
+ * @author Leon Müller (thedevleon)
+ * @author Beat Küng <beat-kueng@gmx.net>
+ *
+ */
+
+#include "output_mavlink.h"
+
+#include <uORB/topics/vehicle_command.h>
+#include <px4_defines.h>
+
+
+namespace vmount
+{
+
+OutputMavlink::OutputMavlink(const OutputConfig &output_config)
+	: OutputBase(output_config)
+{
+}
+
+int OutputMavlink::update(const ControlData *control_data)
+{
+	vehicle_command_s vehicle_command;
+
+	if (control_data) {
+		//got new command
+		_set_angle_setpoints(control_data);
+
+		vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
+		vehicle_command.target_system = _config.mavlink_sys_id;
+		vehicle_command.target_component = _config.mavlink_comp_id;
+
+		if (control_data->type == ControlData::Type::Neutral) {
+			vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
+
+		} else {
+			vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
+		}
+
+		if (_vehicle_command_pub) {
+			orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);
+
+		} else {
+			_vehicle_command_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command, 3);
+		}
+
+	}
+
+	if (!_vehicle_command_pub) {
+		return 0;
+	}
+
+	_handle_position_update();
+
+	hrt_abstime t = hrt_absolute_time();
+	_calculate_output_angles(t);
+
+	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
+	vehicle_command.target_system = _config.mavlink_sys_id;
+	vehicle_command.target_component = _config.mavlink_comp_id;
+
+	vehicle_command.param1 = _angle_outputs[0];
+	vehicle_command.param2 = _angle_outputs[1];
+	vehicle_command.param3 = _angle_outputs[2];
+
+	orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);
+
+	_last_update = t;
+
+	return 0;
+}
+
+void OutputMavlink::print_status()
+{
+	PX4_INFO("Output: Mavlink");
+}
+
+} /* namespace vmount */
+

src/drivers/vmount/output_mavlink.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -32,24 +32,39 @@
 ****************************************************************************/
 
 /**
- * @file vmount_rc
+ * @file output_mavlink.h
- * @author Leon Müller (thedevleon)
+ * @author Beat Küng <beat-kueng@gmx.net>
  *
  */
 
-#ifndef _VMOUNT_RC_H
+#pragma once
-#define _VMOUNT_RC_H
 
-#include <sys/types.h>
+#include "output.h"
-#include <stdbool.h>
 
-// Public functions
+#include <uORB/uORB.h>
-bool vmount_rc_init(void);
-void vmount_rc_deinit(void);
-void vmount_rc_configure(int roi_mode, bool man_control, int normal_mode_new, int locked_mode_new);
-void vmount_rc_set_location(double lat_new, double lon_new, float alt_new);
-void vmount_rc_point(double global_lat, double global_lon, float global_alt);
-void vmount_rc_point_manual(float pitch_new, float roll_new, float yaw_new);
-float vmount_rc_calculate_pitch(double global_lat, double global_lon, float global_alt);
 
-#endif /* _VMOUNT_RC_H */
+
+namespace vmount
+{
+
+
+/**
+ ** class OutputMavlink
+ *  Output via vehicle_command topic
+ */
+class OutputMavlink : public OutputBase
+{
+public:
+	OutputMavlink(const OutputConfig &output_config);
+	virtual ~OutputMavlink() { }
+
+	virtual int update(const ControlData *control_data);
+
+	virtual void print_status();
+
+private:
+	orb_advert_t _vehicle_command_pub = nullptr;
+};
+
+
+} /* namespace vmount */

src/drivers/vmount/output_rc.cpp

@@ -0,0 +1,90 @@
+/****************************************************************************
+*
+*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in
+*    the documentation and/or other materials provided with the
+*    distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+*    used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file output_rc.cpp
+ * @author Leon Müller (thedevleon)
+ * @author Beat Küng <beat-kueng@gmx.net>
+ *
+ */
+
+#include "output_rc.h"
+
+#include <uORB/topics/actuator_controls.h>
+#include <px4_defines.h>
+
+
+namespace vmount
+{
+
+OutputRC::OutputRC(const OutputConfig &output_config)
+	: OutputBase(output_config)
+{
+}
+
+int OutputRC::update(const ControlData *control_data)
+{
+	if (control_data) {
+		//got new command
+		_retract_gimbal = control_data->gimbal_shutter_retract;
+		_set_angle_setpoints(control_data);
+	}
+
+	_handle_position_update();
+
+	hrt_abstime t = hrt_absolute_time();
+	_calculate_output_angles(t);
+
+	actuator_controls_s actuator_controls;
+	actuator_controls.timestamp = hrt_absolute_time();
+	actuator_controls.control[0] = _angle_outputs[0] / M_PI_F;
+	actuator_controls.control[1] = _angle_outputs[1] / M_PI_F;
+	actuator_controls.control[2] = _angle_outputs[2] / M_PI_F;
+	actuator_controls.control[3] = _retract_gimbal ? _config.gimbal_retracted_mode_value : _config.gimbal_normal_mode_value;
+
+	int instance;
+	orb_publish_auto(ORB_ID(actuator_controls_2), &_actuator_controls_pub, &actuator_controls,
+			 &instance, ORB_PRIO_DEFAULT);
+
+	_last_update = t;
+
+	return 0;
+}
+
+void OutputRC::print_status()
+{
+	PX4_INFO("Output: AUX");
+}
+
+} /* namespace vmount */
+

src/drivers/vmount/output_rc.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -32,25 +32,40 @@
 ****************************************************************************/
 
 /**
- * @file vmount_mavlink.h
+ * @file output_rc.h
- * @author Leon Müller (thedevleon)
+ * @author Beat Küng <beat-kueng@gmx.net>
  *
  */
 
-#ifndef _VMOUNT_MAVLINK_H
+#pragma once
-#define _VMOUNT_MAVLINK_H
 
-#include <sys/types.h>
+#include "output.h"
-#include <stdbool.h>
 
-// Public functions
+#include <uORB/uORB.h>
-bool vmount_mavlink_init();
-void vmount_mavlink_deinit(void);
-void vmount_mavlink_configure(int roi_mode, bool man_control, int sysid, int compid);
-void vmount_mavlink_set_location(double lat_new, double lon_new, float alt_new);
-void vmount_mavlink_point(double global_lat, double global_lon, float global_alt);
-void vmount_mavlink_point_manual(float pitch_new, float roll_new, float yaw_new);
-float vmount_mavlink_calculate_pitch(double global_lat, double global_lon, float global_alt);
 
 
-#endif /* _VMOUNT_MAVLINK_H */
+namespace vmount
+{
+
+
+/**
+ ** class OutputRC
+ *  Output via actuator_controls_2 topic
+ */
+class OutputRC : public OutputBase
+{
+public:
+	OutputRC(const OutputConfig &output_config);
+	virtual ~OutputRC() { }
+
+	virtual int update(const ControlData *control_data);
+
+	virtual void print_status();
+
+private:
+	orb_advert_t _actuator_controls_pub = nullptr;
+	bool _retract_gimbal = true;
+};
+
+
+} /* namespace vmount */

src/drivers/vmount/vmount.cpp

@@ -34,6 +34,7 @@
 /**
  * @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.
  *
@@ -49,82 +50,26 @@
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 
-#include "vmount_mavlink.h"
+#include "input_rc.h"
-#include "vmount_rc.h"
+#include "input_mavlink.h"
-#include "vmount_onboard.h"
+#include "output_rc.h"
+#include "output_mavlink.h"
 
 #include <uORB/uORB.h>
-#include <uORB/topics/vehicle_roi.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/manual_control_setpoint.h>
 
 #include <px4_config.h>
-#include <px4_posix.h>
+
-#include <poll.h>
+using namespace vmount;
 
 /* thread state */
 static volatile bool thread_should_exit = false;
 static volatile bool thread_running = false;
-static volatile bool thread_should_restart = false;
+static volatile bool thread_do_report_status = false;
-static int vmount_task;
-typedef enum { IDLE = -1, MAVLINK = 0, RC = 1, ONBOARD = 2 } vmount_state_t;
-static vmount_state_t vmount_state = IDLE;
 
-/* polling */
+struct Parameters {
-px4_pollfd_struct_t polls[7];
+	int mnt_mode_in;
-
+	int mnt_mode_out;
-/* functions */
-static void usage(void);
-static void vmount_update_topics(void);
-static void update_params(void);
-static bool get_params(void);
-static float get_aux_value(int);
-static void ack_mount_command(uint16_t command);
-static int vmount_thread_main(int argc, char *argv[]);
-extern "C" __EXPORT int vmount_main(int argc, char *argv[]);
-
-/* uORB subscriptions */
-static int vehicle_roi_sub = -1;
-static int vehicle_global_position_sub = -1;
-static int vehicle_command_sub = -1;
-static int vehicle_attitude_sub = -1;
-static int position_setpoint_triplet_sub = -1;
-static int manual_control_setpoint_sub = -1;
-static int parameter_update_sub = -1;
-
-/* uORB publication */
-static orb_advert_t vehicle_command_ack_pub;
-
-static struct vehicle_roi_s vehicle_roi;
-static bool   vehicle_roi_updated;
-
-static struct vehicle_global_position_s vehicle_global_position;
-static bool   vehicle_global_position_updated;
-
-static struct vehicle_command_s vehicle_command;
-static bool   vehicle_command_updated;
-
-static struct vehicle_attitude_s vehicle_attitude;
-static bool   vehicle_attitude_updated;
-
-static struct position_setpoint_triplet_s position_setpoint_triplet;
-static bool   position_setpoint_triplet_updated;
-
-static struct manual_control_setpoint_s manual_control_setpoint;
-static bool   manual_control_setpoint_updated;
-
-static struct parameter_update_s parameter_update;
-static bool   parameter_update_updated;
-
-static struct vehicle_command_ack_s vehicle_command_ack;
-
-static struct {
-	int mnt_mode;
 	int mnt_mav_sysid;
 	int mnt_mav_compid;
 	int mnt_ob_lock_mode;
@@ -133,11 +78,25 @@ static struct {
 	int mnt_man_pitch;
 	int mnt_man_roll;
 	int mnt_man_yaw;
-	int mnt_mode_ovr;
-} params;
 
-static struct {
+	bool operator!=(const Parameters &p)
-	param_t mnt_mode;
+	{
+		return mnt_mode_in != p.mnt_mode_in ||
+		       mnt_mode_out != p.mnt_mode_out ||
+		       mnt_mav_sysid != p.mnt_mav_sysid ||
+		       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;
+	}
+};
+
+struct ParameterHandles {
+	param_t mnt_mode_in;
+	param_t mnt_mode_out;
 	param_t mnt_mav_sysid;
 	param_t mnt_mav_compid;
 	param_t mnt_ob_lock_mode;
@@ -146,314 +105,193 @@ static struct {
 	param_t mnt_man_pitch;
 	param_t mnt_man_roll;
 	param_t mnt_man_yaw;
-	param_t mnt_mode_ovr;
+};
-} params_handels;
 
-static bool manual_control_desired;
 
-/**
+/* functions */
- * Print command usage information
+static void usage(void);
- */
+static void update_params(ParameterHandles &param_handles, Parameters &params, bool &got_changes);
+static bool get_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[]);
+
 static void usage()
 {
-	fprintf(stderr,
+	PX4_INFO("usage: vmount {start|stop|status}");
-		"usage: vmount start\n"
-		"       vmount stop\n"
-		"       vmount status\n");
-	exit(1);
 }
 
-/**
- * The daemon thread.
- */
 static int vmount_thread_main(int argc, char *argv[])
 {
-	if (!get_params()) {
+	ParameterHandles param_handles;
-		warnx("could not get mount parameters!");
+	Parameters params;
+	memset(&params, 0, sizeof(params));
+
+	if (!get_params(param_handles, params)) {
+		PX4_ERR("could not get mount parameters!");
+		return -1;
 	}
 
-	if (params.mnt_mode == 0) { vmount_state = IDLE;}
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
-
-	else if (params.mnt_mode == 1) { vmount_state = MAVLINK;}
-
-	else if (params.mnt_mode == 2) { vmount_state = RC;}
-
-	else if (params.mnt_mode == 3) { vmount_state = ONBOARD;}
-
-	//TODO is this needed?
-	memset(&vehicle_roi, 0, sizeof(vehicle_roi));
-	memset(&vehicle_global_position, 0, sizeof(vehicle_global_position));
-	memset(&vehicle_command, 0, sizeof(vehicle_command));
-	memset(&vehicle_attitude, 0, sizeof(vehicle_attitude));
-	memset(&position_setpoint_triplet, 0, sizeof(position_setpoint_triplet));
-	memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint));
-	memset(&parameter_update, 0, sizeof(parameter_update));
-	memset(&vehicle_command_ack, 0, sizeof(vehicle_command_ack));
-
-
-	vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
-	vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
-	vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
-	manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
-
-	vehicle_command_ack_pub = orb_advertise(ORB_ID(vehicle_command_ack), &vehicle_command_ack);
-
-	if (!vehicle_roi_sub || !position_setpoint_triplet_sub ||
-	    !manual_control_setpoint_sub || !vehicle_global_position_sub ||
-	    !vehicle_command_sub || !parameter_update_sub || !vehicle_command_ack_pub ||
-		!vehicle_attitude_sub) {
-		err(1, "could not subscribe to uORB topics");
-	}
-
-	polls[0].fd = 		vehicle_roi_sub;
-	polls[0].events = 	POLLIN;
-	polls[1].fd = 		vehicle_global_position_sub;
-	polls[1].events = 	POLLIN;
-	polls[2].fd = 		vehicle_attitude_sub;
-	polls[2].events = 	POLLIN;
-	polls[3].fd = 		vehicle_command_sub;
-	polls[3].events = 	POLLIN;
-	polls[4].fd = 		position_setpoint_triplet_sub;
-	polls[4].events = 	POLLIN;
-	polls[5].fd = 		manual_control_setpoint_sub;
-	polls[5].events = 	POLLIN;
-	polls[6].fd = 		parameter_update_sub;
-	polls[6].events = 	POLLIN;
-
 	thread_running = true;
+	OutputConfig output_config;
+	ControlData *control_data = nullptr;
+	InputBase *input_obj = nullptr;
+	OutputBase *output_obj = nullptr;
+	InputRC *manual_input = nullptr;
 
-	run: {
+	while (!thread_should_exit) {
-		if (vmount_state == MAVLINK) {
-			if (!vmount_mavlink_init()) {
-				err(1, "could not initiate vmount_mavlink");
-			}
 
-			warnx("running mount driver in mavlink mode");
+		if (!input_obj && params.mnt_mode_in != 0) { //need to initialize
 
-			if (params.mnt_man_control) manual_control_desired = true;
+			output_config.gimbal_normal_mode_value = params.mnt_ob_norm_mode;
-
+			output_config.gimbal_retracted_mode_value = params.mnt_ob_lock_mode;
-			while (!thread_should_exit && !thread_should_restart) {
+			output_config.mavlink_sys_id = params.mnt_mav_sysid;
-				vmount_update_topics();
+			output_config.mavlink_comp_id = params.mnt_mav_compid;
-
-				if (vehicle_roi_updated) {
-					vehicle_roi_updated = false;
-					vmount_mavlink_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_mav_sysid, params.mnt_mav_compid);
-
-					if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
-						if (params.mnt_man_control) manual_control_desired = true;
-
-					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
-						manual_control_desired = false;
-						vmount_mavlink_set_location(
-							position_setpoint_triplet.next.lat,
-							position_setpoint_triplet.next.lon,
-							position_setpoint_triplet.next.alt
-						);
-
-					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
-						manual_control_desired = false;
-						//TODO how to do this?
-
-					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
-						manual_control_desired = false;
-						vmount_mavlink_set_location(
-							vehicle_roi.lat,
-							vehicle_roi.lon,
-							vehicle_roi.alt
-						);
-
-					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
-						manual_control_desired = false;
-						//TODO is this even suported?
-					}
-				}
-
-				else if (manual_control_desired && manual_control_setpoint_updated)
-				{
-					manual_control_setpoint_updated = false;
-					vmount_mavlink_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
-				}
-
-				else if (!manual_control_desired && vehicle_global_position_updated)
-				{
-					vehicle_global_position_updated = false;
-					vmount_mavlink_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
-				}
-
-			vmount_mavlink_deinit();
-			}
-
-		} else if (vmount_state == RC) {
-			if (!vmount_rc_init()) {
-				err(1, "could not initiate vmount_rc");
-			}
-
-			warnx("running mount driver in rc mode");
 
 			if (params.mnt_man_control) {
-				manual_control_desired = true;
+				manual_input = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
-			}
 
-			while (!thread_should_exit && !thread_should_restart) {
+				if (!manual_input) {
-				vmount_update_topics();
+					PX4_ERR("memory allocation failed");
-
+					break;
-				if (vehicle_roi_updated) {
-					vehicle_roi_updated = false;
-					vmount_rc_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_ob_norm_mode, params.mnt_ob_lock_mode);
-
-					if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
-						if (params.mnt_man_control) manual_control_desired = true;
-
-					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
-						manual_control_desired = false;
-						vmount_rc_set_location(
-							position_setpoint_triplet.next.lat,
-							position_setpoint_triplet.next.lon,
-							position_setpoint_triplet.next.alt
-						);
-
-					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
-						manual_control_desired = false;
-						//TODO how to do this?
-					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
-						manual_control_desired = false;
-						vmount_rc_set_location(
-							vehicle_roi.lat,
-							vehicle_roi.lon,
-							vehicle_roi.alt
-						);
-
-					} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
-						manual_control_desired = false;
-						//TODO is this even suported?
-					}
-				}
-
-				else if (manual_control_desired && manual_control_setpoint_updated) {
-					manual_control_setpoint_updated = false;
-					vmount_rc_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
-				}
-
-				else if (!manual_control_desired && vehicle_global_position_updated)
-				{
-					vehicle_global_position_updated = false;
-					vmount_rc_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
 				}
 			}
 
-			vmount_rc_deinit();
+			switch (params.mnt_mode_in) {
+			case 1: //RC
+				if (manual_input) {
+					input_obj = manual_input;
+					manual_input = nullptr;
 
-		} else if (vmount_state == ONBOARD) {
+				} else {
-			if (!vmount_onboard_init()) {
+					input_obj = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
-				err(1, "could not initiate vmount_onboard");
+				}
+
+				break;
+
+			case 2: //MAVLINK_ROI
+				input_obj = new InputMavlinkROI(manual_input);
+				break;
+
+			case 3: //MAVLINK_DO_MOUNT
+				input_obj = new InputMavlinkCmdMount(manual_input);
+				break;
+
+			default:
+				PX4_ERR("invalid input mode %i", params.mnt_mode_in);
+				break;
 			}
 
-			warnx("running mount driver in onboard mode");
+			switch (params.mnt_mode_out) {
+			case 0: //AUX
+				output_obj = new OutputRC(output_config);
+				break;
 
-			if (params.mnt_man_control) manual_control_desired = true;
+			case 1: //MAVLINK
+				output_obj = new OutputMavlink(output_config);
+				break;
 
-			while (!thread_should_exit && !thread_should_restart) {
+			default:
-				vmount_update_topics();
+				PX4_ERR("invalid output mode %i", params.mnt_mode_out);
+				break;
+			}
 
-				if(vehicle_attitude_updated)
+			if (!input_obj || !output_obj) {
-				{
+				PX4_ERR("memory allocation failed");
-					vmount_onboard_update_attitude(vehicle_attitude);
+				break;
-					vehicle_attitude_updated = false;
+			}
+
+			int ret = output_obj->initialize();
+
+			if (ret) {
+				PX4_ERR("failed to initialize output mode (%i)", ret);
+				break;
+			}
+		}
+
+		if (params.mnt_mode_in != 0) {
+
+			//get input: we cannot make the timeout too large, because the output needs to update
+			//periodically for stabilization and angle updates.
+			int ret = input_obj->update(50, &control_data);
+
+			if (ret) {
+				PX4_ERR("failed to read input (%i)", ret);
+				break;
+			}
+
+			//update output
+			ret = output_obj->update(control_data);
+
+			if (ret) {
+				PX4_ERR("failed to write output (%i)", ret);
+				break;
+			}
+
+		} else {
+			//wait for parameter changes. We still need to wake up regularily to check for thread exit requests
+			usleep(1e6);
+		}
+
+		//check for parameter changes
+		bool updated;
+
+		if (orb_check(parameter_update_sub, &updated) == 0 && updated) {
+			parameter_update_s param_update;
+			orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update);
+			update_params(param_handles, params, updated);
+
+			if (updated) {
+				//re-init objects
+				if (input_obj) {
+					delete(input_obj);
+					input_obj = nullptr;
 				}
 
-				if (params.mnt_mode_ovr && manual_control_setpoint_updated) {
+				if (output_obj) {
-					manual_control_setpoint_updated = false;
+					delete(output_obj);
-
+					output_obj = nullptr;
-					float ovr_value = get_aux_value(params.mnt_mode_ovr);
-
-					if(ovr_value < 0.0f)
-					{
-						vmount_onboard_set_mode(vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT);
-					}
-					else if (ovr_value > 0.0f)
-					{
-						vmount_onboard_set_mode(vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING);
-					}
-
 				}
 
-				else if (vehicle_command_updated) {
+				if (manual_input) {
-					vehicle_command_updated = false;
+					delete(manual_input);
-
+					manual_input = nullptr;
-					if(vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL)
-					{
-						switch ((int)vehicle_command.param7) {
-						case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
-						case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
-							manual_control_desired = false;
-							break;
-
-						case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
-							manual_control_desired = false;
-							vmount_onboard_set_mode(vehicle_command.param7);
-							vmount_onboard_set_manual(vehicle_command.param1,
-										 vehicle_command.param2, vehicle_command.param3);
-
-						case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
-							if (params.mnt_man_control) {
-								manual_control_desired = true;
-								vmount_onboard_set_mode(vehicle_command.param7);
-							}
-
-						case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
-							manual_control_desired = false;
-							vmount_onboard_set_mode(vehicle_command.param7);
-							vmount_onboard_set_location(
-								vehicle_command.param1,
-								vehicle_command.param2,
-								vehicle_command.param3);
-
-						default:
-							manual_control_desired = false;
-							break;
-						}
-
-						ack_mount_command(vehicle_command.command);
-					}
-
-					else if(vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE)
-					{
-						vmount_onboard_configure(vehicle_command.param1,
-									((uint8_t) vehicle_command.param2 == 1), ((uint8_t) vehicle_command.param3 == 1), ((uint8_t) vehicle_command.param4 == 1));
-
-						ack_mount_command(vehicle_command.command);
-					}
-				}
-
-				else if (manual_control_desired && manual_control_setpoint_updated)
-				{
-					manual_control_setpoint_updated = false;
-					vmount_onboard_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
-				}
-				else if (!manual_control_desired && vehicle_global_position_updated)
-				{
-					vehicle_global_position_updated = false;
-					vmount_onboard_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
 				}
 			}
-		} else if (vmount_state == IDLE)
+		}
-		{
-			warnx("running mount driver in idle mode");
 
-			while (!thread_should_exit && !thread_should_restart) {
+		if (thread_do_report_status) {
-				vmount_update_topics();
+			if (input_obj) {
+				input_obj->print_status();
+
+			} else {
+				PX4_INFO("Input: None");
 			}
+
+			if (output_obj) {
+				output_obj->print_status();
+
+			} else {
+				PX4_INFO("Output: None");
+			}
+
+			thread_do_report_status = false;
 		}
 	}
 
-	if (thread_should_restart)
+	orb_unsubscribe(parameter_update_sub);
-	{
+
-		thread_should_restart = false;
+	if (input_obj) {
-		warnx("parameter update, restarting mount driver!");
+		delete(input_obj);
-		goto run;
+	}
+
+	if (output_obj) {
+		delete(output_obj);
+	}
+
+	if (manual_input) {
+		delete(manual_input);
 	}
 
 	thread_running = false;
@@ -466,227 +304,116 @@ static int vmount_thread_main(int argc, char *argv[])
  */
 int vmount_main(int argc, char *argv[])
 {
-	if (argc < 1) {
+	if (argc < 2) {
-		warnx("missing command");
+		PX4_ERR("missing command");
 		usage();
+		return -1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
 		/* this is not an error */
 		if (thread_running) {
-			errx(0, "mount driver already running");
+			PX4_WARN("mount driver already running");
+			return 0;
 		}
 
 		thread_should_exit = false;
-		vmount_task = px4_task_spawn_cmd("vmount",
+		int vmount_task = px4_task_spawn_cmd("vmount",
-						SCHED_DEFAULT,
+						     SCHED_DEFAULT,
-						SCHED_PRIORITY_DEFAULT + 40, //TODO we might want a higher priority?
+						     SCHED_PRIORITY_DEFAULT + 40,
-						1100,
+						     1200,
-						vmount_thread_main,
+						     vmount_thread_main,
-						(char *const *)argv);
+						     (char *const *)argv);
 
-		while (!thread_running) {
+		while (!thread_running && vmount_task >= 0) {
 			usleep(200);
 		}
 
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 
 		/* this is not an error */
 		if (!thread_running) {
-			errx(0, "mount driver already stopped");
+			PX4_WARN("mount driver not running");
+			return 0;
 		}
 
 		thread_should_exit = true;
 
 		while (thread_running) {
-			usleep(1000000);
+			usleep(100000);
-			warnx(".");
 		}
 
-		warnx("terminated.");
+		return 0;
-		exit(0);
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			switch (vmount_state) {
+			thread_do_report_status = true;
-			case IDLE:
-				errx(0, "running: IDLE");
-				break;
-
-			case MAVLINK:
-				errx(0, "running: MAVLINK - Manual Control? %d", manual_control_desired);
-				break;
-
-			case RC:
-				errx(0, "running: RC - Manual Control? %d", manual_control_desired);
-				break;
-
-			case ONBOARD:
-				errx(0, "running: ONBOARD");
-				break;
 
+			while (thread_do_report_status) {
+				usleep(10000);
 			}
 
 		} else {
-			errx(1, "not running");
+			PX4_INFO("not running");
 		}
+
+		return 0;
 	}
 
-	warnx("unrecognized command");
+	PX4_ERR("unrecognized command");
 	usage();
-	/* not getting here */
+	return -1;
-	return 0;
 }
 
-/* Update oURB topics */
+void update_params(ParameterHandles &param_handles, Parameters &params, bool &got_changes)
-void vmount_update_topics()
 {
-	/*
+	Parameters prev_params = params;
-	polls = {
+	param_get(param_handles.mnt_mode_in, &params.mnt_mode_in);
-		{ .fd = vehicle_roi_sub,   .events = POLLIN },
+	param_get(param_handles.mnt_mode_out, &params.mnt_mode_out);
-		{ .fd = vehicle_global_position_sub,   .events = POLLIN },
+	param_get(param_handles.mnt_mav_sysid, &params.mnt_mav_sysid);
-		{ .fd = vehicle_attitude_sub,   .events = POLLIN },
+	param_get(param_handles.mnt_mav_compid, &params.mnt_mav_compid);
-		{ .fd = vehicle_command_sub,   .events = POLLIN },
+	param_get(param_handles.mnt_ob_lock_mode, &params.mnt_ob_lock_mode);
-		{ .fd = position_setpoint_triplet_sub,   .events = POLLIN },
+	param_get(param_handles.mnt_ob_norm_mode, &params.mnt_ob_norm_mode);
-		{ .fd = manual_control_setpoint_sub,   .events = POLLIN },
+	param_get(param_handles.mnt_man_control, &params.mnt_man_control);
-		{ .fd = parameter_update_sub,   .events = POLLIN },
+	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);
 
-	/* wait for sensor update of 7 file descriptors for 100 ms */
+	got_changes = prev_params != params;
-	int poll_ret = px4_poll(polls, 7, 100);
-
-	//Nothing updated.
-	if(poll_ret == 0) return;
-
-	if (polls[0].revents & POLLIN) {
-		orb_copy(ORB_ID(vehicle_roi), vehicle_roi_sub, &vehicle_roi);
-		vehicle_roi_updated = true;
-	}
-
-	if (polls[1].revents & POLLIN) {
-		orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &vehicle_global_position);
-		vehicle_global_position_updated = true;
-	}
-
-	if (polls[2].revents & POLLIN) {
-		orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &vehicle_attitude);
-		vehicle_attitude_updated = true;
-	}
-
-	if (polls[3].revents & POLLIN) {
-		orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &vehicle_command);
-		vehicle_command_updated = true;
-	}
-
-	if (polls[4].revents & POLLIN) {
-		orb_copy(ORB_ID(position_setpoint_triplet), position_setpoint_triplet_sub, &position_setpoint_triplet);
-		position_setpoint_triplet_updated = true;
-	}
-
-	if (polls[5].revents & POLLIN) {
-		orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual_control_setpoint);
-		manual_control_setpoint_updated = true;
-	}
-
-	if (polls[6].revents & POLLIN) {
-		orb_copy(ORB_ID(parameter_update), parameter_update_sub, &parameter_update);
-		parameter_update_updated = true;
-		update_params();
-	}
 }
 
-void update_params()
+bool get_params(ParameterHandles &param_handles, Parameters &params)
 {
-	param_get(params_handels.mnt_mode, &params.mnt_mode);
+	param_handles.mnt_mode_in = param_find("MNT_MODE_IN");
-	param_get(params_handels.mnt_mav_sysid, &params.mnt_mav_sysid);
+	param_handles.mnt_mode_out = param_find("MNT_MODE_OUT");
-	param_get(params_handels.mnt_mav_compid, &params.mnt_mav_compid);
+	param_handles.mnt_mav_sysid = param_find("MNT_MAV_SYSID");
-	param_get(params_handels.mnt_ob_lock_mode, &params.mnt_ob_lock_mode);
+	param_handles.mnt_mav_compid = param_find("MNT_MAV_COMPID");
-	param_get(params_handels.mnt_ob_norm_mode, &params.mnt_ob_norm_mode);
+	param_handles.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE");
-	param_get(params_handels.mnt_man_control, &params.mnt_man_control);
+	param_handles.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE");
-	param_get(params_handels.mnt_man_pitch, &params.mnt_man_pitch);
+	param_handles.mnt_man_control = param_find("MNT_MAN_CONTROL");
-	param_get(params_handels.mnt_man_roll, &params.mnt_man_roll);
+	param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH");
-	param_get(params_handels.mnt_man_yaw, &params.mnt_man_yaw);
+	param_handles.mnt_man_roll = param_find("MNT_MAN_ROLL");
-	param_get(params_handels.mnt_mode_ovr, &params.mnt_mode_ovr);
+	param_handles.mnt_man_yaw = param_find("MNT_MAN_YAW");
 
-	if (vmount_state != params.mnt_mode)
+	if (param_handles.mnt_mode_in == PARAM_INVALID ||
-	{
+	    param_handles.mnt_mode_out == PARAM_INVALID ||
-		thread_should_restart = true;
+	    param_handles.mnt_mav_sysid == PARAM_INVALID ||
-	}
+	    param_handles.mnt_mav_compid == PARAM_INVALID ||
-	else if (vmount_state == MAVLINK)
+	    param_handles.mnt_ob_lock_mode == PARAM_INVALID ||
-	{
+	    param_handles.mnt_ob_norm_mode == PARAM_INVALID ||
-		vmount_mavlink_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_mav_sysid, params.mnt_mav_compid);
+	    param_handles.mnt_man_control == PARAM_INVALID ||
-	}
+	    param_handles.mnt_man_pitch == PARAM_INVALID ||
-	else if (vmount_state == RC)
+	    param_handles.mnt_man_roll == PARAM_INVALID ||
-	{
+	    param_handles.mnt_man_yaw == PARAM_INVALID) {
-		vmount_rc_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_ob_norm_mode, params.mnt_ob_lock_mode);
-	}
-	else if(vmount_state == ONBOARD)
-	{
-		//None of the parameter changes require a reconfiguration of the onboard mount.
-	}
-}
-
-bool get_params()
-{
-	params_handels.mnt_mode = param_find("MNT_MODE");
-	params_handels.mnt_mav_sysid = param_find("MNT_MAV_SYSID");
-	params_handels.mnt_mav_compid = param_find("MNT_MAV_COMPID");
-	params_handels.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE");
-	params_handels.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE");
-	params_handels.mnt_man_control = param_find("MNT_MAN_CONTROL");
-	params_handels.mnt_man_pitch = param_find("MNT_MAN_PITCH");
-	params_handels.mnt_man_roll = param_find("MNT_MAN_ROLL");
-	params_handels.mnt_man_yaw = param_find("MNT_MAN_YAW");
-	params_handels.mnt_mode_ovr = param_find("MNT_MODE_OVR");
-
-	if (param_get(params_handels.mnt_mode, &params.mnt_mode) ||
-	    param_get(params_handels.mnt_mav_sysid, &params.mnt_mav_sysid) ||
-	    param_get(params_handels.mnt_mav_compid, &params.mnt_mav_compid) ||
-		param_get(params_handels.mnt_ob_lock_mode, &params.mnt_ob_lock_mode) ||
-		param_get(params_handels.mnt_ob_norm_mode, &params.mnt_ob_norm_mode) ||
-	    param_get(params_handels.mnt_man_control, &params.mnt_man_control) ||
-	    param_get(params_handels.mnt_man_pitch, &params.mnt_man_pitch) ||
-		param_get(params_handels.mnt_man_roll, &params.mnt_man_roll) ||
-	    param_get(params_handels.mnt_man_yaw, &params.mnt_man_yaw) ||
-		param_get(params_handels.mnt_mode_ovr, &params.mnt_mode_ovr)) {
 		return false;
 	}
 
+	bool dummy;
+	update_params(param_handles, params, dummy);
 	return true;
-
-}
-
-void ack_mount_command(uint16_t command)
-{
-	vehicle_command_ack.command = command;
-	vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
-
-	orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack);
-}
-
-float get_aux_value(int aux)
-{
-	switch (aux)
-	{
-		case 0:
-			return 0.0f;
-		case 1:
-			return manual_control_setpoint.aux1;
-		case 2:
-			return manual_control_setpoint.aux2;
-		case 3:
-			return manual_control_setpoint.aux3;
-		case 4:
-			return manual_control_setpoint.aux4;
-		case 5:
-			return manual_control_setpoint.aux5;
-		default:
-			return 0.0f;
-	}
 }

src/drivers/vmount/vmount_mavlink.cpp

@@ -1,178 +0,0 @@
-/****************************************************************************
-*
-*   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* 1. Redistributions of source code must retain the above copyright
-*    notice, this list of conditions and the following disclaimer.
-* 2. Redistributions in binary form must reproduce the above copyright
-*    notice, this list of conditions and the following disclaimer in
-*    the documentation and/or other materials provided with the
-*    distribution.
-* 3. Neither the name PX4 nor the names of its contributors may be
-*    used to endorse or promote products derived from this software
-*    without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-****************************************************************************/
-
-/**
- * @file vmount_mavlink.cpp
- * @author Leon Müller (thedevleon)
- *
- */
-
-#include "vmount_mavlink.h"
-
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-#include <arch/math.h>
-#include <geo/geo.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_roi.h>
-
-#include <px4_posix.h>
-
-/* uORB advertising */
-static struct vehicle_command_s *vehicle_command;
-static orb_advert_t vehicle_command_pub;
-
-static int sys_id;
-static int comp_id;
-static double lat;
-static double lon;
-static float alt;
-
-bool vmount_mavlink_init()
-{
-	memset(&vehicle_command, 0, sizeof(vehicle_command));
-	vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_command);
-
-	if (!vehicle_command_pub) { return false; }
-
-	return true;
-}
-
-
-void vmount_mavlink_deinit()
-{
-	orb_unadvertise(vehicle_command_pub);
-	free(vehicle_command);
-}
-
-/*
- * MAV_CMD_DO_MOUNT_CONFIGURE (#204)
- *   param1 = mount_mode (1 = MAV_MOUNT_MODE_NEUTRAL recenters camera)
- */
-
-void vmount_mavlink_configure(int roi_mode, bool man_control, int sysid, int compid)
-{
-	sys_id = sysid;
-	comp_id = compid;
-
-	vehicle_command->command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
-	vehicle_command->target_system = sys_id;
-	vehicle_command->target_component = comp_id;
-
-	switch (roi_mode) {
-	case vehicle_roi_s::VEHICLE_ROI_NONE:
-		if (man_control) { vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING; }
-
-		else { vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL; }
-
-		break;
-
-	case vehicle_roi_s::VEHICLE_ROI_WPNEXT:
-		vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
-		break;
-
-	case vehicle_roi_s::VEHICLE_ROI_WPINDEX:
-		vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
-		break;
-
-	case vehicle_roi_s::VEHICLE_ROI_LOCATION:
-		vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
-		break;
-
-	case vehicle_roi_s::VEHICLE_ROI_TARGET:
-		vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
-		break;
-
-	default:
-		vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
-		break;
-	}
-
-	orb_publish(ORB_ID(vehicle_command), vehicle_command_pub, vehicle_command);
-}
-
-
-/* MAV_CMD_DO_MOUNT_CONTROL (#205)
- *  param1 = pitch, angle in degree or pwm input
- *  param2 = roll, angle in degree or pwm input
- *  param3 = yaw, angle in degree or pwm input
- *  param6 = typeflags (uint16_t)
- *   0x0001: pitch, 0: unlimited, 1: limited, see CMD_SETANGLE
- *   0x0002: roll, 0: unlimited, 1: limited, see CMD_SETANGLE
- *   0x0004: yaw, 0: unlimited, 1: limited, see CMD_SETANGLE
- *   0x0100: input type, 0: angle input (see CMD_SETANGLE), 1: pwm input (see CMD_SETPITCHROLLYAW)
- */
-
-
-void vmount_mavlink_set_location(double lat_new, double lon_new, float alt_new)
-{
-	lat = lat_new;
-	lon = lon_new;
-	alt = alt_new;
-}
-
-void vmount_mavlink_point(double global_lat, double global_lon, float global_alt)
-{
-	float pitch = vmount_mavlink_calculate_pitch(global_lat, global_lon, global_alt);
-	float roll = 0.0f; // We want a level horizon, so leave roll at 0 degrees.
-	float yaw = get_bearing_to_next_waypoint(global_lat, global_lon, lat, lon) * (float)M_RAD_TO_DEG;
-
-	vmount_mavlink_point_manual(pitch, roll, yaw);
-}
-
-
-void vmount_mavlink_point_manual(float pitch_new, float roll_new, float yaw_new)
-{
-	vehicle_command->command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
-	vehicle_command->target_system = sys_id;
-	vehicle_command->target_component = comp_id;
-
-	vehicle_command->param1 = pitch_new;
-	vehicle_command->param2 = roll_new;
-	vehicle_command->param3 = yaw_new;
-
-	orb_publish(ORB_ID(vehicle_command), vehicle_command_pub, vehicle_command);
-}
-
-float vmount_mavlink_calculate_pitch(double global_lat, double global_lon, float global_alt)
-{
-	float x = (lon - global_lon) * cos(M_DEG_TO_RAD * ((global_lat + lat) * 0.00000005)) * 0.01113195;
-	float y = (lat - global_lat) * 0.01113195;
-	float z = (alt - global_alt);
-	float target_distance = sqrtf(powf(x, 2) + powf(y, 2));
-
-	return atan2(z, target_distance) * M_RAD_TO_DEG;
-}

src/drivers/vmount/vmount_onboard.cpp

@@ -1,193 +0,0 @@
-/****************************************************************************
-*
-*   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* 1. Redistributions of source code must retain the above copyright
-*    notice, this list of conditions and the following disclaimer.
-* 2. Redistributions in binary form must reproduce the above copyright
-*    notice, this list of conditions and the following disclaimer in
-*    the documentation and/or other materials provided with the
-*    distribution.
-* 3. Neither the name PX4 nor the names of its contributors may be
-*    used to endorse or promote products derived from this software
-*    without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-****************************************************************************/
-
-/**
- * @file vmount_onboard.cpp
- * @author Leon Müller (thedevleon)
- *
- */
-
-#include "vmount_onboard.h"
-
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-#include <arch/math.h>
-#include <geo/geo.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_command.h>
-
-
-/* uORB topics */
-static struct actuator_controls_s actuator_controls;
-static orb_advert_t actuator_controls_pub;
-
-static struct vehicle_attitude_s vehicle_attitude;
-
-static int mount_mode;
-static float pitch;
-static float roll;
-static float yaw;
-static float retracts;
-static int stab_pitch;
-static int stab_roll;
-static int stab_yaw;
-static double lat;
-static double lon;
-static float alt;
-
-bool vmount_onboard_init()
-{
-	memset(&actuator_controls, 0, sizeof(actuator_controls));
-	memset(&vehicle_attitude, 0, sizeof(vehicle_attitude));
-	actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls);
-
-	if (!actuator_controls_pub) { return false; }
-
-	vmount_onboard_configure(vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT, 0.0f, 0.0f, 0.0f);
-
-	return true;
-}
-
-void vmount_onboard_deinit()
-{
-	orb_unadvertise(actuator_controls_pub);
-	//free(actuator_controls);
-	//free(vehicle_attitude);
-}
-
-void vmount_onboard_configure(int new_mount_mode, bool new_stab_roll, bool new_stab_pitch, bool new_stab_yaw)
-{
-	mount_mode = new_mount_mode;
-	stab_roll = new_stab_roll;
-	stab_pitch = new_stab_pitch;
-	stab_yaw = new_stab_yaw;
-
-	switch (mount_mode) {
-	case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
-		retracts = -1.0f;
-		vmount_onboard_set_manual(0.0f, 0.0f, 0.0f);
-		break;
-
-	case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
-		retracts = 0.0f;
-		vmount_onboard_set_manual(0.0f, 0.0f, 0.0f);
-		break;
-
-	case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
-	case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
-	case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
-		retracts = 0.0f;
-
-	default:
-		break;
-	}
-}
-
-void vmount_onboard_set_location(double lat_new, double lon_new, float alt_new)
-{
-	lat = lat_new;
-	lon = lon_new;
-	alt = alt_new;
-}
-
-void vmount_onboard_set_manual(double pitch_new, double roll_new, float yaw_new)
-{
-	pitch = pitch_new;
-	roll = roll_new;
-	yaw = yaw_new;
-}
-
-void vmount_onboard_set_mode(int new_mount_mode)
-{
-	vmount_onboard_configure(new_mount_mode, stab_roll, stab_pitch, stab_yaw);
-}
-
-void vmount_onboard_point(double global_lat, double global_lon, float global_alt)
-{
-	if (mount_mode == vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT) {
-		pitch = vmount_onboard_calculate_pitch(global_lat, global_lon, global_alt);
-		roll = 0.0f; // We want a level horizon, so leave roll at 0 degrees.
-		yaw = get_bearing_to_next_waypoint(global_lat, global_lon, lat, lon) * (float)M_RAD_TO_DEG;
-	}
-
-	vmount_onboard_point_manual(pitch, roll, yaw);
-}
-
-void vmount_onboard_point_manual(float pitch_target, float roll_target, float yaw_target)
-{
-	float pitch_new = pitch_target;
-	float roll_new = roll_target;
-	float yaw_new = yaw_target;
-
-	if (mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT &&
-	    mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) {
-		if (stab_roll) {
-			roll_new += 1.0f / M_PI_F * -vehicle_attitude.roll;
-		}
-
-		if (stab_pitch) {
-			pitch_new += 1.0f / M_PI_F * -vehicle_attitude.pitch;
-		}
-
-		if (stab_yaw) {
-			yaw_new += 1.0f / M_PI_F * vehicle_attitude.yaw;
-		}
-	}
-
-	actuator_controls.timestamp = hrt_absolute_time();
-	actuator_controls.control[0] = pitch_new;
-	actuator_controls.control[1] = roll_new;
-	actuator_controls.control[2] = yaw_new;
-	actuator_controls.control[4] = retracts;
-
-	orb_publish(ORB_ID(actuator_controls_2), actuator_controls_pub, &actuator_controls);
-}
-
-void vmount_onboard_update_attitude(vehicle_attitude_s vehicle_attitude_new)
-{
-	vehicle_attitude = vehicle_attitude_new;
-}
-
-float vmount_onboard_calculate_pitch(double global_lat, double global_lon, float global_alt)
-{
-	float x = (lon - global_lon) * cos(M_DEG_TO_RAD * ((global_lat + lat) * 0.00000005)) * 0.01113195;
-	float y = (lat - global_lat) * 0.01113195;
-	float z = (alt - global_alt);
-	float target_distance = sqrtf(powf(x, 2) + powf(y, 2));
-
-	return atan2(z, target_distance) * M_RAD_TO_DEG;
-}

src/drivers/vmount/vmount_params.c

@@ -41,46 +41,42 @@
 #include <systemlib/param/param.h>
 
 /**
-* Mount operation mode
+* Mount input mode
-* MAVLINK and RC use the ROI (or RC input if enabled and no ROI set) to control a mount.
+* RC uses the AUX input channels (see MNT_MAN_* parameters),
-* ONBOARD uses MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL MavLink messages
+* MAVLINK_ROI uses the MAV_CMD_DO_SET_ROI Mavlink message, and MAVLINK_DO_MOUNT the
-* to control a mount.
+* MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL messages to control a mount.
 * @value 0 DISABLE
-* @value 1 MAVLINK
+* @value 1 RC
-* @value 2 RC
+* @value 2 MAVLINK_ROI
-* @value 3 ONBOARD
+* @value 3 MAVLINK_DO_MOUNT
 * @min 0
 * @max 3
 * @group Mount
 */
-PARAM_DEFINE_INT32(MNT_MODE, 0);
+PARAM_DEFINE_INT32(MNT_MODE_IN, 0);
 
 /**
-* Auxiliary channel to override current mount mode (only in ONBOARD mode)
+* Mount output mode
-* if <0.0f then MODE_RETRACT (retracts not retracted)
+* AUX uses the mixer output Control Group #2.
-* if =0.0f then don't override
+* MAVLINK uses the MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL MavLink messages
-* if >0.0f then MODE_RC_TARGETING (retracts retracted)
+* to control a mount (set MNT_MAV_SYSID & MNT_MAV_COMPID)
-* @value 0 Disable
+* @value 0 AUX
-* @value 1 AUX1
+* @value 1 MAVLINK
-* @value 2 AUX2
-* @value 3 AUX3
-* @value 4 AUX4
-* @value 5 AUX5
 * @min 0
-* @max 5
+* @max 1
 * @group Mount
 */
-PARAM_DEFINE_INT32(MNT_MODE_OVR, 0);
+PARAM_DEFINE_INT32(MNT_MODE_OUT, 0);
 
 /**
-* Mavlink System ID
+* Mavlink System ID (if MNT_MODE_OUT is MAVLINK)
 *
 * @group Mount
 */
 PARAM_DEFINE_INT32(MNT_MAV_SYSID, 71);
 
 /**
-* Mavlink Component ID
+* Mavlink Component ID (if MNT_MODE_OUT is MAVLINK)
 *
 * @group Mount
 */
@@ -88,7 +84,7 @@ PARAM_DEFINE_INT32(MNT_MAV_COMPID, 67);
 
 /**
 * Mixer value for selecting normal mode
-* if required by the gimbal (only in RC mode)
+* if required by the gimbal (only in AUX output mode)
 * @min -1.0
 * @max 1.0
 * @decimal 3
@@ -98,7 +94,7 @@ PARAM_DEFINE_FLOAT(MNT_OB_NORM_MODE, -1.0f);
 
 /**
 * Mixer value for selecting a locking mode
-* if required for the gimbal (only in RC mode)
+* if required for the gimbal (only in AUX output mode)
 * @min -1.0
 * @max 1.0
 * @decimal 3
@@ -108,7 +104,7 @@ PARAM_DEFINE_FLOAT(MNT_OB_LOCK_MODE, 0.0f);
 
 
 /**
-* This enables the mount to be controlled when no ROI is set.
+* 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.
@@ -119,7 +115,7 @@ PARAM_DEFINE_FLOAT(MNT_OB_LOCK_MODE, 0.0f);
 PARAM_DEFINE_INT32(MNT_MAN_CONTROL, 0);
 
 /**
-* Auxiliary channel to control roll.
+* Auxiliary channel to control roll (in AUX input or manual mode).
 *
 * @value 0 Disable
 * @value 1 AUX1
@@ -134,7 +130,7 @@ PARAM_DEFINE_INT32(MNT_MAN_CONTROL, 0);
 PARAM_DEFINE_INT32(MNT_MAN_ROLL, 0);
 
 /**
-* Auxiliary channel to control pitch.
+* Auxiliary channel to control pitch (in AUX input or manual mode).
 *
 * @value 0 Disable
 * @value 1 AUX1
@@ -149,7 +145,7 @@ PARAM_DEFINE_INT32(MNT_MAN_ROLL, 0);
 PARAM_DEFINE_INT32(MNT_MAN_PITCH, 0);
 
 /**
-* Auxiliary channel to control yaw.
+* Auxiliary channel to control yaw (in AUX input or manual mode).
 *
 * @value 0 Disable
 * @value 1 AUX1
@@ -162,3 +158,4 @@ PARAM_DEFINE_INT32(MNT_MAN_PITCH, 0);
 * @group Mount
 */
 PARAM_DEFINE_INT32(MNT_MAN_YAW, 0);
+

src/drivers/vmount/vmount_rc.cpp

@@ -1,155 +0,0 @@
-/****************************************************************************
-*
-*   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* 1. Redistributions of source code must retain the above copyright
-*    notice, this list of conditions and the following disclaimer.
-* 2. Redistributions in binary form must reproduce the above copyright
-*    notice, this list of conditions and the following disclaimer in
-*    the documentation and/or other materials provided with the
-*    distribution.
-* 3. Neither the name PX4 nor the names of its contributors may be
-*    used to endorse or promote products derived from this software
-*    without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-****************************************************************************/
-
-/**
- * @file vmount_rc.c
- * @author Leon Müller (thedevleon)
- *
- */
-
-#include "vmount_rc.h"
-
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-#include <arch/math.h>
-#include <geo/geo.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_roi.h>
-#include <uORB/topics/actuator_controls.h>
-
-
-/* uORB advertising */
-static struct actuator_controls_s actuator_controls;
-static orb_advert_t actuator_controls_pub;
-
-static double lon;
-static double lat;
-static float alt;
-static float normal_mode;
-static float locked_mode;
-static bool  locked;
-
-bool vmount_rc_init()
-{
-	memset(&actuator_controls, 0, sizeof(actuator_controls));
-	actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls);
-
-	if (!actuator_controls_pub) { return false; }
-
-	locked = false;
-	normal_mode = -1.0f;
-	locked_mode = 0.0f;
-
-	return true;
-}
-
-void vmount_rc_deinit()
-{
-	orb_unadvertise(actuator_controls_pub);
-	//free(&actuator_controls);
-}
-
-void vmount_rc_configure(int roi_mode, bool man_control, int normal_mode_new, int locked_mode_new)
-{
-	normal_mode = normal_mode_new;
-	locked_mode = locked_mode_new;
-
-	switch (roi_mode) {
-	case vehicle_roi_s::VEHICLE_ROI_NONE:
-		locked = false;
-
-		if (!man_control) {vmount_rc_point_manual(0.0f, 0.0f, 0.0f);}
-
-		break;
-
-	case vehicle_roi_s::VEHICLE_ROI_WPNEXT:
-	case vehicle_roi_s::VEHICLE_ROI_WPINDEX:
-	case vehicle_roi_s::VEHICLE_ROI_LOCATION:
-	case vehicle_roi_s::VEHICLE_ROI_TARGET:
-		locked = true;
-		break;
-
-	default:
-		locked = false;
-		vmount_rc_point_manual(0.0f, 0.0f, 0.0f);
-		break;
-	}
-}
-
-void vmount_rc_set_location(double lat_new, double lon_new, float alt_new)
-{
-	lat = lat_new;
-	lon = lon_new;
-	alt = alt_new;
-}
-
-void vmount_rc_point(double global_lat, double global_lon, float global_alt)
-{
-	float pitch = vmount_rc_calculate_pitch(global_lat, global_lon, global_alt);
-	float roll = 0.0f; // We want a level horizon, so leave roll at 0 degrees.
-	float yaw = get_bearing_to_next_waypoint(global_lat, global_lon, lat, lon) * (float)M_RAD_TO_DEG;
-
-	vmount_rc_point_manual(pitch, roll, yaw);
-}
-
-void vmount_rc_point_manual(float pitch_new, float roll_new, float yaw_new)
-{
-	actuator_controls.timestamp = hrt_absolute_time();
-	actuator_controls.control[0] = pitch_new;
-	actuator_controls.control[1] = roll_new;
-	actuator_controls.control[2] = yaw_new;
-	actuator_controls.control[3] = (locked) ? locked_mode : normal_mode;
-
-	/** for debugging purposes
-	warnx("actuator_controls_2 values:\t%8.4f\t%8.4f\t%8.4f\t%8.4f\t%8.4f",
-		 (double)actuator_controls.control[0],
-		 (double)actuator_controls.control[1],
-		 (double)actuator_controls.control[2],
-	     (double)actuator_controls.control[3],
-	     (double)actuator_controls.control[4]);
-	**/
-
-	orb_publish(ORB_ID(actuator_controls_2), actuator_controls_pub, &actuator_controls);
-}
-
-float vmount_rc_calculate_pitch(double global_lat, double global_lon, float global_alt)
-{
-	float x = (lon - global_lon) * cos(M_DEG_TO_RAD * ((global_lat + lat) * 0.00000005)) * 0.01113195;
-	float y = (lat - global_lat) * 0.01113195;
-	float z = (alt - global_alt);
-	float target_distance = sqrtf(powf(x, 2) + powf(y, 2));
-
-	return atan2(z, target_distance) * M_RAD_TO_DEG;
-}