Updated Message Handling to new MessageBroker

Makefile

@@ -13,6 +13,15 @@ get_code_gen:
 gen_prot:
 	./alox.protogen -i prot.json -o main/uart
 
+switch_to_s3:
+	idf.py set-target esp32s3
+	cp sdkconfig.s3 sdkconfig
+	idf.py build
+
+switch_to_c3:
+	idf.py set-target esp32c3
+	cp sdkconfig.c3 sdkconfig
+	idf.py build
 
 buildIdf:
 	idf.py build 
@@ -26,6 +35,17 @@ flashMini2:
 flashMini3:
 	idf.py flash -p /dev/ttyACM2
 
+flashCluster:
+	idf.py flash -p /dev/ttyACM1
+	idf.py flash -p /dev/ttyACM2
+	idf.py flash -p /dev/ttyACM3
+	idf.py flash -p /dev/ttyACM4
+	idf.py flash -p /dev/ttyACM5
+	idf.py flash -p /dev/ttyACM6
+	idf.py flash -p /dev/ttyACM7
+	idf.py flash -p /dev/ttyACM8
+
+
 monitorMini:
 	idf.py monitor -p /dev/ttyACM0 
 

main/communication_handler.c

@@ -1,3 +1,4 @@
+#include "esp_err.h"
 #include "esp_log.h"
 #include "esp_now.h"
 #include "esp_timer.h"
@@ -8,15 +9,37 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdlib.h>
+#include <string.h>
 #include <sys/types.h>
 
 static const char *TAG = "ALOX - COM";
 
 static struct ESP_MessageBroker mr;
+static QueueHandle_t ESP_recieved_message_queue;
 
