OptoTest/OpticalChannelTester/Radio.cpp

#include "Radio.h"
#include "Config.h"
#include "Log.h"
#include <WiFi.h>
#include <esp_wifi.h>
#include <string.h>

Radio *Radio::instance_ = nullptr;
static const uint8_t BROADCAST_MAC[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};

bool Radio::begin() {
  if (active_) { Log::event("ESP-NOW", "already active"); return true; }
  // Keep both devices on a dedicated, fixed STA channel. Stored access-point
  // credentials must not reconnect in the background and move ESP-NOW to the
  // AP's channel.
  WiFi.persistent(false);
  WiFi.mode(WIFI_STA);
  WiFi.setAutoReconnect(false);
  WiFi.disconnect(false, false);
  WiFi.setSleep(false);
  const bool txPowerOk = WiFi.setTxPower(WIFI_POWER_8_5dBm);
  delay(10);
  if (esp_wifi_set_channel(ESPNOW_WIFI_CHANNEL, WIFI_SECOND_CHAN_NONE) != ESP_OK) {
    Log::event("ESP-NOW", "Wi-Fi channel setup FAILED"); return false;
  }
  if (!queue_) queue_ = xQueueCreate(8, sizeof(ReceivedPacket));
  if (!heartbeatQueue_) heartbeatQueue_ = xQueueCreate(4, sizeof(ReceivedPacket));
  if (!heartbeatTask_ && xTaskCreate(heartbeatTaskEntry, "radio-hb", 2048, this, 5, &heartbeatTask_) != pdPASS)
    heartbeatTask_ = nullptr;
  if (!queue_ || !heartbeatQueue_ || !heartbeatTask_) {
    Log::event("ESP-NOW", "receive/heartbeat service creation FAILED"); return false;
  }
  if (esp_now_init() != ESP_OK) {
    Log::event("ESP-NOW", "initialization FAILED"); return false;
  }
  const esp_err_t rateResult = esp_wifi_config_espnow_rate(WIFI_IF_STA, WIFI_PHY_RATE_1M_L);
  instance_ = this;
  if (esp_now_register_recv_cb(onReceive) != ESP_OK) {
    instance_ = nullptr; esp_now_deinit(); return false;
  }
  active_ = true; __atomic_store_n(&lastValidRxMs_, millis(), __ATOMIC_RELAXED);
  const bool ok = ensurePeer(BROADCAST_MAC);
  uint8_t primaryChannel = 0;
  wifi_second_chan_t secondaryChannel = WIFI_SECOND_CHAN_NONE;
  esp_wifi_get_channel(&primaryChannel, &secondaryChannel);
  Log::printf("ESP-NOW", "started channel=%u expected=%u rate=1M%s tx-power=max%s broadcast-peer=%s",
    primaryChannel, ESPNOW_WIFI_CHANNEL, rateResult == ESP_OK ? "" : "-FAILED",
    txPowerOk ? "" : "-FAILED", ok ? "OK" : "FAILED");
  return ok;
}

void Radio::end() {
  const bool wasActive = active_; active_ = false;
  if (wasActive) { esp_now_unregister_recv_cb(); esp_now_deinit(); }
  if (queue_) xQueueReset(queue_);
  if (heartbeatQueue_) xQueueReset(heartbeatQueue_);
  if (instance_ == this) instance_ = nullptr;
  windowedReceive_ = false;
  if (wasActive) WiFi.mode(WIFI_OFF);
  Log::event("ESP-NOW", "stopped");
}

bool Radio::setWindowedReceive(bool enabled) {
  if (!active_) return false;
  if (windowedReceive_ == enabled) return true;

  const uint16_t window = enabled ? SLAVE_LISTEN_WINDOW_MS : UINT16_MAX;
  const uint16_t interval = enabled ? SLAVE_LISTEN_INTERVAL_MS :
    ESP_WIFI_CONNECTIONLESS_INTERVAL_DEFAULT_MODE;
  const esp_err_t windowResult = esp_now_set_wake_window(window);
  const esp_err_t intervalResult = esp_wifi_connectionless_module_set_wake_interval(interval);
  const bool sleepResult = WiFi.setSleep(enabled);
  const bool ok = windowResult == ESP_OK && intervalResult == ESP_OK && sleepResult;

  bool continuousRestored = true;
  if (!ok) {
    continuousRestored = esp_now_set_wake_window(UINT16_MAX) == ESP_OK;
    continuousRestored =
      esp_wifi_connectionless_module_set_wake_interval(
        ESP_WIFI_CONNECTIONLESS_INTERVAL_DEFAULT_MODE) == ESP_OK && continuousRestored;
    continuousRestored = WiFi.setSleep(false) && continuousRestored;
    windowedReceive_ = false;
  } else windowedReceive_ = enabled;

  Log::printf("ESP-NOW", "RX power-save %s window=%ums interval=%ums %s%s",
    enabled ? "ON" : "OFF", window, interval,
    ok ? "OK" : "FAILED; continuous RX restore ",
    ok ? "" : (continuousRestored ? "OK" : "FAILED"));
  return ok;
}

