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