From 5cc05c2dd721f48e08e52daf1b1dc1b0a9d6d190 Mon Sep 17 00:00:00 2001 From: Bryce Gruber Date: Wed, 18 Jun 2025 23:38:20 -0500 Subject: [PATCH 1/4] add espnow peer manager --- components/espnow_server/CMakeLists.txt | 2 +- components/espnow_server/espnow_server.cpp | 3 - components/peer_manager/CMakeLists.txt | 5 + .../peer_manager/include/peer_manager.h | 37 ++++ components/peer_manager/peer_manager.cpp | 194 ++++++++++++++++++ 5 files changed, 237 insertions(+), 4 deletions(-) create mode 100644 components/peer_manager/CMakeLists.txt create mode 100644 components/peer_manager/include/peer_manager.h create mode 100644 components/peer_manager/peer_manager.cpp diff --git a/components/espnow_server/CMakeLists.txt b/components/espnow_server/CMakeLists.txt index 3dd7bdd..8596913 100644 --- a/components/espnow_server/CMakeLists.txt +++ b/components/espnow_server/CMakeLists.txt @@ -1,5 +1,5 @@ idf_component_register( SRCS "espnow_server.cpp" INCLUDE_DIRS "include" - REQUIRES esp_app_format esp_netif nvs_flash esp_wifi esp_event esp_ringbuf tasks msp + REQUIRES esp_app_format esp_netif nvs_flash esp_wifi esp_event esp_ringbuf tasks msp peer_manager ) diff --git a/components/espnow_server/espnow_server.cpp b/components/espnow_server/espnow_server.cpp index a569cf8..8d4ba8d 100644 --- a/components/espnow_server/espnow_server.cpp +++ b/components/espnow_server/espnow_server.cpp @@ -86,9 +86,6 @@ static int sendMSPViaEspnow(mspPacket_t *packet) return esp_err; } - esp_now_peer_num_t pn; - esp_now_get_peer_num(&pn); - esp_err = esp_now_send(sendAddress, (uint8_t *)&nowDataOutput, packetSize); ESP_LOGI(TAG, "Sent ESPNOW message"); diff --git a/components/peer_manager/CMakeLists.txt b/components/peer_manager/CMakeLists.txt new file mode 100644 index 0000000..d970848 --- /dev/null +++ b/components/peer_manager/CMakeLists.txt @@ -0,0 +1,5 @@ +idf_component_register( + SRCS "peer_manager.cpp" + INCLUDE_DIRS "include" + REQUIRES esp_wifi esp_event esp_ringbuf msp +) diff --git a/components/peer_manager/include/peer_manager.h b/components/peer_manager/include/peer_manager.h new file mode 100644 index 0000000..9762f0a --- /dev/null +++ b/components/peer_manager/include/peer_manager.h @@ -0,0 +1,37 @@ +#include +#include +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/ringbuf.h" +#include "esp_now.h" +#include "msp.h" + +#define MAX_RETRIES 5 +#define BUFFER_MAX_ITEMS 64 + +typedef struct Peer +{ + esp_now_peer_info_t peerInfo; + TaskHandle_t taskHandle = NULL; + RingbufHandle_t buffer = xRingbufferCreateNoSplit(sizeof(mspPacket_t), BUFFER_MAX_ITEMS); + + Peer(uint8_t peer_address[]); + ~Peer(); +} Peer_t; + +class PeerManager +{ +private: + std::vector peers; + SemaphoreHandle_t xSemaphore = NULL; + const Peer_t* findPeer(const uint8_t *address); + +public: + PeerManager(); + void addPeer(uint8_t address[]); + void removePeer(uint8_t *address); + void clearPeers(); + void sendToPeer(uint8_t *address, mspPacket_t *packet); + void sendToPeers(mspPacket_t *packet); + void notifyPeer(const uint8_t *address, esp_now_send_status_t status); +} peerManager; \ No newline at end of file diff --git a/components/peer_manager/peer_manager.cpp b/components/peer_manager/peer_manager.cpp new file mode 100644 index 0000000..c997e0d --- /dev/null +++ b/components/peer_manager/peer_manager.cpp @@ -0,0 +1,194 @@ +#include +#include +#include +#include "esp_log.h" +#include "peer_manager.h" +#include "msp.h" + +static const char *TAG = "peer_manager"; + +static void espnowSendCB(const uint8_t *mac_addr, esp_now_send_status_t status) +{ + peerManager.notifyPeer(mac_addr, status); +} + +static void sendToPeerTask(void *pvParameters) +{ + MSP msp; + int sendStatus; + uint8_t sendAttempts; + uint32_t sendSuccess; + + Peer_t *peer = (Peer_t *)pvParameters; + + while (1) + { + size_t item_size; + mspPacket_t *packet = (mspPacket_t *)xRingbufferReceive(peer->buffer, &item_size, portMAX_DELAY); + + if (packet != NULL) + { + uint8_t packetSize = msp.getTotalPacketSize(packet); + uint8_t nowDataOutput[packetSize]; + + uint8_t result = msp.convertToByteArray(packet, nowDataOutput); + if (!result) + { + ESP_LOGE(TAG, "Packet could not be converted to array"); + vRingbufferReturnItem(peer->buffer, (void *)packet); + continue; + } + + sendAttempts = 0; + do + { + sendStatus = esp_now_send(peer->peerInfo.peer_addr, (uint8_t *)&nowDataOutput, packetSize); + + if (sendStatus == ESP_OK) + xTaskNotifyWait(0x00, ULONG_MAX, &sendSuccess, portMAX_DELAY); + else + { + ESP_LOGW(TAG, "ESPNOW message send status: %d", sendStatus); + break; + } + + } while (++sendAttempts < MAX_RETRIES && !sendSuccess); + + vRingbufferReturnItem(peer->buffer, (void *)packet); + } + } +} + +Peer::Peer(uint8_t address[]) +{ + memset(&peerInfo, 0, sizeof(peerInfo)); + memcpy(peerInfo.peer_addr, address, 6); + peerInfo.channel = 0; + peerInfo.encrypt = false; +} + +Peer::~Peer() +{ + vTaskDelete(taskHandle); + vRingbufferDelete(buffer); + esp_now_del_peer(peerInfo.peer_addr); +} + +PeerManager::PeerManager() +{ + vSemaphoreCreateBinary(xSemaphore); + ESP_ERROR_CHECK(esp_now_register_send_cb(espnowSendCB)); +} + +const Peer_t * +PeerManager::findPeer(const uint8_t *address) +{ + for (const Peer_t &peer : peers) + { + if (peer.peerInfo.peer_addr[0] == address[0] && + peer.peerInfo.peer_addr[1] == address[1] && + peer.peerInfo.peer_addr[2] == address[2] && + peer.peerInfo.peer_addr[3] == address[3] && + peer.peerInfo.peer_addr[4] == address[4] && + peer.peerInfo.peer_addr[5] == address[5]) + return &peer; + } + return nullptr; +} + +void PeerManager::addPeer(uint8_t address[]) +{ + if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) + { + Peer_t peer(address); + + int status = esp_now_add_peer(&peer.peerInfo); + if (status != ESP_OK) + { + ESP_LOGE(TAG, "Failed to register new peer: %d", status); + xSemaphoreGive(xSemaphore); + return; + } + + xTaskCreate(sendToPeerTask, "sendToPeerTask", 4096, (void *)&peer, 9, &peer.taskHandle); + peers.push_back(peer); + + xSemaphoreGive(xSemaphore); + } +} + +void PeerManager::removePeer(uint8_t *address) +{ + if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) + { + for (int i = 0; i < peers.size(); i++) + { + const Peer_t *peer = &peers[i]; + + if (peer->peerInfo.peer_addr[0] == address[0] && + peer->peerInfo.peer_addr[1] == address[1] && + peer->peerInfo.peer_addr[2] == address[2] && + peer->peerInfo.peer_addr[3] == address[3] && + peer->peerInfo.peer_addr[4] == address[4] && + peer->peerInfo.peer_addr[5] == address[5]) + { + peers.erase(peers.begin() + i); + xSemaphoreGive(xSemaphore); + return; + } + } + xSemaphoreGive(xSemaphore); + } +} + +void PeerManager::clearPeers() +{ + if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) + { + peers.clear(); + xSemaphoreGive(xSemaphore); + } +} + +void PeerManager::sendToPeer(uint8_t *address, mspPacket_t *packet) +{ + if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) + { + const Peer_t *peer = findPeer(address); + if (peer != nullptr) + xRingbufferSend(peer->buffer, packet, sizeof(mspPacket_t), pdMS_TO_TICKS(1000)); + xSemaphoreGive(xSemaphore); + } +} + +void PeerManager::sendToPeers(mspPacket_t *packet) +{ + if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) + { + for (const Peer_t &peer : peers) + { + xRingbufferSend(peer.buffer, packet, sizeof(mspPacket_t), pdMS_TO_TICKS(1000)); + } + xSemaphoreGive(xSemaphore); + } +} + +void PeerManager::notifyPeer(const uint8_t *address, esp_now_send_status_t status) +{ + if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) + { + const Peer_t *peer = findPeer(address); + if (peer == nullptr) + { + xSemaphoreGive(xSemaphore); + return; + } + + if (status == ESP_NOW_SEND_SUCCESS) + xTaskNotify(peer->taskHandle, (uint32_t)1, eSetValueWithOverwrite); + else + xTaskNotify(peer->taskHandle, (uint32_t)0, eSetValueWithOverwrite); + + xSemaphoreGive(xSemaphore); + } +} \ No newline at end of file From 7b96fa8ccf4db171a768b9a71e938c1b37b692c5 Mon Sep 17 00:00:00 2001 From: Bryce Gruber Date: Wed, 18 Jun 2025 23:58:13 -0500 Subject: [PATCH 2/4] store peer in vector --- components/peer_manager/peer_manager.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/components/peer_manager/peer_manager.cpp b/components/peer_manager/peer_manager.cpp index c997e0d..d5d616c 100644 --- a/components/peer_manager/peer_manager.cpp +++ b/components/peer_manager/peer_manager.cpp @@ -100,18 +100,18 @@ void PeerManager::addPeer(uint8_t address[]) { if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) { - Peer_t peer(address); + peers.push_back({address}); - int status = esp_now_add_peer(&peer.peerInfo); + int status = esp_now_add_peer(&peers.back().peerInfo); if (status != ESP_OK) { ESP_LOGE(TAG, "Failed to register new peer: %d", status); + peers.pop_back(); xSemaphoreGive(xSemaphore); return; } - xTaskCreate(sendToPeerTask, "sendToPeerTask", 4096, (void *)&peer, 9, &peer.taskHandle); - peers.push_back(peer); + xTaskCreate(sendToPeerTask, "sendToPeerTask", 4096, (void *)&peers.back(), 9, &peers.back().taskHandle); xSemaphoreGive(xSemaphore); } From ffe5aaaed06cebe16d7b080afe5db238ef469aa9 Mon Sep 17 00:00:00 2001 From: Bryce Gruber Date: Sat, 21 Jun 2025 23:58:38 -0500 Subject: [PATCH 3/4] update vector practices --- components/peer_manager/include/peer_manager.h | 13 +++++++------ components/peer_manager/peer_manager.cpp | 15 ++++++++++----- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/components/peer_manager/include/peer_manager.h b/components/peer_manager/include/peer_manager.h index 9762f0a..7fcb32c 100644 --- a/components/peer_manager/include/peer_manager.h +++ b/components/peer_manager/include/peer_manager.h @@ -16,22 +16,23 @@ typedef struct Peer RingbufHandle_t buffer = xRingbufferCreateNoSplit(sizeof(mspPacket_t), BUFFER_MAX_ITEMS); Peer(uint8_t peer_address[]); - ~Peer(); + virtual ~Peer(); } Peer_t; class PeerManager { -private: - std::vector peers; - SemaphoreHandle_t xSemaphore = NULL; - const Peer_t* findPeer(const uint8_t *address); - public: PeerManager(); + virtual ~PeerManager(); void addPeer(uint8_t address[]); void removePeer(uint8_t *address); void clearPeers(); void sendToPeer(uint8_t *address, mspPacket_t *packet); void sendToPeers(mspPacket_t *packet); void notifyPeer(const uint8_t *address, esp_now_send_status_t status); + +private: + std::vector peers; + SemaphoreHandle_t xSemaphore = NULL; + const Peer_t *findPeer(const uint8_t *address); } peerManager; \ No newline at end of file diff --git a/components/peer_manager/peer_manager.cpp b/components/peer_manager/peer_manager.cpp index d5d616c..f1cdcb6 100644 --- a/components/peer_manager/peer_manager.cpp +++ b/components/peer_manager/peer_manager.cpp @@ -16,7 +16,7 @@ static void sendToPeerTask(void *pvParameters) { MSP msp; int sendStatus; - uint8_t sendAttempts; + uint8_t sendAttempt; uint32_t sendSuccess; Peer_t *peer = (Peer_t *)pvParameters; @@ -39,7 +39,7 @@ static void sendToPeerTask(void *pvParameters) continue; } - sendAttempts = 0; + sendAttempt = 0; do { sendStatus = esp_now_send(peer->peerInfo.peer_addr, (uint8_t *)&nowDataOutput, packetSize); @@ -52,7 +52,7 @@ static void sendToPeerTask(void *pvParameters) break; } - } while (++sendAttempts < MAX_RETRIES && !sendSuccess); + } while (++sendAttempt < MAX_RETRIES && !sendSuccess); vRingbufferReturnItem(peer->buffer, (void *)packet); } @@ -80,6 +80,11 @@ PeerManager::PeerManager() ESP_ERROR_CHECK(esp_now_register_send_cb(espnowSendCB)); } +PeerManager::~PeerManager() +{ + vSemaphoreDelete(xSemaphore); +} + const Peer_t * PeerManager::findPeer(const uint8_t *address) { @@ -100,7 +105,7 @@ void PeerManager::addPeer(uint8_t address[]) { if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) { - peers.push_back({address}); + peers.emplace_back(address); int status = esp_now_add_peer(&peers.back().peerInfo); if (status != ESP_OK) @@ -111,7 +116,7 @@ void PeerManager::addPeer(uint8_t address[]) return; } - xTaskCreate(sendToPeerTask, "sendToPeerTask", 4096, (void *)&peers.back(), 9, &peers.back().taskHandle); + xTaskCreate(sendToPeerTask, "PeerSenderTask", 4096, (void *)&peers.back(), 9, &peers.back().taskHandle); xSemaphoreGive(xSemaphore); } From db9841e19478d9dba9d31d0562c08edf16484ee9 Mon Sep 17 00:00:00 2001 From: Bryce Gruber Date: Sun, 22 Jun 2025 22:44:59 -0500 Subject: [PATCH 4/4] integrate into testing --- components/espnow_server/espnow_server.cpp | 308 ++++++++---------- components/msp/include/msptypes.h | 3 +- .../peer_manager/include/peer_manager.h | 4 +- components/peer_manager/peer_manager.cpp | 14 +- 4 files changed, 145 insertions(+), 184 deletions(-) diff --git a/components/espnow_server/espnow_server.cpp b/components/espnow_server/espnow_server.cpp index 8d4ba8d..c696202 100644 --- a/components/espnow_server/espnow_server.cpp +++ b/components/espnow_server/espnow_server.cpp @@ -14,6 +14,7 @@ #include "tasks.h" #include "msptypes.h" #include "msp.h" +#include "peer_manager.h" #define NO_BINDING_TIMEOUT 120000 / portTICK_PERIOD_MS #define STORAGE_NAMESPACE "netpack" @@ -24,8 +25,6 @@ static const char *TAG = "espnow_server"; const esp_app_desc_t *description = esp_app_get_description(); const TickType_t espnowDelay = CONFIG_ESPNOW_SEND_DELAY / portTICK_PERIOD_MS; -static uint8_t bindAddress[6]; -static uint8_t sendAddress[6]; static nvs_handle_t bp_mac_handle; static TaskHandle_t espnowTaskHandle = NULL; @@ -58,52 +57,9 @@ static void runBindTask(void *pvParameters) isBinding = false; } -static void registerPeer(uint8_t *address) -{ - esp_now_peer_info_t peerInfo; - memset(&peerInfo, 0, sizeof(peerInfo)); - memcpy(peerInfo.peer_addr, address, 6); - peerInfo.channel = 0; - peerInfo.encrypt = false; - if (esp_now_add_peer(&peerInfo) != ESP_OK) - { - ESP_LOGE(TAG, "ESP-NOW failed to add peer"); - } -} - -static int sendMSPViaEspnow(mspPacket_t *packet) -{ - MSP msp; - int esp_err = -1; - uint8_t packetSize = msp.getTotalPacketSize(packet); - uint8_t nowDataOutput[packetSize]; - - uint8_t result = msp.convertToByteArray(packet, nowDataOutput); - - if (!result) - { - ESP_LOGE(TAG, "Packet could not be converted to array"); - return esp_err; - } - - esp_err = esp_now_send(sendAddress, (uint8_t *)&nowDataOutput, packetSize); - - ESP_LOGI(TAG, "Sent ESPNOW message"); - return esp_err; -} - -static void espnowSendCB(const uint8_t *mac_addr, esp_now_send_status_t status) -{ - if (status == ESP_NOW_SEND_SUCCESS) - xTaskNotify(espnowTaskHandle, (uint32_t)1, eSetValueWithOverwrite); - else - xTaskNotify(espnowTaskHandle, (uint32_t)0, eSetValueWithOverwrite); -} - static void espnowRecvCB(const esp_now_recv_info_t *recv_info, const uint8_t *data, int len) { MSP msp; - esp_now_peer_info_t peerInfo; for (int i = 0; i < len; i++) { if (msp.processReceivedByte(data[i])) @@ -130,8 +86,6 @@ static void espnowRecvCB(const esp_now_recv_info_t *recv_info, const uint8_t *da recievedAddress[i] = packet->payload[i]; } - recievedAddress[0] = recievedAddress[0] & ~0x01; - if (recievedAddress[0] == 0 && recievedAddress[1] == 0 && recievedAddress[2] == 0 && recievedAddress[3] == 0 && recievedAddress[4] == 0 && recievedAddress[5] == 0) { @@ -156,23 +110,13 @@ static void espnowRecvCB(const esp_now_recv_info_t *recv_info, const uint8_t *da nvs_close(bp_mac_handle); - if (bindAddress[0] == sendAddress[0] && bindAddress[1] == sendAddress[1] && bindAddress[2] == sendAddress[2] && - bindAddress[3] == sendAddress[3] && bindAddress[4] == sendAddress[4] && bindAddress[5] == sendAddress[5]) - { - memset(&sendAddress, 0, sizeof(sendAddress)); - memcpy(sendAddress, recievedAddress, 6); - - if (esp_now_fetch_peer(true, &peerInfo) == ESP_OK) - esp_now_del_peer(peerInfo.peer_addr); - - registerPeer(sendAddress); - } + recievedAddress[0] = recievedAddress[0] & ~0x01; + recievedAddress[5] = 3; - memset(&bindAddress, 0, sizeof(bindAddress)); - memcpy(bindAddress, recievedAddress, 6); + ESP_ERROR_CHECK(esp_wifi_set_mac(WIFI_IF_STA, recievedAddress)); - ESP_LOGI(TAG, "Backpack UID set to: [%d,%d,%d,%d,%d,%d]", bindAddress[0], bindAddress[1], - bindAddress[2], bindAddress[3], bindAddress[4], bindAddress[5]); + ESP_LOGI(TAG, "Backpack UID set to: [%d,%d,%d,%d,%d,%d]", recievedAddress[0], recievedAddress[1], + recievedAddress[2], recievedAddress[3], recievedAddress[4], recievedAddress[5]); break; } @@ -190,6 +134,117 @@ static void espnowRecvCB(const esp_now_recv_info_t *recv_info, const uint8_t *da } } +void processPacketFromHost(mspPacket_t *packet) +{ + switch (packet->function) + { + case MSP_ELRS_GET_BACKPACK_VERSION: + { + ESP_LOGI(TAG, "Processing MSP_ELRS_GET_BACKPACK_VERSION..."); + + mspPacket_t out; + out.reset(); + out.makeResponse(); + out.function = MSP_ELRS_GET_BACKPACK_VERSION; + for (size_t i = 0; i < sizeof(description->version); i++) + { + out.addByte(description->version[i]); + } + + if (xRingbufferSend(xRingReceivedEspnow, &out, sizeof(mspPacket_t), pdMS_TO_TICKS(1000)) == pdTRUE) + ESP_LOGI(TAG, "Added device version to ring buffer"); + else + ESP_LOGE(TAG, "Failed to add item to ring buffer"); + + break; + } + case MSP_ELRS_BACKPACK_SET_MODE: + { + if (packet->payloadSize == 1) + { + if (packet->payload[0] == 'B') + { + ESP_LOGI(TAG, "Enter binding mode..."); + isBinding = true; + } + if (bindTaskHandle != NULL) + { + vTaskDelete(bindTaskHandle); + bindTaskHandle = NULL; + } + + xTaskCreate(runBindTask, "BindTask", 4096, NULL, 10, &bindTaskHandle); + sendInProgressResponse(); + } + break; + } + case MSP_ELRS_REGISTER_ESPNOW_PEER: + { + ESP_LOGI(TAG, "Processing MSP_ELRS_REGISTER_ESPNOW_PEER..."); + uint8_t mode = packet->readByte(); + + // Set target send address + if (mode == 0x01) + { + uint8_t receivedAddress[6]; + receivedAddress[0] = packet->readByte(); + receivedAddress[1] = packet->readByte(); + receivedAddress[2] = packet->readByte(); + receivedAddress[3] = packet->readByte(); + receivedAddress[4] = packet->readByte(); + receivedAddress[5] = packet->readByte(); + + peerManager.addPeer(receivedAddress); + } + else if (mode == 0x02) + { + uint8_t receivedAddress[6]; + receivedAddress[0] = packet->readByte(); + receivedAddress[1] = packet->readByte(); + receivedAddress[2] = packet->readByte(); + receivedAddress[3] = packet->readByte(); + receivedAddress[4] = packet->readByte(); + receivedAddress[5] = packet->readByte(); + + peerManager.removePeer(receivedAddress); + } + else if (mode == 0x03) + { + peerManager.clearPeers(); + } + break; + } + case MSP_ELRS_SEND_RACE_OSD: + { + uint8_t receivedAddress[6]; + receivedAddress[0] = packet->readByte(); + receivedAddress[1] = packet->readByte(); + receivedAddress[2] = packet->readByte(); + receivedAddress[3] = packet->readByte(); + receivedAddress[4] = packet->readByte(); + receivedAddress[5] = packet->readByte(); + + if (receivedAddress[0] == 0 && + receivedAddress[1] == 0 && + receivedAddress[2] == 0 && + receivedAddress[3] == 0 && + receivedAddress[4] == 0 && + receivedAddress[5] == 0) + peerManager.sendToPeers(packet); + + else + peerManager.sendToPeer(receivedAddress, packet); + + break; + } + default: + { + peerManager.sendToPeers(packet); + break; + } + } +} + void runESPNOWServer(void *pvParameters) { @@ -198,10 +253,9 @@ void runESPNOWServer(void *pvParameters) espnowTaskHandle = xTaskGetCurrentTaskHandle(); - uint8_t sendAttempt = 0; - uint32_t sendSuccess = 0; - int sendStatus = -1; MSP msp; + uint8_t macAddress[6]; + esp_err_t err; @@ -220,15 +274,15 @@ void runESPNOWServer(void *pvParameters) { uint8_t mac_addr[6]; size_t size = sizeof(mac_addr); - err = nvs_get_blob(bp_mac_handle, STORAGE_MAC_KEY, bindAddress, &size); + err = nvs_get_blob(bp_mac_handle, STORAGE_MAC_KEY, macAddress, &size); if (err != ESP_OK && err != ESP_ERR_NVS_NOT_FOUND) ESP_LOGW(TAG, "Unable to retreive mac address from nvs"); else { - memcpy(sendAddress, bindAddress, 6); - bindAddress[0] = bindAddress[0] & ~0x01; - ESP_LOGI(TAG, "Backpack UID: [%d,%d,%d,%d,%d,%d]", bindAddress[0], bindAddress[1], - bindAddress[2], bindAddress[3], bindAddress[4], bindAddress[5]); + macAddress[0] = macAddress[0] & ~0x01; + macAddress[5] = 3; + ESP_LOGI(TAG, "Backpack UID: [%d,%d,%d,%d,%d,%d]", macAddress[0], macAddress[1], + macAddress[2], macAddress[3], macAddress[4], macAddress[5]); } } else @@ -251,12 +305,7 @@ void runESPNOWServer(void *pvParameters) ESP_ERROR_CHECK(esp_now_register_send_cb(espnowSendCB)); ESP_ERROR_CHECK(esp_now_register_recv_cb(espnowRecvCB)); - ESP_ERROR_CHECK(esp_wifi_set_mac(WIFI_IF_STA, bindAddress)); - - // Register default peer if UID set - if (bindAddress[0] != 0 || bindAddress[1] != 0 || bindAddress[2] != 0 || - bindAddress[3] != 0 || bindAddress[4] != 0 || bindAddress[5] != 0) - registerPeer(bindAddress); + ESP_ERROR_CHECK(esp_wifi_set_mac(WIFI_IF_STA, macAddress)); while (1) { @@ -271,111 +320,10 @@ void runESPNOWServer(void *pvParameters) if (result) { - switch (packet->function) - { - case MSP_ELRS_GET_BACKPACK_VERSION: - { - ESP_LOGI(TAG, "Processing MSP_ELRS_GET_BACKPACK_VERSION..."); - - mspPacket_t out; - out.reset(); - out.makeResponse(); - out.function = MSP_ELRS_GET_BACKPACK_VERSION; - for (size_t i = 0; i < sizeof(description->version); i++) - { - out.addByte(description->version[i]); - } - - if (xRingbufferSend(xRingReceivedEspnow, &out, sizeof(mspPacket_t), pdMS_TO_TICKS(1000)) == pdTRUE) - ESP_LOGI(TAG, "Added device version to ring buffer"); - else - ESP_LOGE(TAG, "Failed to add item to ring buffer"); - - break; - } - case MSP_ELRS_BACKPACK_SET_MODE: - { - if (packet->payloadSize == 1) - { - if (packet->payload[0] == 'B') - { - ESP_LOGI(TAG, "Enter binding mode..."); - isBinding = true; - } - if (bindTaskHandle != NULL) - { - vTaskDelete(bindTaskHandle); - bindTaskHandle = NULL; - } - - xTaskCreate(runBindTask, "BindTask", 4096, NULL, 10, &bindTaskHandle); - sendInProgressResponse(); - } - break; - } - case MSP_ELRS_SET_SEND_UID: - { - ESP_LOGI(TAG, "Processing MSP_ELRS_SET_SEND_UID..."); - uint8_t mode = packet->readByte(); - - // Unregister current peer - esp_now_del_peer(sendAddress); - memset(&sendAddress, 0, sizeof(sendAddress)); - - // Set target send address - if (mode == 0x01) - { - uint8_t receivedAddress[6]; - receivedAddress[0] = packet->readByte(); - receivedAddress[1] = packet->readByte(); - receivedAddress[2] = packet->readByte(); - receivedAddress[3] = packet->readByte(); - receivedAddress[4] = packet->readByte(); - receivedAddress[5] = packet->readByte(); - - ESP_LOGI(TAG, "Setting to recieved address"); - memcpy(sendAddress, receivedAddress, 6); - } - else - { - ESP_LOGI(TAG, "Resetting to default address"); - memcpy(sendAddress, bindAddress, 6); - } - - ESP_ERROR_CHECK(esp_wifi_set_mac(WIFI_IF_STA, sendAddress)); - - if (sendAddress[0] != 0 || sendAddress[1] != 0 || sendAddress[2] != 0 || - sendAddress[3] != 0 || sendAddress[4] != 0 || sendAddress[5] != 0) - registerPeer(sendAddress); - - ESP_LOGI(TAG, "Send UID set to: [%d,%d,%d,%d,%d,%d]", sendAddress[0], sendAddress[1], - sendAddress[2], sendAddress[3], sendAddress[4], sendAddress[5]); - - break; - } - default: - { - sendAttempt = 0; - do - { - sendStatus = sendMSPViaEspnow(packet); - if (sendStatus == ESP_OK) - xTaskNotifyWait(0x00, ULONG_MAX, &sendSuccess, portMAX_DELAY); - else - { - ESP_LOGW(TAG, "ESPNOW message send status: %d", sendStatus); - break; - } - vTaskDelay(espnowDelay); - } while (++sendAttempt < CONFIG_ESPNOW_MAX_SEND_ATTEMPTS && !sendSuccess); - - ESP_LOGI(TAG, "ESPNOW message send attempts: %d", sendAttempt); - break; - } - } + processPacketFromHost(packet); } - - vRingbufferReturnItem(buffers->read, (void *)packet); } + + vRingbufferReturnItem(buffers->read, (void *)packet); } } diff --git a/components/msp/include/msptypes.h b/components/msp/include/msptypes.h index 0cda911..356a87d 100644 --- a/components/msp/include/msptypes.h +++ b/components/msp/include/msptypes.h @@ -20,8 +20,9 @@ #define MSP_ELRS_SET_RX_LOAN_MODE 0x0F #define MSP_ELRS_GET_BACKPACK_VERSION 0x10 #define MSP_ELRS_BACKPACK_CRSF_TLM 0x11 -#define MSP_ELRS_SET_SEND_UID 0x00B5 +#define MSP_ELRS_REGISTER_ESPNOW_PEER 0x00B5 #define MSP_ELRS_SET_OSD 0x00B6 +#define MSP_ELRS_SEND_RACE_OSD 0x00B7 // Config opcodes #define MSP_ELRS_BACKPACK_CONFIG 0x30 diff --git a/components/peer_manager/include/peer_manager.h b/components/peer_manager/include/peer_manager.h index 7fcb32c..d2080c6 100644 --- a/components/peer_manager/include/peer_manager.h +++ b/components/peer_manager/include/peer_manager.h @@ -9,6 +9,8 @@ #define MAX_RETRIES 5 #define BUFFER_MAX_ITEMS 64 +extern void espnowSendCB(const uint8_t *mac_addr, esp_now_send_status_t status); + typedef struct Peer { esp_now_peer_info_t peerInfo; @@ -35,4 +37,4 @@ class PeerManager std::vector peers; SemaphoreHandle_t xSemaphore = NULL; const Peer_t *findPeer(const uint8_t *address); -} peerManager; \ No newline at end of file +} inline peerManager; \ No newline at end of file diff --git a/components/peer_manager/peer_manager.cpp b/components/peer_manager/peer_manager.cpp index f1cdcb6..0099b35 100644 --- a/components/peer_manager/peer_manager.cpp +++ b/components/peer_manager/peer_manager.cpp @@ -7,7 +7,7 @@ static const char *TAG = "peer_manager"; -static void espnowSendCB(const uint8_t *mac_addr, esp_now_send_status_t status) +void espnowSendCB(const uint8_t *mac_addr, esp_now_send_status_t status) { peerManager.notifyPeer(mac_addr, status); } @@ -77,7 +77,6 @@ Peer::~Peer() PeerManager::PeerManager() { vSemaphoreCreateBinary(xSemaphore); - ESP_ERROR_CHECK(esp_now_register_send_cb(espnowSendCB)); } PeerManager::~PeerManager() @@ -103,6 +102,12 @@ PeerManager::findPeer(const uint8_t *address) void PeerManager::addPeer(uint8_t address[]) { + if (esp_now_is_peer_exist(address)) + { + ESP_LOGE(TAG, "Espnow peer already registerd"); + return; + } + if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) { peers.emplace_back(address); @@ -116,6 +121,9 @@ void PeerManager::addPeer(uint8_t address[]) return; } + ESP_LOGI(TAG, "Registered espnow peer: [%d.%d.%d.%d.%d.%d]", address[0], address[1], address[2], address[3], address[4], address[5]); + ESP_LOGI(TAG, "Peers in vector: %d", peers.size()); + xTaskCreate(sendToPeerTask, "PeerSenderTask", 4096, (void *)&peers.back(), 9, &peers.back().taskHandle); xSemaphoreGive(xSemaphore); @@ -138,6 +146,7 @@ void PeerManager::removePeer(uint8_t *address) peer->peerInfo.peer_addr[5] == address[5]) { peers.erase(peers.begin() + i); + ESP_LOGI(TAG, "Removed espnow peer"); xSemaphoreGive(xSemaphore); return; } @@ -151,6 +160,7 @@ void PeerManager::clearPeers() if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) { peers.clear(); + ESP_LOGI(TAG, "Registered espnow peers cleared"); xSemaphoreGive(xSemaphore); } }