bool Radio::ensurePeer(const uint8_t mac[6]) {
  if (esp_now_is_peer_exist(mac)) return true;
  esp_now_peer_info_t peer = {};
  memcpy(peer.peer_addr, mac, 6);
  peer.channel = ESPNOW_WIFI_CHANNEL;
  peer.ifidx = WIFI_IF_STA;
  peer.encrypt = false;
  return esp_now_add_peer(&peer) == ESP_OK;
}

bool Radio::sendBroadcast(ProtocolPacket p) { return sendTo(BROADCAST_MAC, p); }

bool Radio::sendTo(const uint8_t mac[6], ProtocolPacket p) {
  maintainChannel();
  char peer[20]; macText(mac, peer, sizeof(peer));
  if (!active_ || !ensurePeer(mac)) {
    Log::printf("ESP-NOW", "TX %s to %s FAILED: inactive/peer", messageName(static_cast<MessageType>(p.type)), peer);
    return false;
  }
  finalizePacket(p);
  const bool ok = esp_now_send(mac, reinterpret_cast<const uint8_t *>(&p), sizeof(p)) == ESP_OK;
  const MessageType type = static_cast<MessageType>(p.type);
  if (type != MessageType::DISCOVER && type != MessageType::HEARTBEAT &&
      type != MessageType::HEARTBEAT_ACK && type != MessageType::PROGRESS)
    Log::printf("ESP-NOW", "TX %s to %s session=%08lX stage=%u seq=%u %s",
      messageName(type), peer, p.session, p.stage, p.sequence, ok ? "QUEUED" : "FAILED");
  return ok;
}

bool Radio::receive(ReceivedPacket &r) {
  maintainChannel();
  return queue_ && xQueueReceive(queue_, &r, 0) == pdTRUE;
}

void Radio::maintainChannel() {
  if (!active_) return;
  const uint32_t now = millis();
  if (now - lastChannelCheckMs_ < 250) return;
  lastChannelCheckMs_ = now;
  uint8_t primaryChannel = 0;
  wifi_second_chan_t secondaryChannel = WIFI_SECOND_CHAN_NONE;
  if (esp_wifi_get_channel(&primaryChannel, &secondaryChannel) != ESP_OK ||
      primaryChannel == ESPNOW_WIFI_CHANNEL) return;
  const bool restored = esp_wifi_set_channel(ESPNOW_WIFI_CHANNEL, WIFI_SECOND_CHAN_NONE) == ESP_OK;
  Log::printf("ESP-NOW", "channel drift %u->%u %s", primaryChannel, ESPNOW_WIFI_CHANNEL,
    restored ? "RESTORED" : "FAILED");
}

void Radio::flush() {
  if (queue_) xQueueReset(queue_);
  if (heartbeatQueue_) xQueueReset(heartbeatQueue_);
}

void Radio::onReceive(const esp_now_recv_info_t *info, const uint8_t *data, int length) {
  if (!instance_ || !instance_->queue_ || !info || length != sizeof(ProtocolPacket)) return;
  ReceivedPacket item;
  memcpy(item.mac, info->src_addr, 6); memcpy(&item.packet, data, sizeof(item.packet));
  if (!validPacket(item.packet)) return;
  __atomic_store_n(&instance_->lastValidRxMs_, millis(), __ATOMIC_RELAXED);
  if (static_cast<MessageType>(item.packet.type) == MessageType::HEARTBEAT && instance_->heartbeatQueue_)
    xQueueSend(instance_->heartbeatQueue_, &item, 0);
  xQueueSend(instance_->queue_, &item, 0); // Wi-Fi task callback: copy only, never block
}

void Radio::heartbeatTaskEntry(void *context) {
  static_cast<Radio *>(context)->heartbeatTaskLoop();
}

void Radio::heartbeatTaskLoop() {
  ReceivedPacket item;
  for (;;) {
    if (xQueueReceive(heartbeatQueue_, &item, portMAX_DELAY) != pdTRUE || !active_) continue;
    ProtocolPacket ack = item.packet;
    ack.type = static_cast<uint8_t>(MessageType::HEARTBEAT_ACK);
    finalizePacket(ack);
    esp_now_send(item.mac, reinterpret_cast<const uint8_t *>(&ack), sizeof(ack));
  }
}

void Radio::macText(const uint8_t mac[6], char *out, size_t n) {
  snprintf(out, n, "%02X:%02X:%02X:%02X:%02X:%02X", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
}