diff --git a/platforms/common/uORB/Subscription.cpp b/platforms/common/uORB/Subscription.cpp index 9ca5a8f9448..837feed5c78 100644 --- a/platforms/common/uORB/Subscription.cpp +++ b/platforms/common/uORB/Subscription.cpp @@ -55,7 +55,7 @@ bool Subscription::subscribe() if (node) { _node = node; - _last_generation.store(initial_generation); + _last_generation = initial_generation; return true; } } @@ -70,15 +70,15 @@ void Subscription::unsubscribe() } _node = nullptr; - _last_generation.store(0); + _last_generation = 0; } bool Subscription::update(void *dst) { if (subscribe()) { - unsigned gen = _last_generation.load(); + unsigned gen = _last_generation; bool ret = Manager::orb_data_copy(_node, dst, gen, true); - _last_generation.store(gen); + _last_generation = gen; return ret; } @@ -88,9 +88,9 @@ bool Subscription::update(void *dst) bool Subscription::copy(void *dst) { if (subscribe()) { - unsigned gen = _last_generation.load(); + unsigned gen = _last_generation; bool ret = Manager::orb_data_copy(_node, dst, gen, false); - _last_generation.store(gen); + _last_generation = gen; return ret; } @@ -172,7 +172,7 @@ bool Subscription::advertised() bool Subscription::updated() { if (subscribe()) { - return Manager::updates_available(_node, _last_generation.load()); + return Manager::updates_available(_node, _last_generation); } return false; diff --git a/platforms/common/uORB/Subscription.hpp b/platforms/common/uORB/Subscription.hpp index a40ba3a93e6..41a863e6f7c 100644 --- a/platforms/common/uORB/Subscription.hpp +++ b/platforms/common/uORB/Subscription.hpp @@ -118,7 +118,7 @@ public: bool ChangeInstance(uint8_t instance); uint8_t get_instance() const { return _instance; } - unsigned get_last_generation() const { return _last_generation.load(); } + unsigned get_last_generation() const { return _last_generation; } orb_id_t get_topic() const { return get_orb_meta(_orb_id); } ORB_ID orb_id() const { return _orb_id; } @@ -132,7 +132,7 @@ protected: void *_node{nullptr}; - px4::atomic _last_generation{0}; /**< last generation the subscriber has seen */ + unsigned _last_generation{0}; /**< last generation the subscriber has seen */ ORB_ID _orb_id{ORB_ID::INVALID}; uint8_t _instance{0}; diff --git a/platforms/common/uORB/SubscriptionBlocking.hpp b/platforms/common/uORB/SubscriptionBlocking.hpp index 0df895bc01f..abe6d7442c2 100644 --- a/platforms/common/uORB/SubscriptionBlocking.hpp +++ b/platforms/common/uORB/SubscriptionBlocking.hpp @@ -64,7 +64,7 @@ public: pthread_cond_destroy(&_cv); } - void call() override + void call(unsigned generation) override { // signal immediately if no interval, otherwise only if interval has elapsed hrt_abstime last_update = _last_update.load(); diff --git a/platforms/common/uORB/SubscriptionCallback.hpp b/platforms/common/uORB/SubscriptionCallback.hpp index f3e2b172755..ce0e809831c 100644 --- a/platforms/common/uORB/SubscriptionCallback.hpp +++ b/platforms/common/uORB/SubscriptionCallback.hpp @@ -41,6 +41,7 @@ #include #include #include +#include namespace uORB { @@ -101,14 +102,19 @@ public: return ret; } - virtual void call() = 0; + virtual void call(unsigned generation) = 0; bool registered() const { return _registered; } protected: bool _registered{false}; - + // Node generation at our last ScheduleNow(), used by the count gate. Only ever + // touched from call() (which runs under the publishing node's lock, so it is + // serialized) - do NOT write it from the subscriber thread (e.g. in + // registerCallback()), or it becomes a cross-thread race. It self-initializes: + // the first call() sees a large delta and schedules once, then tracks normally. + unsigned _last_scheduled_generation{0}; }; // Subscription with callback that schedules a WorkItem @@ -130,14 +136,20 @@ public: virtual ~SubscriptionCallbackWorkItem() = default; - void call() override + void call(unsigned generation) override { - // schedule immediately if updated (queue depth or subscription interval) - uint8_t req = _required_updates.load(); + // 'generation' is the publishing node's generation, handed in by the + // publisher - so unlike before we never read the subscriber's own cursor + // (_last_generation, mutated on the subscriber's thread) from here. + // Schedule once enough new samples have accrued since our last schedule + // (count gate), respecting the optional interval (interval gate). + const uint8_t req = _required_updates.load(); - if ((req == 0) - || (Manager::updates_available(_subscription.get_node(), _subscription.get_last_generation()) >= req)) { - if (updated()) { + if ((generation - _last_scheduled_generation) >= req) { + const hrt_abstime last_update = _last_update.load(); + + if ((_interval_us == 0) || (hrt_elapsed_time(&last_update) >= _interval_us)) { + _last_scheduled_generation = generation; _work_item->ScheduleNow(); } } diff --git a/platforms/common/uORB/uORBDeviceNode.cpp b/platforms/common/uORB/uORBDeviceNode.cpp index 306c6fba668..d92612f2b40 100644 --- a/platforms/common/uORB/uORBDeviceNode.cpp +++ b/platforms/common/uORB/uORBDeviceNode.cpp @@ -196,7 +196,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen) // callbacks for (auto item : _callbacks) { - item->call(); + item->call(generation + 1); } /* Mark at least one data has been published */ diff --git a/src/lib/drivers/mcp_common/CallbackHandler.hpp b/src/lib/drivers/mcp_common/CallbackHandler.hpp index f0aa770cec4..b5b6caa141d 100644 --- a/src/lib/drivers/mcp_common/CallbackHandler.hpp +++ b/src/lib/drivers/mcp_common/CallbackHandler.hpp @@ -45,7 +45,7 @@ public: CallbackHandler(orb_id_t id) : uORB::SubscriptionCallback(id) {} virtual ~CallbackHandler() {} - void call() override + void call(unsigned generation) override { px4::msg::GpioIn new_input;