int uORBTest::UnitTest::test_unadvertise()
{
test_note("Testing unadvertise");
//we still have the advertisements from the previous test_multi calls.
for (int i = 0; i < 4; ++i) {
int ret = orb_unadvertise(_pfd[i]);
if (ret != PX4_OK) {
return test_fail("orb_unadvertise failed (%i)", ret);
}
}
//try to advertise and see whether we get the right instance
int instance_test[4];
struct orb_test t;
for (int i = 0; i < 4; ++i) {
_pfd[i] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance_test[i], ORB_PRIO_MAX);
if (instance_test[i] != i) {
return test_fail("got wrong instance (should be %i, is %i)", i, instance_test[i]);
}
}
for (int i = 0; i < 4; ++i) {
orb_unadvertise(_pfd[i]);
}
return test_note("PASS unadvertise");
}
int uORBTest::UnitTest::test_multi2()
{
test_note("Testing multi-topic 2 test (queue simulation)");
//test: first subscribe, then advertise
_thread_should_exit = false;
const int num_instances = 3;
int orb_data_fd[num_instances];
int orb_data_next = 0;
for (int i = 0; i < num_instances; ++i) {
// PX4_WARN("subscribe %i, t=%" PRIu64, i, hrt_absolute_time());
orb_data_fd[i] = orb_subscribe_multi(ORB_ID(orb_test_medium_multi), i);
}
char *const args[1] = { NULL };
int pubsub_task = px4_task_spawn_cmd("uorb_test_multi",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
(px4_main_t)&uORBTest::UnitTest::pub_test_multi2_entry,
args);
if (pubsub_task < 0) {
return test_fail("failed launching task");
}
hrt_abstime last_time = 0;
while (!_thread_should_exit) {
bool updated = false;
int orb_data_cur_fd = orb_data_fd[orb_data_next];
orb_check(orb_data_cur_fd, &updated);
if (updated) {
struct orb_test_medium msg;
orb_copy(ORB_ID(orb_test_medium_multi), orb_data_cur_fd, &msg);
usleep(1000);
if (last_time >= msg.time && last_time != 0) {
return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.time);
}
last_time = msg.time;
// PX4_WARN(" got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.time);
orb_data_next = (orb_data_next + 1) % num_instances;
}
}
for (int i = 0; i < num_instances; ++i) {
orb_unsubscribe(orb_data_fd[i]);
}
return test_note("PASS multi-topic 2 test (queue simulation)");
}
int uORBTest::UnitTest::test_queue_poll_notify()
{
test_note("Testing orb queuing (poll & notify)");
struct orb_test_medium t;
int sfd;
if ((sfd = orb_subscribe(ORB_ID(orb_test_medium_queue_poll))) < 0) {
return test_fail("subscribe failed: %d", errno);
}
_thread_should_exit = false;
char *const args[1] = { nullptr };
int pubsub_task = px4_task_spawn_cmd("uorb_test_queue",
SCHED_DEFAULT,
SCHED_PRIORITY_MIN + 5,
1500,
(px4_main_t)&uORBTest::UnitTest::pub_test_queue_entry,
args);
if (pubsub_task < 0) {
return test_fail("failed launching task");
}
int next_expected_val = 0;
px4_pollfd_struct_t fds[1];
fds[0].fd = sfd;
fds[0].events = POLLIN;
while (!_thread_should_exit) {
int poll_ret = px4_poll(fds, 1, 500);
if (poll_ret == 0) {
if (_thread_should_exit) {
break;
}
return test_fail("poll timeout");
} else if (poll_ret < 0) {
return test_fail("poll error (%d, %d)", poll_ret, errno);
}
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(orb_test_medium_queue_poll), sfd, &t);
if (next_expected_val != t.val) {
return test_fail("copy mismatch: %d expected %d", t.val, next_expected_val);
}
++next_expected_val;
}
}
if (_num_messages_sent != next_expected_val) {
return test_fail("number of sent and received messages mismatch (sent: %i, received: %i)",
_num_messages_sent, next_expected_val);
}
return test_note("PASS orb queuing (poll & notify), got %i messages", next_expected_val);
}
int uORBTest::UnitTest::test_queue()
{
test_note("Testing orb queuing");
struct orb_test_medium t, u;
int sfd;
orb_advert_t ptopic;
bool updated;
sfd = orb_subscribe(ORB_ID(orb_test_medium_queue));
if (sfd < 0) {
return test_fail("subscribe failed: %d", errno);
}
const unsigned int queue_size = 11;
t.val = 0;
ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size);
if (ptopic == nullptr) {
return test_fail("advertise failed: %d", errno);
}
orb_check(sfd, &updated);
if (!updated) {
return test_fail("update flag not set");
}
if (PX4_OK != orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u)) {
return test_fail("copy(1) failed: %d", errno);
}
if (u.val != t.val) {
return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val);
}
orb_check(sfd, &updated);
if (updated) {
return test_fail("spurious updated flag");
}
#define CHECK_UPDATED(element) \
orb_check(sfd, &updated); \
if (!updated) { \
return test_fail("update flag not set, element %i", element); \
}
#define CHECK_NOT_UPDATED(element) \
orb_check(sfd, &updated); \
if (updated) { \
return test_fail("update flag set, element %i", element); \
}
#define CHECK_COPY(i_got, i_correct) \
orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u); \
if (i_got != i_correct) { \
return test_fail("got wrong element from the queue (got %i, should be %i)", i_got, i_correct); \
}
//no messages in the queue anymore
test_note(" Testing to write some elements...");
for (unsigned int i = 0; i < queue_size - 2; ++i) {
t.val = i;
orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
}
for (unsigned int i = 0; i < queue_size - 2; ++i) {
CHECK_UPDATED(i);
CHECK_COPY(u.val, i);
}
CHECK_NOT_UPDATED(queue_size);
test_note(" Testing overflow...");
int overflow_by = 3;
for (unsigned int i = 0; i < queue_size + overflow_by; ++i) {
t.val = i;
orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
}
for (unsigned int i = 0; i < queue_size; ++i) {
CHECK_UPDATED(i);
CHECK_COPY(u.val, i + overflow_by);
}
CHECK_NOT_UPDATED(queue_size);
test_note(" Testing underflow...");
for (unsigned int i = 0; i < queue_size; ++i) {
CHECK_NOT_UPDATED(i);
CHECK_COPY(u.val, queue_size + overflow_by - 1);
}
t.val = 943;
orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
CHECK_UPDATED(-1);
CHECK_COPY(u.val, t.val);
#undef CHECK_COPY
#undef CHECK_UPDATED
#undef CHECK_NOT_UPDATED
orb_unadvertise(ptopic);
return test_note("PASS orb queuing");
}
int uORBTest::UnitTest::test_multi_reversed()
{
test_note("try multi-topic support subscribing before publishing");
/* For these tests 0 and 1 instances are taken from before, therefore continue with 2 and 3. */
/* Subscribe first and advertise afterwards. */
int sfd2 = orb_subscribe_multi(ORB_ID(orb_multitest), 2);
if (sfd2 < 0) {
return test_fail("sub. id2: ret: %d", sfd2);
}
struct orb_test t {}, u {};
t.val = 0;
int instance2;
_pfd[2] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance2, ORB_PRIO_MAX);
int instance3;
_pfd[3] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance3, ORB_PRIO_MIN);
test_note("advertised");
if (instance2 != 2) {
return test_fail("mult. id2: %d", instance2);
}
if (instance3 != 3) {
return test_fail("mult. id3: %d", instance3);
}
t.val = 204;
if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[2], &t)) {
return test_fail("mult. pub0 fail");
}
t.val = 304;
if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[3], &t)) {
return test_fail("mult. pub1 fail");
}
test_note("published");
if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd2, &u)) {
return test_fail("sub #2 copy failed: %d", errno);
}
if (u.val != 204) {
return test_fail("sub #3 val. mismatch: %d", u.val);
}
int sfd3 = orb_subscribe_multi(ORB_ID(orb_multitest), 3);
if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd3, &u)) {
return test_fail("sub #3 copy failed: %d", errno);
}
if (u.val != 304) {
return test_fail("sub #3 val. mismatch: %d", u.val);
}
return test_note("PASS multi-topic reversed");
}
int uORBTest::UnitTest::test_multi()
{
/* this routine tests the multi-topic support */
test_note("try multi-topic support");
struct orb_test t {}, u {};
t.val = 0;
int instance0;
_pfd[0] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);
test_note("advertised");
int instance1;
_pfd[1] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
if (instance0 != 0) {
return test_fail("mult. id0: %d", instance0);
}
if (instance1 != 1) {
return test_fail("mult. id1: %d", instance1);
}
t.val = 103;
if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[0], &t)) {
return test_fail("mult. pub0 fail");
}
test_note("published");
t.val = 203;
if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[1], &t)) {
return test_fail("mult. pub1 fail");
}
/* subscribe to both topics and ensure valid data is received */
int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) {
return test_fail("sub #0 copy failed: %d", errno);
}
if (u.val != 103) {
return test_fail("sub #0 val. mismatch: %d", u.val);
}
int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);
if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) {
return test_fail("sub #1 copy failed: %d", errno);
}
if (u.val != 203) {
return test_fail("sub #1 val. mismatch: %d", u.val);
}
/* test priorities */
int prio;
if (PX4_OK != orb_priority(sfd0, &prio)) {
return test_fail("prio #0");
}
if (prio != ORB_PRIO_MAX) {
return test_fail("prio: %d", prio);
}
if (PX4_OK != orb_priority(sfd1, &prio)) {
return test_fail("prio #1");
}
if (prio != ORB_PRIO_MIN) {
return test_fail("prio: %d", prio);
}
if (PX4_OK != latency_test<struct orb_test>(ORB_ID(orb_test), false)) {
return test_fail("latency test failed");
}
orb_unsubscribe(sfd0);
orb_unsubscribe(sfd1);
return test_note("PASS multi-topic test");
}
int uORBTest::UnitTest::test_single()
{
test_note("try single-topic support");
struct orb_test t, u;
int sfd;
orb_advert_t ptopic;
bool updated;
t.val = 0;
ptopic = orb_advertise(ORB_ID(orb_test), &t);
if (ptopic == nullptr) {
return test_fail("advertise failed: %d", errno);
}
test_note("publish handle 0x%08x", ptopic);
sfd = orb_subscribe(ORB_ID(orb_test));
if (sfd < 0) {
return test_fail("subscribe failed: %d", errno);
}
test_note("subscribe fd %d", sfd);
u.val = 1;
if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) {
return test_fail("copy(1) failed: %d", errno);
}
if (u.val != t.val) {
return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val);
}
if (PX4_OK != orb_check(sfd, &updated)) {
return test_fail("check(1) failed");
}
if (updated) {
return test_fail("spurious updated flag");
}
t.val = 2;
test_note("try publish");
if (PX4_OK != orb_publish(ORB_ID(orb_test), ptopic, &t)) {
return test_fail("publish failed");
}
if (PX4_OK != orb_check(sfd, &updated)) {
return test_fail("check(2) failed");
}
if (!updated) {
return test_fail("missing updated flag");
}
if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) {
return test_fail("copy(2) failed: %d", errno);
}
if (u.val != t.val) {
return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val);
}
orb_unsubscribe(sfd);
int ret = orb_unadvertise(ptopic);
if (ret != PX4_OK) {
return test_fail("orb_unadvertise failed: %i", ret);
}
return test_note("PASS single-topic test");
}
bool test_task_random_RT_AAT_FAI(TaskManager& task_manager,
const Waypoints &waypoints,
const unsigned _num_points)
{
const Waypoint *wp;
OrderedTaskPoint *tp;
char tmp[255];
char tskType[20];
tskType[0] = '\0';
switch (rand() %3) {
case 0:
task_manager.SetFactory(TaskFactoryType::AAT);
strcpy(tskType,"AAT");
test_note("# creating random AAT task\n");
break;
case 1:
task_manager.SetFactory(TaskFactoryType::RACING);
strcpy(tskType,"RT");
test_note("# creating random RT task\n");
break;
case 2:
task_manager.SetFactory(TaskFactoryType::FAI_GENERAL);
strcpy(tskType,"FAI");
test_note("# creating random FAI GENERAL\n");
break;
}
AbstractTaskFactory &fact = task_manager.GetFactory();
//max points includes start & finish
const TaskFactoryConstraints &constraints =
task_manager.GetOrderedTask().GetFactoryConstraints();
const unsigned num_points_total =
std::max(constraints.min_points,
_num_points % constraints.max_points) + 1;
const unsigned num_int_points = num_points_total - 2;
test_note("# adding start\n");
wp = random_waypoint(waypoints);
if (wp) {
const TaskPointFactoryType s = GetRandomType(fact.GetStartTypes());
tp = fact.CreateStart(s,*wp);
if (!fact.Append(*tp,false)) {
return false;
}
delete tp;
}
for (unsigned i=0; i<num_int_points; i++) {
test_note("# adding intermediate\n");
wp = random_waypoint(waypoints);
if (wp) {
const TaskPointFactoryType s = GetRandomType(fact.GetIntermediateTypes());
tp = fact.CreateIntermediate(s,*wp);
if (!fact.Append(*tp,false)) {
return false;
}
delete tp;
}
}
test_note("# adding finish\n");
wp = random_waypoint(waypoints);
if (wp) {
const TaskPointFactoryType s = GetRandomType(fact.GetFinishTypes());
tp = fact.CreateFinish(s,*wp);
if (!fact.Append(*tp,false)) {
return false;
}
delete tp;
}
fact.UpdateStatsGeometry();
test_note("# validating task..\n");
if (!fact.Validate()) {
return false;
}
if (task_manager.GetOrderedTask().GetFactoryType()
== TaskFactoryType::FAI_GENERAL)
{
test_note("# checking OZs for FAI General..\n");
if (!fact.ValidateFAIOZs())
return false;
}
if (task_manager.GetOrderedTask().GetFactoryType()
== TaskFactoryType::MAT)
{
test_note("# checking OZs for MAT General..\n");
if (!fact.ValidateMATOZs())
return false;
}
task_manager.Resume();
sprintf(tmp, "# SUCCESS CREATING %s task! task_size():%d..\n",
tskType,
task_manager.TaskSize());
test_note(tmp);
return true;
}
bool test_task_type_manip(TaskManager& task_manager,
const Waypoints &waypoints, unsigned n_points)
{
if (!test_task_random_RT_AAT_FAI(task_manager, waypoints, n_points))
return false;
switch (rand() %4) {
case 0:
task_manager.SetFactory(TaskFactoryType::AAT);
test_note("# switched FACTORY TYPE to AAT\n");
break;
case 1:
task_manager.SetFactory(TaskFactoryType::RACING);
test_note("# switched FACTORY TYPE to RT\n");
break;
case 2:
task_manager.SetFactory(TaskFactoryType::FAI_GENERAL);
test_note("# switched FACTORY TYPE to FAI GENERAL\n");
break;
case 3:
task_manager.SetFactory(TaskFactoryType::MAT);
test_note("# switched FACTORY TYPE to MAT\n");
break;
default:
test_note("# unknown task type\n");
}
AbstractTaskFactory &fact = task_manager.GetFactory();
fact.MutateTPsToTaskType();
fact.UpdateStatsGeometry();
test_note("# checking mutated start..\n");
if (!fact.IsValidStartType(fact.GetType(task_manager.GetOrderedTask().GetTaskPoint(0))))
return false;
char tmp[255];
sprintf(tmp, "# checking mutated intermediates. task_size():%d..\n",
task_manager.TaskSize());
test_note(tmp);
for (unsigned i = 1; i < (task_manager.TaskSize() - 1); i++) {
sprintf(tmp, "# checking mutated intermediate point %d..\n", i);
test_note(tmp);
if (!fact.IsValidIntermediateType(fact.GetType(task_manager.GetOrderedTask().GetTaskPoint(i))))
return false;
}
test_note("# checking mutated finish..\n");
if (!fact.IsValidFinishType(
fact.GetType(task_manager.GetOrderedTask().GetTaskPoint(task_manager.TaskSize() - 1))))
return false;
test_note("# validating task..\n");
if (!fact.Validate()) {
return false;
}
test_note("# checking task..\n");
if (!task_manager.CheckOrderedTask()) {
return false;
}
if (task_manager.GetOrderedTask().GetFactoryType() ==
TaskFactoryType::FAI_GENERAL) {
test_note("# checking OZs for FAI task..\n");
if (!fact.ValidateFAIOZs())
return false;
}
if (task_manager.GetOrderedTask().GetFactoryType() ==
TaskFactoryType::MAT) {
test_note("# checking OZs for MAT task..\n");
if (!fact.ValidateMATOZs())
return false;
}
return true;
}
bool test_task_random_RT_AAT_FAI(TaskManager& task_manager,
const Waypoints &waypoints,
const unsigned _num_points)
{
const Waypoint *wp;
OrderedTaskPoint *tp;
AbstractTaskFactory &fact = task_manager.get_factory();
char tmp[255];
char tskType[20];
tskType[0] = '\0';
switch (rand() %3) {
case 0:
task_manager.set_factory(TaskBehaviour::FACTORY_AAT);
strcpy(tskType,"AAT");
test_note("# creating random AAT task\n");
break;
case 1:
task_manager.set_factory(TaskBehaviour::FACTORY_RT);
strcpy(tskType,"RT");
test_note("# creating random RT task\n");
break;
case 2:
task_manager.set_factory(TaskBehaviour::FACTORY_FAI_GENERAL);
strcpy(tskType,"FAI");
test_note("# creating random FAI GENERAL\n");
break;
}
//max points includes start & finish
const unsigned num_points_total = (
max(task_manager.get_ordered_task_behaviour().min_points,
(_num_points % task_manager.get_ordered_task_behaviour().max_points) + 1));
const unsigned num_int_points = num_points_total - 2;
test_note("# adding start\n");
wp = random_waypoint(waypoints);
if (wp) {
AbstractTaskFactory::LegalPointType_t s =
fact.getStartTypes()[(rand() % fact.getStartTypes().size())];
tp = fact.createStart(s,*wp);
if (!fact.append(*tp,false)) {
return false;
}
delete tp;
}
for (unsigned i=0; i<num_int_points; i++) {
test_note("# adding intermediate\n");
wp = random_waypoint(waypoints);
if (wp) {
AbstractTaskFactory::LegalPointType_t s =
fact.getIntermediateTypes()[(rand() % fact.getIntermediateTypes().size())];
tp = fact.createIntermediate(s,*wp);
if (!fact.append(*tp,false)) {
return false;
}
delete tp;
}
}
test_note("# adding finish\n");
wp = random_waypoint(waypoints);
if (wp) {
AbstractTaskFactory::LegalPointType_t s =
fact.getFinishTypes()[(rand() % fact.getFinishTypes().size())];
tp = fact.createFinish(s,*wp);
if (!fact.append(*tp,false)) {
return false;
}
delete tp;
}
test_note("# validating task..\n");
if (!fact.validate()) {
return false;
}
if (task_manager.get_ordered_task().get_factory_type()
== TaskBehaviour::FACTORY_FAI_GENERAL)
{
test_note("# checking OZs for FAI General..\n");
if (!fact.validateFAIOZs())
return false;
}
task_manager.resume();
sprintf(tmp, "# SUCCESS CREATING %s task! task_size():%d..\n",
tskType,
task_manager.task_size());
test_note(tmp);
return true;
}
bool test_task_type_manip(TaskManager& task_manager,
const Waypoints &waypoints, unsigned n_points)
{
if (!test_task_random_RT_AAT_FAI(task_manager, waypoints, n_points))
return false;
AbstractTaskFactory &fact = task_manager.get_factory();
switch (rand() %3) {
case 0:
task_manager.set_factory(TaskBehaviour::FACTORY_AAT);
test_note("# switched FACTORY TYPE to AAT\n");
break;
case 1:
task_manager.set_factory(TaskBehaviour::FACTORY_RT);
test_note("# switched FACTORY TYPE to RT\n");
break;
case 2:
task_manager.set_factory(TaskBehaviour::FACTORY_FAI_GENERAL);
test_note("# switched FACTORY TYPE to FAI GENERAL\n");
break;
default:
test_note("# unknown task type\n");
}
fact.mutate_tps_to_task_type();
test_note("# checking mutated start..\n");
if (!fact.validStartType(
fact.getType(*task_manager.get_ordered_task().getTaskPoint(0))))
return false;
char tmp[255];
sprintf(tmp, "# checking mutated intermediates. task_size():%d..\n",
task_manager.task_size());
test_note(tmp);
for (unsigned i = 1; i < (task_manager.task_size() - 1); i++) {
sprintf(tmp, "# checking mutated intermediate point %d..\n", i);
test_note(tmp);
if (!fact.validIntermediateType(
fact.getType(*task_manager.get_ordered_task().getTaskPoint(i))))
return false;
}
test_note("# checking mutated finish..\n");
if (!fact.validFinishType(
fact.getType(*task_manager.get_ordered_task().getTaskPoint(
task_manager.task_size() - 1))))
return false;
test_note("# validating task..\n");
if (!fact.validate()) {
return false;
}
test_note("# checking task..\n");
if (!task_manager.check_ordered_task()) {
return false;
}
if (task_manager.get_ordered_task().get_factory_type() ==
TaskBehaviour::FACTORY_FAI_GENERAL) {
test_note("# checking OZs for FAI task..\n");
if (!fact.validateFAIOZs())
return false;
}
return true;
}