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