esp32-cluster/firmware/ble_node/ble_node.ino

#include <WiFi.h>
#include <WiFiUdp.h>
#include <BLEDevice.h>
#include <BLEScan.h>
#include <BLEAdvertisedDevice.h>
#include "esp_efuse.h"
#include "esp_mac.h"
#include "freertos/queue.h"
#include "config.h"

WiFiUDP udp;
String  nodeId;

unsigned long lastHeartbeat = 0;
unsigned long lastFlush     = 0;

static volatile bool scanDone  = false;
static volatile uint32_t bleDrops = 0;

// ─── BLE event ───────────────────────────────────────────────────────────────

struct BleEvent {
    char    mac[18];
    uint8_t addr_type;    // 0 = public, 1 = random
    char    name[32];
    int8_t  rssi;
    int32_t mfr_id;       // company ID (little-endian), -1 if absent
    char    mfr_data[33]; // hex string of bytes after company ID, up to 16 bytes
};

static QueueHandle_t bleQueue;

// ─── Dedup ───────────────────────────────────────────────────────────────────

struct BleDedup { char mac[18]; uint32_t last_ms; };
static BleDedup dedupCache[BLE_DEDUP_CACHE_SIZE];
static int      dedupNext = 0;

static bool isDuplicate(const char* mac) {
    uint32_t now = millis();
    for (int i = 0; i < BLE_DEDUP_CACHE_SIZE; i++) {
        if (dedupCache[i].last_ms == 0) continue;
        if (strcmp(dedupCache[i].mac, mac) == 0) {
            if ((now - dedupCache[i].last_ms) < (uint32_t)(BLE_DEDUP_SECS * 1000))
                return true;
            dedupCache[i].last_ms = now;
            return false;
        }
    }
    strncpy(dedupCache[dedupNext].mac, mac, 17);
    dedupCache[dedupNext].mac[17] = '\0';
    dedupCache[dedupNext].last_ms  = now;
    dedupNext = (dedupNext + 1) % BLE_DEDUP_CACHE_SIZE;
    return false;
}

// ─── Advertisement callback ──────────────────────────────────────────────────
// Runs in the BLE task — must be fast and must not call UDP.

class AdvCallback : public BLEAdvertisedDeviceCallbacks {
    void onResult(BLEAdvertisedDevice dev) override {
        char mac[18];
        strncpy(mac, dev.getAddress().toString().c_str(), 17);
        mac[17] = '\0';

        if (isDuplicate(mac)) return;

        BleEvent ev;
        memcpy(ev.mac, mac, 18);

        // BLE_ADDR_TYPE_PUBLIC = 0, everything else treated as random
        ev.addr_type = (dev.getAddressType() == BLE_ADDR_PUBLIC) ? 0 : 1;
        ev.rssi      = (int8_t)dev.getRSSI();
        ev.mfr_id    = -1;
        ev.mfr_data[0] = '\0';
        ev.name[0]     = '\0';

        // Device name — strip non-printable bytes
        if (dev.haveName()) {
            String raw = dev.getName();
            int j = 0;
            for (int i = 0; i < (int)raw.length() && j < 31; i++) {
                char c = raw[i];
                ev.name[j++] = (c >= 32 && c <= 126) ? c : '?';
            }
            ev.name[j] = '\0';
        }

        // Manufacturer specific data (AD type 0xFF)
        // First 2 bytes (little-endian) are the company ID; rest is payload.
        if (dev.haveManufacturerData()) {
            String md = dev.getManufacturerData();
            if (md.length() >= 2) {
                ev.mfr_id = (int32_t)((uint8_t)md[0] | ((uint8_t)md[1] << 8));
                int payload_len = min((int)16, (int)md.length() - 2);
                for (int i = 0; i < payload_len; i++) {
                    snprintf(ev.mfr_data + i * 2, 3, "%02x", (uint8_t)md[2 + i]);
                }
                ev.mfr_data[payload_len * 2] = '\0';
            }
        }

        if (xQueueSend(bleQueue, &ev, 0) != pdTRUE) bleDrops++;
    }
};

static AdvCallback advCallback;
static BLEScan*    pBLEScan = nullptr;

// Called from BLE task when a scan window completes.
static void onScanComplete(BLEScanResults) {
    pBLEScan->clearResults();
    scanDone = true;
}

// ─── Setup ───────────────────────────────────────────────────────────────────

void setup() {
    Serial.begin(115200);
    delay(500);

    uint8_t mac[6];
    esp_efuse_mac_get_default(mac);
    char macBuf[9];
    snprintf(macBuf, sizeof(macBuf), "%02X%02X%02X%02X", mac[2], mac[3], mac[4], mac[5]);
    nodeId = String(macBuf);

    Serial.printf("\n[BLE NODE] ID: %s\n", nodeId.c_str());

    memset(dedupCache, 0, sizeof(dedupCache));
    bleQueue = xQueueCreate(BLE_QUEUE_SIZE, sizeof(BleEvent));

    connectWiFi();

    BLEDevice::init("");
    pBLEScan = BLEDevice::getScan();
    pBLEScan->setAdvertisedDeviceCallbacks(&advCallback, true); // true = keep duplicates in scan window
    pBLEScan->setActiveScan(false);   // passive — do not send scan requests
    pBLEScan->setInterval(100);       // scan interval (ms × 0.625 = 62.5ms)
    pBLEScan->setWindow(99);          // scan window — near-continuous coverage

    pBLEScan->start(BLE_SCAN_DURATION_SECS, onScanComplete, false);
    Serial.println("[BLE] Passive scan started");
}

