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..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,55 +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_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"); - 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])) @@ -133,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) { @@ -159,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; } @@ -193,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) { @@ -201,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; @@ -223,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 @@ -254,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) { @@ -274,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/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..d2080c6 --- /dev/null +++ b/components/peer_manager/include/peer_manager.h @@ -0,0 +1,40 @@ +#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 + +extern void espnowSendCB(const uint8_t *mac_addr, esp_now_send_status_t status); + +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[]); + virtual ~Peer(); +} Peer_t; + +class PeerManager +{ +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); +} inline 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..0099b35 --- /dev/null +++ b/components/peer_manager/peer_manager.cpp @@ -0,0 +1,209 @@ +#include +#include +#include +#include "esp_log.h" +#include "peer_manager.h" +#include "msp.h" + +static const char *TAG = "peer_manager"; + +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 sendAttempt; + 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; + } + + sendAttempt = 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 (++sendAttempt < 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); +} + +PeerManager::~PeerManager() +{ + vSemaphoreDelete(xSemaphore); +} + +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 (esp_now_is_peer_exist(address)) + { + ESP_LOGE(TAG, "Espnow peer already registerd"); + return; + } + + if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) + { + peers.emplace_back(address); + + 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; + } + + 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); + } +} + +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); + ESP_LOGI(TAG, "Removed espnow peer"); + xSemaphoreGive(xSemaphore); + return; + } + } + xSemaphoreGive(xSemaphore); + } +} + +void PeerManager::clearPeers() +{ + if (xSemaphoreTake(xSemaphore, portMAX_DELAY) == pdTRUE) + { + peers.clear(); + ESP_LOGI(TAG, "Registered espnow peers cleared"); + 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