diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake
index 48ab8ff9a8234e6f36daf844af3eefed5f03d566..ad2ee2b11e101a0bb7ee308abf25b0c8e7d246e5 100644
--- a/cmake/configs/nuttx_mindpx-v2_default.cmake
+++ b/cmake/configs/nuttx_mindpx-v2_default.cmake
@@ -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
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index 89e066472ef1ebb4389df7d75871160aa504c451..5012b93b05b3e0ab0e9616613622960773f63f6e 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -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
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index 7cddcbc0333f4747f0ed6c2e9c5ad42aeb56ee57..faf19aa8ffa2cc1f5e2066898eddb87a09ebc2c2 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -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
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 cac636ea58a179d82cec7cae4bd3b46883371aa7..87c73aeaccff0f5cc3fe6d30876dfc6d72142a62 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -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 */
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
index cd415febbb168333801bfc052f65ff41eb0bfd50..122d21cf578f2f1a2b9dc2c5236b113f1b650a11 100644
--- 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
@@ -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;
}
diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
index 42be09c16514ce2f896bc2ed0c47fa465fd791c3..b40cc582f1172f582acfcc4bcc642fad30aae0a2 100644
--- a/src/modules/unit_test/unit_test.h
+++ b/src/modules/unit_test/unit_test.h
@@ -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.