diff --git a/src/helpers/esp32/ESPNOWRadio.cpp b/src/helpers/esp32/ESPNOWRadio.cpp index ced19f9110..39caf6e717 100644 --- a/src/helpers/esp32/ESPNOWRadio.cpp +++ b/src/helpers/esp32/ESPNOWRadio.cpp @@ -7,22 +7,31 @@ static uint8_t broadcastAddress[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; static esp_now_peer_info_t peerInfo; static volatile bool is_send_complete = false; static esp_err_t last_send_result; -static uint8_t rx_buf[256]; -static uint8_t last_rx_len = 0; +static ESPNOWRadio* espnow_instance = nullptr; // callback when data is sent static void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) { + (void)mac_addr; is_send_complete = true; ESPNOW_DEBUG_PRINTLN("Send Status: %d", (int)status); } static void OnDataRecv(const uint8_t *mac, const uint8_t *data, int len) { + (void)mac; ESPNOW_DEBUG_PRINTLN("Recv: len = %d", len); - memcpy(rx_buf, data, len); - last_rx_len = len; + if (!espnow_instance || len <= 0 || len > 256) { + return; + } + espnow_instance->enqueueRxFrame(data, len); } void ESPNOWRadio::init() { + espnow_instance = this; + _rx_read_idx = 0; + _rx_write_idx = 0; + _rx_count = 0; + _rx_drop_count = 0; + // Set device as a Wi-Fi Station WiFi.mode(WIFI_STA); // Long Range mode @@ -99,13 +108,49 @@ float ESPNOWRadio::getLastRSSI() const { return 0; } float ESPNOWRadio::getLastSNR() const { return 0; } int ESPNOWRadio::recvRaw(uint8_t* bytes, int sz) { - int len = last_rx_len; - if (last_rx_len > 0) { - memcpy(bytes, rx_buf, last_rx_len); - last_rx_len = 0; + PendingFrame frame; + if (popPendingFrame(frame)) { + int len = frame.len; + if (len > sz) { + len = sz; + } + memcpy(bytes, frame.buf, len); n_recv++; + return len; + } + return 0; +} + +bool ESPNOWRadio::enqueueRxFrame(const uint8_t* data, int len) { + portENTER_CRITICAL(&_rx_lock); + if (_rx_count >= RX_QUEUE_SIZE) { + _rx_drop_count++; + portEXIT_CRITICAL(&_rx_lock); + ESPNOW_DEBUG_PRINTLN("Recv queue full, dropping frame len=%d (drops=%lu)", len, + (unsigned long)_rx_drop_count); + return false; + } + + PendingFrame& slot = _rx_queue[_rx_write_idx]; + slot.len = len; + memcpy(slot.buf, data, len); + _rx_write_idx = (_rx_write_idx + 1) % RX_QUEUE_SIZE; + _rx_count++; + portEXIT_CRITICAL(&_rx_lock); + return true; +} + +bool ESPNOWRadio::popPendingFrame(PendingFrame& frame) { + bool have_frame = false; + portENTER_CRITICAL(&_rx_lock); + if (_rx_count > 0) { + frame = _rx_queue[_rx_read_idx]; + _rx_read_idx = (_rx_read_idx + 1) % RX_QUEUE_SIZE; + _rx_count--; + have_frame = true; } - return len; + portEXIT_CRITICAL(&_rx_lock); + return have_frame; } uint32_t ESPNOWRadio::getEstAirtimeFor(int len_bytes) { diff --git a/src/helpers/esp32/ESPNOWRadio.h b/src/helpers/esp32/ESPNOWRadio.h index c696da3a87..35b76c4da8 100644 --- a/src/helpers/esp32/ESPNOWRadio.h +++ b/src/helpers/esp32/ESPNOWRadio.h @@ -1,15 +1,31 @@ #pragma once #include +#include +#include class ESPNOWRadio : public mesh::Radio { protected: uint32_t n_recv, n_sent; + struct PendingFrame { + uint16_t len; + uint8_t buf[256]; + }; + + static const uint8_t RX_QUEUE_SIZE = 4; + PendingFrame _rx_queue[RX_QUEUE_SIZE]; + uint8_t _rx_read_idx; + uint8_t _rx_write_idx; + uint8_t _rx_count; + uint32_t _rx_drop_count; + portMUX_TYPE _rx_lock; + bool popPendingFrame(PendingFrame& frame); public: - ESPNOWRadio() { n_recv = n_sent = 0; } + ESPNOWRadio() : _rx_read_idx(0), _rx_write_idx(0), _rx_count(0), _rx_drop_count(0), _rx_lock(portMUX_INITIALIZER_UNLOCKED) { n_recv = n_sent = 0; } void init(); + bool enqueueRxFrame(const uint8_t* data, int len); int recvRaw(uint8_t* bytes, int sz) override; uint32_t getEstAirtimeFor(int len_bytes) override; bool startSendRaw(const uint8_t* bytes, int len) override; @@ -18,8 +34,11 @@ class ESPNOWRadio : public mesh::Radio { bool isInRecvMode() const override; uint32_t getPacketsRecv() const { return n_recv; } + uint32_t getPacketsRecvErrors() const { return 0; } uint32_t getPacketsSent() const { return n_sent; } void resetStats() { n_recv = n_sent = 0; } + void setRxBoostedGainMode(bool) { } + void powerOff() { } virtual float getLastRSSI() const override; virtual float getLastSNR() const override;