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::update(uint64_t *time, void *data)
{
// TODO this is NOT atomic operation, we can get data newer than time
// if topic was published between orb_stat and orb_copy calls.
uint64_t time_topic;
if (orb_stat(_fd, &time_topic)) {
/* error getting last topic publication time */
time_topic = 0;
}
if (update(data)) {
/* data copied successfully */
if (time_topic == 0 || (time_topic != *time)) {
*time = time_topic;
return true;
} else {
return false;
}
}
return false;
}
bool
MavlinkOrbSubscription::update(uint64_t *time, void* data)
{
// TODO this is NOT atomic operation, we can get data newer than time
// if topic was published between orb_stat and orb_copy calls.
uint64_t time_topic;
if (orb_stat(_fd, &time_topic)) {
/* error getting last topic publication time */
time_topic = 0;
}
if (orb_copy(_topic, _fd, data)) {
if (data != nullptr) {
/* error copying topic data */
memset(data, 0, _topic->o_size);
}
return false;
} else {
/* data copied successfully */
_published = true;
if (time_topic != *time) {
*time = time_topic;
return true;
} else {
return false;
}
}
}
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;
}
