diff --git a/microros_crazyflie21_extensions/src/custom_memory_manager.c b/microros_crazyflie21_extensions/src/custom_memory_manager.c
index 924dc503..c04a76ff 100644
--- a/microros_crazyflie21_extensions/src/custom_memory_manager.c
+++ b/microros_crazyflie21_extensions/src/custom_memory_manager.c
@@ -300,21 +300,25 @@ void *pvPortRealloc( void *pv, size_t xWantedSize )
 
 	void * newmem = pvPortMalloc(xWantedSize);
 
-	uint8_t *puc = ( uint8_t * ) pv;
-	BlockLink_t *pxLink;
+	if (newmem != NULL && pv != NULL)
+	{
+		uint8_t *puc = ( uint8_t * ) pv;
+		BlockLink_t *pxLink;
 
-	puc -= xHeapStructSize;
-	pxLink = ( void * ) puc;
+		puc -= xHeapStructSize;
+		pxLink = ( void * ) puc;
 
 
-	char *in_src = (char*)pv;
-  	char *in_dest = (char*)newmem;
-	size_t count = pxLink->xBlockSize & ~xBlockAllocatedBit;
+		char *in_src = (char*)pv;
+		char *in_dest = (char*)newmem;
 
-  	while(count--)
-    	*in_dest++ = *in_src++;
+		size_t count = (pxLink->xBlockSize & ~xBlockAllocatedBit) - xHeapStructSize;
+		if (xWantedSize < count) count = xWantedSize;
 
-	vPortFree(pv);
+		while (count--) *in_dest++ = *in_src++;
+
+		vPortFree(pv);
+	}
 
 	( void ) xTaskResumeAll();
 
@@ -463,4 +467,3 @@ uint8_t *puc;
 		mtCOVERAGE_TEST_MARKER();
 	}
 }
