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; }