Skip to content

Individual Peer Addressing #7

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
i-am-grub wants to merge 4 commits into
base: main
Choose a base branch
from
Open

Individual Peer Addressing #7

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
5cc05c2
add espnow peer manager
i-am-grub Jun 19, 2025
7b96fa8
store peer in vector
i-am-grub Jun 19, 2025
ffe5aaa
update vector practices
i-am-grub Jun 22, 2025
db9841e
integrate into testing
i-am-grub Jun 23, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion components/espnow_server/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
)
311 changes: 128 additions & 183 deletions components/espnow_server/espnow_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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;
Expand Down Expand Up @@ -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]))
Expand All @@ -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)
{
Expand All @@ -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;
}
Expand All @@ -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)
{

Expand All @@ -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;

Expand All @@ -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
Expand All @@ -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)
{
Expand All @@ -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);
}
}
Loading