AeroAlign/firmware/master/src/espnow_master.cpp

// SkyLogic AeroAlign - ESP-NOW Master (Receiver) Implementation
//
// Receives sensor data from Slave nodes via ESP-NOW protocol.
// Automatically registers new nodes on first packet received (no manual pairing).

#include "espnow_master.h"
#include "config.h"

// Static instance pointer for callback
ESPNowMaster* ESPNowMaster::instance = nullptr;

ESPNowMaster::ESPNowMaster()
    : total_packets_received(0), total_checksum_errors(0) {
    // Initialize nodes array
    memset(nodes, 0, sizeof(nodes));

    // Set static instance pointer
    instance = this;
}

bool ESPNowMaster::begin() {
    #ifdef DEBUG_SERIAL_ENABLED
    Serial.println("[ESP-NOW] Initializing Master receiver...");
    #endif

    // Initialize ESP-NOW
    if (esp_now_init() != ESP_OK) {
        last_error = "ESP-NOW initialization failed";
        #ifdef DEBUG_SERIAL_ENABLED
        Serial.printf("[ESP-NOW] ERROR: %s\n", last_error.c_str());
        #endif
        return false;
    }

    // Register receive callback
    esp_now_register_recv_cb(onDataRecv);

    #ifdef DEBUG_SERIAL_ENABLED
    Serial.println("[ESP-NOW] Master receiver initialized");
    Serial.printf("[ESP-NOW] Master MAC: %s\n", WiFi.macAddress().c_str());
    #endif

    return true;
}

void ESPNowMaster::update() {
    // Check for node timeouts
    uint32_t now = millis();

    for (int i = 0; i < 9; i++) {
        if (nodes[i].node_id != 0 && nodes[i].is_connected) {
            // Check if node timed out (no packet for ESPNOW_TIMEOUT_MS)
            if (now - nodes[i].last_update_ms > ESPNOW_TIMEOUT_MS) {
                nodes[i].is_connected = false;

                #ifdef DEBUG_SERIAL_ENABLED
                Serial.printf("[ESP-NOW] Node 0x%02X disconnected (timeout)\n", nodes[i].node_id);
                #endif
            }
        }
    }
}

SensorNode* ESPNowMaster::getNode(uint8_t node_id) {
    // Find node by ID
    for (int i = 0; i < 9; i++) {
        if (nodes[i].node_id == node_id) {
            return &nodes[i];
        }
    }
    return nullptr;
}

SensorNode* ESPNowMaster::getAllNodes(uint8_t &count) {
    count = 0;

    // Count registered nodes
    for (int i = 0; i < 9; i++) {
        if (nodes[i].node_id != 0) {
            count++;
        }
    }

    return nodes;
}

void ESPNowMaster::getStatistics(uint32_t &total_received, uint32_t &total_errors, float &loss_rate) {
    total_received = total_packets_received;
    total_errors = total_checksum_errors;

    if (total_received > 0) {
        loss_rate = (float)total_errors / (float)total_received;
    } else {
        loss_rate = 0.0;
    }
}

bool ESPNowMaster::isNodeConnected(uint8_t node_id) {
    SensorNode* node = getNode(node_id);
    if (node) {
        return node->is_connected;
    }
    return false;
}

String ESPNowMaster::getLastError() const {
    return last_error;
}

// ========================================
// Private Methods
// ========================================

void ESPNowMaster::onDataRecv(const uint8_t *mac, const uint8_t *data, int len) {
    // Static callback - forward to instance
    if (instance) {
        instance->handleReceivedPacket(mac, data, len);
    }
}

