Refactored cross sphere line tracking and added a unittest to verify correct operation

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

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();
 					}
 				}

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 :

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;
+}

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

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