diff --git a/main/app_networking.cpp b/main/app_networking.cpp index 3efe0cf..6af5d3b 100644 --- a/main/app_networking.cpp +++ b/main/app_networking.cpp @@ -10,6 +10,7 @@ #include "esp_now.h" #include "esp_wifi.h" #include "esp_log.h" +#include "esp_wifi.h" namespace { @@ -27,13 +28,12 @@ constexpr auto STR_LEN(const char (&s)[N]) return N; } -static_assert(STR_LEN(CONFIG_ESPTNL_PMK) == 16 + 1, "CONFIG_ESPTNL_PMK must be of length 16 bytes + 1 null terminator."); -static_assert(STR_LEN(CONFIG_ESPTNL_LMK) == 16 + 1, "CONFIG_ESPTNL_LMK must be of length 16 bytes + 1 null terminator."); +static_assert(STR_LEN(CONFIG_ESPTNL_PMK) == ESP_NOW_KEY_LEN + 1, "CONFIG_ESPTNL_PMK must be of length 16 bytes + 1 null terminator."); +static_assert(STR_LEN(CONFIG_ESPTNL_LMK) == ESP_NOW_KEY_LEN + 1, "CONFIG_ESPTNL_LMK must be of length 16 bytes + 1 null terminator."); QueueHandle_t s_esp_now_queue = nullptr; std::array s_rx_buffer; std::variant s_peer; -bool s_tx_inflight = false; bool s_isBroadcastAddress(const std::uint8_t* mac) { @@ -46,13 +46,28 @@ bool s_isBroadcastAddress(const esp_now_recv_info_t* sender) return s_isBroadcastAddress(sender->src_addr); } +bool s_addPeer(const MacAddress& peer_address) +{ + esp_now_peer_info_t broadcast_peer; + std::memset(&broadcast_peer, 0, sizeof(broadcast_peer)); + + broadcast_peer.channel = CONFIG_ESPTNL_CHANNEL; + broadcast_peer.ifidx = WIFI_IF_STA; + broadcast_peer.encrypt = false; + std::memcpy(broadcast_peer.lmk, CONFIG_ESPTNL_LMK, ESP_NOW_KEY_LEN); + std::memcpy(broadcast_peer.peer_addr, peer_address.data(), peer_address.size()); + + auto successful = esp_now_add_peer(&broadcast_peer); + ESP_ERROR_CHECK_WITHOUT_ABORT(successful); + + return successful == ESP_OK; +} + void s_cbEspNowSendComplete(const std::uint8_t* peer_mac, esp_now_send_status_t status) { - s_tx_inflight = false; - EspNowEvent event = { EspNowEvent::MSG_SEND_COMPLETE, 0 }; xQueueSend(s_esp_now_queue, &event, 0); - ESP_LOGD(TAG, "Send complete. s_tx_inflight = false."); + ESP_LOGD(TAG, "Send complete."); } void s_cbEspNowReceiveComplete(const esp_now_recv_info_t* recv_info, const std::uint8_t* data, int len) @@ -95,7 +110,10 @@ QueueHandle_t setupEspNow() ESP_ERROR_CHECK(esp_now_register_send_cb(s_cbEspNowSendComplete)); ESP_ERROR_CHECK(esp_now_register_recv_cb(s_cbEspNowReceiveComplete)); - s_peer = MacAddress{ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; + const auto broadcast_mac = MacAddress{ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; + s_peer = broadcast_mac; + + ESP_ERROR_CHECK(s_addPeer(broadcast_mac)); return s_esp_now_queue; } @@ -108,13 +126,11 @@ const std::array& getRxBuffer() void sendData(const std::array& buffer, const std::size_t length) { if (const MacAddress* peer_mac = std::get_if(&s_peer); - !s_tx_inflight && peer_mac != nullptr) + peer_mac != nullptr) { - esp_now_send(peer_mac->data(), buffer.data(), length); - if (!s_isBroadcastAddress(peer_mac->data())) - s_tx_inflight = true; + ESP_ERROR_CHECK_WITHOUT_ABORT(esp_now_send(peer_mac->data(), buffer.data(), length)); - ESP_LOGD(TAG, "Message send started. s_tx_inflight = %d.", s_tx_inflight); + ESP_LOGD(TAG, "Message send started."); } else {