ESP-NOW Utils (Arduino framework)

espnow_utils.cpp

#include "espnow_utils.h"

#ifdef ESP32
#include <WiFi.h>
#else
#include <ESP8266WiFi.h>
#endif

String get_mac_as_arduino_str(const uint8_t *address);

bool init_espnow(esp_now_recv_cb_t on_recv_data, esp_now_send_cb_t on_data_sent)
{
  Serial.print("ESP-NOW Init: Setting WiFi mode... ");

  // Initialize WiFi mode
  wifi_mode_t oldmode = WiFi.getMode();
  if (!oldmode)
  {
    Serial.print("Null->STA (Disconnected) ");
    WiFi.mode(WIFI_STA);
    WiFi.disconnect();
  }
  else if (oldmode == WIFI_AP)
  {
    Serial.print("AP -> AP_STA ");
    WiFi.mode(WIFI_AP_STA);
  }
  else
  {
    Serial.print("[OK] ");
  }
  Serial.println("done!");

  // Init ESP-NOW
  if (esp_now_init() != ESP_OK)
  {
    Serial.println("Error initializing ESP-NOW");
    WiFi.mode(oldmode); // revert any mode changes
    return false;
  }

  bool err = false;
  if (on_recv_data && esp_now_register_recv_cb(on_recv_data) != ESP_OK)
  {
    Serial.printf("ERROR: Failed to register recv callback!");
    err = true;
  }

  if (on_data_sent && esp_now_register_send_cb(on_data_sent) != ESP_OK)
  {
    Serial.printf("ERROR: Failed to register send callback!");
    err = true;
  }

  Serial.println("ESP-NOW init done.");
  return !err;
}

void add_peer_from_addr(uint8_t addr[6])
{
  // TODO: Do I need to allocate on the stack so this persists? internet code didn't.
  esp_now_peer_info_t peerInfo = {};

  memcpy(peerInfo.peer_addr, addr, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;

  if (esp_now_add_peer(&peerInfo) != ESP_OK)
  {
    Serial.printf("Failed to add peer! (%s)\n", get_mac_as_arduino_str(addr).c_str());
  }

  Serial.printf("Registering peer for: %s\n", get_mac_as_arduino_str(addr).c_str());
}

String parse_esp_now_error(esp_err_t result)
{
  switch (result)
  {
    case ESP_OK: return "ESP_OK (Success)";
    
    case ESP_ERR_ESPNOW_NOT_INIT: return "ESP_ERR_ESPNOW_NOT_INIT (ESPNOW is not initialized.)";
    case ESP_ERR_ESPNOW_ARG: return "ESP_ERR_ESPNOW_ARG (Invalid argument)";
    case ESP_ERR_ESPNOW_NO_MEM: return "ESP_ERR_ESPNOW_NO_MEM (Out of memory)";
    case ESP_ERR_ESPNOW_FULL: return "ESP_ERR_ESPNOW_FULL (ESPNOW peer list is full)";
    case ESP_ERR_ESPNOW_NOT_FOUND: return "ESP_ERR_ESPNOW_NOT_FOUND (ESPNOW peer is not found)";
    case ESP_ERR_ESPNOW_INTERNAL: return "ESP_ERR_ESPNOW_INTERNAL (Internal error)";
    case ESP_ERR_ESPNOW_EXIST: return "ESP_ERR_ESPNOW_EXIST (ESPNOW peer has existed)";
    case ESP_ERR_ESPNOW_IF: return "ESP_ERR_ESPNOW_IF (Interface error)";
    default:
      return "Unknown error. code: " + String(result);
  }
}

String get_mac_as_arduino_str(const uint8_t *address)
{
  char buf[18];

  for (size_t i = 0; i < 6; i++)
  {
    char num_str[3];

    // print upper to match WiFi.macAddress() format
    sprintf(num_str, "%X", address[i]); // will add null terminal, replace it with ':'
    num_str[2] = ':';
    // Serial.printf("a[%i] = %s\n", i, num_str);
    memcpy(&buf[i * 3], num_str, 3);
  }

  buf[17] = '\0';
  return String(buf);
}

espnow_utils.h

#ifndef ESP_NOW_UTILS_H
#define ESP_NOW_UTILS_H

#include <Arduino.h>
#include <esp_now.h>

void add_peer_from_addr(uint8_t espnow_addr[6]);
bool init_espnow(esp_now_recv_cb_t on_recv_data, esp_now_send_cb_t on_data_sent);
String parse_esp_now_error(esp_err_t result);

#endif