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

mc_pos_control move to matrix lib (#9141)

Browse Source
This commit is contained in:
Daniel Agar
2018-03-23 12:53:49 -04:00
committed by GitHub
parent cc26c34691
commit d2712dcb05
2 changed files with 76 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp
+14 -14
View File
@@ -51,8 +51,8 @@ bool mcPosControlTests();
class MulticopterPositionControl
{
public:
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);
bool cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r,
const matrix::Vector3f &line_a, const matrix::Vector3f &line_b, matrix::Vector3f &res);
};
class McPosControlTests : public UnitTest
@@ -79,9 +79,9 @@ bool McPosControlTests::cross_sphere_line_test()
{
MulticopterPositionControl control = MulticopterPositionControl();
math::Vector<3> prev = math::Vector<3>(0, 0, 0);
math::Vector<3> curr = math::Vector<3>(0, 0, 2);
math::Vector<3> res;
matrix::Vector3f prev = matrix::Vector3f(0.0f, 0.0f, 0.0f);
matrix::Vector3f curr = matrix::Vector3f(0.0f, 0.0f, 2.0f);
matrix::Vector3f res;
bool retval = false;
/*
@@ -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(matrix::Vector3f(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()
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(matrix::Vector3f(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()
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(matrix::Vector3f(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()
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(matrix::Vector3f(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()
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(matrix::Vector3f(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()
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(matrix::Vector3f(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()
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(matrix::Vector3f(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()
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(matrix::Vector3f(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()
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(matrix::Vector3f(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);