void ESPNowMaster::handleReceivedPacket(const uint8_t *mac, const uint8_t *data, int len) {
    // Validate packet length
    if (len != ESPNOW_PACKET_SIZE) {
        #ifdef DEBUG_ESPNOW_PACKETS
        Serial.printf("[ESP-NOW] Invalid packet size: %d (expected %d)\n", len, ESPNOW_PACKET_SIZE);
        #endif
        total_checksum_errors++;
        return;
    }

    // Validate checksum
    if (!validateChecksum(data, len)) {
        #ifdef DEBUG_ESPNOW_PACKETS
        Serial.printf("[ESP-NOW] Checksum error from %02X:%02X:%02X:%02X:%02X:%02X\n",
                     mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
        #endif
        total_checksum_errors++;
        return;
    }

    // Parse packet
    ESPNowPacket packet;
    memcpy(&packet, data, sizeof(ESPNowPacket));

    // Validate node ID (0x02-0x09)
    if (packet.node_id < 0x02 || packet.node_id > 0x09) {
        #ifdef DEBUG_ESPNOW_PACKETS
        Serial.printf("[ESP-NOW] Invalid node ID: 0x%02X\n", packet.node_id);
        #endif
        total_checksum_errors++;
        return;
    }

    // Find or register node
    SensorNode* node = getNode(packet.node_id);
    if (!node) {
        // New node - register it
        registerNode(packet.node_id, mac);
        node = getNode(packet.node_id);
    }

    if (node) {
        // Update node data
        node->device_type = static_cast<DeviceType>(packet.device_type);
        node->pitch = packet.pitch;
        node->roll = packet.roll;
        node->yaw = packet.yaw;
        node->front_weight_g = 0.0f;
        node->rear_weight_g = 0.0f;
        node->cog_position_mm = 0.0f;
        if (node->device_type == DEVICE_TYPE_COG_SCALE || node->device_type == DEVICE_TYPE_HYBRID) {
            node->front_weight_g = packet.pitch;
            node->rear_weight_g = packet.roll;
            node->cog_position_mm = packet.yaw;
        }
        node->battery_percent = packet.battery;
        node->is_connected = true;
        node->last_update_ms = millis();
        node->packets_received++;

        // Calculate RSSI (signal strength)
        // Note: ESP-NOW doesn't provide direct RSSI, estimate from WiFi
        node->rssi = WiFi.RSSI();

        total_packets_received++;

        #ifdef DEBUG_ESPNOW_PACKETS
        Serial.printf("[ESP-NOW] RX from 0x%02X (%s): a=%.2f b=%.2f c=%.2f battery=%d%% RSSI=%ddBm\n",
                     packet.node_id, deviceTypeToString(node->device_type),
                     packet.pitch, packet.roll, packet.yaw, packet.battery, node->rssi);
        #endif
    }
}

bool ESPNowMaster::validateChecksum(const uint8_t *data, int len) {
    // Calculate expected checksum (XOR of bytes 0 to len-2)
    uint8_t calculated = calculateChecksum(data, len - 1);

    // Compare with received checksum (last byte)
    uint8_t received = data[len - 1];

    return (calculated == received);
}

uint8_t ESPNowMaster::calculateChecksum(const uint8_t *data, int len) {
    uint8_t checksum = 0;
    for (int i = 0; i < len; i++) {
        checksum ^= data[i];
    }
    return checksum;
}

void ESPNowMaster::registerNode(uint8_t node_id, const uint8_t *mac) {
    // Find first empty slot
    for (int i = 0; i < 9; i++) {
        if (nodes[i].node_id == 0) {
            // Initialize new node
            nodes[i].node_id = node_id;
            nodes[i].device_type = DEVICE_TYPE_UNKNOWN;
            memcpy(nodes[i].mac_address, mac, 6);
            snprintf(nodes[i].label, sizeof(nodes[i].label), "Sensor %d", node_id - 1);
            nodes[i].pitch = 0.0;
            nodes[i].roll = 0.0;
            nodes[i].yaw = 0.0;
            nodes[i].front_weight_g = 0.0;
            nodes[i].rear_weight_g = 0.0;
            nodes[i].cog_position_mm = 0.0;
            nodes[i].pitch_offset = 0.0;
            nodes[i].roll_offset = 0.0;
            nodes[i].yaw_offset = 0.0;
            nodes[i].battery_percent = 0;
            nodes[i].battery_voltage = 0.0;
            nodes[i].rssi = 0;
            nodes[i].is_connected = false;
            nodes[i].last_update_ms = 0;
            nodes[i].packets_received = 0;
            nodes[i].packets_lost = 0;

            #ifdef DEBUG_SERIAL_ENABLED
            Serial.printf("[ESP-NOW] Registered new node 0x%02X (MAC: %02X:%02X:%02X:%02X:%02X:%02X)\n",
                         node_id, mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
            #endif

            break;
        }
    }
}