// ─── Loop ────────────────────────────────────────────────────────────────────

void loop() {
    if (WiFi.status() != WL_CONNECTED) {
        Serial.println("[WIFI] Lost connection, reconnecting...");
        connectWiFi();
        return;
    }

    unsigned long now = millis();

    if (now - lastHeartbeat >= HEARTBEAT_INTERVAL_MS) {
        lastHeartbeat = now;
        sendHeartbeat();
    }

    if (now - lastFlush >= BLE_FLUSH_INTERVAL_MS) {
        lastFlush = now;
        flushBleQueue();
    }

    // Restart scan after each window completes
    if (scanDone) {
        scanDone = false;
        pBLEScan->start(BLE_SCAN_DURATION_SECS, onScanComplete, false);
    }

    delay(10);
}

// ─── WiFi ────────────────────────────────────────────────────────────────────

void connectWiFi() {
    Serial.printf("[WIFI] Connecting to %s\n", WIFI_SSID);
    WiFi.mode(WIFI_STA);
    WiFi.begin(WIFI_SSID, WIFI_PASSWORD);

    int attempts = 0;
    while (WiFi.status() != WL_CONNECTED && attempts < 20) {
        delay(500);
        Serial.print(".");
        attempts++;
    }

    if (WiFi.status() == WL_CONNECTED) {
        Serial.printf("\n[WIFI] Connected — IP: %s\n", WiFi.localIP().toString().c_str());
    } else {
        Serial.println("\n[WIFI] Failed to connect, will retry in loop");
    }
}

// ─── Send ────────────────────────────────────────────────────────────────────

void sendBleEvent(const BleEvent& ev) {
    // Escape name for JSON
    char nameEsc[66];
    int j = 0;
    for (int i = 0; ev.name[i] && j < 64; i++) {
        if (ev.name[i] == '"' || ev.name[i] == '\\') nameEsc[j++] = '\\';
        nameEsc[j++] = ev.name[i];
    }
    nameEsc[j] = '\0';

    char buf[320];
    snprintf(buf, sizeof(buf),
        "{\"node_id\":\"%s\",\"ts\":%lu,\"type\":\"ble_adv\","
        "\"mac\":\"%s\",\"addr_type\":%u,"
        "\"name\":\"%s\",\"rssi\":%d,"
        "\"mfr_id\":%ld,\"mfr_data\":\"%s\"}",
        nodeId.c_str(), millis(),
        ev.mac, (unsigned)ev.addr_type,
        nameEsc, (int)ev.rssi,
        (long)ev.mfr_id, ev.mfr_data
    );

    udp.beginPacket(COORDINATOR_IP, COORDINATOR_PORT);
    udp.print(buf);
    udp.endPacket();

    const char* at = ev.addr_type == 0 ? "pub" : "rnd";
    Serial.printf("  [BLE] %s (%s)  \"%s\"  %ddBm  mfr=0x%04lX  data=%s\n",
        ev.mac, at, ev.name[0] ? ev.name : "<anon>",
        (int)ev.rssi, (long)(ev.mfr_id >= 0 ? ev.mfr_id : 0), ev.mfr_data);
}

void flushBleQueue() {
    if (WiFi.status() != WL_CONNECTED) return;
    BleEvent ev;
    while (xQueueReceive(bleQueue, &ev, 0) == pdTRUE) {
        sendBleEvent(ev);
    }
}

// ─── Heartbeat ───────────────────────────────────────────────────────────────

void sendHeartbeat() {
    unsigned long now = millis();
    char buf[256];
    snprintf(buf, sizeof(buf),
        "{\"node_id\":\"%s\",\"ts\":%lu,\"type\":\"heartbeat\","
        "\"uptime_ms\":%lu,\"free_heap\":%lu,\"wifi_rssi\":%d,"
        "\"probe_drops\":0,\"deauth_drops\":0,\"assoc_drops\":0,"
        "\"ble_drops\":%lu}",
        nodeId.c_str(), now,
        now,
        (unsigned long)ESP.getFreeHeap(),
        WiFi.RSSI(),
        (unsigned long)bleDrops
    );
    udp.beginPacket(COORDINATOR_IP, COORDINATOR_PORT);
    udp.print(buf);
    udp.endPacket();
    Serial.printf("[HB] uptime=%lus  heap=%luK  ap=%ddBm  ble_drops=%lu\n",
        now / 1000,
        (unsigned long)ESP.getFreeHeap() / 1024,
        WiFi.RSSI(),
        (unsigned long)bleDrops
    );
}