|
|
|
@ -51,8 +51,8 @@ bool mcPosControlTests();
@@ -51,8 +51,8 @@ bool mcPosControlTests();
|
|
|
|
|
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); |
|
|
|
|
bool cross_sphere_line(const math::Vector<3> &sphere_c, const float sphere_r, |
|
|
|
|
const math::Vector<3> &line_a, const math::Vector<3> &line_b, math::Vector<3> &res); |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
class McPosControlTests : public UnitTest |
|
|
|
@ -77,7 +77,7 @@ McPosControlTests::~McPosControlTests()
@@ -77,7 +77,7 @@ McPosControlTests::~McPosControlTests()
|
|
|
|
|
|
|
|
|
|
bool McPosControlTests::cross_sphere_line_test() |
|
|
|
|
{ |
|
|
|
|
MulticopterPositionControl *control = {}; |
|
|
|
|
MulticopterPositionControl control = MulticopterPositionControl(); |
|
|
|
|
|
|
|
|
|
math::Vector<3> prev = math::Vector<3>(0, 0, 0); |
|
|
|
|
math::Vector<3> curr = math::Vector<3>(0, 0, 2); |
|
|
|
@ -108,7 +108,7 @@ bool McPosControlTests::cross_sphere_line_test()
@@ -108,7 +108,7 @@ bool McPosControlTests::cross_sphere_line_test()
|
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
// 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); |
|
|
|
|
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_compare_float("target A 0", res(0), 0.0f, 2); |
|
|
|
@ -116,7 +116,7 @@ bool McPosControlTests::cross_sphere_line_test()
@@ -116,7 +116,7 @@ bool McPosControlTests::cross_sphere_line_test()
|
|
|
|
|
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); |
|
|
|
|
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_compare_float("target B 0", res(0), 0.0f, 2); |
|
|
|
@ -124,7 +124,7 @@ bool McPosControlTests::cross_sphere_line_test()
@@ -124,7 +124,7 @@ bool McPosControlTests::cross_sphere_line_test()
|
|
|
|
|
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); |
|
|
|
|
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_compare_float("target C 0", res(0), 0.0f, 2); |
|
|
|
@ -132,7 +132,7 @@ bool McPosControlTests::cross_sphere_line_test()
@@ -132,7 +132,7 @@ bool McPosControlTests::cross_sphere_line_test()
|
|
|
|
|
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); |
|
|
|
|
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_compare_float("target D 0", res(0), 0.0f, 2); |
|
|
|
@ -140,7 +140,7 @@ bool McPosControlTests::cross_sphere_line_test()
@@ -140,7 +140,7 @@ bool McPosControlTests::cross_sphere_line_test()
|
|
|
|
|
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); |
|
|
|
|
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_compare_float("target E 0", res(0), 0.0f, 2); |
|
|
|
@ -148,7 +148,7 @@ bool McPosControlTests::cross_sphere_line_test()
@@ -148,7 +148,7 @@ bool McPosControlTests::cross_sphere_line_test()
|
|
|
|
|
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); |
|
|
|
|
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_compare_float("target F 0", res(0), 0.0f, 2); |
|
|
|
@ -156,7 +156,7 @@ bool McPosControlTests::cross_sphere_line_test()
@@ -156,7 +156,7 @@ bool McPosControlTests::cross_sphere_line_test()
|
|
|
|
|
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); |
|
|
|
|
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_compare_float("target G 0", res(0), 0.0f, 2); |
|
|
|
@ -164,7 +164,7 @@ bool McPosControlTests::cross_sphere_line_test()
@@ -164,7 +164,7 @@ bool McPosControlTests::cross_sphere_line_test()
|
|
|
|
|
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); |
|
|
|
|
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_compare_float("target H 0", res(0), 0.0f, 2); |
|
|
|
@ -172,7 +172,7 @@ bool McPosControlTests::cross_sphere_line_test()
@@ -172,7 +172,7 @@ bool McPosControlTests::cross_sphere_line_test()
|
|
|
|
|
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); |
|
|
|
|
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_compare_float("target I 0", res(0), 0.0f, 2); |
|
|
|
|