vmount: add mount and ROI implementation

MavLink spec implementation

implemented vehicle_roi topic

rename old gimbal to rc_gimbal

little changes

corrected RC Gimbal group

Starting ROI implementation in commander

implementation done, needs to be tested

uhm..

add todo

Change to float32 for x,y and z

remove mission topic again, not needed

change roi coordinates to lat, lon and alt

adjust to float64

starting mount implementation

correcting small mistakes, compiles now

add todos

further progress

implementing parameters

adjust default parameters

started implementation of mavlink

fix typo

change to lat, lon and alt

fix typo :D

change to double (to represent float64)

add global_position_

add mount topic

commander mount implementation done

cleanup

almost finished

little fix

codestyle fixes

leave pitch at 0 degrees

added pitch calculation

codestyle changes

Undo vehicle_mount, react to updated parameters, parsing of CMD_DO_MOUNT_* in mount.cpp

start implementing mode override

forgot a semikolon.

add debug

Finish implementation of mount override and manual control.

fix codestyle

correct cleanup

rename to vmount

works now

fix rebase error

fix polling

refactoring and custom airframe for gimbal

couple changes

remove warnx

almost done

finally

What is going on?

change back to actuator_controls_2

working

bump parameter version number and some clarification

fix submodules

src/drivers/vmount/CMakeLists.txt

@@ -0,0 +1,48 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__vmount
+	MAIN vmount
+	STACK_MAIN 1024
+	COMPILE_FLAGS
+		-Os
+	SRCS
+        vmount.cpp
+        vmount_params.c
+        vmount_rc.cpp
+        vmount_mavlink.cpp
+        vmount_onboard.cpp
+	DEPENDS
+		platforms__common
+	)
+ # vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/vmount/vmount.cpp

@@ -0,0 +1,692 @@
+/****************************************************************************
+ *
+ *   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.cpp
+ * @author Leon Müller (thedevleon)
+ * MAV_MOUNT driver for controlling mavlink gimbals, rc gimbals/servors and
+ * future kinds of mounts.
+ *
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <string.h>
+#include <sys/types.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include "vmount_mavlink.h"
+#include "vmount_rc.h"
+#include "vmount_onboard.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>
+
+/* thread state */
+static volatile bool thread_should_exit = false;
+static volatile bool thread_running = false;
+static volatile bool thread_should_restart = 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 */
+px4_pollfd_struct_t polls[7];
+
+/* 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;
+	int mnt_ob_norm_mode;
+	int mnt_man_control;
+	int mnt_man_pitch;
+	int mnt_man_roll;
+	int mnt_man_yaw;
+	int mnt_mode_ovr;
+} params;
+
+static struct {
+	param_t mnt_mode;
+	param_t mnt_mav_sysid;
+	param_t mnt_mav_compid;
+	param_t mnt_ob_lock_mode;
+	param_t mnt_ob_norm_mode;
+	param_t mnt_man_control;
+	param_t mnt_man_pitch;
+	param_t mnt_man_roll;
+	param_t mnt_man_yaw;
+	param_t mnt_mode_ovr;
+} params_handels;
+
+static bool manual_control_desired;
+
+/**
+ * Print command usage information
+ */
+static void usage()
+{
+	fprintf(stderr,
+		"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()) {
+		warnx("could not get mount parameters!");
+	}
+
+	if (params.mnt_mode == 0) { vmount_state = IDLE;}
+
+	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;
+
+	run: {
+		if (vmount_state == MAVLINK) {
+			if (!vmount_mavlink_init()) {
+				err(1, "could not initiate vmount_mavlink");
+			}
+
+			warnx("running mount driver in mavlink mode");
+
+			if (params.mnt_man_control) manual_control_desired = true;
+
+			while (!thread_should_exit && !thread_should_restart) {
+				vmount_update_topics();
+
+				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;
+			}
+
+			while (!thread_should_exit && !thread_should_restart) {
+				vmount_update_topics();
+
+				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();
+
+		} else if (vmount_state == ONBOARD) {
+			if (!vmount_onboard_init()) {
+				err(1, "could not initiate vmount_onboard");
+			}
+
+			warnx("running mount driver in onboard mode");
+
+			if (params.mnt_man_control) manual_control_desired = true;
+
+			while (!thread_should_exit && !thread_should_restart) {
+				vmount_update_topics();
+
+				if(vehicle_attitude_updated)
+				{
+					vmount_onboard_update_attitude(vehicle_attitude);
+					vehicle_attitude_updated = false;
+				}
+
+				if (params.mnt_mode_ovr && manual_control_setpoint_updated) {
+					manual_control_setpoint_updated = false;
+
+					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) {
+					vehicle_command_updated = false;
+
+					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) {
+				vmount_update_topics();
+			}
+		}
+	}
+
+	if (thread_should_restart)
+	{
+		thread_should_restart = false;
+		warnx("parameter update, restarting mount driver!");
+		goto run;
+	}
+
+	thread_running = false;
+	return 0;
+}
+
+/**
+ * The main command function.
+ * Processes command line arguments and starts the daemon.
+ */
+int vmount_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		warnx("missing command");
+		usage();
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		/* this is not an error */
+		if (thread_running) {
+			errx(0, "mount driver already running");
+		}
+
+		thread_should_exit = false;
+		vmount_task = px4_task_spawn_cmd("vmount",
+						SCHED_DEFAULT,
+						SCHED_PRIORITY_DEFAULT + 40, //TODO we might want a higher priority?
+						1100,
+						vmount_thread_main,
+						(char *const *)argv);
+
+		while (!thread_running) {
+			usleep(200);
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+
+		/* this is not an error */
+		if (!thread_running) {
+			errx(0, "mount driver already stopped");
+		}
+
+		thread_should_exit = true;
+
+		while (thread_running) {
+			usleep(1000000);
+			warnx(".");
+		}
+
+		warnx("terminated.");
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			switch (vmount_state) {
+			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;
+
+			}
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	usage();
+	/* not getting here */
+	return 0;
+}
+
+/* Update oURB topics */
+void vmount_update_topics()
+{
+	/*
+	polls = {
+		{ .fd = vehicle_roi_sub,   .events = POLLIN },
+		{ .fd = vehicle_global_position_sub,   .events = POLLIN },
+		{ .fd = vehicle_attitude_sub,   .events = POLLIN },
+		{ .fd = vehicle_command_sub,   .events = POLLIN },
+		{ .fd = position_setpoint_triplet_sub,   .events = POLLIN },
+		{ .fd = manual_control_setpoint_sub,   .events = POLLIN },
+		{ .fd = parameter_update_sub,   .events = POLLIN },
+	};
+	*/
+
+	/* wait for sensor update of 7 file descriptors for 100 ms */
+	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()
+{
+	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);
+
+	if (vmount_state != params.mnt_mode)
+	{
+		thread_should_restart = true;
+	}
+	else if (vmount_state == MAVLINK)
+	{
+		vmount_mavlink_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_mav_sysid, params.mnt_mav_compid);
+	}
+	else if (vmount_state == RC)
+	{
+		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;
+	}
+
+	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

@@ -0,0 +1,178 @@
+/****************************************************************************
+*
+*   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_mavlink.h

@@ -0,0 +1,56 @@
+/****************************************************************************
+*
+*   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.h
+ * @author Leon Müller (thedevleon)
+ *
+ */
+
+#ifndef _VMOUNT_MAVLINK_H
+#define _VMOUNT_MAVLINK_H
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+// Public functions
+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 */

src/drivers/vmount/vmount_onboard.cpp

@@ -0,0 +1,193 @@
+/****************************************************************************
+*
+*   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_onboard.h

@@ -0,0 +1,59 @@
+/****************************************************************************
+*
+*   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
+ * @author Leon Müller (thedevleon)
+ *
+ */
+
+#ifndef _VMOUNT_ONBOARD_H
+#define _VMOUNT_ONBOARD_H
+
+#include <sys/types.h>
+#include <stdbool.h>
+#include <uORB/topics/vehicle_attitude.h>
+
+// Public functions
+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);
+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);
+void vmount_onboard_set_mode(int new_mount_mode);
+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);
+void vmount_onboard_update_attitude(vehicle_attitude_s vehicle_attitude_new);
+float vmount_onboard_calculate_pitch(double global_lat, double global_lon, float global_alt);
+
+#endif /* _VMOUNT_ONBOARD_H */

src/drivers/vmount/vmount_params.c

@@ -0,0 +1,164 @@
+/****************************************************************************
+*
+*   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_params.c
+* @author Leon Müller (thedevleon)
+*
+*/
+
+#include <px4_config.h>
+#include <systemlib/param/param.h>
+
+/**
+* Mount operation mode
+* MAVLINK and RC use the ROI (or RC input if enabled and no ROI set) to control a mount.
+* ONBOARD uses MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL MavLink messages
+* to control a mount.
+* @value 0 DISABLE
+* @value 1 MAVLINK
+* @value 2 RC
+* @value 3 ONBOARD
+* @min 0
+* @max 3
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_MODE, 0);
+
+/**
+* Auxiliary channel to override current mount mode (only in ONBOARD mode)
+* if <0.0f then MODE_RETRACT (retracts not retracted)
+* if =0.0f then don't override
+* if >0.0f then MODE_RC_TARGETING (retracts retracted)
+* @value 0 Disable
+* @value 1 AUX1
+* @value 2 AUX2
+* @value 3 AUX3
+* @value 4 AUX4
+* @value 5 AUX5
+* @min 0
+* @max 5
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_MODE_OVR, 0);
+
+/**
+* Mavlink System ID
+*
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_MAV_SYSID, 71);
+
+/**
+* Mavlink Component ID
+*
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_MAV_COMPID, 67);
+
+/**
+* Mixer value for selecting normal mode
+* if required by the gimbal (only in RC mode)
+* @min -1.0
+* @max 1.0
+* @decimal 3
+* @group Mount
+*/
+PARAM_DEFINE_FLOAT(MNT_OB_NORM_MODE, -1.0f);
+
+/**
+* Mixer value for selecting a locking mode
+* if required for the gimbal (only in RC mode)
+* @min -1.0
+* @max 1.0
+* @decimal 3
+* @group Mount
+*/
+PARAM_DEFINE_FLOAT(MNT_OB_LOCK_MODE, 0.0f);
+
+
+/**
+* This enables the mount to be controlled when no ROI is set.
+*
+* If set to 1, the mount will be controlled by the AUX channels below
+* when no ROI is set.
+*
+* @boolean
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_MAN_CONTROL, 0);
+
+/**
+* Auxiliary channel to control roll.
+*
+* @value 0 Disable
+* @value 1 AUX1
+* @value 2 AUX2
+* @value 3 AUX3
+* @value 4 AUX4
+* @value 5 AUX5
+* @min 0
+* @max 5
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_MAN_ROLL, 0);
+
+/**
+* Auxiliary channel to control pitch.
+*
+* @value 0 Disable
+* @value 1 AUX1
+* @value 2 AUX2
+* @value 3 AUX3
+* @value 4 AUX4
+* @value 5 AUX5
+* @min 0
+* @max 5
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_MAN_PITCH, 0);
+
+/**
+* Auxiliary channel to control yaw.
+*
+* @value 0 Disable
+* @value 1 AUX1
+* @value 2 AUX2
+* @value 3 AUX3
+* @value 4 AUX4
+* @value 5 AUX5
+* @min 0
+* @max 5
+* @group Mount
+*/
+PARAM_DEFINE_INT32(MNT_MAN_YAW, 0);

src/drivers/vmount/vmount_rc.cpp

@@ -0,0 +1,155 @@
+/****************************************************************************
+*
+*   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;
+}

src/drivers/vmount/vmount_rc.h

@@ -0,0 +1,55 @@
+/****************************************************************************
+*
+*   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
+ * @author Leon Müller (thedevleon)
+ *
+ */
+
+#ifndef _VMOUNT_RC_H
+#define _VMOUNT_RC_H
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+// Public functions
+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 */