-void ESP_InitMessageBroker() {
+void free_ESPNOW_MessageInfo(ESPNOW_MessageInfo *msg) {
+  if (msg->esp_now_info.src_addr) {
+    free(msg->esp_now_info.src_addr);
+    msg->esp_now_info.src_addr = NULL;
+  }
+  if (msg->esp_now_info.des_addr) {
+    free(msg->esp_now_info.des_addr);
+    msg->esp_now_info.des_addr = NULL;
+  }
+  if (msg->esp_now_info.rx_ctrl) {
+    free(msg->esp_now_info.rx_ctrl);
+    msg->esp_now_info.rx_ctrl = NULL;
+  }
+  if (msg->data) {
+    free(msg->data);
+    msg->data = NULL;
+  }
+}
+
+void ESP_InitMessageBroker(QueueHandle_t msg_queue_handle) {
   mr.num_direct_callbacks = 0;
   mr.num_task_callbacks = 0;
+  ESP_recieved_message_queue = msg_queue_handle;
   return;
 }
 
@@ -35,15 +58,12 @@ void ESP_RegisterTask(CommandPages command, ESP_RegisterTaskCallback callback) {
 }
 
 void ESP_MessageBrokerTask(void *param) {
-  ParsedMessage_t received_msg;
+  ESPNOW_MessageInfo received_msg;
-  MessageBrokerTaskParams_t *task_params = (MessageBrokerTaskParams_t *)param;
+  ESP_MessageBrokerTaskParams_t *task_params =
+      (ESP_MessageBrokerTaskParams_t *)param;
 
   // Extrahiere die einzelnen Parameter
   QueueHandle_t msg_queue = task_params->message_queue;
-  uint8_t *send_message_buffer = task_params->send_buffer;
-  size_t send_message_buffer_size = task_params->send_buffer_size;
-  uint8_t *send_payload_buffer = task_params->payload_buffer;
-  size_t send_payload_buffer_size = task_params->payload_buffer_size;
 
   if (msg_queue == NULL) {
     ESP_LOGE(TAG, "Message queue not initialized. Terminating task.");
@@ -54,23 +74,27 @@ void ESP_MessageBrokerTask(void *param) {
 
   while (1) {
     if (xQueueReceive(msg_queue, &received_msg, portMAX_DELAY)) {
-      //ESP_LOGI(TAG, "Received message from queue: MSGID=0x%02X, Length=%u",
-      //         received_msg.msgid, received_msg.payload_len);
 
-      for (int i = 0; i < mr.num_direct_callbacks; i++) {
+      ESP_LOGI(TAG, "Broker got message trying to relay it now");
-        if (mr.FunctionList[i].MSGID == received_msg) {
+      const BaseMessage *message = (const BaseMessage *)received_msg.data;
-          mr.FunctionList[i].callback(
+      ESP_LOGI(TAG, "Broker searching for command page %d",
-              received_msg.msgid, received_msg.data, received_msg.payload_len,
+               message->commandPage);
-              send_payload_buffer, send_payload_buffer_size,
+      for (int i = 0; i < mr.num_direct_callbacks;
-              send_message_buffer, send_message_buffer_size);
+           i++) { // TODO: there should not be a loop needed here
+        if (mr.FunctionList[i].MSGID == message->commandPage) {
+          mr.FunctionList[i].callback(&received_msg.esp_now_info,
+                                      received_msg.data, received_msg.data_len);
+          ESP_LOGI(TAG, "Broker found matching msgid %d",
+                   mr.FunctionList[i].MSGID);
+          free_ESPNOW_MessageInfo(&received_msg);
         }
       }
       for (int i = 0; i < mr.num_direct_callbacks; i++) {
-        if (mr.FunctionList[i].MSGID == received_msg.msgid) {
+        // if (mr.FunctionList[i].MSGID == received_msg.msgid) {
-          // TODO: Not yet implemented
+        //  TODO: Not yet implemented
-          // Only send data to task, task should be created beforhead and wait
+        //  Only send data to task, task should be created beforhead and wait
-          // for new data in the queue.
+        //  for new data in the queue.
-        }
+        //}
       }
     }
   }
@@ -85,7 +109,7 @@ static uint8_t channelNumber = 0;
 
 int init_com(ClientList *clients, uint8_t wifi_channel) {
   // Initialisiere die Kommunikations-Warteschlange
-  messageQueue = xQueueCreate(MESSAGE_QUEUE_SIZE, sizeof(BaseMessage));
+  messageQueue = xQueueCreate(MESSAGE_QUEUE_SIZE, sizeof(ESPNOW_MessageInfo));
   if (messageQueue == NULL) {
     ESP_LOGE(TAG, "Message queue creation failed");
     return -1;
@@ -101,7 +125,7 @@ int add_peer(uint8_t *macAddr) {
   esp_now_peer_info_t peerInfo = {
       .channel = channelNumber,
       .ifidx = ESP_IF_WIFI_STA,
-      .encrypt = false, // Keine Verschlüsselung (kann geändert werden)
+      .encrypt = false, // Keine Verschlüsselung // TODO: should be changed
   };
   memcpy(peerInfo.peer_addr, macAddr, ESP_NOW_ETH_ALEN);
 
@@ -137,13 +161,11 @@ BaseMessage MessageBuilder(CommandPages commandPage, PayloadUnion payload,
                            size_t payload_size) {
   BaseMessage message;
 
-  // Initialisierung der BaseMessage
   message.commandPage = commandPage;
   message.version = 1;
   message.length = (uint16_t)payload_size;
 
-  // Kopieren des Payloads in die Union
+  memset(&message.payload, 0, sizeof(message.payload));
-  memset(&message.payload, 0, sizeof(message.payload)); // Sicherheitsmaßnahme
   memcpy(&message.payload, &payload, payload_size);
 
   return message;
@@ -158,7 +180,8 @@ void master_broadcast_task(void *param) {
 
     ESP_ERROR_CHECK(esp_now_send(broadcast_address, (uint8_t *)&message,
                                  sizeof(BaseMessage)));
-    ESP_LOGI(TAG, "Broadcast Message sent");
+
+    // ESP_LOGI(TAG, "Broadcast Message sent");
     vTaskDelay(pdMS_TO_TICKS(5000));
   }
 }
@@ -171,7 +194,7 @@ void master_broadcast_ping(void *param) {
         MessageBuilder(PingPage, *(PayloadUnion *)&payload, sizeof(payload));
     ESP_ERROR_CHECK(esp_now_send(broadcast_address, (uint8_t *)&message,
                                  sizeof(BaseMessage)));
-    ESP_LOGI(TAG, "Broadcast PING Message sent");
+    // ESP_LOGI(TAG, "Broadcast PING Message sent");
     vTaskDelay(pdMS_TO_TICKS(2500));
   }
 }
@@ -195,86 +218,207 @@ void master_ping_task(void *param) {
   }
 }
 
+void master_StatusCallback(const esp_now_recv_info_t *esp_now_info,
+                           const uint8_t *data, int data_len) {
+  const BaseMessage *message = (const BaseMessage *)data;
+
+  ESP_LOGI(TAG, "SRC " MACSTR, MAC2STR(esp_now_info->src_addr));
+  ESP_LOGI(TAG,
+           "Status Message Received: status: %d, runningPartition: %d, uptime: "
+           "%d, version: %d",
+           message->payload.status_payload.status,
+           message->payload.status_payload.runningPartition,
+           message->payload.status_payload.uptime,
+           message->payload.status_payload.version);
+}
+
+void master_RegisterCallback(const esp_now_recv_info_t *esp_now_info,
+                             const uint8_t *data, int data_len) {
+  BaseMessage replyMessage = {};
+  const BaseMessage *message = (const BaseMessage *)data;
+
+  ESP_LOGI(TAG, "WILL REGISTER DEVICE");
+  esp_now_peer_info_t checkPeerInfo;
+  esp_err_t checkPeer =
+      esp_now_get_peer(esp_now_info->src_addr, &checkPeerInfo);
+  switch (checkPeer) {
+  case (ESP_OK):
+    ESP_LOGI(TAG, "CLIENT BEKANNT");
+    int id = get_client_id(esp_client_list, esp_now_info->src_addr);
+    esp_client_list->Clients[id].isAvailable = true;
+    esp_client_list->Clients[id].lastSuccessfullPing = xTaskGetTickCount();
+    ESP_LOGI(TAG, "Updated client %d last ping time to %lu", id,
+             esp_client_list->Clients[id].lastSuccessfullPing);
+    break;
+  case (ESP_ERR_ESPNOW_NOT_INIT):
+    ESP_LOGI(TAG, "Not initalised");
+    break;
+  case (ESP_ERR_ESPNOW_ARG):
+    ESP_LOGI(TAG, "ESP ERR ESPNOW_ARG");
+    break;
+  case (ESP_ERR_ESPNOW_NOT_FOUND):
+    ESP_LOGI(TAG, "CLIENT WIRD IN DIE LISTE AUFGENOMMEN " MACSTR,
+             MAC2STR(esp_now_info->src_addr));
+    int peer_err = add_peer(esp_now_info->src_addr);
+    if (peer_err < 0) {
+      ESP_LOGE(TAG, "Could not add ESP TO ClientList %d", peer_err);
+    }
+    ESP_LOGI(TAG, "FRAGE CLIENT STATUS AN");
+
+    GetStatusPayload payload = {};
+    replyMessage = MessageBuilder(GetStatusPage, *(PayloadUnion *)&payload,
+                                  sizeof(payload));
+    esp_err_t err = esp_now_send(esp_now_info->src_addr,
+                                 (uint8_t *)&replyMessage, sizeof(BaseMessage));
+    if (err != ESP_OK) {
+      ESP_LOGE(TAG, "Could not send Message Error %s", esp_err_to_name(err));
+    }
+
+    break;
+  default:
+    ESP_LOGI(TAG, "Unknown Message %i", checkPeer);
+  }
+}
+
+void master_pingCallback(const esp_now_recv_info_t *esp_now_info,
+                         const uint8_t *data, int data_len) {
+  BaseMessage replyMessage = {};
+  const BaseMessage *message = (const BaseMessage *)data;
+
+  ESP_LOGI(TAG, "GOT PING MESSAGE");
+  uint32_t currentTime = esp_timer_get_time();
+  uint32_t diff = currentTime - message->payload.ping_payload.timestamp;
+
+  ESP_LOGI(TAG, "Start: %lu, End: %lu, Diff: %lu, Ping: %lu",
+           message->payload.ping_payload.timestamp, currentTime, diff,
+           diff / 1000); // ping in ms
+
+  int id = get_client_id(esp_client_list, esp_now_info->src_addr);
+  if (id >= 0) {
+    esp_client_list->Clients[id].lastSuccessfullPing = xTaskGetTickCount();
+    esp_client_list->Clients[id].lastPing = (diff / 1000);
+    ESP_LOGI(TAG, "Updated client %d: " MACSTR " last ping time to %lu", id,
+             MAC2STR(esp_now_info->src_addr),
+             esp_client_list->Clients[id].lastSuccessfullPing);
+  }
+}
+
+void master_broadcastCallback(const esp_now_recv_info_t *esp_now_info,
+                              const uint8_t *data, int data_len) {
+  ESP_LOGI(TAG,
+           "Master should not recieve Broadcast is there another master "
+           "Calling got message from " MACSTR,
+           MAC2STR(esp_now_info->src_addr));
+}
+
+void ESPNOW_RegisterMasterCallbacks() {
+  ESP_RegisterFunction(StatusPage, master_StatusCallback);
+  ESP_RegisterFunction(RegisterPage, master_RegisterCallback);
+  ESP_RegisterFunction(PingPage, master_pingCallback);
+  ESP_RegisterFunction(BroadCastPage, master_broadcastCallback);
+}
+
+void slave_broadcastCallback(const esp_now_recv_info_t *esp_now_info,
+                             const uint8_t *data, int data_len) {
+  BaseMessage replyMessage = {};
+  const BaseMessage *message = (const BaseMessage *)data;
+
+  ESP_LOGI(TAG, "GOT BROADCAST MESSAGE");
+  if (!hasMaster) {
+    if (IS_BROADCAST_ADDR(esp_now_info->des_addr)) {
+      ESP_LOGI(TAG, "GOT BROADCAST MESSAGE ATTEMPTING TO REGISTER TO MASTER!");
+      add_peer(esp_now_info->src_addr);
+      replyMessage =
+          MessageBuilder(RegisterPage, *(PayloadUnion *)&message->payload,
+                         sizeof(message->payload));
+      ESP_ERROR_CHECK(esp_now_send(esp_now_info->src_addr,
+                                   (uint8_t *)&replyMessage,
+                                   sizeof(BaseMessage)));
+      hasMaster = true;
+    }
+  } else {
+    ESP_LOGI(TAG, "Already have master wont register by the new one");
+  }
+}
+void slave_getstatusCallback(const esp_now_recv_info_t *esp_now_info,
+                             const uint8_t *data, int data_len) {
+  BaseMessage replyMessage = {};
+  const BaseMessage *message = (const BaseMessage *)data;
+
+  StatusPayload payload = {
+      .status = 1,
+      .runningPartition = 1,
+      .uptime = 100,
+      .version = 0x0002,
+
+  };
+  replyMessage =
+      MessageBuilder(StatusPage, *(PayloadUnion *)&payload, sizeof(payload));
+  ESP_ERROR_CHECK(esp_now_send(esp_now_info->src_addr, (uint8_t *)&replyMessage,
+                               sizeof(BaseMessage)));
+}
+void slave_pingCallback(const esp_now_recv_info_t *esp_now_info,
+                        const uint8_t *data, int data_len) {
+  if (!hasMaster)
+    return;
+
+  BaseMessage replyMessage = {};
+  const BaseMessage *message = (const BaseMessage *)data;
+
+  ESP_LOGI(TAG, "GOT PING MESSAGE");
+  replyMessage = MessageBuilder(PingPage, *(PayloadUnion *)&message->payload,
+                                sizeof(message->payload));
+  ESP_ERROR_CHECK(esp_now_send(esp_now_info->src_addr, (uint8_t *)&replyMessage,
+                               sizeof(BaseMessage)));
+}
+
+void ESPNOW_RegisterSlaveCallbacks() {
+  ESP_RegisterFunction(BroadCastPage, slave_broadcastCallback);
+  ESP_RegisterFunction(GetStatusPage, slave_getstatusCallback);
+  ESP_RegisterFunction(PingPage, slave_pingCallback);
+}
+
 void master_receive_callback(const esp_now_recv_info_t *esp_now_info,
                              const uint8_t *data, int data_len) {
   ESP_LOGI(TAG, "MASTER GOT MESSAGE");
 
-  BaseMessage replyMessage = {};
+  // Allokiere Speicher für die Daten und kopiere sie
-  const BaseMessage *message = (const BaseMessage *)data;
+  uint8_t *copied_data = (uint8_t *)malloc(data_len);
-  int id;
+  if (copied_data == NULL) {
-  switch (message->commandPage) {
+    ESP_LOGE(TAG, "Failed to allocate memory for message data.");
-  case StatusPage:
+    return;
-    ESP_LOGI(TAG, "GOT STATUS MESSAGE");
-    id = get_client_id(esp_client_list, esp_now_info->src_addr);
-    if (id >= 0) {
-      esp_client_list->Clients[id].clientVersion =
-          message->payload.status_payload.version;
-    }
-
-    break;
-  case PingPage:
-    ESP_LOGI(TAG, "GOT PING MESSAGE");
-    uint32_t currentTime = esp_timer_get_time();
-    uint32_t diff = currentTime - message->payload.ping_payload.timestamp;
-
-    ESP_LOGI(TAG, "Start: %lu, End: %lu, Diff: %lu, Ping: %lu",
-             message->payload.ping_payload.timestamp, currentTime, diff,
-             diff / 1000); // ping in ms
-
-    id = get_client_id(esp_client_list, esp_now_info->src_addr);
-    if (id >= 0) {
-      esp_client_list->Clients[id].lastSuccessfullPing = xTaskGetTickCount();
-      esp_client_list->Clients[id].lastPing = (diff / 1000);
-      ESP_LOGI(TAG, "Updated client %d: " MACSTR " last ping time to %lu", id,
-               MAC2STR(esp_now_info->src_addr),
-               esp_client_list->Clients[id].lastSuccessfullPing);
-    }
-    break;
-  case BroadCastPage:
-    ESP_LOGI(TAG, "MASTER SHOULD NOT GET BROADCAST MESSAGE, is there another "
-                  "master calling?");
-    break;
-  case RegisterPage:
-    ESP_LOGI(TAG, "WILL REGISTER DEVICE");
-    esp_now_peer_info_t checkPeerInfo;
-    esp_err_t checkPeer =
-        esp_now_get_peer(esp_now_info->src_addr, &checkPeerInfo);
-    switch (checkPeer) {
-    case (ESP_OK):
-      ESP_LOGI(TAG, "CLIENT BEKANNT");
-      int id = get_client_id(esp_client_list, esp_now_info->src_addr);
-      esp_client_list->Clients[id].isAvailable = true;
-      esp_client_list->Clients[id].lastSuccessfullPing = xTaskGetTickCount();
-      ESP_LOGI(TAG, "Updated client %d last ping time to %lu", id,
-               esp_client_list->Clients[id].lastSuccessfullPing);
-      break;
-    case (ESP_ERR_ESPNOW_NOT_INIT):
-      ESP_LOGI(TAG, "Not initalised");
-      break;
-    case (ESP_ERR_ESPNOW_ARG):
-      ESP_LOGI(TAG, "ESP ERR ESPNOW_ARG");
-      break;
-    case (ESP_ERR_ESPNOW_NOT_FOUND):
-      ESP_LOGI(TAG, "CLIENT WIRD IN DIE LISTE AUFGENOMMEN");
-      add_peer(esp_now_info->src_addr);
-      ESP_LOGI(TAG, "FRAGE CLIENT STATUS AN");
-
-      GetStatusPayload payload = {};
-      replyMessage = MessageBuilder(GetStatusPage, *(PayloadUnion *)&payload,
-                                    sizeof(payload));
-      ESP_ERROR_CHECK(esp_now_send(esp_now_info->src_addr,
-                                   (uint8_t *)&replyMessage,
-                                   sizeof(BaseMessage)));
-
-      break;
-    default:
-      ESP_LOGI(TAG, "Unknown Message %i", checkPeer);
-    }
-    break;
-  default:
-    ESP_LOGI(TAG, "Unknown CommandPage %i", message->commandPage);
-    break;
   }
+  memcpy(copied_data, data, data_len);
+
+  // Fülle die neue Struktur mit kopierten Daten
+  ESPNOW_MessageInfo msg_info;
+  msg_info.esp_now_info.src_addr = malloc(6);
+  if (msg_info.esp_now_info.src_addr) {
+    memcpy(msg_info.esp_now_info.src_addr, esp_now_info->src_addr, 6);
+  }
+
+  // Speicher für des_addr kopieren
+  msg_info.esp_now_info.des_addr = malloc(6);
+  if (msg_info.esp_now_info.des_addr) {
+    memcpy(msg_info.esp_now_info.des_addr, esp_now_info->des_addr, 6);
+  }
+
+  // rx_ctrl Struktur kopieren
+  msg_info.esp_now_info.rx_ctrl = malloc(sizeof(wifi_pkt_rx_ctrl_t));
+  if (msg_info.esp_now_info.rx_ctrl) {
+    memcpy(msg_info.esp_now_info.rx_ctrl, esp_now_info->rx_ctrl,
+           sizeof(wifi_pkt_rx_ctrl_t));
+  }
+
+  msg_info.data = copied_data;
+  msg_info.data_len = data_len;
+
+  if (xQueueSend(ESP_recieved_message_queue, &msg_info, portMAX_DELAY) !=
+      pdPASS) {
+    // Fehlerbehandlung: Queue voll oder Senden fehlgeschlagen
+    ESP_LOGE(TAG, "Failed to send parsed message to queue.");
+  }
+  return;
 }
 
 void client_receive_callback(const esp_now_recv_info_t *esp_now_info,
@@ -282,62 +426,26 @@ void client_receive_callback(const esp_now_recv_info_t *esp_now_info,
   ESP_LOGI(TAG, "SLAVE GOT MESSAGE");
   ESP_LOGI(TAG, "Received message from: " MACSTR,
            MAC2STR(esp_now_info->src_addr));
-  ESP_LOGI(TAG, "Message: %.*s", data_len, data);
 
-  BaseMessage replyMessage = {};
+  uint8_t *copied_data = (uint8_t *)malloc(data_len);
-
+  if (copied_data == NULL) {
-  const BaseMessage *message = (const BaseMessage *)data;
+    ESP_LOGE(TAG, "Failed to allocate memory for message data.");
-  switch (message->commandPage) {
+    return;
-  case StatusPage:
-    ESP_LOGI(TAG, "GOT STATUS MESSAGE");
-    break;
-  case GetStatusPage: {
-    StatusPayload payload = {
-        .status = 1,
-        .runningPartition = 1,
-        .uptime = 100,
-        .version = 0x0002,
-
-    };
-    replyMessage =
-        MessageBuilder(StatusPage, *(PayloadUnion *)&payload, sizeof(payload));
-    ESP_ERROR_CHECK(esp_now_send(
-        esp_now_info->src_addr, (uint8_t *)&replyMessage, sizeof(BaseMessage)));
-
-  } break;
-  case PingPage:
-    ESP_LOGI(TAG, "GOT PING MESSAGE");
-    replyMessage = MessageBuilder(PingPage, *(PayloadUnion *)&message->payload,
-                                  sizeof(message->payload));
-    ESP_ERROR_CHECK(esp_now_send(
-        esp_now_info->src_addr, (uint8_t *)&replyMessage, sizeof(BaseMessage)));
-    break;
-  case BroadCastPage:
-    ESP_LOGI(TAG, "GOT BROADCAST MESSAGE");
-    if (!hasMaster) {
-      if (IS_BROADCAST_ADDR(esp_now_info->des_addr)) {
-        ESP_LOGI(TAG,
-                 "GOT BROADCAST MESSAGE ATTEMPTING TO REGISTER TO MASTER!");
-        add_peer(esp_now_info->src_addr);
-        replyMessage =
-            MessageBuilder(RegisterPage, *(PayloadUnion *)&message->payload,
-                           sizeof(message->payload));
-        ESP_ERROR_CHECK(esp_now_send(esp_now_info->src_addr,
-                                     (uint8_t *)&replyMessage,
-                                     sizeof(BaseMessage)));
-        hasMaster = true;
-      }
-      break;
-    }
-    // check if its an update message
-
-    break;
-  case RegisterPage:
-    break;
-  default:
-    ESP_LOGI(TAG, "GOT UNKONW MESSAGE");
-    break;
   }
+  memcpy(copied_data, data, data_len);
+
+  // Fülle die neue Struktur mit kopierten Daten
+  ESPNOW_MessageInfo msg_info;
+  memcpy(&msg_info.esp_now_info, esp_now_info, sizeof(esp_now_recv_info_t));
+  msg_info.data = copied_data;
+  msg_info.data_len = data_len;
+
+  if (xQueueSend(ESP_recieved_message_queue, &msg_info, portMAX_DELAY) !=
+      pdPASS) {
+    // Fehlerbehandlung: Queue voll oder Senden fehlgeschlagen
+    ESP_LOGE(TAG, "Failed to send parsed message to queue.");
+  }
+  return;
 }
 
 void client_data_sending_task(void *param) {

main/communication_handler.h

@@ -138,10 +138,24 @@ struct ESP_MessageBroker {
   uint8_t num_task_callbacks;
 };
 
-void ESP_InitMessageBroker();
+typedef struct {
+  QueueHandle_t message_queue;
+} ESP_MessageBrokerTaskParams_t;
+
+typedef struct {
+  esp_now_recv_info_t esp_now_info;
+  uint8_t *data;
+  int data_len;
+} ESPNOW_MessageInfo;
+
+void ESP_InitMessageBroker(QueueHandle_t msg_queue_handle);
 void ESP_RegisterFunction(CommandPages command,
                           ESP_RegisterFunctionCallback callback);
 void ESP_RegisterTask(CommandPages command, ESP_RegisterTaskCallback callback);
+void ESP_MessageBrokerTask(void *param);
+
+void ESPNOW_RegisterMasterCallbacks();
+void ESPNOW_RegisterSlaveCallbacks();
 
 int init_com(ClientList *clients, uint8_t wifi_channel);
 int getNextFreeClientId();

main/main.c

@@ -36,6 +36,7 @@ static uint8_t send_message_buffer[1024];
 static uint8_t send_message_payload_buffer[512];
 
 static MessageBrokerTaskParams_t broker_task_params;
+static ESP_MessageBrokerTaskParams_t esp_broker_task_params;
 
 ClientList clientList = {.Clients = {{0}}, .ClientCount = 0};
 
@@ -141,7 +142,7 @@ void clientInfoCallback(uint8_t msgid, const uint8_t *payload,
   int len = build_message(UART_CLIENT_INFO, send_payload_buffer,
                           needed_buffer_size, send_buffer, send_buffer_size);
 
-  //ESP_LOG_BUFFER_HEX("SEND BUFFER: ", send_buffer, send_buffer_size);
+  // ESP_LOG_BUFFER_HEX("SEND BUFFER: ", send_buffer, send_buffer_size);
 
   if (len < 0) {
     ESP_LOGE(TAG,
@@ -212,7 +213,7 @@ void app_main(void) {
   uint8_t ota_part_count = esp_ota_get_app_partition_count();
   ESP_LOGI(TAG, "OTA: Got %d OTA Partitions", ota_part_count);
 
-	esp_ota_img_states_t ota_state;
+  esp_ota_img_states_t ota_state;
   if (esp_ota_get_state_partition(running, &ota_state) == ESP_OK) {
     ESP_LOGI(TAG, "OTA: Partition State : %d", ota_state);
     if (ota_state == ESP_OTA_IMG_PENDING_VERIFY) {
@@ -257,9 +258,19 @@ void app_main(void) {
   }
   nvs_close(nt);
 
+  QueueHandle_t espnow_message_queue =
+      xQueueCreate(10, sizeof(ESPNOW_MessageInfo));
+  ESP_InitMessageBroker(espnow_message_queue);
+
+  esp_broker_task_params.message_queue = espnow_message_queue;
+  xTaskCreate(ESP_MessageBrokerTask, "espnow_message_broker_task", 4096,
+              (void *)&esp_broker_task_params, 4, NULL);
+
   // Tasks starten basierend auf Master/Client
   if (isMaster) {
     ESP_LOGI(TAG, "Started in Mastermode");
+    ESPNOW_RegisterMasterCallbacks();
+
     add_peer(broadcast_address);
     xTaskCreate(master_broadcast_task, "MasterBroadcast", 4096, NULL, 1, NULL);
     //  xTaskCreate(master_ping_task, "MasterPing", 4096, NULL, 1, NULL);
@@ -295,6 +306,7 @@ void app_main(void) {
     // NULL);
   } else {
     ESP_LOGI(TAG, "Started in Slavemode");
+    ESPNOW_RegisterSlaveCallbacks();
     // xTaskCreate(client_data_sending_task, "ClientDataSending", 4096, NULL, 1,
     //             NULL);
   }