moving everyting over to an updated branch

components/message_bus/CMakeLists.txt

@@ -0,0 +1,3 @@
+idf_component_register(SRCS "message_bus.c"
+                    INCLUDE_DIRS "include"
+                    TEST_SRCS "test/test_message_bus.c")

components/message_bus/include/message_bus.h

@@ -0,0 +1,25 @@
+#ifndef MESSAGE_BUS_H
+#define MESSAGE_BUS_H
+
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+
+#define MAX_SUBSCRIBERS 10
+
+typedef enum {
+    TOPIC_SERVO_COMMAND,
+    TOPIC_SERVO_TELEMETRY,
+    TOPIC_SYSTEM_STATE,
+    // Add other topics here
+} MessageTopic_t;
+
+typedef struct {
+    MessageTopic_t topic;
+    void* data;
+} Message_t;
+
+void message_bus_init(void);
+void message_bus_subscribe(MessageTopic_t topic, QueueHandle_t queue);
+void message_bus_publish(Message_t* message);
+
+#endif // MESSAGE_BUS_H

components/message_bus/message_bus.c

@@ -0,0 +1,32 @@
+#include "message_bus.h"
+#include "esp_log.h"
+
+static const char *TAG = "MESSAGE_BUS";
+
+static QueueHandle_t s_subscriber_queues[MAX_SUBSCRIBERS];
+static MessageTopic_t s_subscriber_topics[MAX_SUBSCRIBERS];
+static int s_num_subscribers = 0;
+
+void message_bus_init(void) {
+    // Initialization logic, if any
+}
+
+void message_bus_subscribe(MessageTopic_t topic, QueueHandle_t queue) {
+    if (s_num_subscribers < MAX_SUBSCRIBERS) {
+        s_subscriber_topics[s_num_subscribers] = topic;
+        s_subscriber_queues[s_num_subscribers] = queue;
+        s_num_subscribers++;
+    } else {
+        ESP_LOGE(TAG, "Cannot subscribe, max subscribers reached");
+    }
+}
+
+void message_bus_publish(Message_t* message) {
+    for (int i = 0; i < s_num_subscribers; i++) {
+        if (s_subscriber_topics[i] == message->topic) {
+            if (xQueueSend(s_subscriber_queues[i], message, (TickType_t)0) != pdPASS) {
+                ESP_LOGE(TAG, "Failed to send message to subscriber");
+            }
+        }
+    }
+}

components/message_bus/test/test_message_bus.c

@@ -0,0 +1,26 @@
+#include "unity.h"
+#include "message_bus.h"
+
+TEST_CASE("Message bus publish and subscribe", "[message_bus]")
+{
+    message_bus_init();
+
+    QueueHandle_t queue = xQueueCreate(1, sizeof(Message_t));
+    TEST_ASSERT_NOT_NULL(queue);
+
+    message_bus_subscribe(TOPIC_SYSTEM_STATE, queue);
+
+    Message_t msg_to_publish = {
+        .topic = TOPIC_SYSTEM_STATE,
+        .data = (void*)42,
+    };
+
+    message_bus_publish(&msg_to_publish);
+
+    Message_t received_msg;
+    TEST_ASSERT_EQUAL(pdTRUE, xQueueReceive(queue, &received_msg, pdMS_TO_TICKS(100)));
+    TEST_ASSERT_EQUAL(TOPIC_SYSTEM_STATE, received_msg.topic);
+    TEST_ASSERT_EQUAL(42, (int)received_msg.data);
+
+    vQueueDelete(queue);
+}

components/servo_controller/CMakeLists.txt

@@ -0,0 +1,3 @@
+idf_component_register(SRCS "servo_controller.c"
+                    INCLUDE_DIRS "include"
+                    REQUIRES message_bus feetech_protocol)

components/servo_controller/include/servo_controller.h

