fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

UT: added method to test float values

Browse Source 
MC pos control tests: added tests to all configs that include them
This commit is contained in:
Andreas Antener 2016-09-13 11:19:00 +02:00 committed by Lorenz Meier
parent 9a219da9c2
commit f3b5c243e5
6 changed files with 59 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cmake/configs/nuttx_mindpx-v2_default.cmake
View File

@ -80,6 +80,7 @@ set(config_module_list
drivers/sf0x/sf0x_tests
drivers/test_ppm
modules/commander/commander_tests
modules/mc_pos_control/mc_pos_control_tests
modules/controllib_test
modules/mavlink/mavlink_tests
modules/unit_test

1
cmake/configs/nuttx_px4fmu-v2_test.cmake
View File

@ -76,6 +76,7 @@ set(config_module_list
drivers/test_ppm
#lib/rc/rc_tests
modules/commander/commander_tests
modules/mc_pos_control/mc_pos_control_tests
modules/controllib_test
modules/mavlink/mavlink_tests
modules/unit_test

1
cmake/configs/nuttx_px4fmu-v4_default.cmake
View File

@ -77,6 +77,7 @@ set(config_module_list
drivers/sf0x/sf0x_tests
drivers/test_ppm
modules/commander/commander_tests
modules/mc_pos_control/mc_pos_control_tests
modules/controllib_test
modules/mavlink/mavlink_tests
modules/unit_test

4
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -976,9 +976,7 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, f
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
res = line_b;
} else {
/* target is in front of us */

67
src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp
View File

@ -92,79 +92,92 @@ bool McPosControlTests::cross_sphere_line_test(void)
* Near + + +
* On trajectory --+----o---------+---------o----+--
* prev curr
*
* Expected targets (1, 2, 3):
* Far + + +
*
*
* On trajectory -------1---------2---------3-------
*
*
* Near + + +
* On trajectory -------o---1---------2-----3-------
*
*
* On trajectory --+----o----1----+--------2/3---+--
*/
// 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);
ut_compare_float("target A 0", res(0), 0.0f, 2);
ut_compare_float("target A 1", res(1), 0.0f, 2);
ut_compare_float("target A 2", res(2), 0.5f, 2);
// 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);
ut_compare_float("target B 0", res(0), 0.0f, 2);
ut_compare_float("target B 1", res(1), 0.0f, 2);
ut_compare_float("target B 2", res(2), 2.0f, 2);
// 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);
ut_compare_float("target C 0", res(0), 0.0f, 2);
ut_compare_float("target C 1", res(1), 0.0f, 2);
ut_compare_float("target C 2", res(2), 2.0f, 2);
// 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);
ut_compare_float("target D 0", res(0), 0.0f, 2);
ut_compare_float("target D 1", res(1), 0.0f, 2);
ut_compare_float("target D 2", res(2), 0.37f, 2);
// 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);
ut_compare_float("target E 0", res(0), 0.0f, 2);
ut_compare_float("target E 1", res(1), 0.0f, 2);
ut_compare_float("target E 2", res(2), 1.87f, 2);
// 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);
ut_compare_float("target F 0", res(0), 0.0f, 2);
ut_compare_float("target F 1", res(1), 0.0f, 2);
ut_compare_float("target F 2", res(2), 2.0f, 2);
// 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);
ut_compare_float("target G 0", res(0), 0.0f, 2);
ut_compare_float("target G 1", res(1), 0.0f, 2);
ut_compare_float("target G 2", res(2), 0.0f, 2);
// 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);
ut_compare_float("target H 0", res(0), 0.0f, 2);
ut_compare_float("target H 1", res(1), 0.0f, 2);
ut_compare_float("target H 2", res(2), 1.0f, 2);
// 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);
ut_compare_float("target I 0", res(0), 0.0f, 2);
ut_compare_float("target I 1", res(1), 0.0f, 2);
ut_compare_float("target I 2", res(2), 2.0f, 2);
return true;
}

15
src/modules/unit_test/unit_test.h
View File

@ -142,6 +142,21 @@ protected:
} \
} while (0)
/// @brief Used to compare two float values within a unit test. If possible use ut_compare_float instead of ut_assert
/// since it will give you better error reporting of the actual values being compared.
#define ut_compare_float(message, v1, v2, precision) \
do { \
int _p = pow(10, precision); \
int _v1 = (int)(v1 * _p + 0.5f); \
int _v2 = (int)(v2 * _p + 0.5f); \
if (_v1 != _v2) { \
_print_compare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \
return false; \
} else { \
_assertions++; \
} \
} while (0)
virtual void _init(void) { }; ///< Run before each unit test. Override to provide custom behavior.
virtual void _cleanup(void) { }; ///< Run after each unit test. Override to provide custom behavior.