diff --git a/common/utils/itti/intertask_interface.c b/common/utils/itti/intertask_interface.c
index b7d6b4400f92841ab1d816469d29e165be249da8..31c0cd65d3c7ae15572288d90b782432f180464f 100644
--- a/common/utils/itti/intertask_interface.c
+++ b/common/utils/itti/intertask_interface.c
@@ -137,23 +137,23 @@ int send_broadcast_message(MessageDef *message_p)
         MessageDef *new_message_p;
 
         /* Skip tasks which are not running */
-        if (itti_desc.tasks[i].task_state != TASK_STATE_READY)
-            continue;
-
-        new_message_p = malloc(sizeof(MessageDef));
+        if (itti_desc.tasks[i].task_state == TASK_STATE_READY)
+        {
+            new_message_p = malloc(sizeof(MessageDef));
 
-        if (new_message_p == NULL) {
-            ITTI_ERROR("Failed to allocate memory (%s:%d)\n",
-                       __FILE__, __LINE__);
-            return -1;
-        }
-        memcpy(new_message_p, message_p, sizeof(MessageDef));
-        temp = send_msg_to_task(i, INSTANCE_DEFAULT, new_message_p);
-        if (temp < 0) {
-            ITTI_ERROR("Failed to send broadcast message (%s) to queue (%u:%s)\n",
-                       itti_desc.messages_info[message_p->header.messageId].name, i, itti_desc.threads_name[i]);
-            ret = temp;
-            free(new_message_p);
+            if (new_message_p == NULL) {
+                ITTI_ERROR("Failed to allocate memory (%s:%d)\n",
+                           __FILE__, __LINE__);
+                return -1;
+            }
+            memcpy(new_message_p, message_p, sizeof(MessageDef));
+            temp = send_msg_to_task(TASK_SHIFT_THREAD_ID(i), INSTANCE_DEFAULT, new_message_p);
+            if (temp < 0) {
+                ITTI_ERROR("Failed to send broadcast message (%s) to queue (%u:%s)\n",
+                           itti_desc.messages_info[message_p->header.messageId].name, i, itti_desc.threads_name[i]);
+                ret = temp;
+                free(new_message_p);
+            }
         }
     }
 
@@ -444,16 +444,9 @@ int intertask_interface_init(thread_id_t thread_max, MessagesIds messages_id_max
 
 void intertask_interface_send_quit_signal(void)
 {
-    int i;
     MessageDef *terminate_message_p;
 
     terminate_message_p = alloc_new_message(TASK_UNKNOWN, TERMINATE_MESSAGE);
 
     send_broadcast_message(terminate_message_p);
-
-    for (i = THREAD_FIRST; i < itti_desc.thread_max; i++) {
-        /* Skip tasks which are not running */
-        if (itti_desc.tasks[i].task_state != TASK_STATE_READY)
-            continue;
-    }
 }
diff --git a/common/utils/itti/intertask_interface_types.h b/common/utils/itti/intertask_interface_types.h
index c7495d44da8313b47b1cee0905560d695b36abcf..7c51a443578d43a353bc3145fe1611df81d45442 100644
--- a/common/utils/itti/intertask_interface_types.h
+++ b/common/utils/itti/intertask_interface_types.h
@@ -59,6 +59,10 @@
 #define TASK_GET_THREAD_ID(tASKiD)          UL_FIELD_EXTRACT(tASKiD, TASK_THREAD_ID_OFFSET, TASK_THREAD_ID_LENGTH)
 #define TASK_GET_SUB_TASK_ID(tASKiD)        UL_FIELD_EXTRACT(tASKiD, TASK_SUB_TASK_ID_OFFSET, TASK_SUB_TASK_ID_LENGTH)
 
+/* Defines to shift task ID fields */
+#define TASK_SHIFT_THREAD_ID(tHREADiD)      UL_BIT_SHIFT(tHREADiD, TASK_THREAD_ID_OFFSET)
+#define TASK_SHIFT_SUB_TASK_ID(sUBtASKiD)   UL_BIT_SHIFT(sUBtASKiD, TASK_SUB_TASK_ID_OFFSET)
+
 #include <messages_types.h>
 
 /* This enum defines messages ids. Each one is unique. */
@@ -98,8 +102,8 @@ typedef enum
 //! Tasks id of each task
 typedef enum
 {
-#define TASK_DEF(tHREADiD, pRIO)            tHREADiD = UL_BIT_SHIFT(THREAD_##tHREADiD, TASK_THREAD_ID_OFFSET),
-#define SUB_TASK_DEF(tHREADiD, sUBtASKiD)   sUBtASKiD = (UL_BIT_SHIFT(THREAD_##tHREADiD, TASK_THREAD_ID_OFFSET) | UL_BIT_SHIFT(SUB_TASK_OFFSET_##sUBtASKiD, TASK_SUB_TASK_ID_OFFSET)),
+#define TASK_DEF(tHREADiD, pRIO)            tHREADiD = TASK_SHIFT_THREAD_ID(THREAD_##tHREADiD),
+#define SUB_TASK_DEF(tHREADiD, sUBtASKiD)   sUBtASKiD = (TASK_SHIFT_THREAD_ID(THREAD_##tHREADiD) | TASK_SHIFT_SUB_TASK_ID(SUB_TASK_OFFSET_##sUBtASKiD)),
 #include <tasks_def.h>
 #undef SUB_TASK_DEF
 #undef TASK_DEF