@@ -0,0 +1,18 @@
+#ifndef SERVO_CONTROLLER_H
+#define SERVO_CONTROLLER_H
+
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+#include "feetech_protocol.h"
+
+typedef struct {
+    uint8_t servo_id;
+    uint8_t command;
+    uint8_t reg_address;
+    uint16_t value;
+    QueueHandle_t response_queue;
+} ServoCommand_t;
+
+void servo_controller_init(void);
+
+#endif // SERVO_CONTROLLER_H

components/servo_controller/servo_controller.c

@@ -0,0 +1,74 @@
+#include "servo_controller.h"
+#include "message_bus.h"
+#include "esp_log.h"
+#include "feetech_protocol.h"
+#include "driver/uart.h"
+
+static const char *TAG = "SERVO_CONTROLLER";
+static QueueHandle_t s_servo_command_queue;
+
+#define SERVO_UART_PORT UART_NUM_1
+#define SERVO_TXD_PIN 17
+#define SERVO_RXD_PIN 16
+
+static void servo_controller_task(void *pvParameters) {
+    Message_t msg;
+    while (1) {
+        if (xQueueReceive(s_servo_command_queue, &msg, portMAX_DELAY)) {
+            if (msg.topic == TOPIC_SERVO_COMMAND) {
+                ServoCommand_t* cmd = (ServoCommand_t*)msg.data;
+
+                BusResponse_t response;
+                response.status = ESP_FAIL;
+                response.value = 0;
+
+                switch (cmd->command) {
+                    case CMD_WRITE_BYTE:
+                        scs_write_byte(SERVO_UART_PORT, cmd->servo_id, cmd->reg_address, cmd->value);
+                        response.status = ESP_OK;
+                        break;
+                    case CMD_WRITE_WORD:
+                        scs_write_word(SERVO_UART_PORT, cmd->servo_id, cmd->reg_address, cmd->value);
+                        response.status = ESP_OK;
+                        break;
+                    case CMD_READ_BYTE:
+                        response.value = scs_read_byte(SERVO_UART_PORT, cmd->servo_id, cmd->reg_address);
+                        response.status = ESP_OK;
+                        break;
+                    case CMD_READ_WORD:
+                        response.value = scs_read_word(SERVO_UART_PORT, cmd->servo_id, cmd->reg_address);
+                        response.status = ESP_OK;
+                        break;
+                    default:
+                        ESP_LOGE(TAG, "Unknown servo command: %d", cmd->command);
+                        break;
+                }
+
+                if (cmd->response_queue != NULL) {
+                    xQueueSend(cmd->response_queue, &response, (TickType_t)0);
+                }
+
+                free(cmd);
+            }
+        }
+    }
+}
+
+void servo_controller_init(void) {
+    s_servo_command_queue = xQueueCreate(10, sizeof(Message_t));
+    message_bus_subscribe(TOPIC_SERVO_COMMAND, s_servo_command_queue);
+
+    uart_config_t uart_config = {
+        .baud_rate = 1000000,
+        .data_bits = UART_DATA_8_BITS,
+        .parity = UART_PARITY_DISABLE,
+        .stop_bits = UART_STOP_BITS_1,
+        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
+        .source_clk = UART_SCLK_DEFAULT,
+    };
+    uart_param_config(SERVO_UART_PORT, &uart_config);
+    uart_set_pin(SERVO_UART_PORT, SERVO_TXD_PIN, SERVO_RXD_PIN, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
+    uart_driver_install(SERVO_UART_PORT, 256, 256, 0, NULL, 0);
+
+    xTaskCreate(servo_controller_task, "servo_controller_task", 4096, NULL, 5, NULL);
+}

main/CMakeLists.txt

@@ -1,5 +1,6 @@
 idf_component_register(SRCS "main.c" "feetech_protocol.c" "bma400_driver.c" "led_indicator.c" "nvs_storage.c" "mcp_server.c" "console.c" "config.c" "planner.c" "behavior.c" "omni_base.c" "synsense_driver.c"
                     INCLUDE_DIRS "."
+                    TEST_SRCS "test/test_console.c"
 
                     REQUIRES driver nvs_flash console esp_timer esp_wifi esp_event esp_netif lwip json
                     PRIV_REQUIRES mbedtls)