zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

move cross_sphere_line_test method into test_controlmath.cpp 

remove mc_pos_control_tests since empty
move test_controlmath.cpp into systemcmds/tests directory
sbg
Dennis Mannhart 7 years ago committed by Lorenz Meier
parent
09b7d0f659
commit
c2343589d8
3 changed files with 212 additions and 296 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 190
      src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp
  2. 106
      src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp
  3. 212
      src/systemcmds/tests/test_controlmath.cpp

190
src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp
Unescape View File

@ -1,190 +0,0 @@ @@ -1,190 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mc_pos_control_tests.cpp
* Commander unit tests. Run the tests as follows:
* nsh> mc_pos_control_tests
*
*/
#include <systemlib/err.h>
#include <unit_test.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
extern "C" __EXPORT int mc_pos_control_tests_main(int argc, char *argv[]);
bool mcPosControlTests();
//#include "../mc_pos_control_main.cpp"
class MulticopterPositionControl
{
public:
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
{
public:
McPosControlTests() = default;
virtual ~McPosControlTests() = default;
virtual bool run_tests();
private:
bool cross_sphere_line_test();
};
bool McPosControlTests::cross_sphere_line_test()
{
MulticopterPositionControl control = MulticopterPositionControl();
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;
/*
* Testing 9 positions (+) around waypoints (o):
*
* Far + + +
*
* 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(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);
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(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);
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(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);
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(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);
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(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);
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(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);
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(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);
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(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);
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(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);
ut_compare_float("target I 1", res(1), 0.0f, 2);
ut_compare_float("target I 2", res(2), 2.0f, 2);
return true;
}
bool McPosControlTests::run_tests()
{
ut_run_test(cross_sphere_line_test);
return (_tests_failed == 0);
}
ut_declare_test(mcPosControlTests, McPosControlTests);
int mc_pos_control_tests_main(int argc, char *argv[])
{
return mcPosControlTests() ? 0 : -1;
}

106
src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp
Unescape View File

@ -1,106 +0,0 @@ @@ -1,106 +0,0 @@
#include <unit_test.h>
#include "../Utility/ControlMath.hpp"
#include <mathlib/mathlib.h>
#include <cfloat>
class ControlMathTest : public UnitTest
{
public:
virtual bool run_tests();
private:
bool testThrAttMapping();
bool testPrioritizeVector();
};
bool ControlMathTest::run_tests()
{
ut_run_test(testThrAttMapping);
ut_run_test(testPrioritizeVector);
return (_tests_failed == 0);
}
bool ControlMathTest::testThrAttMapping()
{
/* expected: zero roll, zero pitch, zero yaw, full thr mag
* reasone: thrust pointing full upward
*/
matrix::Vector3f thr{0.0f, 0.0f, -1.0f};
float yaw = 0.0f;
vehicle_attitude_setpoint_s att = ControlMath::thrustToAttitude(thr, yaw);
ut_assert_true(att.roll_body < FLT_EPSILON);
ut_assert_true(att.pitch_body < FLT_EPSILON);
ut_assert_true(att.yaw_body < FLT_EPSILON);
ut_assert_true(att.thrust - 1.0f < FLT_EPSILON);
/* expected: same as before but with 90 yaw
* reason: only yaw changed
*/
yaw = M_PI_2_F;
att = ControlMath::thrustToAttitude(thr, yaw);
ut_assert_true(att.roll_body < FLT_EPSILON);
ut_assert_true(att.pitch_body < FLT_EPSILON);
ut_assert_true(att.yaw_body - M_PI_2_F < FLT_EPSILON);
ut_assert_true(att.thrust - 1.0f < FLT_EPSILON);
/* expected: same as before but roll 180
* reason: thrust points straight down and order Euler
* order is: 1. roll, 2. pitch, 3. yaw
*/
thr = matrix::Vector3f(0.0f, 0.0f, 1.0f);
att = ControlMath::thrustToAttitude(thr, yaw);
ut_assert_true(fabsf(att.roll_body) - M_PI_F < FLT_EPSILON);
ut_assert_true(fabsf(att.pitch_body) < FLT_EPSILON);
ut_assert_true(att.yaw_body - M_PI_2_F < FLT_EPSILON);
ut_assert_true(att.thrust - 1.0f < FLT_EPSILON);
/* TODO: find a good way to test it */
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));
// the static keywork is a workaround for an internal bug of GCC
// "internal compiler error: in trunc_int_for_mode, at explow.c:55"
static matrix::Vector2f v_r = ControlMath::constrainXY(v0, v1, max);
ut_assert_true(fabsf(v_r(0)) - max < FLT_EPSILON && v_r(0) > 0.0f);
ut_assert_true(fabsf(v_r(1) - 0.0f) < FLT_EPSILON);
// v1 exceeds max but v0 is zero
v0.zero();
v_r = ControlMath::constrainXY(v0, v1, max);
ut_assert_true(fabsf(v_r(1)) - max < FLT_EPSILON && v_r(1) < 0.0f);
ut_assert_true(fabsf(v_r(0) - 0.0f) < FLT_EPSILON);
// 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 < FLT_EPSILON);
// 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) < FLT_EPSILON && v_r(0) > 0.0f);
float remaining = sqrtf(max * max - (v0(0) * v0(0)));
ut_assert_true(fabsf(v_r(1)) - remaining < FLT_EPSILON && v_r(1) < FLT_EPSILON);
//TODO: add more tests with vectors not perpendicular
return true;
}
ut_declare_test_c(test_controlmath, ControlMathTest)

