diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 8e5f096c02..abbc16291f 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -88,4 +88,29 @@ void Subscription::unsubscribe() _last_generation = 0; } +bool Subscription::ChangeInstance(uint8_t instance) +{ + if (instance != _instance) { + DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); + + if (device_master != nullptr) { + if (!device_master->deviceNodeExists(_orb_id, _instance)) { + return false; + } + + // if desired new instance exists, unsubscribe from current + unsubscribe(); + _instance = instance; + subscribe(); + return true; + } + + } else { + // already on desired index + return true; + } + + return false; +} + } // namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 06c8fa45a1..c7ab1548f2 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -127,6 +127,12 @@ public: */ bool copy(void *dst) { return advertised() && _node->copy(dst, _last_generation); } + /** + * Change subscription instance + * @param instance The new multi-Subscription instance + */ + bool ChangeInstance(uint8_t instance); + uint8_t get_instance() const { return _instance; } unsigned get_last_generation() const { return _last_generation; } orb_id_t get_topic() const { return get_orb_meta(_orb_id); } diff --git a/src/modules/uORB/SubscriptionCallback.hpp b/src/modules/uORB/SubscriptionCallback.hpp index 5ed54c8ec2..63bb0771c7 100644 --- a/src/modules/uORB/SubscriptionCallback.hpp +++ b/src/modules/uORB/SubscriptionCallback.hpp @@ -98,6 +98,37 @@ public: _registered = false; } + /** + * Change subscription instance + * @param instance The new multi-Subscription instance + */ + bool ChangeInstance(uint8_t instance) + { + bool ret = false; + + if (instance != get_instance()) { + const bool registered = _registered; + + if (registered) { + unregisterCallback(); + } + + if (_subscription.ChangeInstance(instance)) { + ret = true; + } + + if (registered) { + registerCallback(); + } + + } else { + // already on desired index + return true; + } + + return ret; + } + virtual void call() = 0; protected: