canard: implement a lightweight way to share mutex between same messa…

…ge type subscribers
dronecan · Dec 27, 2023 · 7d4cac5 · 7d4cac5
1 parent 50dcf28
commit 7d4cac5
Showing 1 changed file with 22 additions and 7 deletions.
diff --git a/canard/subscriber.h b/canard/subscriber.h
@@ -41,8 +41,17 @@ class Subscriber : public HandlerList {
     Subscriber(Callback<msgtype> &_cb, uint8_t _index) :
     HandlerList(CanardTransferTypeBroadcast, msgtype::cxx_iface::ID, msgtype::cxx_iface::SIGNATURE, _index),
     cb (_cb) {
+        if (branch_head[index] == nullptr) {
 #ifdef CANARD_MUTEX_ENABLED
-        WITH_SEMAPHORE(sem[index]);
+            sem = allocate<Canard::Semaphore>(); // allocate a semaphore
+#endif
+        } else {
+#ifdef CANARD_MUTEX_ENABLED
+            sem = branch_head[index]->sem;
+#endif
+        }
+#ifdef CANARD_MUTEX_ENABLED
+        WITH_SEMAPHORE(*sem);
 #endif
         next = branch_head[index];
         branch_head[index] = this;
@@ -53,9 +62,17 @@ class Subscriber : public HandlerList {
 
     // destructor, remove the entry from the singly-linked list
     ~Subscriber() {
+#ifdef CANARD_MUTEX_ENABLED
+        WITH_SEMAPHORE(*sem);
+#endif
         Subscriber<msgtype>* entry = branch_head[index];
         if (entry == this) {
             branch_head[index] = next;
+            if (branch_head[index] == nullptr) {
+#ifdef CANARD_MUTEX_ENABLED
+                deallocate(sem);
+#endif
+            }
             return;
         }
         while (entry != nullptr) {
@@ -70,6 +87,9 @@ class Subscriber : public HandlerList {
     /// @brief parse the message and call the callback
     /// @param transfer transfer object
     void handle_message(const CanardRxTransfer& transfer) override {
+#ifdef CANARD_MUTEX_ENABLED
+        WITH_SEMAPHORE(*sem);
+#endif
         msgtype msg {};
         msgtype::cxx_iface::decode(&transfer, &msg);
         // call all registered callbacks in one go
@@ -84,19 +104,14 @@ class Subscriber : public HandlerList {
     Subscriber<msgtype>* next;
     static Subscriber<msgtype> *branch_head[CANARD_NUM_HANDLERS];
 #ifdef CANARD_MUTEX_ENABLED
-    static Canard::Semaphore sem[CANARD_NUM_HANDLERS];
+    Canard::Semaphore *sem;
 #endif
     Callback<msgtype> &cb;
 };
 
 template <typename msgtype>
 Subscriber<msgtype>* Subscriber<msgtype>::branch_head[] = {nullptr};
 
-#ifdef CANARD_MUTEX_ENABLED
-template <typename msgtype>
-Canard::Semaphore Subscriber<msgtype>::sem[CANARD_NUM_HANDLERS];
-#endif
-
 template <typename T, typename msgtype>
 class SubscriberArgCb {
 public: