bool HysteresisTest::_change_after_multiple_sets()
{
systemlib::Hysteresis hysteresis(false);
hysteresis.set_hysteresis_time_from(true, 5000 * f);
hysteresis.set_hysteresis_time_from(false, 5000 * f);
// Change to true.
hysteresis.set_state_and_update(true);
ut_assert_false(hysteresis.get_state());
usleep(3000 * f);
hysteresis.set_state_and_update(true);
ut_assert_false(hysteresis.get_state());
usleep(3000 * f);
hysteresis.set_state_and_update(true);
ut_assert_true(hysteresis.get_state());
// Change to false.
hysteresis.set_state_and_update(false);
ut_assert_true(hysteresis.get_state());
usleep(3000 * f);
hysteresis.set_state_and_update(false);
ut_assert_true(hysteresis.get_state());
usleep(3000 * f);
hysteresis.set_state_and_update(false);
ut_assert_false(hysteresis.get_state());
return true;
}
bool HysteresisTest::_change_after_time()
{
systemlib::Hysteresis hysteresis(false);
hysteresis.set_hysteresis_time_from(false, 5000 * f);
hysteresis.set_hysteresis_time_from(true, 3000 * f);
// Change to true.
hysteresis.set_state_and_update(true);
ut_assert_false(hysteresis.get_state());
px4_usleep(4000 * f);
hysteresis.update();
ut_assert_false(hysteresis.get_state());
px4_usleep(2000 * f);
hysteresis.update();
ut_assert_true(hysteresis.get_state());
// Change back to false.
hysteresis.set_state_and_update(false);
ut_assert_true(hysteresis.get_state());
px4_usleep(1000 * f);
hysteresis.update();
ut_assert_true(hysteresis.get_state());
px4_usleep(3000 * f);
hysteresis.update();
ut_assert_false(hysteresis.get_state());
return true;
}
bool HysteresisTest::_hysteresis_changed()
{
systemlib::Hysteresis hysteresis(false);
hysteresis.set_hysteresis_time_from(true, 2000 * f);
hysteresis.set_hysteresis_time_from(false, 5000 * f);
// Change to true.
hysteresis.set_state_and_update(true);
ut_assert_false(hysteresis.get_state());
usleep(3000 * f);
hysteresis.update();
ut_assert_false(hysteresis.get_state());
usleep(3000 * f);
hysteresis.update();
ut_assert_true(hysteresis.get_state());
// Change hysteresis time.
hysteresis.set_hysteresis_time_from(true, 10000 * f);
// Change back to false.
hysteresis.set_state_and_update(false);
ut_assert_true(hysteresis.get_state());
usleep(7000 * f);
hysteresis.update();
ut_assert_true(hysteresis.get_state());
usleep(5000 * f);
hysteresis.update();
ut_assert_false(hysteresis.get_state());
return true;
}
bool HysteresisTest::_init_true()
{
systemlib::Hysteresis hysteresis(true);
ut_assert_true(hysteresis.get_state());
return true;
}
bool HysteresisTest::_take_change_back()
{
systemlib::Hysteresis hysteresis(false);
hysteresis.set_hysteresis_time_from(false, 5000 * f);
// Change to true.
hysteresis.set_state_and_update(true);
ut_assert_false(hysteresis.get_state());
px4_usleep(3000 * f);
hysteresis.update();
ut_assert_false(hysteresis.get_state());
// Change your mind to false.
hysteresis.set_state_and_update(false);
ut_assert_false(hysteresis.get_state());
px4_usleep(6000 * f);
hysteresis.update();
ut_assert_false(hysteresis.get_state());
// And true again
hysteresis.set_state_and_update(true);
ut_assert_false(hysteresis.get_state());
px4_usleep(3000 * f);
hysteresis.update();
ut_assert_false(hysteresis.get_state());
px4_usleep(3000 * f);
hysteresis.update();
ut_assert_true(hysteresis.get_state());
// The other directory is immediate.
hysteresis.set_state_and_update(false);
ut_assert_false(hysteresis.get_state());
return true;
}
bool IntrusiveQueueTest::test_pop()
{
IntrusiveQueue<testContainer *> q1;
// size should be 0 initially
ut_compare("size initially 0", q1.size(), 0);
ut_assert_true(q1.empty());
// insert 100
for (int i = 0; i < 100; i++) {
testContainer *t = new testContainer();
t->i = i;
q1.push(t);
}
// verify full size (100)
ut_assert_true(q1.size() == 100);
for (int i = 0; i < 100; i++) {
auto node = q1.front();
ut_compare("stored i", i, node->i);
ut_compare("size check", q1.size(), 100 - i);
q1.pop();
ut_compare("size check", q1.size(), 100 - i - 1);
delete node;
ut_compare("size check", q1.size(), 100 - i - 1);
}
// verify list has been cleared
ut_assert_true(q1.empty());
ut_compare("size check", q1.size(), 0);
// pop an empty queue
auto T = q1.pop();
ut_assert_true(T == nullptr);
ut_assert_true(q1.empty());
ut_compare("size check", q1.size(), 0);
return true;
}
bool HysteresisTest::_zero_case()
{
// Default is 0 hysteresis.
systemlib::Hysteresis hysteresis(false);
ut_assert_false(hysteresis.get_state());
// Change and see result immediately.
hysteresis.set_state_and_update(true);
ut_assert_true(hysteresis.get_state());
hysteresis.set_state_and_update(false);
ut_assert_false(hysteresis.get_state());
hysteresis.set_state_and_update(true);
ut_assert_true(hysteresis.get_state());
// A wait won't change anything.
px4_usleep(1000 * f);
hysteresis.update();
ut_assert_true(hysteresis.get_state());
return true;
}
bool ParameterTest::SimpleFind()
{
param_t param = param_find("TEST_2");
ut_assert_true(PARAM_INVALID != param);
int32_t value;
int result = param_get(param, &value);
ut_compare("param_get did not return parameter", 0, result);
ut_compare("value of returned parameter does not match", 4, value);
return true;
}
bool IntrusiveQueueTest::test_push()
{
IntrusiveQueue<testContainer *> q1;
// size should be 0 initially
ut_compare("size initially 0", q1.size(), 0);
ut_assert_true(q1.empty());
// insert 100
for (int i = 0; i < 100; i++) {
testContainer *t = new testContainer();
t->i = i;
ut_compare("size increasing with i", q1.size(), i);
q1.push(t);
ut_compare("size increasing with i", q1.size(), i + 1);
ut_assert_true(!q1.empty());
}
// verify full size (100)
ut_compare("size 100", q1.size(), 100);
// pop all elements
for (int i = 0; i < 100; i++) {
auto node = q1.front();
q1.pop();
delete node;
}
// verify list has been cleared
ut_compare("size 0", q1.size(), 0);
ut_assert_true(q1.empty());
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"
matrix::Vector2f v_r = ControlMath::constrainXY(v0, v1, max);
ut_assert_true(fabsf(v_r(0)) - max < SIGMA_SINGLE_OP && v_r(0) > 0.0f);
ut_assert_true(fabsf(v_r(1) - 0.0f) < SIGMA_SINGLE_OP);
// v1 exceeds max but v0 is zero
v0.zero();
v_r = ControlMath::constrainXY(v0, v1, max);
ut_assert_true(fabsf(v_r(1)) - max < SIGMA_SINGLE_OP && v_r(1) < 0.0f);
ut_assert_true(fabsf(v_r(0) - 0.0f) < SIGMA_SINGLE_OP);
// v0 and v1 are below max
v0 = matrix::Vector2f(0.5f, 0.5f);
v1(0) = v0(1); v1(1) = -v0(0);
v_r = ControlMath::constrainXY(v0, v1, max);
float diff = matrix::Vector2f(v_r - (v0 + v1)).length();
ut_assert_true(diff < SIGMA_SINGLE_OP);
// 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) < SIGMA_SINGLE_OP && v_r(0) > 0.0f);
float remaining = sqrtf(max * max - (v0(0) * v0(0)));
ut_assert_true(fabsf(v_r(1)) - remaining < SIGMA_SINGLE_OP && v_r(1) < SIGMA_SINGLE_OP);
//TODO: add more tests with vectors not perpendicular
return true;
}
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 IntrusiveQueueTest::test_push_duplicate()
{
IntrusiveQueue<testContainer *> q1;
// size should be 0 initially
ut_compare("size initially 0", q1.size(), 0);
ut_assert_true(q1.empty());
// insert 100
for (int i = 0; i < 100; i++) {
testContainer *t = new testContainer();
t->i = i;
ut_compare("size increasing with i", q1.size(), i);
q1.push(t);
ut_compare("size increasing with i", q1.size(), i + 1);
ut_assert_true(!q1.empty());
}
// verify full size (100)
ut_compare("size 100", q1.size(), 100);
// attempt to insert front again
const auto q1_front = q1.front();
const auto q1_front_i = q1_front->i; // copy i value
const auto q1_back = q1.back();
const auto q1_back_i = q1_back->i; // copy i value
// push front and back aagain
q1.push(q1_front);
q1.push(q1_back);
// verify no change
ut_compare("size 100", q1.size(), 100);
ut_compare("q front not reinserted", q1.front()->i, q1_front->i);
ut_compare("q back not reinserted", q1.back()->i, q1_back->i);
// pop the head
const auto q1_head = q1.pop();
// verfify size should now be 99
ut_compare("size 99", q1.size(), 99);
// push back on
q1.push(q1_head);
// verify size now 100 again
ut_compare("size 100", q1.size(), 100);
// pop all elements
for (int i = 0; i < 100; i++) {
auto node = q1.front();
q1.pop();
delete node;
}
// verify list has been cleared
ut_compare("size 0", q1.size(), 0);
ut_assert_true(q1.empty());
return true;
}
bool McPosControlTests::cross_sphere_line_test(void)
{
MulticopterPositionControl *control = {};
math::Vector<3> prev = math::Vector<3>(0, 0, 0);
math::Vector<3> curr = math::Vector<3>(0, 0, 2);
math::Vector<3> 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(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);
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_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_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_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_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_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_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_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_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;
}
