From 8f5b7de49874cd750477d784b6bce3f7e7e9fbf5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 6 Aug 2019 10:28:49 -0400 Subject: [PATCH] uORB::Subscription minor API cleanup * the forceInit() method was combined with the existing subscribe() * delete unused last_update() --- src/modules/commander/Commander.cpp | 2 +- .../mavlink/mavlink_orb_subscription.cpp | 2 +- src/modules/uORB/Subscription.cpp | 59 +++++++++++-------- src/modules/uORB/Subscription.hpp | 18 +++--- src/modules/uORB/SubscriptionCallback.hpp | 4 +- src/modules/uORB/SubscriptionInterval.hpp | 21 +++++-- src/modules/uORB/uORBDeviceMaster.cpp | 4 ++ src/modules/uORB/uORBDeviceNode.cpp | 12 ---- src/modules/uORB/uORBDeviceNode.hpp | 8 --- src/systemcmds/tests/test_microbench_uorb.cpp | 3 - 10 files changed, 69 insertions(+), 64 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 3bbd2e5acc..8e24ce1c49 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -4290,7 +4290,7 @@ Commander::offboard_control_update(bool &status_changed) if (commander_state_s::MAIN_STATE_OFFBOARD) { if (offboard_control_mode.timestamp == 0) { - _offboard_control_mode_sub.forceInit(); + _offboard_control_mode_sub.subscribe(); force_update = true; } } diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index d2ac9d2c5a..3173accaa3 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -50,7 +50,7 @@ MavlinkOrbSubscription::is_published() } else if (!published && _subscribe_from_beginning) { // 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. - return _sub.forceInit(); + return _sub.subscribe(); } return false; diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index bc4987c895..9268a21ad9 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -42,36 +42,59 @@ namespace uORB { -bool Subscription::subscribe() +bool +Subscription::subscribe() { - DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); - _node = device_master->getDeviceNode(_meta, _instance); + // valid ORB_ID required + if (_meta == nullptr) { + return false; + } + // check if already subscribed if (_node != nullptr) { - _node->add_internal_subscriber(); + return true; + } + + DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); - // If there were any previous publications, allow the subscriber to read them - const unsigned curr_gen = _node->published_message_count(); - const unsigned q_size = _node->get_queue_size(); + if (device_master != nullptr) { + uORB::DeviceNode *node = device_master->getDeviceNode(_meta, _instance); - _last_generation = curr_gen - (q_size < curr_gen ? q_size : curr_gen); + if (node != nullptr) { + _node = node; + _node->add_internal_subscriber(); - return true; + // If there were any previous publications, allow the subscriber to read them + const unsigned curr_gen = _node->published_message_count(); + const uint8_t q_size = _node->get_queue_size(); + + if (q_size < curr_gen) { + _last_generation = curr_gen - q_size; + + } else { + _last_generation = 0; + } + + return true; + } } return false; } -void Subscription::unsubscribe() +void +Subscription::unsubscribe() { if (_node != nullptr) { _node->remove_internal_subscriber(); } + _node = nullptr; _last_generation = 0; } -bool Subscription::init() +bool +Subscription::init() { if (_meta != nullptr) { // this throttles the relatively expensive calls to getDeviceNode() @@ -90,18 +113,8 @@ bool Subscription::init() return false; } -bool Subscription::forceInit() -{ - if (_node == nullptr) { - // reset generation to force subscription attempt - _last_generation = 0; - return subscribe(); - } - - return false; -} - -bool Subscription::update(uint64_t *time, void *dst) +bool +Subscription::update(uint64_t *time, void *dst) { if ((time != nullptr) && (dst != nullptr) && published()) { // always copy data to dst regardless of update diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 6c98d38718..2f3a26b379 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -63,13 +63,16 @@ public: */ Subscription(const orb_metadata *meta, uint8_t instance = 0) : _meta(meta), _instance(instance) { - init(); + subscribe(); } - ~Subscription() { unsubscribe(); } + ~Subscription() + { + unsubscribe(); + } - bool init(); - bool forceInit(); + bool subscribe(); + void unsubscribe(); bool valid() const { return _node != nullptr; } bool published() { return valid() ? _node->is_published() : init(); } @@ -102,8 +105,6 @@ public: */ bool copy(void *dst) { return published() ? _node->copy(dst, _last_generation) : false; } - hrt_abstime last_update() { return published() ? _node->last_update() : 0; } - uint8_t get_instance() const { return _instance; } orb_id_t get_topic() const { return _meta; } @@ -111,10 +112,9 @@ protected: friend class SubscriptionCallback; - DeviceNode *get_node() { return _node; } + DeviceNode *get_node() { return _node; } - bool subscribe(); - void unsubscribe(); + bool init(); DeviceNode *_node{nullptr}; const orb_metadata *_meta{nullptr}; diff --git a/src/modules/uORB/SubscriptionCallback.hpp b/src/modules/uORB/SubscriptionCallback.hpp index 01722bc681..d282b92a5e 100644 --- a/src/modules/uORB/SubscriptionCallback.hpp +++ b/src/modules/uORB/SubscriptionCallback.hpp @@ -78,7 +78,7 @@ public: int fd = orb_subscribe_multi(_subscription.get_topic(), _subscription.get_instance()); // try to register callback again - if (_subscription.forceInit()) { + if (_subscription.subscribe()) { if (_subscription.get_node() && _subscription.get_node()->register_callback(this)) { ret = true; } @@ -124,7 +124,7 @@ public: void call() override { // schedule immediately if no interval, otherwise check time elapsed - if ((_interval == 0) || (hrt_elapsed_time(&_last_update) >= _interval)) { + if ((_interval_us == 0) || (hrt_elapsed_time(&_last_update) >= _interval_us)) { _work_item->ScheduleNow(); } } diff --git a/src/modules/uORB/SubscriptionInterval.hpp b/src/modules/uORB/SubscriptionInterval.hpp index 253c0570d2..5c701eb986 100644 --- a/src/modules/uORB/SubscriptionInterval.hpp +++ b/src/modules/uORB/SubscriptionInterval.hpp @@ -64,13 +64,15 @@ public: */ SubscriptionInterval(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) : _subscription{meta, instance}, - _interval(interval_us) + _interval_us(interval_us) {} SubscriptionInterval() : _subscription{nullptr} {} ~SubscriptionInterval() = default; + bool subscribe() { return _subscription.subscribe(); } + bool published() { return _subscription.published(); } /** @@ -78,7 +80,7 @@ public: * */ bool updated() { - if (published() && (hrt_elapsed_time(&_last_update) >= _interval)) { + if (published() && (hrt_elapsed_time(&_last_update) >= _interval_us)) { return _subscription.updated(); } @@ -118,15 +120,24 @@ public: uint8_t get_instance() const { return _subscription.get_instance(); } orb_id_t get_topic() const { return _subscription.get_topic(); } - uint32_t get_interval() const { return _interval; } - void set_interval(uint32_t interval) { _interval = interval; } + /** + * Set the interval in microseconds + * @param interval The interval in microseconds. + */ + void set_interval_us(uint32_t interval) { _interval_us = interval; } + + /** + * Set the interval in milliseconds + * @param interval The interval in milliseconds. + */ + void set_interval_ms(uint32_t interval) { _interval_us = interval * 1000; } protected: Subscription _subscription; uint64_t _last_update{0}; // last update in microseconds - uint32_t _interval{0}; // maximum update interval in microseconds + uint32_t _interval_us{0}; // maximum update interval in microseconds }; diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index 6ccdc4d016..278f0f7255 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/src/modules/uORB/uORBDeviceMaster.cpp @@ -430,6 +430,10 @@ uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath) uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const struct orb_metadata *meta, const uint8_t instance) { + if (meta == nullptr) { + return nullptr; + } + lock(); uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance); unlock(); diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index d6aad04398..c2f2c51d90 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -220,18 +220,6 @@ uORB::DeviceNode::copy_and_get_timestamp(void *dst, unsigned &generation) return update_time; } -hrt_abstime -uORB::DeviceNode::last_update() -{ - ATOMIC_ENTER; - - const hrt_abstime update_time = _last_update; - - ATOMIC_LEAVE; - - return update_time; -} - ssize_t uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen) { diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index ef0a7b08db..cad2672dd8 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -196,14 +196,6 @@ public: int get_priority() const { return _priority; } void set_priority(uint8_t priority) { _priority = priority; } - /** - * Copies the timestamp of the last update atomically. - * - * @return uint64_t - * Returns the timestamp of the most recent data. - */ - hrt_abstime last_update(); - /** * Copies data and the corresponding generation * from a node to the buffer provided. diff --git a/src/systemcmds/tests/test_microbench_uorb.cpp b/src/systemcmds/tests/test_microbench_uorb.cpp index 8d324347e5..47daa3bc72 100644 --- a/src/systemcmds/tests/test_microbench_uorb.cpp +++ b/src/systemcmds/tests/test_microbench_uorb.cpp @@ -188,21 +188,18 @@ bool MicroBenchORB::time_px4_uorb_direct() uORB::Subscription vstatus{ORB_ID(vehicle_status)}; PERF("uORB::Subscription orb_check vehicle_status", ret = vstatus.updated(), 1000); - PERF("uORB::Subscription orb_stat vehicle_status", time = vstatus.last_update(), 1000); PERF("uORB::Subscription orb_copy vehicle_status", ret = vstatus.copy(&status), 1000); printf("\n"); uORB::Subscription local_pos{ORB_ID(vehicle_local_position)}; PERF("uORB::Subscription orb_check vehicle_local_position", ret = local_pos.updated(), 1000); - PERF("uORB::Subscription orb_stat vehicle_local_position", time = local_pos.last_update(), 1000); PERF("uORB::Subscription orb_copy vehicle_local_position", ret = local_pos.copy(&lpos), 1000); printf("\n"); uORB::Subscription sens_gyro{ORB_ID(sensor_gyro)}; PERF("uORB::Subscription orb_check sensor_gyro", ret = sens_gyro.updated(), 1000); - PERF("uORB::Subscription orb_stat sensor_gyro", time = sens_gyro.last_update(), 1000); PERF("uORB::Subscription orb_copy sensor_gyro", ret = sens_gyro.copy(&gyro), 1000); return true;