212
src/systemcmds/tests/test_controlmath.cpp
Unescape View File

@ -0,0 +1,212 @@ @@ -0,0 +1,212 @@
#include <unit_test.h>
#include <mc_pos_control/Utility/ControlMath.hpp>
#include <mathlib/mathlib.h>
#include <cfloat>
class ControlMathTest : public UnitTest
{
public:
virtual bool run_tests();
private:
bool testThrAttMapping();
bool testPrioritizeVector();
bool crossSphereLineTest();
};
bool ControlMathTest::run_tests()
{
ut_run_test(testThrAttMapping);
ut_run_test(testPrioritizeVector);
ut_run_test(crossSphereLineTest);
return (_tests_failed == 0);
}
bool ControlMathTest::testThrAttMapping()
{
/* expected: zero roll, zero pitch, zero yaw, full thr mag
* reasone: thrust pointing full upward
*/
matrix::Vector3f thr{0.0f, 0.0f, -1.0f};
float yaw = 0.0f;
vehicle_attitude_setpoint_s att = ControlMath::thrustToAttitude(thr, yaw);
ut_assert_true(att.roll_body < FLT_EPSILON);
ut_assert_true(att.pitch_body < FLT_EPSILON);
ut_assert_true(att.yaw_body < FLT_EPSILON);
ut_assert_true(att.thrust - 1.0f < FLT_EPSILON);
/* expected: same as before but with 90 yaw
* reason: only yaw changed
*/
yaw = M_PI_2_F;
att = ControlMath::thrustToAttitude(thr, yaw);
ut_assert_true(att.roll_body < FLT_EPSILON);
ut_assert_true(att.pitch_body < FLT_EPSILON);
ut_assert_true(att.yaw_body - M_PI_2_F < FLT_EPSILON);
ut_assert_true(att.thrust - 1.0f < FLT_EPSILON);
/* expected: same as before but roll 180
* reason: thrust points straight down and order Euler
* order is: 1. roll, 2. pitch, 3. yaw
*/
thr = matrix::Vector3f(0.0f, 0.0f, 1.0f);
att = ControlMath::thrustToAttitude(thr, yaw);
ut_assert_true(fabsf(att.roll_body) - M_PI_F < FLT_EPSILON);
ut_assert_true(fabsf(att.pitch_body) < FLT_EPSILON);
ut_assert_true(att.yaw_body - M_PI_2_F < FLT_EPSILON);
ut_assert_true(att.thrust - 1.0f < FLT_EPSILON);
/* TODO: find a good way to test it */
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));
// the static keywork is a workaround for an internal bug of GCC
// "internal compiler error: in trunc_int_for_mode, at explow.c:55"
static matrix::Vector2f v_r = ControlMath::constrainXY(v0, v1, max);
ut_assert_true(fabsf(v_r(0)) - max < FLT_EPSILON && v_r(0) > 0.0f);
ut_assert_true(fabsf(v_r(1) - 0.0f) < FLT_EPSILON);
// v1 exceeds max but v0 is zero
v0.zero();
v_r = ControlMath::constrainXY(v0, v1, max);
ut_assert_true(fabsf(v_r(1)) - max < FLT_EPSILON && v_r(1) < 0.0f);
ut_assert_true(fabsf(v_r(0) - 0.0f) < FLT_EPSILON);
// 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 < FLT_EPSILON);
// 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) < FLT_EPSILON && v_r(0) > 0.0f);
float remaining = sqrtf(max * max - (v0(0) * v0(0)));
ut_assert_true(fabsf(v_r(1)) - remaining < FLT_EPSILON && v_r(1) < FLT_EPSILON);
//TODO: add more tests with vectors not perpendicular
return true;
}
bool ControlMathTest::crossSphereLineTest()
{
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;
/*
* Testing 9 positions (+) around waypoints (o):
*
* Far + + +
*
* 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 = ControlMath::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);
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 = ControlMath::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);
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 = ControlMath::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);
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 = ControlMath::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);
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 = ControlMath::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);
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 = ControlMath::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);
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 = ControlMath::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);
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 = ControlMath::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);
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 = ControlMath::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);
ut_compare_float("target I 1", res(1), 0.0f, 2);
ut_compare_float("target I 2", res(2), 2.0f, 2);
return true;
}
ut_declare_test_c(test_controlmath, ControlMathTest)
Write Preview
Loading…
Cancel
Save