fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:18:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

test_controlmath: add first few tests for constrain function in xy

Browse Source
This commit is contained in:
Dennis Mannhart
2018-07-20 09:26:18 +02:00
committed by Lorenz Meier
parent c60481614e
commit c6b5731086
1 changed files with 40 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp
+40
View File
@@ -10,11 +10,13 @@ public:
private:
bool testThrAttMapping();
bool testPrioritizeVector();
};
bool ControlMathTest::run_tests()
{
ut_run_test(testThrAttMapping);
ut_run_test(testPrioritizeVector);
return (_tests_failed == 0);
}
@@ -60,4 +62,42 @@ bool ControlMathTest::testThrAttMapping()
return true;
}
bool ControlMathTest::testPrioritizeVector()
{
float max = 5.0f;
// v0 already at max
matrix::Vector2f v0(max, 0);
matrix::Vector2f v1(v0(1), -v0(0));
matrix::Vector2f v_r = ControlMath::constrainXY(v0, v1, max);
ut_assert_true(fabsf(v_r(0)) - max < EPS && v_r(0) > 0.0f);
ut_assert_true(fabsf(v_r(1) - 0.0f) < EPS);
// v1 exceeds max but v0 is zero
v0.zero();
v_r = ControlMath::constrainXY(v0, v1, max);
ut_assert_true(fabsf(v_r(1)) - max < EPS && v_r(1) < 0.0f);
ut_assert_true(fabsf(v_r(0) - 0.0f) < EPS);
// v0 and v1 are below max
v0 = matrix::Vector2f(0.5f, 0.5f);
v1 = matrix::Vector2f(v0(1), -v0(0));
v_r = ControlMath::constrainXY(v0, v1, max);
float diff = matrix::Vector2f(v_r - (v0 + v1)).length();
ut_assert_true(diff < EPS);
// v0 and v1 exceed max and are perpendicular
v0 = matrix::Vector2f(4.0f, 0.0f);
v1 = matrix::Vector2f(0.0f, -4.0f);
v_r = ControlMath::constrainXY(v0, v1, max);
ut_assert_true(v_r(0) - v0(0) < EPS && v_r(0) > 0.0f);
float remaining = sqrtf(max * max - (v0(0) * v0(0)));
ut_assert_true(fabsf(v_r(1)) - remaining < EPS && v_r(1) < EPS);
//TODO: add more tests with vectors not perpendicular
return true;
}
ut_declare_test_c(test_controlmath, ControlMathTest)