From 9a219da9c2f6338e2a15bca4187558061ae15d81 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 13 Sep 2016 10:06:19 +0200
Subject: [PATCH] Refactored cross sphere line tracking and added a unittest to
verify correct operation
---
cmake/configs/posix_sitl_default.cmake | 1 +
.../mc_pos_control/mc_pos_control_main.cpp | 57 +++---
.../mc_pos_control_tests/CMakeLists.txt | 42 ++++
.../mc_pos_control_tests.cpp | 184 ++++++++++++++++++
src/systemcmds/tests/tests_main.c | 1 +
src/systemcmds/tests/tests_main.h | 1 +
6 files changed, 259 insertions(+), 27 deletions(-)
create mode 100644 src/modules/mc_pos_control/mc_pos_control_tests/CMakeLists.txt
create mode 100644 src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index ec40efff9b..97049318dd 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -84,6 +84,7 @@ set(config_module_list
drivers/sf0x/sf0x_tests
lib/rc/rc_tests
modules/commander/commander_tests
+ modules/mc_pos_control/mc_pos_control_tests
modules/controllib_test
#modules/mavlink/mavlink_tests #TODO: fix mavlink_tests
modules/unit_test
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index ba740b59dd..cac636ea58 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -121,6 +121,9 @@ public:
*/
int start();
+ bool cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r,
+ const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res);
+
private:
bool _task_should_exit; /**< if true, task should exit */
int _control_task; /**< task handle for task */
@@ -324,9 +327,6 @@ private:
*/
void control_offboard(float dt);
- bool cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r,
- const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res);
-
/**
* Set position setpoint for AUTO
*/
@@ -970,16 +970,37 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, f
math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
float cd_len = (sphere_c - d).length();
- /* we have triangle CDX with known CD and CX = R, find DX */
if (sphere_r > cd_len) {
- /* have two roots, select one in A->B direction from D */
+ /* we have triangle CDX with known CD and CX = R, find DX */
float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
- res = d + ab_norm * dx_len;
+
+ if ((sphere_c - line_b) * ab_norm > 0.0f) {
+ /* target waypoint is already behind us */
+ math::Vector<3> ba_norm = line_a - line_b;
+ ba_norm.normalize();
+ res = d + ba_norm * dx_len; // vector B->A on line
+
+ } else {
+ /* target is in front of us */
+ res = d + ab_norm * dx_len; // vector A->B on line
+ }
+
return true;
} else {
/* have no roots, return D */
- res = d;
+ res = d; /* go directly to line */
+
+ /* previous waypoint is still in front of us */
+ if ((sphere_c - line_a) * ab_norm < 0.0f) {
+ res = line_a;
+ }
+
+ /* target waypoint is already behind us */
+ if ((sphere_c - line_b) * ab_norm > 0.0f) {
+ res = line_b;
+ }
+
return false;
}
}
@@ -1125,26 +1146,8 @@ void MulticopterPositionControl::control_auto(float dt)
} else {
bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
- if (near) {
- /* unit sphere crosses trajectory */
- /* if copter is in front of curr waypoint, go directly to curr waypoint */
- if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
- pos_sp_s = curr_sp_s;
- pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
- }
-
- } else {
- /* copter is too far from trajectory */
- /* if copter is behind prev waypoint, go directly to prev waypoint */
- if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) {
- pos_sp_s = prev_sp_s;
- }
-
- /* if copter is in front of curr waypoint, go directly to curr waypoint */
- if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
- pos_sp_s = curr_sp_s;
- }
-
+ if (!near) {
+ /* we're far away from trajectory, pos_sp_s is set to the nearest point on the trajectory */
pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
}
}
diff --git a/src/modules/mc_pos_control/mc_pos_control_tests/CMakeLists.txt b/src/modules/mc_pos_control/mc_pos_control_tests/CMakeLists.txt
new file mode 100644
index 0000000000..0a0a80f60b
--- /dev/null
+++ b/src/modules/mc_pos_control/mc_pos_control_tests/CMakeLists.txt
@@ -0,0 +1,42 @@
+############################################################################
+#
+# Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+ MODULE modules__mc_pos_control__mc_pos_control_tests
+ MAIN mc_pos_control_tests
+ SRCS
+ mc_pos_control_tests.cpp
+ ../mc_pos_control_main.cpp
+ DEPENDS
+ platforms__common
+ )
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp b/src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp
new file mode 100644
index 0000000000..cd415febbb
--- /dev/null
+++ b/src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp
@@ -0,0 +1,184 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * 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 mc_pos_control_tests.cpp
+ * Commander unit tests. Run the tests as follows:
+ * nsh> mc_pos_control_tests
+ *
+ */
+
+#include <systemlib/err.h>
+#include <unit_test/unit_test.h>
+#include <mathlib/mathlib.h>
+
+extern "C" __EXPORT int mc_pos_control_tests_main(int argc, char *argv[]);
+
+bool mcPosControlTests(void);
+
+//#include "../mc_pos_control_main.cpp"
+class MulticopterPositionControl
+{
+public:
+ bool cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r,
+ const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res);
+};
+
+class McPosControlTests : public UnitTest
+{
+public:
+ McPosControlTests();
+ virtual ~McPosControlTests();
+
+ virtual bool run_tests(void);
+
+private:
+ bool cross_sphere_line_test();
+};
+
+McPosControlTests::McPosControlTests()
+{
+}
+
+McPosControlTests::~McPosControlTests()
+{
+}
+
+bool McPosControlTests::cross_sphere_line_test(void)
+{
+ MulticopterPositionControl *control = {};
+
+ math::Vector<3> prev = math::Vector<3>(0, 0, 0);
+ math::Vector<3> curr = math::Vector<3>(0, 0, 2);
+ math::Vector<3> res;
+ bool retval = false;
+
+ /*
+ * Testing 9 positions (+) around waypoints (o):
+ *
+ * Far + + +
+ *
+ * Near + + +
+ * On trajectory --+----o---------+---------o----+--
+ * prev curr
+ */
+
+ // on line, near, before previous waypoint
+ retval = control->cross_sphere_line(math::Vector<3>(0.0f, 0.0f, -0.5f), 1.0f, prev, curr, res);
+ PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
+ ut_assert_true(retval);
+ ut_assert_true(res(0) == 0.0f);
+ ut_assert_true(res(1) == 0.0f);
+ ut_assert_true((int)(res(2) * 1e2f + 0.5f) == 50);
+
+ // on line, near, before target waypoint
+ retval = control->cross_sphere_line(math::Vector<3>(0.0f, 0.0f, 1.0f), 1.0f, prev, curr, res);
+ PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
+ ut_assert_true(retval);
+ ut_assert_true(res(0) == 0.0f);
+ ut_assert_true(res(1) == 0.0f);
+ ut_assert_true((int)(res(2) * 1e2f + 0.5f) == 200);
+
+ // on line, near, after target waypoint
+ retval = control->cross_sphere_line(math::Vector<3>(0.0f, 0.0f, 2.5f), 1.0f, prev, curr, res);
+ PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
+ ut_assert_true(retval);
+ ut_assert_true(res(0) == 0.0f);
+ ut_assert_true(res(1) == 0.0f);
+ ut_assert_true((int)(res(2) * 1e2f + 0.5f) == 150);
+
+ // near, before previous waypoint
+ retval = control->cross_sphere_line(math::Vector<3>(0.0f, 0.5f, -0.5f), 1.0f, prev, curr, res);
+ PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
+ ut_assert_true(retval);
+ ut_assert_true(res(0) == 0.0f);
+ ut_assert_true(res(1) == 0.0f);
+ ut_assert_true((int)(res(2) * 1e2f + 0.5f) == 37);
+
+ // near, before target waypoint
+ retval = control->cross_sphere_line(math::Vector<3>(0.0f, 0.5f, 1.0f), 1.0f, prev, curr, res);
+ PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
+ ut_assert_true(retval);
+ ut_assert_true(res(0) == 0.0f);
+ ut_assert_true(res(1) == 0.0f);
+ ut_assert_true((int)(res(2) * 1e2f + 0.5f) == 187);
+
+ // near, after target waypoint
+ retval = control->cross_sphere_line(math::Vector<3>(0.0f, 0.5f, 2.5f), 1.0f, prev, curr, res);
+ PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
+ ut_assert_true(retval);
+ ut_assert_true(res(0) == 0.0f);
+ ut_assert_true(res(1) == 0.0f);
+ ut_assert_true((int)(res(2) * 1e2f + 0.5f) == 163);
+
+ // far, before previous waypoint
+ retval = control->cross_sphere_line(math::Vector<3>(0.0f, 2.0f, -0.5f), 1.0f, prev, curr, res);
+ PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
+ ut_assert_false(retval);
+ ut_assert_true(res(0) == 0.0f);
+ ut_assert_true(res(1) == 0.0f);
+ ut_assert_true(res(2) == 0.0f);
+
+ // far, before target waypoint
+ retval = control->cross_sphere_line(math::Vector<3>(0.0f, 2.0f, 1.0f), 1.0f, prev, curr, res);
+ PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
+ ut_assert_false(retval);
+ ut_assert_true(res(0) == 0.0f);
+ ut_assert_true(res(1) == 0.0f);
+ ut_assert_true((int)(res(2) * 1e2f + 0.5f) == 100);
+
+ // far, after target waypoint
+ retval = control->cross_sphere_line(math::Vector<3>(0.0f, 2.0f, 2.5f), 1.0f, prev, curr, res);
+ PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
+ ut_assert_false(retval);
+ ut_assert_true(res(0) == 0.0f);
+ ut_assert_true(res(1) == 0.0f);
+ ut_assert_true((int)(res(2) * 1e2f + 0.5f) == 200);
+
+ return true;
+}
+
+bool McPosControlTests::run_tests(void)
+{
+ ut_run_test(cross_sphere_line_test);
+
+ return (_tests_failed == 0);
+}
+
+ut_declare_test(mcPosControlTests, McPosControlTests);
+
+int mc_pos_control_tests_main(int argc, char *argv[])
+{
+ return mcPosControlTests() ? 0 : -1;
+}
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index a7b4759d38..dd647b3e8c 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -90,6 +90,7 @@ const struct {
/* external tests */
{"commander", commander_tests_main, 0},
{"controllib", controllib_test_main, 0},
+ {"mc_pos_control", mc_pos_control_tests_main, 0},
//{"mavlink", mavlink_tests_main, 0}, // TODO: fix mavlink_tests
{"sf0x", sf0x_tests_main, 0},
#ifndef __PX4_DARWIN
diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h
index c9cf30ab17..3d1adc1096 100644
--- a/src/systemcmds/tests/tests_main.h
+++ b/src/systemcmds/tests/tests_main.h
@@ -92,6 +92,7 @@ extern int controllib_test_main(int argc, char *argv[]);
extern int uorb_tests_main(int argc, char *argv[]);
extern int rc_tests_main(int argc, char *argv[]);
extern int sf0x_tests_main(int argc, char *argv[]);
+extern int mc_pos_control_tests_main(int argc, char *argv[]);
__END_DECLS
--
GitLab