bool MicroBenchORB::time_px4_uorb()
{
int fd_status = orb_subscribe(ORB_ID(vehicle_status));
int fd_lpos = orb_subscribe(ORB_ID(vehicle_local_position));
int fd_gyro = orb_subscribe(ORB_ID(sensor_gyro));
int ret = 0;
bool updated = false;
uint64_t time = 0;
PERF("orb_check vehicle_status", ret = orb_check(fd_status, &updated), 1000);
PERF("orb_stat vehicle_status", ret = orb_stat(fd_status, &time), 1000);
PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status), fd_status, &status), 1000);
PERF("orb_check vehicle_local_position", ret = orb_check(fd_lpos, &updated), 1000);
PERF("orb_stat vehicle_local_position", ret = orb_stat(fd_lpos, &time), 1000);
PERF("orb_copy vehicle_local_position", ret = orb_copy(ORB_ID(vehicle_local_position), fd_lpos, &lpos), 1000);
PERF("orb_check sensor_gyro", ret = orb_check(fd_gyro, &updated), 1000);
PERF("orb_stat sensor_gyro", ret = orb_stat(fd_gyro, &time), 1000);
PERF("orb_copy sensor_gyro", ret = orb_copy(ORB_ID(sensor_gyro), fd_gyro, &lpos), 1000);
PERF("orb_exists sensor_accel 0", ret = orb_exists(ORB_ID(sensor_accel), 0), 100);
PERF("orb_exists sensor_accel 1", ret = orb_exists(ORB_ID(sensor_accel), 1), 100);
PERF("orb_exists sensor_accel 2", ret = orb_exists(ORB_ID(sensor_accel), 2), 100);
PERF("orb_exists sensor_accel 3", ret = orb_exists(ORB_ID(sensor_accel), 3), 100);
PERF("orb_exists sensor_accel 4", ret = orb_exists(ORB_ID(sensor_accel), 4), 100);
orb_unsubscribe(fd_status);
orb_unsubscribe(fd_lpos);
orb_unsubscribe(fd_gyro);
return true;
}
bool
MavlinkOrbSubscription::is_published()
{
// If we marked it as published no need to check again
if (_published) {
return true;
}
// Telemetry can sustain an initial published check at 10 Hz
hrt_abstime now = hrt_absolute_time();
if (now - _last_pub_check < 100000) {
return false;
}
// We are checking now
_last_pub_check = now;
// If it does not exist its not published
if (orb_exists(_topic, _instance)) {
return false;
} else if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
bool updated;
orb_check(_fd, &updated);
if (updated) {
_published = true;
}
return _published;
}
void listener(const orb_id_t &id, unsigned num_msgs, unsigned topic_instance, unsigned topic_interval)
{
if (orb_exists(id, topic_instance) != 0) {
printf("never published\n");
return;
}
int sub = orb_subscribe_multi(id, topic_instance);
orb_set_interval(sub, topic_interval);
bool updated = false;
unsigned i = 0;
hrt_abstime start_time = hrt_absolute_time();
while (i < num_msgs) {
orb_check(sub, &updated);
if (i == 0) {
updated = true;
} else {
usleep(500);
}
if (updated) {
start_time = hrt_absolute_time();
i++;
printf("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, i);
T container;
if (orb_copy(id, sub, &container) == PX4_OK) {
print_message(container);
} else {
PX4_ERR("orb_copy failed");
}
} else {
if (hrt_elapsed_time(&start_time) > 2 * 1000 * 1000) {
printf("Waited for 2 seconds without a message. Giving up.\n");
break;
}
}
}
orb_unsubscribe(sub);
}
bool
MavlinkOrbSubscription::is_published()
{
// If we marked it as published no need to check again
if (_published) {
return true;
}
// Telemetry can sustain an initial published check at 10 Hz
hrt_abstime now = hrt_absolute_time();
if (now - _last_pub_check < 100000) {
return false;
}
// We are checking now
_last_pub_check = now;
// We don't want to subscribe to anything that does not exist
// in order to save memory and file descriptors.
// However, for some topics like vehicle_command_ack, we want to subscribe
// from the beginning in order not to miss the first publish respective advertise.
if (!_subscribe_from_beginning && orb_exists(_topic, _instance)) {
return false;
}
if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
bool updated;
orb_check(_fd, &updated);
if (updated) {
_published = true;
}
// topic may have been last published before we subscribed
uint64_t time_topic = 0;
if (!_published && orb_stat(_fd, &time_topic) == PX4_OK) {
if (time_topic != 0) {
_published = true;
}
}
return _published;
}
bool
MavlinkOrbSubscription::is_published()
{
// If we marked it as published no need to check again
if (_published) {
return true;
}
// Telemetry can sustain an initial published check at 10 Hz
hrt_abstime now = hrt_absolute_time();
if (now - _last_pub_check < 100000) {
return false;
}
// We are checking now
_last_pub_check = now;
#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE)
// Snapdragon has currently no support for orb_exists, therefore
// we're not using it.
if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
#else
// We don't want to subscribe to anything that does not exist
// in order to save memory and file descriptors.
// However, for some topics like vehicle_command_ack, we want to subscribe
// from the beginning in order not to miss the first publish respective advertise.
if (!_subscribe_from_beginning && orb_exists(_topic, _instance)) {
return false;
}
if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
#endif
bool updated;
orb_check(_fd, &updated);
if (updated) {
_published = true;
}
return _published;
}
bool
MavlinkOrbSubscription::is_published()
{
// If we marked it as published no need to check again
if (_published) {
return true;
}
hrt_abstime now = hrt_absolute_time();
if (now - _last_pub_check < 300000) {
return false;
}
// We are checking now
_last_pub_check = now;
// We don't want to subscribe to anything that does not exist
// in order to save memory and file descriptors.
// However, for some topics like vehicle_command_ack, we want to subscribe
// from the beginning in order not to miss or delay the first publish respective advertise.
if (!_subscribe_from_beginning && orb_exists(_topic, _instance)) {
return false;
}
if (_fd < 0) {
_fd = orb_subscribe_multi(_topic, _instance);
}
bool updated;
orb_check(_fd, &updated);
if (updated) {
_published = true;
}
return _published;
}
