From e342859053e63d26d9fd9da1037fe169dfdb1384 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 23 Sep 2020 12:18:46 +0200
Subject: [PATCH 1/2] Using rtos::MemoryPool to store the queued data elements
 rather than handling the heap memory management oneself

---
 Arduino_Threads.h | 25 +++++++++++++++++++------
 1 file changed, 19 insertions(+), 6 deletions(-)

diff --git a/Arduino_Threads.h b/Arduino_Threads.h
index 38c8634..c85bdf8 100644
--- a/Arduino_Threads.h
+++ b/Arduino_Threads.h
@@ -1,3 +1,6 @@
+#include <MemoryPool.h>
+
+
 template<class T>
 class Shared // template definition
 {
@@ -7,9 +10,14 @@ class Shared // template definition
     operator T() {
       osEvent evt = queue.get();
       if (evt.status == osEventMessage) {
-        T x = *((T*)evt.value.p);
-        delete (T*)evt.value.p;
-        return x;
+        /* Obtain the oldest inserted element from the queue. */
+        T * val_ptr = reinterpret_cast<T *>(evt.value.p);
+        /* Copy the content of T stored in the memory pool since we'll have to free the memory pool afterwards. */
+        T const tmp_val = *val_ptr;
+        /* Free the allocated memory in the memory pool. */
+        memory_pool.free(val_ptr);
+        /* Return obtained value from queue. */
+        return tmp_val;
       }
       return val;
     }
@@ -19,9 +27,13 @@ class Shared // template definition
         T discard = *this;
       }
       val = other;
-      T* obj = new T(val);
-      queue.put(obj);
-      return (*obj);
+      /* Allocate memory in the memory pool. */
+      T * val_ptr = memory_pool.alloc();
+      /* Copy the content of 'other' into the freshly allocated message. */
+      *val_ptr = other;
+      /* Insert into queue. */
+      queue.put(val_ptr);
+      return (*val_ptr);
     }
     T& peek() {
       return val;
@@ -31,6 +43,7 @@ class Shared // template definition
     }
   private:
     T val;
+    rtos::MemoryPool<T, 16> memory_pool;
     rtos::Queue<T, 16> queue;
 };
 

From 83e3aae4e3d2f2e526203f03bcbe8a5499105fb0 Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Wed, 23 Sep 2020 12:20:21 +0200
Subject: [PATCH 2/2] Replace magic number for queue size with constexpr
 constant

---
 Arduino_Threads.h | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/Arduino_Threads.h b/Arduino_Threads.h
index c85bdf8..6d313cc 100644
--- a/Arduino_Threads.h
+++ b/Arduino_Threads.h
@@ -42,9 +42,11 @@ class Shared // template definition
       return peek();
     }
   private:
+    static size_t constexpr QUEUE_SIZE = 16;
+
     T val;
-    rtos::MemoryPool<T, 16> memory_pool;
-    rtos::Queue<T, 16> queue;
+    rtos::MemoryPool<T, QUEUE_SIZE> memory_pool;
+    rtos::Queue<T, QUEUE_SIZE> queue;
 };
 
 #define CONCAT2(x,y) x##y