From 9a219da9c2f6338e2a15bca4187558061ae15d81 Mon Sep 17 00:00:00 2001 From: Andreas Antener 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 + * + * 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 +#include +#include + +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