-
diff --git a/microros_nucleo_f446re_extensions/Core/Src/custom_memory_manager.c b/microros_nucleo_f446re_extensions/Core/Src/custom_memory_manager.c
index 5605064c..683680fc 100644
--- a/microros_nucleo_f446re_extensions/Core/Src/custom_memory_manager.c
+++ b/microros_nucleo_f446re_extensions/Core/Src/custom_memory_manager.c
@@ -265,19 +265,24 @@ void *pvPortRealloc(void *pv, size_t xWantedSize) {
 
   void *newmem = pvPortMalloc(xWantedSize);
 
-  uint8_t *puc = (uint8_t *)pv;
-  BlockLink_t *pxLink;
+  if (newmem != NULL && pv != NULL)
+	{
+    uint8_t *puc = (uint8_t *)pv;
+    BlockLink_t *pxLink;
 
-  puc -= xHeapStructSize;
-  pxLink = (void *)puc;
+    puc -= xHeapStructSize;
+    pxLink = (void *)puc;
 
-  char *in_src = (char *)pv;
-  char *in_dest = (char *)newmem;
-  size_t count = pxLink->xBlockSize & ~xBlockAllocatedBit;
+    char *in_src = (char *)pv;
+    char *in_dest = (char *)newmem;
+
+    size_t count = (pxLink->xBlockSize & ~xBlockAllocatedBit) - xHeapStructSize;
+    if (xWantedSize < count) count = xWantedSize;
 
-  while (count--) *in_dest++ = *in_src++;
+    while (count--) *in_dest++ = *in_src++;
 
-  vPortFree(pv);
+    vPortFree(pv);
+	}
 
   (void)xTaskResumeAll();
 
diff --git a/microros_nucleo_f446ze_extensions/Src/custom_memory_manager.c b/microros_nucleo_f446ze_extensions/Src/custom_memory_manager.c
index 5605064c..683680fc 100644
--- a/microros_nucleo_f446ze_extensions/Src/custom_memory_manager.c
+++ b/microros_nucleo_f446ze_extensions/Src/custom_memory_manager.c
@@ -265,19 +265,24 @@ void *pvPortRealloc(void *pv, size_t xWantedSize) {
 
   void *newmem = pvPortMalloc(xWantedSize);
 
-  uint8_t *puc = (uint8_t *)pv;
-  BlockLink_t *pxLink;
+  if (newmem != NULL && pv != NULL)
+	{
+    uint8_t *puc = (uint8_t *)pv;
+    BlockLink_t *pxLink;
 
-  puc -= xHeapStructSize;
-  pxLink = (void *)puc;
+    puc -= xHeapStructSize;
+    pxLink = (void *)puc;
 
-  char *in_src = (char *)pv;
-  char *in_dest = (char *)newmem;
-  size_t count = pxLink->xBlockSize & ~xBlockAllocatedBit;
+    char *in_src = (char *)pv;
+    char *in_dest = (char *)newmem;
+
+    size_t count = (pxLink->xBlockSize & ~xBlockAllocatedBit) - xHeapStructSize;
+    if (xWantedSize < count) count = xWantedSize;
 
-  while (count--) *in_dest++ = *in_src++;
+    while (count--) *in_dest++ = *in_src++;
 
-  vPortFree(pv);
+    vPortFree(pv);
+	}
 
   (void)xTaskResumeAll();
 
diff --git a/microros_nucleo_f746zg_extensions/Src/custom_memory_manager.c b/microros_nucleo_f746zg_extensions/Src/custom_memory_manager.c
index 5605064c..2509bab4 100644
--- a/microros_nucleo_f746zg_extensions/Src/custom_memory_manager.c
+++ b/microros_nucleo_f746zg_extensions/Src/custom_memory_manager.c
@@ -265,19 +265,24 @@ void *pvPortRealloc(void *pv, size_t xWantedSize) {
 
   void *newmem = pvPortMalloc(xWantedSize);
 
-  uint8_t *puc = (uint8_t *)pv;
-  BlockLink_t *pxLink;
+  if (newmem != NULL && pv != NULL)
+	{
+    uint8_t *puc = (uint8_t *)pv;
+    BlockLink_t *pxLink;
 
-  puc -= xHeapStructSize;
-  pxLink = (void *)puc;
+    puc -= xHeapStructSize;
+    pxLink = (void *)puc;
 
-  char *in_src = (char *)pv;
-  char *in_dest = (char *)newmem;
-  size_t count = pxLink->xBlockSize & ~xBlockAllocatedBit;
+    char *in_src = (char *)pv;
+    char *in_dest = (char *)newmem;
+
+    size_t count = (pxLink->xBlockSize & ~xBlockAllocatedBit) - xHeapStructSize;
+    if (xWantedSize < count) count = xWantedSize;
 
-  while (count--) *in_dest++ = *in_src++;
+    while (count--) *in_dest++ = *in_src++;
 
-  vPortFree(pv);
+		vPortFree(pv);
+	}
 
   (void)xTaskResumeAll();
 
diff --git a/microros_nucleo_f767zi_extensions/Src/custom_memory_manager.c b/microros_nucleo_f767zi_extensions/Src/custom_memory_manager.c
index 5605064c..683680fc 100644
--- a/microros_nucleo_f767zi_extensions/Src/custom_memory_manager.c
+++ b/microros_nucleo_f767zi_extensions/Src/custom_memory_manager.c
@@ -265,19 +265,24 @@ void *pvPortRealloc(void *pv, size_t xWantedSize) {
 
   void *newmem = pvPortMalloc(xWantedSize);
 
-  uint8_t *puc = (uint8_t *)pv;
-  BlockLink_t *pxLink;
+  if (newmem != NULL && pv != NULL)
+	{
+    uint8_t *puc = (uint8_t *)pv;
+    BlockLink_t *pxLink;
 
-  puc -= xHeapStructSize;
-  pxLink = (void *)puc;
+    puc -= xHeapStructSize;
+    pxLink = (void *)puc;
 
-  char *in_src = (char *)pv;
-  char *in_dest = (char *)newmem;
-  size_t count = pxLink->xBlockSize & ~xBlockAllocatedBit;
+    char *in_src = (char *)pv;
+    char *in_dest = (char *)newmem;
+
+    size_t count = (pxLink->xBlockSize & ~xBlockAllocatedBit) - xHeapStructSize;
+    if (xWantedSize < count) count = xWantedSize;
 
-  while (count--) *in_dest++ = *in_src++;
+    while (count--) *in_dest++ = *in_src++;
 
-  vPortFree(pv);
+    vPortFree(pv);
+	}
 
   (void)xTaskResumeAll();
 
diff --git a/microros_olimex_e407_extensions/Src/custom_memory_manager.c b/microros_olimex_e407_extensions/Src/custom_memory_manager.c
index 2eac0b57..c2c8fac7 100644
--- a/microros_olimex_e407_extensions/Src/custom_memory_manager.c
+++ b/microros_olimex_e407_extensions/Src/custom_memory_manager.c
@@ -300,21 +300,24 @@ void *pvPortRealloc( void *pv, size_t xWantedSize )
 
 	void * newmem = pvPortMalloc(xWantedSize);
 
-	uint8_t *puc = ( uint8_t * ) pv;
-	BlockLink_t *pxLink;
+	if (newmem != NULL && pv != NULL)
+	{
+		uint8_t *puc = ( uint8_t * ) pv;
+		BlockLink_t *pxLink;
 
-	puc -= xHeapStructSize;
-	pxLink = ( void * ) puc;
+		puc -= xHeapStructSize;
+		pxLink = ( void * ) puc;
 
+		char *in_src = (char*)pv;
+		char *in_dest = (char*)newmem;
 
-	char *in_src = (char*)pv;
-  	char *in_dest = (char*)newmem;
-	size_t count = pxLink->xBlockSize & ~xBlockAllocatedBit;
+		size_t count = (pxLink->xBlockSize & ~xBlockAllocatedBit) - xHeapStructSize;
+		if (xWantedSize < count) count = xWantedSize;
 
-  	while(count--)
-    	*in_dest++ = *in_src++;
+		while (count--) *in_dest++ = *in_src++;
 
-	vPortFree(pv);
+		vPortFree(pv);
+	}
 
 	( void ) xTaskResumeAll();