diff --git a/src/drivers/vmount/CMakeLists.txt b/src/drivers/vmount/CMakeLists.txt
index 7e9b3ed05c267ef079fd1d6d86aa598efad615f5..058ca9e5ddf5aadaf2404d40f156123c57bd8b2d 100644
--- a/src/drivers/vmount/CMakeLists.txt
+++ b/src/drivers/vmount/CMakeLists.txt
@@ -40,6 +40,7 @@ px4_add_module(
input.cpp
input_mavlink.cpp
input_rc.cpp
+ input_test.cpp
output.cpp
output_mavlink.cpp
output_rc.cpp
diff --git a/src/drivers/vmount/input_test.cpp b/src/drivers/vmount/input_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8c1df28419aa44e9667d05f2cb98d639e6533191
--- /dev/null
+++ b/src/drivers/vmount/input_test.cpp
@@ -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_test.cpp
+ * @author Beat Küng <beat-kueng@gmx.net>
+ *
+ */
+
+#include "input_test.h"
+
+#include <px4_posix.h>
+
+
+namespace vmount
+{
+
+InputTest::InputTest(float roll_deg, float pitch_deg, float yaw_deg)
+{
+ _angles[0] = roll_deg;
+ _angles[1] = pitch_deg;
+ _angles[2] = yaw_deg;
+}
+
+bool InputTest::finished()
+{
+ return true; /* only a single-shot test (for now) */
+}
+
+int InputTest::update(unsigned int timeout_ms, ControlData **control_data)
+{
+ //we directly override the update() here, since we don't need the initialization from the base class
+
+ _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] = _angles[i] * M_DEG_TO_RAD_F;
+
+ _control_data.stabilize_axis[i] = false;
+ }
+
+ _control_data.gimbal_shutter_retract = false;
+ *control_data = &_control_data;
+ return 0;
+}
+
+int InputTest::initialize()
+{
+ return 0;
+}
+
+void InputTest::print_status()
+{
+ PX4_INFO("Input: Test");
+}
+
+} /* namespace vmount */
diff --git a/src/drivers/vmount/input_test.h b/src/drivers/vmount/input_test.h
new file mode 100644
index 0000000000000000000000000000000000000000..e6fba3a3d1c4556fcf82eba902f37275623eee2f
--- /dev/null
+++ b/src/drivers/vmount/input_test.h
@@ -0,0 +1,78 @@
+/****************************************************************************
+*
+* 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_test.h
+ * @author Beat Küng <beat-kueng@gmx.net>
+ *
+ */
+
+#pragma once
+
+#include "input.h"
+
+namespace vmount
+{
+
+/**
+ ** class InputTest
+ * Send a single control command, configured via constructor arguments
+ */
+class InputTest : public InputBase
+{
+public:
+
+ /**
+ * set to a fixed angle
+ */
+ InputTest(float roll_deg, float pitch_deg, float yaw_deg);
+ virtual ~InputTest() {}
+
+ /** check whether the test finished, and thus the main thread can quit */
+ bool finished();
+
+ virtual int update(unsigned int timeout_ms, ControlData **control_data);
+
+protected:
+ virtual int update_impl(unsigned int timeout_ms, ControlData **control_data) { return 0; } //not needed
+
+ virtual int initialize();
+
+ virtual void print_status();
+
+private:
+ float _angles[3]; /**< desired angles in [deg] */
+};
+
+
+} /* namespace vmount */
diff --git a/src/drivers/vmount/output_rc.cpp b/src/drivers/vmount/output_rc.cpp
index 2bfd49e36318ad42addd2382d20eda3d2e1024c2..5c0010365c0522217403be80a8c82a58c3b5640c 100644
--- a/src/drivers/vmount/output_rc.cpp
+++ b/src/drivers/vmount/output_rc.cpp
@@ -51,6 +51,12 @@ OutputRC::OutputRC(const OutputConfig &output_config)
: OutputBase(output_config)
{
}
+OutputRC::~OutputRC()
+{
+ if (_actuator_controls_pub) {
+ orb_unadvertise(_actuator_controls_pub);
+ }
+}
int OutputRC::update(const ControlData *control_data)
{
diff --git a/src/drivers/vmount/output_rc.h b/src/drivers/vmount/output_rc.h
index 02b11d9a104ef4e8c802787f0f601fbd5264d1b0..7e9220af4cbe77ffbbf706a4d00d358ef6ed0e34 100644
--- a/src/drivers/vmount/output_rc.h
+++ b/src/drivers/vmount/output_rc.h
@@ -56,7 +56,7 @@ class OutputRC : public OutputBase
{
public:
OutputRC(const OutputConfig &output_config);
- virtual ~OutputRC() { }
+ virtual ~OutputRC();
virtual int update(const ControlData *control_data);
diff --git a/src/drivers/vmount/vmount.cpp b/src/drivers/vmount/vmount.cpp
index 9530b859918ef61505dc0dbe6bfce50889a5b4f9..ede648fd721a98fba24e822a4b07696020ee50dd 100644
--- a/src/drivers/vmount/vmount.cpp
+++ b/src/drivers/vmount/vmount.cpp
@@ -49,9 +49,11 @@
#include <unistd.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
+#include <px4_defines.h>
-#include "input_rc.h"
#include "input_mavlink.h"
+#include "input_rc.h"
+#include "input_test.h"
#include "output_rc.h"
#include "output_mavlink.h"
@@ -118,7 +120,8 @@ extern "C" __EXPORT int vmount_main(int argc, char *argv[]);
static void usage()
{
- PX4_INFO("usage: vmount {start|stop|status}");
+ PX4_INFO("usage: vmount {start|stop|status|test}");
+ PX4_INFO(" vmount test {roll|pitch|yaw} <angle_deg>");
}
static int vmount_thread_main(int argc, char *argv[])
@@ -127,6 +130,44 @@ static int vmount_thread_main(int argc, char *argv[])
Parameters params;
memset(¶ms, 0, sizeof(params));
+
+ InputTest *test_input = nullptr;
+
+ if (argc > 0 && !strcmp(argv[0], "test")) {
+ PX4_INFO("Starting in test mode");
+
+ const char *axis_names[3] = {"roll", "pitch", "yaw"};
+ float angles[3] = { 0.f, 0.f, 0.f };
+
+ if (argc == 3) {
+ bool found_axis = false;
+
+ for (int i = 0 ; i < 3; ++i) {
+ if (!strcmp(argv[1], axis_names[i])) {
+ long angle_deg = strtol(argv[2], NULL, 0);
+ angles[i] = (float)angle_deg;
+ found_axis = true;
+ }
+ }
+
+ if (!found_axis) {
+ usage();
+ return -1;
+ }
+
+ test_input = new InputTest(angles[0], angles[1], angles[2]);
+
+ if (!test_input) {
+ PX4_ERR("memory allocation failed");
+ return -1;
+ }
+
+ } else {
+ usage();
+ return -1;
+ }
+ }
+
if (!get_params(param_handles, params)) {
PX4_ERR("could not get mount parameters!");
return -1;
@@ -149,38 +190,43 @@ static int vmount_thread_main(int argc, char *argv[])
output_config.mavlink_sys_id = params.mnt_mav_sysid;
output_config.mavlink_comp_id = params.mnt_mav_compid;
- if (params.mnt_man_control) {
- manual_input = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
+ if (test_input) {
+ input_obj = test_input;
- if (!manual_input) {
- PX4_ERR("memory allocation failed");
- break;
+ } else {
+ if (params.mnt_man_control) {
+ manual_input = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
+
+ if (!manual_input) {
+ PX4_ERR("memory allocation failed");
+ break;
+ }
}
- }
- switch (params.mnt_mode_in) {
- case 1: //RC
- if (manual_input) {
- input_obj = manual_input;
- manual_input = nullptr;
+ switch (params.mnt_mode_in) {
+ case 1: //RC
+ if (manual_input) {
+ input_obj = manual_input;
+ manual_input = nullptr;
- } else {
- input_obj = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
- }
+ } else {
+ input_obj = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
+ }
- break;
+ break;
- case 2: //MAVLINK_ROI
- input_obj = new InputMavlinkROI(manual_input);
- break;
+ case 2: //MAVLINK_ROI
+ input_obj = new InputMavlinkROI(manual_input);
+ break;
- case 3: //MAVLINK_DO_MOUNT
- input_obj = new InputMavlinkCmdMount(manual_input);
- break;
+ case 3: //MAVLINK_DO_MOUNT
+ input_obj = new InputMavlinkCmdMount(manual_input);
+ break;
- default:
- PX4_ERR("invalid input mode %i", params.mnt_mode_in);
- break;
+ default:
+ PX4_ERR("invalid input mode %i", params.mnt_mode_in);
+ break;
+ }
}
switch (params.mnt_mode_out) {
@@ -199,6 +245,7 @@ static int vmount_thread_main(int argc, char *argv[])
if (!input_obj || !output_obj) {
PX4_ERR("memory allocation failed");
+ thread_should_exit = true;
break;
}
@@ -206,6 +253,7 @@ static int vmount_thread_main(int argc, char *argv[])
if (ret) {
PX4_ERR("failed to initialize output mode (%i)", ret);
+ thread_should_exit = true;
break;
}
}
@@ -234,6 +282,12 @@ static int vmount_thread_main(int argc, char *argv[])
usleep(1e6);
}
+ if (test_input && test_input->finished()) {
+ thread_should_exit = true;
+ break;
+ }
+
+
//check for parameter changes
bool updated;
@@ -310,7 +364,7 @@ int vmount_main(int argc, char *argv[])
return -1;
}
- if (!strcmp(argv[1], "start")) {
+ if (!strcmp(argv[1], "start") || !strcmp(argv[1], "test")) {
/* this is not an error */
if (thread_running) {
@@ -324,13 +378,19 @@ int vmount_main(int argc, char *argv[])
SCHED_PRIORITY_DEFAULT + 40,
1200,
vmount_thread_main,
- (char *const *)argv);
+ (char *const *)argv + 1);
+
+ int counter = 0;
while (!thread_running && vmount_task >= 0) {
- usleep(200);
+ usleep(5000);
+
+ if (++counter >= 100) {
+ break;
+ }
}
- return 0;
+ return counter < 100 || thread_should_exit ? 0 : -1;
}
if (!strcmp(argv[1], "stop")) {