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::_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::_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_false()
{
systemlib::Hysteresis hysteresis(false);
ut_assert_false(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 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 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;
}
