Added Message Builder with Tests

main/main.c

@@ -7,6 +7,8 @@
 #include "esp_wifi.h"
 #include "freertos/idf_additions.h"
 #include "hal/uart_types.h"
+#include "message_handler.h"
+#include "message_parser.h"
 #include "nvs_flash.h"
 
 #include "communication_handler.h"
@@ -14,22 +16,44 @@
 #include "portmacro.h"
 #include "uart_handler.h"
 #include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <string.h>
+#include <sys/types.h>
+
+#include "message_builder.h"
 
 static const char *TAG = "ALOX - MAIN";
+static const uint16_t version = 0x0001;
 
-void SendClientInfoTask() {
-  int clientId = 0;
-  while (1) {
-    ClientInfo info = *get_client_info(clientId);
-    send_client_info(clientId, info.isAvailable, info.lastPing);
-    clientId += 1;
-    clientId = clientId%20;
-    vTaskDelay(100 / portTICK_PERIOD_MS); 
-  }
+void echoCallback(uint8_t msgid, const uint8_t *payload, size_t payload_len) {
+  // Code für den Button-Press
+  ESP_LOGI(TAG, "Echo command 0x01...");
+  uint8_t send_buffer[64];
+  size_t len = build_message(0x01, payload, payload_len, send_buffer,
+                             sizeof(send_buffer));
+  uart_write_bytes(MASTER_UART, send_buffer, len);
+}
+
+void versionCallback(uint8_t msgid, const uint8_t *payload,
+                     size_t payload_len) {
+  // Code für den Button-Press
+  ESP_LOGI(TAG, "Version command 0x02...");
+  uint8_t send_buffer[64];
+  size_t git_build_hash_len = strlen(BUILD_GIT_HASH);
+
+  uint8_t send_payload[2 + git_build_hash_len];
+  send_payload[0] = (uint8_t)(version & 0xFF);
+  send_payload[1] = (uint8_t)((version >> 8) & 0xFF);
+  memcpy(&send_payload[2], &BUILD_GIT_HASH, git_build_hash_len);
+
+  size_t len = build_message(0x02, send_payload, sizeof(send_payload),
+                             send_buffer, sizeof(send_buffer));
+  uart_write_bytes(MASTER_UART, send_buffer, len);
 }
 
 void app_main(void) {
-ESP_LOGI(TAG, "Starting Alox Powerpod Version %d Build: %s", version,
+  ESP_LOGI(TAG, "Starting Alox Powerpod Version %d Build: %s", version,
            BUILD_GIT_HASH);
 
   esp_err_t ret = nvs_flash_init();
@@ -74,17 +98,30 @@ ESP_LOGI(TAG, "Starting Alox Powerpod Version %d Build: %s", version,
   if (isMaster) {
     ESP_LOGI(TAG, "Started in Mastermode");
     add_peer(broadcast_address);
-    xTaskCreate(master_broadcast_task, "MasterBroadcast", 4096, NULL, 1, NULL);
-    // xTaskCreate(master_ping_task, "MasterPing", 4096, NULL, 1, NULL);
-    xTaskCreate(master_broadcast_ping, "MasterBroadcastPing", 4096, NULL, 1,
-                NULL);
-    xTaskCreate(client_monitor_task, "MonitorClientTask", 4096, NULL, 1, NULL);
-    init_uart();
-    //xTaskCreate(uart_status_task, "MasterUartStatusTask", 4096, NULL, 1, NULL);
-    xTaskCreate(SendClientInfoTask, "SendCientInfo", 4096, NULL, 1, NULL);
+    // xTaskCreate(master_broadcast_task, "MasterBroadcast", 4096, NULL, 1,
+    // NULL);
+    //  xTaskCreate(master_ping_task, "MasterPing", 4096, NULL, 1, NULL);
+    // xTaskCreate(master_broadcast_ping, "MasterBroadcastPing", 4096, NULL, 1,
+    //             NULL);
+    // xTaskCreate(client_monitor_task, "MonitorClientTask", 4096, NULL, 1,
+    // NULL);
+    QueueHandle_t parsed_message_queue =
+        xQueueCreate(10, sizeof(ParsedMessage_t));
+    init_uart(parsed_message_queue);
+    InitMessageBroker();
+
+    xTaskCreate(MessageBrokerTask, "message_handler_task", 4096,
+                (void *)&parsed_message_queue, 5, NULL);
+
+    RegisterCallback(0x01, echoCallback);
+    RegisterCallback(0x02, versionCallback);
+
+    // xTaskCreate(uart_status_task, "MasterUartStatusTask", 4096, NULL, 1,
+    // NULL); xTaskCreate(SendClientInfoTask, "SendCientInfo", 4096, NULL, 1,
+    // NULL);
   } else {
     ESP_LOGI(TAG, "Started in Slavemode");
-    xTaskCreate(client_data_sending_task, "ClientDataSending", 4096, NULL, 1,
-                NULL);
+    //xTaskCreate(client_data_sending_task, "ClientDataSending", 4096, NULL, 1,
+    //             NULL);
   }
 }

main/message_builder.c

@@ -0,0 +1,76 @@
+#include "message_builder.h"
+#include "message_parser.h"
+#include <stdbool.h>
+#include <stddef.h>
+
+bool needs_stuffing_byte(uint8_t byte) {
+  return (byte == StartByte || byte == EscapeByte || byte == EndByte);
+}
+
+bool add_byte_with_length_check(uint8_t byte, uint8_t write_index,
+                                uint8_t *data, uint8_t max_length) {
+  if (write_index >= max_length) {
+    return false;
+  }
+  data[write_index] = byte;
+  return true;
+}
+
+int build_message(uint8_t msgid, const uint8_t *payload, size_t payload_len,
+                  uint8_t *msg_buffer, size_t msg_buffer_size) {
+
+  if (payload_len + 4 > msg_buffer_size) {
+    return PayloadBiggerThenBuffer;
+  }
+
+  uint8_t checksum = 0;
+  size_t write_index = 0;
+  msg_buffer[write_index++] = StartByte;
+
+  if (needs_stuffing_byte(msgid)) {
+    if (!add_byte_with_length_check(EscapeByte, write_index, msg_buffer,
+                                    msg_buffer_size)) {
+      return BufferOverFlow;
+    }
+    write_index++;
+  }
+  if (!add_byte_with_length_check(msgid, write_index, msg_buffer,
+                                  msg_buffer_size)) {
+    return BufferOverFlow;
+  }
+  write_index++;
+  checksum ^= msgid;
+
+  for (size_t i = 0; i < payload_len; i++) {
+    if (needs_stuffing_byte(payload[i])) {
+      if (!add_byte_with_length_check(EscapeByte, write_index, msg_buffer,
+                                      msg_buffer_size)) {
+        return BufferOverFlow;
+      }
+      write_index++;
+    }
+    if (!add_byte_with_length_check(payload[i], write_index, msg_buffer,
+                                    msg_buffer_size)) {
+      return BufferOverFlow;
+    }
+
+    write_index++;
+    checksum ^= payload[i];
+  }
+
+  if (needs_stuffing_byte(checksum)) {
+    if (!add_byte_with_length_check(EscapeByte, write_index, msg_buffer,
+                                    msg_buffer_size)) {
+      return BufferOverFlow;
+    }
+    write_index++;
+  }
+
+  if (write_index + 2 > msg_buffer_size) {
+    return BufferOverFlow;
+  }
+  msg_buffer[write_index++] = checksum;
+  msg_buffer[write_index++] = EndByte;
+
+  return write_index;
+}

main/message_builder.h

@@ -0,0 +1,18 @@
+#ifndef _MESSAGE_BUILDER_HEADER
+#define _MESSAGE_BUILDER_HEADER
+
+#include "message_parser.h"
+#include <stddef.h>
+#include <stdint.h>
+
+enum BuildMessageErrors {
+  NoBuildError = 0,
+  PayloadBiggerThenBuffer = -1,
+  BufferOverFlow = -2,
+};
+
+// returns the length of msg_buffer
+int build_message(uint8_t msgid, const uint8_t *payload, size_t payload_len,
+                  uint8_t *msg_buffer, size_t msg_buffer_length);
+
+#endif