canard: use common semaphore from handler_list inside subscriber

dronecan · Jan 22, 2024 · b5cfe56 · b5cfe56
1 parent 50dcf28
commit b5cfe56
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 10 deletions.
diff --git a/canard/handler_list.h b/canard/handler_list.h
@@ -68,7 +68,9 @@ class HandlerList {
 protected:
     uint8_t index;
     HandlerList* next;
-
+#ifdef CANARD_MUTEX_ENABLED
+    Canard::Semaphore& get_sem() { return sem[index]; }
+#endif
 private:
     static HandlerList* head[CANARD_NUM_HANDLERS];
 #ifdef CANARD_MUTEX_ENABLED

diff --git a/canard/subscriber.h b/canard/subscriber.h
@@ -42,7 +42,7 @@ class Subscriber : public HandlerList {
     HandlerList(CanardTransferTypeBroadcast, msgtype::cxx_iface::ID, msgtype::cxx_iface::SIGNATURE, _index),
     cb (_cb) {
 #ifdef CANARD_MUTEX_ENABLED
-        WITH_SEMAPHORE(sem[index]);
+        WITH_SEMAPHORE(get_sem());
 #endif
         next = branch_head[index];
         branch_head[index] = this;
@@ -53,6 +53,9 @@ class Subscriber : public HandlerList {
 
     // destructor, remove the entry from the singly-linked list
     ~Subscriber() {
+#ifdef CANARD_MUTEX_ENABLED
+        WITH_SEMAPHORE(get_sem());
+#endif
         Subscriber<msgtype>* entry = branch_head[index];
         if (entry == this) {
             branch_head[index] = next;
@@ -70,6 +73,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(get_sem());
+#endif
         msgtype msg {};
         msgtype::cxx_iface::decode(&transfer, &msg);
         // call all registered callbacks in one go
@@ -83,20 +89,12 @@ class Subscriber : public HandlerList {
 private:
     Subscriber<msgtype>* next;
     static Subscriber<msgtype> *branch_head[CANARD_NUM_HANDLERS];
-#ifdef CANARD_MUTEX_ENABLED
-    static Canard::Semaphore sem[CANARD_NUM_HANDLERS];
-#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: