diff --git a/src/drivers/vmount/CMakeLists.txt b/src/drivers/vmount/CMakeLists.txt
index ca6ad367953165efe04532e90dfc96715d54164c..7e9b3ed05c267ef079fd1d6d86aa598efad615f5 100644
--- a/src/drivers/vmount/CMakeLists.txt
+++ b/src/drivers/vmount/CMakeLists.txt
@@ -37,11 +37,14 @@ px4_add_module(
COMPILE_FLAGS
-Os
SRCS
- vmount.cpp
- vmount_params.c
- vmount_rc.cpp
- vmount_mavlink.cpp
- vmount_onboard.cpp
+ input.cpp
+ input_mavlink.cpp
+ input_rc.cpp
+ output.cpp
+ output_mavlink.cpp
+ output_rc.cpp
+ vmount.cpp
+ vmount_params.c
DEPENDS
platforms__common
)
diff --git a/src/drivers/vmount/common.h b/src/drivers/vmount/common.h
new file mode 100644
index 0000000000000000000000000000000000000000..77401ed6f5fc925232bedc0e827486d9ff365e76
--- /dev/null
+++ b/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 */
diff --git a/src/drivers/vmount/vmount_onboard.h b/src/drivers/vmount/input.cpp
similarity index 60%
rename from src/drivers/vmount/vmount_onboard.h
rename to src/drivers/vmount/input.cpp
index 3f525ba98fe9b9aac24b437d7cbc2d5e11fda8e8..16011b57029e2f0853841554c8d480dbe9274d69 100644
--- a/src/drivers/vmount/vmount_onboard.h
+++ b/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
- * @author Leon Müller (thedevleon)
+ * @file input.cpp
+ * @author Beat Küng <beat-kueng@gmx.net>
*
*/
-#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 */
+#include "input.h"
+
+
+namespace vmount
+{
+
+int InputBase::update(unsigned int timeout_ms, ControlData **control_data)
+{
+ if (!_initialized) {
+ int ret = initialize();
+
+ if (ret) {
+ return ret;
+ }
+
+ //on startup, set the mount to a neutral position
+ _control_data.type = ControlData::Type::Neutral;
+ _control_data.gimbal_shutter_retract = true;
+ *control_data = &_control_data;
+ _initialized = true;
+ return 0;
+ }
+
+ return update_impl(timeout_ms, control_data);
+}
+
+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 */
+
diff --git a/src/drivers/vmount/input.h b/src/drivers/vmount/input.h
new file mode 100644
index 0000000000000000000000000000000000000000..5606a056b661d59701531055bc011535d8115e43
--- /dev/null
+++ b/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 */
diff --git a/src/drivers/vmount/input_mavlink.cpp b/src/drivers/vmount/input_mavlink.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..12e5da7b2b13c469c05acb676dcbe98b2ad1afcc
--- /dev/null
+++ b/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 */
+
diff --git a/src/drivers/vmount/input_mavlink.h b/src/drivers/vmount/input_mavlink.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c243a73ea1608eed77fa0d09312c7100f9141c1
--- /dev/null
+++ b/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 */
diff --git a/src/drivers/vmount/input_rc.cpp b/src/drivers/vmount/input_rc.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c0d61742cc7d993194109d91c591c0d6239c8267
--- /dev/null
+++ b/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 */
diff --git a/src/drivers/vmount/input_rc.h b/src/drivers/vmount/input_rc.h
new file mode 100644
index 0000000000000000000000000000000000000000..845bd81faece63a032257ee1306aa03d0517389c
--- /dev/null
+++ b/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 */
diff --git a/src/drivers/vmount/output.cpp b/src/drivers/vmount/output.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a1cd0e91a94645a162171369dbdbd709e489d5a1
--- /dev/null
+++ b/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 */
+
diff --git a/src/drivers/vmount/output.h b/src/drivers/vmount/output.h
new file mode 100644
index 0000000000000000000000000000000000000000..9bcaa30c155c89e17e15bc19eb5ffb03c22c8b3c
--- /dev/null
+++ b/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 */
diff --git a/src/drivers/vmount/output_mavlink.cpp b/src/drivers/vmount/output_mavlink.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9cf5378571c5d59b4d6a13f3f937e9913efc7462
--- /dev/null
+++ b/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 */
+
diff --git a/src/drivers/vmount/vmount_rc.h b/src/drivers/vmount/output_mavlink.h
similarity index 69%
rename from src/drivers/vmount/vmount_rc.h
rename to src/drivers/vmount/output_mavlink.h
index c8affedbbd87c21256d55a6ad5d023e1882b68eb..90d91c68e54eeca7e154160ffe264272207ccee7 100644
--- a/src/drivers/vmount/vmount_rc.h
+++ b/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
- * @author Leon Müller (thedevleon)
+ * @file output_mavlink.h
+ * @author Beat Küng <beat-kueng@gmx.net>
*
*/
-#ifndef _VMOUNT_RC_H
-#define _VMOUNT_RC_H
+#pragma once
-#include <sys/types.h>
-#include <stdbool.h>
+#include "output.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);
+#include <uORB/uORB.h>
-#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 */
diff --git a/src/drivers/vmount/output_rc.cpp b/src/drivers/vmount/output_rc.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2bfd49e36318ad42addd2382d20eda3d2e1024c2
--- /dev/null
+++ b/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 */
+
diff --git a/src/drivers/vmount/vmount_mavlink.h b/src/drivers/vmount/output_rc.h
similarity index 68%
rename from src/drivers/vmount/vmount_mavlink.h
rename to src/drivers/vmount/output_rc.h
index 1a1daefa711c0d76eac2302c6777b0a0d6a55def..02b11d9a104ef4e8c802787f0f601fbd5264d1b0 100644
--- a/src/drivers/vmount/vmount_mavlink.h
+++ b/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
- * @author Leon Müller (thedevleon)
+ * @file output_rc.h
+ * @author Beat Küng <beat-kueng@gmx.net>
*
*/
-#ifndef _VMOUNT_MAVLINK_H
-#define _VMOUNT_MAVLINK_H
+#pragma once
-#include <sys/types.h>
-#include <stdbool.h>
+#include "output.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);
+#include <uORB/uORB.h>
-#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 */
diff --git a/src/drivers/vmount/vmount.cpp b/src/drivers/vmount/vmount.cpp
index 566b7c2b205d3ac9aef7dfed6553da0fead03b02..9530b859918ef61505dc0dbe6bfce50889a5b4f9 100644
--- a/src/drivers/vmount/vmount.cpp
+++ b/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 "vmount_rc.h"
-#include "vmount_onboard.h"
+#include "input_rc.h"
+#include "input_mavlink.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 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 volatile bool thread_do_report_status = false;
-static struct vehicle_command_ack_s vehicle_command_ack;
-
-static struct {
- int mnt_mode;
+struct Parameters {
+ int mnt_mode_in;
+ int mnt_mode_out;
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 {
- param_t mnt_mode;
+ bool operator!=(const Parameters &p)
+ {
+ 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;
-/**
- * Print command usage information
- */
+/* functions */
+static void usage(void);
+static void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &got_changes);
+static bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms);
+
+static int vmount_thread_main(int argc, char *argv[]);
+extern "C" __EXPORT int vmount_main(int argc, char *argv[]);
+
static void usage()
{
- fprintf(stderr,
- "usage: vmount start\n"
- " vmount stop\n"
- " vmount status\n");
- exit(1);
+ PX4_INFO("usage: vmount {start|stop|status}");
}
-/**
- * The daemon thread.
- */
static int vmount_thread_main(int argc, char *argv[])
{
- if (!get_params()) {
- warnx("could not get mount parameters!");
+ ParameterHandles param_handles;
+ Parameters params;
+ memset(¶ms, 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));
+ thread_running = true;
+ OutputConfig output_config;
+ ControlData *control_data = nullptr;
+ InputBase *input_obj = nullptr;
+ OutputBase *output_obj = nullptr;
+ InputRC *manual_input = nullptr;
- else if (params.mnt_mode == 1) { vmount_state = MAVLINK;}
+ while (!thread_should_exit) {
- else if (params.mnt_mode == 2) { vmount_state = RC;}
+ if (!input_obj && params.mnt_mode_in != 0) { //need to initialize
- else if (params.mnt_mode == 3) { vmount_state = ONBOARD;}
+ output_config.gimbal_normal_mode_value = params.mnt_ob_norm_mode;
+ output_config.gimbal_retracted_mode_value = params.mnt_ob_lock_mode;
+ output_config.mavlink_sys_id = params.mnt_mav_sysid;
+ output_config.mavlink_comp_id = params.mnt_mav_compid;
- //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(¶meter_update, 0, sizeof(parameter_update));
- memset(&vehicle_command_ack, 0, sizeof(vehicle_command_ack));
+ if (params.mnt_man_control) {
+ manual_input = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
+ if (!manual_input) {
+ PX4_ERR("memory allocation failed");
+ break;
+ }
+ }
- 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));
+ switch (params.mnt_mode_in) {
+ case 1: //RC
+ if (manual_input) {
+ input_obj = manual_input;
+ manual_input = nullptr;
- vehicle_command_ack_pub = orb_advertise(ORB_ID(vehicle_command_ack), &vehicle_command_ack);
+ } else {
+ input_obj = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
+ }
- 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");
- }
+ break;
- 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;
+ case 2: //MAVLINK_ROI
+ input_obj = new InputMavlinkROI(manual_input);
+ break;
- thread_running = true;
+ case 3: //MAVLINK_DO_MOUNT
+ input_obj = new InputMavlinkCmdMount(manual_input);
+ break;
- run: {
- if (vmount_state == MAVLINK) {
- if (!vmount_mavlink_init()) {
- err(1, "could not initiate vmount_mavlink");
+ default:
+ PX4_ERR("invalid input mode %i", params.mnt_mode_in);
+ break;
}
- warnx("running mount driver in mavlink mode");
-
- if (params.mnt_man_control) manual_control_desired = true;
+ switch (params.mnt_mode_out) {
+ case 0: //AUX
+ output_obj = new OutputRC(output_config);
+ break;
- while (!thread_should_exit && !thread_should_restart) {
- vmount_update_topics();
+ case 1: //MAVLINK
+ output_obj = new OutputMavlink(output_config);
+ break;
- 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);
+ default:
+ PX4_ERR("invalid output mode %i", params.mnt_mode_out);
+ break;
+ }
- if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
- if (params.mnt_man_control) manual_control_desired = true;
+ if (!input_obj || !output_obj) {
+ PX4_ERR("memory allocation failed");
+ break;
+ }
- } 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
- );
+ int ret = output_obj->initialize();
- } else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
- manual_control_desired = false;
- //TODO how to do this?
+ if (ret) {
+ PX4_ERR("failed to initialize output mode (%i)", ret);
+ break;
+ }
+ }
- } 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
- );
+ if (params.mnt_mode_in != 0) {
- } else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
- manual_control_desired = false;
- //TODO is this even suported?
- }
- }
+ //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);
- 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));
- }
+ if (ret) {
+ PX4_ERR("failed to read input (%i)", ret);
+ break;
+ }
- 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);
- }
+ //update output
+ ret = output_obj->update(control_data);
- vmount_mavlink_deinit();
+ if (ret) {
+ PX4_ERR("failed to write output (%i)", ret);
+ break;
}
- } else if (vmount_state == RC) {
- if (!vmount_rc_init()) {
- err(1, "could not initiate vmount_rc");
- }
+ } else {
+ //wait for parameter changes. We still need to wake up regularily to check for thread exit requests
+ usleep(1e6);
+ }
- warnx("running mount driver in rc mode");
+ //check for parameter changes
+ bool updated;
- if (params.mnt_man_control) {
- manual_control_desired = true;
- }
+ if (orb_check(parameter_update_sub, &updated) == 0 && updated) {
+ parameter_update_s param_update;
+ orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update);
+ update_params(param_handles, params, updated);
- 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?
- }
+ if (updated) {
+ //re-init objects
+ if (input_obj) {
+ delete(input_obj);
+ input_obj = nullptr;
}
- 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));
+ if (output_obj) {
+ delete(output_obj);
+ output_obj = nullptr;
}
- 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);
+ if (manual_input) {
+ delete(manual_input);
+ manual_input = nullptr;
}
}
+ }
- vmount_rc_deinit();
+ if (thread_do_report_status) {
+ if (input_obj) {
+ input_obj->print_status();
- } else if (vmount_state == ONBOARD) {
- if (!vmount_onboard_init()) {
- err(1, "could not initiate vmount_onboard");
+ } else {
+ PX4_INFO("Input: None");
}
- 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;
+ if (output_obj) {
+ output_obj->print_status();
- 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 {
+ PX4_INFO("Output: None");
+ }
- }
+ thread_do_report_status = false;
+ }
+ }
- 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);
- }
- }
+ orb_unsubscribe(parameter_update_sub);
- 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");
+ if (input_obj) {
+ delete(input_obj);
+ }
- while (!thread_should_exit && !thread_should_restart) {
- vmount_update_topics();
- }
- }
+ if (output_obj) {
+ delete(output_obj);
}
- if (thread_should_restart)
- {
- thread_should_restart = false;
- warnx("parameter update, restarting mount driver!");
- goto run;
+ 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) {
- warnx("missing command");
+ if (argc < 2) {
+ 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",
- SCHED_DEFAULT,
- SCHED_PRIORITY_DEFAULT + 40, //TODO we might want a higher priority?
- 1100,
- vmount_thread_main,
- (char *const *)argv);
-
- while (!thread_running) {
+ int vmount_task = px4_task_spawn_cmd("vmount",
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_DEFAULT + 40,
+ 1200,
+ vmount_thread_main,
+ (char *const *)argv);
+
+ 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);
- warnx(".");
+ usleep(100000);
}
- warnx("terminated.");
- exit(0);
+ return 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;
+ thread_do_report_status = true;
+ while (thread_do_report_status) {
+ usleep(10000);
}
} else {
- errx(1, "not running");
+ PX4_INFO("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;
+ return 0;
}
- if (polls[6].revents & POLLIN) {
- orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶meter_update);
- parameter_update_updated = true;
- update_params();
- }
+ PX4_ERR("unrecognized command");
+ usage();
+ return -1;
}
-void update_params()
+void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &got_changes)
{
- param_get(params_handels.mnt_mode, ¶ms.mnt_mode);
- param_get(params_handels.mnt_mav_sysid, ¶ms.mnt_mav_sysid);
- param_get(params_handels.mnt_mav_compid, ¶ms.mnt_mav_compid);
- param_get(params_handels.mnt_ob_lock_mode, ¶ms.mnt_ob_lock_mode);
- param_get(params_handels.mnt_ob_norm_mode, ¶ms.mnt_ob_norm_mode);
- param_get(params_handels.mnt_man_control, ¶ms.mnt_man_control);
- param_get(params_handels.mnt_man_pitch, ¶ms.mnt_man_pitch);
- param_get(params_handels.mnt_man_roll, ¶ms.mnt_man_roll);
- param_get(params_handels.mnt_man_yaw, ¶ms.mnt_man_yaw);
- param_get(params_handels.mnt_mode_ovr, ¶ms.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.
- }
+ Parameters prev_params = params;
+ param_get(param_handles.mnt_mode_in, ¶ms.mnt_mode_in);
+ param_get(param_handles.mnt_mode_out, ¶ms.mnt_mode_out);
+ param_get(param_handles.mnt_mav_sysid, ¶ms.mnt_mav_sysid);
+ param_get(param_handles.mnt_mav_compid, ¶ms.mnt_mav_compid);
+ param_get(param_handles.mnt_ob_lock_mode, ¶ms.mnt_ob_lock_mode);
+ param_get(param_handles.mnt_ob_norm_mode, ¶ms.mnt_ob_norm_mode);
+ param_get(param_handles.mnt_man_control, ¶ms.mnt_man_control);
+ param_get(param_handles.mnt_man_pitch, ¶ms.mnt_man_pitch);
+ param_get(param_handles.mnt_man_roll, ¶ms.mnt_man_roll);
+ param_get(param_handles.mnt_man_yaw, ¶ms.mnt_man_yaw);
+
+ got_changes = prev_params != params;
}
-bool get_params()
+bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms)
{
- 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, ¶ms.mnt_mode) ||
- param_get(params_handels.mnt_mav_sysid, ¶ms.mnt_mav_sysid) ||
- param_get(params_handels.mnt_mav_compid, ¶ms.mnt_mav_compid) ||
- param_get(params_handels.mnt_ob_lock_mode, ¶ms.mnt_ob_lock_mode) ||
- param_get(params_handels.mnt_ob_norm_mode, ¶ms.mnt_ob_norm_mode) ||
- param_get(params_handels.mnt_man_control, ¶ms.mnt_man_control) ||
- param_get(params_handels.mnt_man_pitch, ¶ms.mnt_man_pitch) ||
- param_get(params_handels.mnt_man_roll, ¶ms.mnt_man_roll) ||
- param_get(params_handels.mnt_man_yaw, ¶ms.mnt_man_yaw) ||
- param_get(params_handels.mnt_mode_ovr, ¶ms.mnt_mode_ovr)) {
+ param_handles.mnt_mode_in = param_find("MNT_MODE_IN");
+ param_handles.mnt_mode_out = param_find("MNT_MODE_OUT");
+ param_handles.mnt_mav_sysid = param_find("MNT_MAV_SYSID");
+ param_handles.mnt_mav_compid = param_find("MNT_MAV_COMPID");
+ param_handles.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE");
+ param_handles.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE");
+ param_handles.mnt_man_control = param_find("MNT_MAN_CONTROL");
+ param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH");
+ param_handles.mnt_man_roll = param_find("MNT_MAN_ROLL");
+ param_handles.mnt_man_yaw = param_find("MNT_MAN_YAW");
+
+ if (param_handles.mnt_mode_in == PARAM_INVALID ||
+ param_handles.mnt_mode_out == PARAM_INVALID ||
+ param_handles.mnt_mav_sysid == PARAM_INVALID ||
+ param_handles.mnt_mav_compid == PARAM_INVALID ||
+ param_handles.mnt_ob_lock_mode == PARAM_INVALID ||
+ param_handles.mnt_ob_norm_mode == PARAM_INVALID ||
+ param_handles.mnt_man_control == PARAM_INVALID ||
+ param_handles.mnt_man_pitch == PARAM_INVALID ||
+ param_handles.mnt_man_roll == PARAM_INVALID ||
+ param_handles.mnt_man_yaw == PARAM_INVALID) {
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;
- }
}
diff --git a/src/drivers/vmount/vmount_mavlink.cpp b/src/drivers/vmount/vmount_mavlink.cpp
deleted file mode 100644
index 6ed0be2a8cc40086e40393929abfa6b05d288272..0000000000000000000000000000000000000000
--- a/src/drivers/vmount/vmount_mavlink.cpp
+++ /dev/null
@@ -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;
-}
diff --git a/src/drivers/vmount/vmount_onboard.cpp b/src/drivers/vmount/vmount_onboard.cpp
deleted file mode 100644
index 2451dfb4d8a1d2a8116485cda0ceebe491e94fc2..0000000000000000000000000000000000000000
--- a/src/drivers/vmount/vmount_onboard.cpp
+++ /dev/null
@@ -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;
-}
diff --git a/src/drivers/vmount/vmount_params.c b/src/drivers/vmount/vmount_params.c
index f79634948dae70fc1d2943939536a4ca2e1836e0..39a0687d1112d4fee4675ace9b382821303df9ee 100644
--- a/src/drivers/vmount/vmount_params.c
+++ b/src/drivers/vmount/vmount_params.c
@@ -41,46 +41,42 @@
#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.
+* Mount input mode
+* RC uses the AUX input channels (see MNT_MAN_* parameters),
+* MAVLINK_ROI uses the MAV_CMD_DO_SET_ROI Mavlink message, and MAVLINK_DO_MOUNT the
+* MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL messages to control a mount.
* @value 0 DISABLE
-* @value 1 MAVLINK
-* @value 2 RC
-* @value 3 ONBOARD
+* @value 1 RC
+* @value 2 MAVLINK_ROI
+* @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)
-* 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
+* Mount output mode
+* AUX uses the mixer output Control Group #2.
+* MAVLINK uses the MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL MavLink messages
+* to control a mount (set MNT_MAV_SYSID & MNT_MAV_COMPID)
+* @value 0 AUX
+* @value 1 MAVLINK
* @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);
+
diff --git a/src/drivers/vmount/vmount_rc.cpp b/src/drivers/vmount/vmount_rc.cpp
deleted file mode 100644
index 0d174f20e7922fc3659aeac2c05f44b3dc92b2ea..0000000000000000000000000000000000000000
--- a/src/drivers/vmount/vmount_rc.cpp
+++ /dev/null
@@ -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;
-}