bool ParameterTest::_assert_parameter_float_value(param_t param, float expected)
{
float value;
int result = param_get(param, &value);
ut_compare("param_get did not return parameter", 0, result);
ut_compare_float("value for param doesn't match default value", expected, value, 0.001);
return true;
}
bool ParameterTest::exportImport()
{
static constexpr float MAGIC_FLOAT_VAL = 0.314159f;
bool ret = true;
param_t test_params[] = {p0, p1, p2, p3, p4};
// set all params to corresponding param_t value
for (auto p : test_params) {
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = p;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = (float)p + MAGIC_FLOAT_VAL;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
}
}
// save
if (param_save_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
return false;
}
// zero all params and verify, but don't save
for (auto p : test_params) {
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = 0;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = -1;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = 0.0f;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = -1.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare_float("value for param doesn't match default value", set_val, get_val, 0.001f);
}
}
// load saved params
if (param_load_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
ret = true;
}
// check every param
for (auto p : test_params) {
if (param_type(p) == PARAM_TYPE_INT32) {
int32_t get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare_float("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL, 0.001f);
}
}
return ret;
}
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;
}
