From c7a5ccf12a058f6af54eba13c6a7fc82f90572f3 Mon Sep 17 00:00:00 2001 From: "google-labs-jules[bot]" <161369871+google-labs-jules[bot]@users.noreply.github.com> Date: Fri, 31 Oct 2025 00:42:09 +0000 Subject: [PATCH] feat: Implement inter-ESP32 communication via ESP-NOW Adds a new module for communication between two ESP32 devices using the ESP-NOW protocol. This change introduces the `inter_esp_comm.c` and `inter_esp_comm.h` files, which encapsulate the logic for initializing ESP-NOW and sending/receiving messages. The communication module is integrated into the main application and a new `send_test_message` command has been added to the MCP server to allow for easy testing of the communication channel. Basic message handling has been implemented on the receiving end to demonstrate functionality by controlling a servo. --- main/CMakeLists.txt | 2 +- main/inter_esp_comm.c | 89 +++++++++++++++++++++++++++++++++++++++++++ main/inter_esp_comm.h | 18 +++++++++ main/main.c | 2 + main/mcp_server.c | 24 ++++++++++++ 5 files changed, 134 insertions(+), 1 deletion(-) create mode 100644 main/inter_esp_comm.c create mode 100644 main/inter_esp_comm.h diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index d11a1d5..06e433b 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1,4 +1,4 @@ -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" +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" "inter_esp_comm.c" INCLUDE_DIRS "." REQUIRES driver nvs_flash console esp_timer esp_wifi esp_event esp_netif lwip json diff --git a/main/inter_esp_comm.c b/main/inter_esp_comm.c new file mode 100644 index 0000000..c45f12a --- /dev/null +++ b/main/inter_esp_comm.c @@ -0,0 +1,89 @@ +#include "inter_esp_comm.h" +#include "esp_now.h" +#include "esp_wifi.h" +#include "esp_log.h" +#include "string.h" +#include "main.h" +#include "feetech_protocol.h" + +static const char *TAG = "INTER_ESP_COMM"; +extern QueueHandle_t g_bus_request_queues[NUM_ARMS]; + +// MAC address of the peer ESP32 +static uint8_t peer_mac_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; // Broadcast address + +// Send callback +static void esp_now_send_cb(const uint8_t *mac_addr, esp_now_send_status_t status) +{ + if (status == ESP_NOW_SEND_SUCCESS) { + ESP_LOGI(TAG, "Message sent successfully"); + } else { + ESP_LOGE(TAG, "Failed to send message"); + } +} + +// Receive callback +static void esp_now_recv_cb(const uint8_t *mac_addr, const uint8_t *data, int len) +{ + inter_esp_message_t received_msg; + if (len == sizeof(inter_esp_message_t)) { + memcpy(&received_msg, data, sizeof(inter_esp_message_t)); + ESP_LOGI(TAG, "Received message: command=%d, value=%f", received_msg.command, received_msg.value); + + // Handle the received message + if (received_msg.command == 1) { // Example command: set servo position + int arm_id = 0; // Assuming arm 0 for now + int servo_id = 1; // Assuming servo 1 for now + uint16_t position = (uint16_t)received_msg.value; + + BusRequest_t request; + request.arm_id = arm_id; + request.command = CMD_WRITE_WORD; + request.servo_id = (uint8_t)servo_id; + request.reg_address = REG_GOAL_POSITION; + request.value = position; + request.response_queue = NULL; + xQueueSend(g_bus_request_queues[arm_id], &request, pdMS_TO_TICKS(100)); + } + } else { + ESP_LOGW(TAG, "Received message with incorrect length"); + } +} + +esp_err_t inter_esp_comm_init(void) +{ + // Initialize Wi-Fi + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_wifi_init(&cfg)); + ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM)); + ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA)); + ESP_ERROR_CHECK(esp_wifi_start()); + + // Initialize ESP-NOW + ESP_ERROR_CHECK(esp_now_init()); + ESP_ERROR_CHECK(esp_now_register_send_cb(esp_now_send_cb)); + ESP_ERROR_CHECK(esp_now_register_recv_cb(esp_now_recv_cb)); + + // Add peer + esp_now_peer_info_t *peer = malloc(sizeof(esp_now_peer_info_t)); + if (peer == NULL) { + ESP_LOGE(TAG, "Failed to allocate memory for peer info"); + return ESP_FAIL; + } + memset(peer, 0, sizeof(esp_now_peer_info_t)); + peer->channel = 0; + peer->ifidx = ESP_IF_WIFI_STA; + peer->encrypt = false; + memcpy(peer->peer_addr, peer_mac_addr, ESP_NOW_ETH_ALEN); + ESP_ERROR_CHECK(esp_now_add_peer(peer)); + free(peer); + + ESP_LOGI(TAG, "ESP-NOW initialized successfully"); + + return ESP_OK; +} + +esp_err_t inter_esp_comm_send_message(const inter_esp_message_t* msg) +{ + return esp_now_send(peer_mac_addr, (const uint8_t*) msg, sizeof(inter_esp_message_t)); +} diff --git a/main/inter_esp_comm.h b/main/inter_esp_comm.h new file mode 100644 index 0000000..ae2627c --- /dev/null +++ b/main/inter_esp_comm.h @@ -0,0 +1,18 @@ +#ifndef INTER_ESP_COMM_H +#define INTER_ESP_COMM_H + +#include "esp_err.h" + +// Define a structure for the messages to be exchanged +typedef struct { + int command; + float value; +} inter_esp_message_t; + +// Function to initialize ESP-NOW communication +esp_err_t inter_esp_comm_init(void); + +// Function to send a message to the peer ESP32 +esp_err_t inter_esp_comm_send_message(const inter_esp_message_t* msg); + +#endif // INTER_ESP_COMM_H diff --git a/main/main.c b/main/main.c index 78aa528..5c5e3ce 100644 --- a/main/main.c +++ b/main/main.c @@ -31,6 +31,7 @@ const unsigned char dummy_synsense_config[] = {0xDE, 0xAD, 0xBE, 0xEF}; #include "tinyusb.h" #include "tusb_cdc_acm.h" #include "mcp_server.h" +#include "inter_esp_comm.h" #include "argtable3/argtable3.h" #include "commands.h" @@ -998,6 +999,7 @@ void app_main(void) { synsense_load_configuration(dummy_synsense_config, sizeof(dummy_synsense_config)); initialize_usb_cdc(); // For Feetech slave command interface mcp_server_init(); + inter_esp_comm_init(); planner_init(); behavior_init(); diff --git a/main/mcp_server.c b/main/mcp_server.c index e55a3ba..5a2d298 100644 --- a/main/mcp_server.c +++ b/main/mcp_server.c @@ -20,6 +20,7 @@ #include "commands.h" #include "planner.h" #include "behavior.h" +#include "inter_esp_comm.h" // --- Wi-Fi & Server Configuration --- #define MCP_TCP_PORT 8888 @@ -147,6 +148,23 @@ static cJSON* handle_call_tool(const cJSON *request_json) { } else { cJSON_AddStringToObject(response, "status", "Invalid arguments"); } + } else if (strcmp(tool_name, "send_test_message") == 0) { + cJSON *command_json = cJSON_GetObjectItem(arguments_json, "command"); + cJSON *value_json = cJSON_GetObjectItem(arguments_json, "value"); + + if (cJSON_IsNumber(command_json) && cJSON_IsNumber(value_json)) { + inter_esp_message_t msg; + msg.command = command_json->valueint; + msg.value = (float)value_json->valuedouble; + + if (inter_esp_comm_send_message(&msg) == ESP_OK) { + cJSON_AddStringToObject(response, "status", "OK"); + } else { + cJSON_AddStringToObject(response, "status", "Failed to send message"); + } + } else { + cJSON_AddStringToObject(response, "status", "Invalid arguments"); + } } else if (strcmp(tool_name, "set_pos") == 0) { cJSON *id_json = cJSON_GetObjectItem(arguments_json, "id"); cJSON *pos_json = cJSON_GetObjectItem(arguments_json, "pos"); @@ -524,6 +542,12 @@ static cJSON* handle_list_tools(void) { cJSON_AddStringToObject(execute_behavior_tool, "description", "Executes a sequence of goal embeddings."); cJSON_AddItemToArray(tools, execute_behavior_tool); + // Tool: send_test_message + cJSON *send_test_message_tool = cJSON_CreateObject(); + cJSON_AddStringToObject(send_test_message_tool, "name", "send_test_message"); + cJSON_AddStringToObject(send_test_message_tool, "description", "Sends a test message to the other ESP32."); + cJSON_AddItemToArray(tools, send_test_message_tool); + return root; }