449  UAV/FANET Interactive Lab

449.1 Learning Objectives

By the end of this lab, you will be able to:

• Implement FANET Mesh Networking: Build and understand how drones discover neighbors and form ad-hoc networks
• Design Multi-Hop Message Relay: Forward data through intermediate drone nodes to reach distant destinations
• Program Swarm Coordination: Implement distributed algorithms for collective behavior
• Handle GPS Data: Parse, validate, and share location data between network nodes
• Integrate Ground Station Communication: Aggregate telemetry from multiple UAVs at a central command
• Implement Leader Election: Select coordinator nodes in distributed systems when the current leader fails
• Control Formation: Maintain relative positions in a swarm using various formation patterns

449.2 Prerequisites

Before diving into this lab, you should be familiar with:

• UAV Introduction: Basic UAV network concepts and FANET fundamentals
• UAV Topologies: Star and mesh topology concepts
• UAV Swarm Coordination: Coordination algorithms and formation control

449.3 Interactive Lab: UAV/Drone Communication Network

TipHands-On Learning: FANET Simulation with ESP32

This interactive lab simulates a Flying Ad-hoc Network (FANET) using ESP32 microcontrollers. You’ll explore drone-to-drone communication, ground station coordination, swarm behavior patterns, and GPS coordinate handling - all concepts central to UAV network design.

What You’ll Learn:

1. FANET Mesh Networking - How drones discover neighbors and form ad-hoc networks
2. Multi-Hop Message Relay - Forwarding data through intermediate drone nodes
3. Swarm Coordination - Distributed algorithms for collective behavior
4. GPS Data Handling - Parsing, validating, and sharing location data
5. Ground Station Integration - Central command and telemetry aggregation
6. Leader Election - Selecting coordinator nodes in distributed systems
7. Formation Control - Maintaining relative positions in a swarm

Lab Scenario: You are deploying a 4-drone search and rescue swarm. Each drone must: - Broadcast its position to neighbors - Relay emergency beacon signals toward the ground station - Maintain formation while searching - Elect a leader if the current leader goes offline

449.3.1 Wokwi Simulation Environment

NoteSetup Instructions

1. Copy the code below into the Wokwi editor
2. Click “Start Simulation” to run the FANET simulation
3. Open the Serial Monitor (115200 baud) to observe drone communications
4. Watch the console for swarm coordination messages, GPS updates, and relay operations

The simulation models 4 virtual drones communicating through a simulated mesh network, demonstrating real FANET concepts on a single ESP32.

449.3.2 Complete Lab Code: FANET Drone Communication System

/*
 * ============================================================================
 * UAV/FANET Communication Lab - ESP32 Drone Network Simulation
 * ============================================================================
 *
 * This lab simulates a Flying Ad-hoc Network (FANET) with 4 virtual drones
 * demonstrating key UAV networking concepts:
 *
 * 1. Mesh Network Formation - Neighbor discovery and link establishment
 * 2. Multi-Hop Routing - Message relay through intermediate nodes
 * 3. Swarm Coordination - Distributed behavior patterns
 * 4. GPS Handling - Position tracking and sharing
 * 5. Ground Station Communication - Telemetry and command relay
 * 6. Leader Election - Coordinator selection algorithms
 * 7. Formation Control - Relative position maintenance
 *
 * Educational IoT Course - UAV Networks Module
 * ============================================================================
 */

#include <Arduino.h>
#include <math.h>

// ============================================================================
// CONFIGURATION CONSTANTS
// ============================================================================

// Network Configuration
#define NUM_DRONES          4
#define GROUND_STATION_ID   0xFF
#define BROADCAST_ID        0xFE
#define MAX_NEIGHBORS       6
#define COMM_RANGE_METERS   2000.0    // 2 km air-to-air range
#define MESSAGE_QUEUE_SIZE  10
#define MAX_HOPS            5

// Timing Configuration (milliseconds)
#define BEACON_INTERVAL     2000      // Position broadcast interval
#define HEARTBEAT_INTERVAL  1000      // Alive signal interval
#define SWARM_UPDATE_INTERVAL 500     // Formation update rate
#define TELEMETRY_INTERVAL  3000      // Ground station report
#define LEADER_TIMEOUT      5000      // Leader heartbeat timeout
#define NEIGHBOR_TIMEOUT    6000      // Link expiry time

// GPS Simulation Bounds (decimal degrees)
#define BASE_LATITUDE       37.7749   // San Francisco area
#define BASE_LONGITUDE      -122.4194
#define GPS_VARIANCE        0.005     // ~500m movement range

// Swarm Formation Parameters
#define FORMATION_SPACING   100.0     // meters between drones
#define SEARCH_ALTITUDE     120.0     // meters (regulatory max)

// ============================================================================
// DATA STRUCTURES
// ============================================================================

// Message Types for FANET Protocol
enum MessageType {
    MSG_BEACON,           // Position broadcast
    MSG_HEARTBEAT,        // Alive signal
    MSG_TELEMETRY,        // Sensor data to ground
    MSG_COMMAND,          // Ground station command
    MSG_RELAY,            // Multi-hop forwarded message
    MSG_EMERGENCY,        // High-priority alert
    MSG_LEADER_ANNOUNCE,  // Leader election announcement
    MSG_LEADER_VOTE,      // Leader election vote
    MSG_FORMATION_CMD,    // Formation control command
    MSG_SWARM_STATE       // Collective swarm status
};

// Drone Operational States
enum DroneState {
    STATE_INITIALIZING,
    STATE_SEARCHING,
    STATE_HOVERING,
    STATE_RETURNING,
    STATE_RELAYING,
    STATE_EMERGENCY,
    STATE_LEADER
};

// Swarm Formation Types
enum FormationType {
    FORM_LINE,
    FORM_WEDGE,
    FORM_CIRCLE,
    FORM_GRID,
    FORM_SEARCH_PATTERN
};

// GPS Coordinates Structure
struct GPSCoordinate {
    double latitude;
    double longitude;
    float altitude;       // meters
    float accuracy;       // meters (GPS precision)
    unsigned long timestamp;
    bool valid;
};

// Neighbor Information
struct Neighbor {
    uint8_t droneId;
    GPSCoordinate position;
    float signalStrength;  // dBm
    float distance;        // meters
    unsigned long lastSeen;
    bool isAlive;
    DroneState state;
    float batteryLevel;
};

// Network Message Structure
struct FANETMessage {
    uint8_t sourceId;
    uint8_t destId;
    MessageType type;
    uint8_t hopCount;
    uint8_t ttl;          // Time to live (max hops)
    unsigned long timestamp;
    uint8_t dataLength;
    uint8_t data[64];
};

// Drone Telemetry Data
struct DroneTelemetry {
    uint8_t droneId;
    GPSCoordinate position;
    float batteryLevel;       // 0-100%
    float batteryVoltage;     // Volts
    DroneState state;
    float heading;            // degrees
    float speed;              // m/s
    float verticalSpeed;      // m/s
    uint8_t neighborCount;
    uint8_t signalQuality;    // 0-100%
    bool hasTarget;           // Search target detected
    float targetBearing;      // degrees to target
};

// Swarm State
struct SwarmState {
    uint8_t leaderId;
    FormationType formation;
    GPSCoordinate centroid;   // Swarm center
    uint8_t activeDrones;
    float coverageArea;       // km squared
    unsigned long missionTime;
    bool targetFound;
    GPSCoordinate targetLocation;
};

// ============================================================================
// VIRTUAL DRONE CLASS
// ============================================================================

class VirtualDrone {
public:
    uint8_t id;
    GPSCoordinate position;
    DroneTelemetry telemetry;
    Neighbor neighbors[MAX_NEIGHBORS];
    uint8_t neighborCount;
    DroneState state;
    bool isLeader;

    // Timing
    unsigned long lastBeacon;
    unsigned long lastHeartbeat;
    unsigned long lastTelemetry;
    unsigned long lastSwarmUpdate;
    unsigned long lastLeaderHeartbeat;

    // Formation position
    int formationIndex;
    GPSCoordinate targetPosition;

    // Mission state
    float searchProgress;
    bool targetDetected;

    VirtualDrone() {
        neighborCount = 0;
        state = STATE_INITIALIZING;
        isLeader = false;
        lastBeacon = 0;
        lastHeartbeat = 0;
        lastTelemetry = 0;
        lastSwarmUpdate = 0;
        lastLeaderHeartbeat = 0;
        searchProgress = 0;
        targetDetected = false;
    }

    void initialize(uint8_t droneId, int formIdx) {
        id = droneId;
        formationIndex = formIdx;

        // Initialize GPS position with offset based on formation index
        position.latitude = BASE_LATITUDE + (formIdx * 0.001);
        position.longitude = BASE_LONGITUDE + (formIdx * 0.001);
        position.altitude = SEARCH_ALTITUDE;
        position.accuracy = 2.5;  // meters
        position.timestamp = millis();
        position.valid = true;

        // Initialize telemetry
        telemetry.droneId = droneId;
        telemetry.position = position;
        telemetry.batteryLevel = 95.0 - (droneId * 5);  // Varied battery levels
        telemetry.batteryVoltage = 22.2 * (telemetry.batteryLevel / 100.0);
        telemetry.state = state;
        telemetry.heading = random(0, 360);
        telemetry.speed = 8.0;  // m/s survey speed
        telemetry.verticalSpeed = 0;
        telemetry.neighborCount = 0;
        telemetry.signalQuality = 85;
        telemetry.hasTarget = false;

        // Set target position (search waypoint)
        targetPosition = position;
        targetPosition.latitude += 0.002;  // ~200m north

        state = STATE_SEARCHING;

        Serial.printf("[DRONE-%d] Initialized at (%.6f, %.6f) alt=%.1fm\n",
                      id, position.latitude, position.longitude, position.altitude);
    }

    // Update GPS position (simulated movement)
    void updatePosition() {
        // Simulate movement toward target
        double dLat = targetPosition.latitude - position.latitude;
        double dLon = targetPosition.longitude - position.longitude;
        double dist = sqrt(dLat*dLat + dLon*dLon);

        if (dist > 0.0001) {  // ~11 meters
            // Move 1% toward target each update
            position.latitude += dLat * 0.01;
            position.longitude += dLon * 0.01;

            // Add small random variance (GPS jitter)
            position.latitude += (random(-100, 100) / 1000000.0);
            position.longitude += (random(-100, 100) / 1000000.0);
        } else {
            // Reached waypoint - set new random target
            targetPosition.latitude = BASE_LATITUDE + (random(-500, 500) / 100000.0);
            targetPosition.longitude = BASE_LONGITUDE + (random(-500, 500) / 100000.0);
            searchProgress += 5.0;
        }

        // Update telemetry
        position.timestamp = millis();
        telemetry.position = position;
        telemetry.heading = atan2(dLon, dLat) * 180.0 / PI;
        if (telemetry.heading < 0) telemetry.heading += 360;

        // Simulate battery drain
        telemetry.batteryLevel -= 0.01;
        telemetry.batteryVoltage = 22.2 * (telemetry.batteryLevel / 100.0);

        if (telemetry.batteryLevel < 20.0) {
            state = STATE_RETURNING;
        }
    }

    // Calculate distance to another drone
    float distanceTo(GPSCoordinate& other) {
        // Haversine formula for GPS distance
        double R = 6371000;  // Earth radius in meters
        double dLat = (other.latitude - position.latitude) * PI / 180.0;
        double dLon = (other.longitude - position.longitude) * PI / 180.0;
        double a = sin(dLat/2) * sin(dLat/2) +
                   cos(position.latitude * PI / 180.0) *
                   cos(other.latitude * PI / 180.0) *
                   sin(dLon/2) * sin(dLon/2);
        double c = 2 * atan2(sqrt(a), sqrt(1-a));
        float horizontalDist = R * c;

        // Include altitude difference
        float altDiff = other.altitude - position.altitude;
        return sqrt(horizontalDist * horizontalDist + altDiff * altDiff);
    }

    // Check if another position is within communication range
    bool isInRange(GPSCoordinate& other) {
        return distanceTo(other) <= COMM_RANGE_METERS;
    }

    // Update neighbor information
    void updateNeighbor(uint8_t neighborId, GPSCoordinate& pos,
                        DroneState neighborState, float battery) {
        // Check if already in neighbor list
        for (int i = 0; i < neighborCount; i++) {
            if (neighbors[i].droneId == neighborId) {
                neighbors[i].position = pos;
                neighbors[i].distance = distanceTo(pos);
                neighbors[i].lastSeen = millis();
                neighbors[i].isAlive = true;
                neighbors[i].state = neighborState;
                neighbors[i].batteryLevel = battery;
                // Simulate signal strength based on distance
                neighbors[i].signalStrength = -50.0 - (neighbors[i].distance / 50.0);
                return;
            }
        }

        // Add new neighbor
        if (neighborCount < MAX_NEIGHBORS) {
            neighbors[neighborCount].droneId = neighborId;
            neighbors[neighborCount].position = pos;
            neighbors[neighborCount].distance = distanceTo(pos);
            neighbors[neighborCount].lastSeen = millis();
            neighbors[neighborCount].isAlive = true;
            neighbors[neighborCount].state = neighborState;
            neighbors[neighborCount].batteryLevel = battery;
            neighbors[neighborCount].signalStrength = -50.0 - (neighbors[neighborCount].distance / 50.0);
            neighborCount++;
        }
    }

    // Remove stale neighbors
    void pruneNeighbors() {
        unsigned long now = millis();
        for (int i = neighborCount - 1; i >= 0; i--) {
            if (now - neighbors[i].lastSeen > NEIGHBOR_TIMEOUT) {
                Serial.printf("[DRONE-%d] Lost contact with Drone-%d\n",
                              id, neighbors[i].droneId);
                // Remove by shifting array
                for (int j = i; j < neighborCount - 1; j++) {
                    neighbors[j] = neighbors[j + 1];
                }
                neighborCount--;
            }
        }
        telemetry.neighborCount = neighborCount;
    }

    // Simulate target detection (for search missions)
    void checkForTarget() {
        // Random chance of detecting target based on search progress
        if (!targetDetected && random(0, 1000) < 2) {  // 0.2% chance per check
            targetDetected = true;
            telemetry.hasTarget = true;
            telemetry.targetBearing = random(0, 360);
            state = STATE_HOVERING;
            Serial.printf("\n[DRONE-%d] *** TARGET DETECTED at bearing %.0f deg ***\n\n",
                          id, telemetry.targetBearing);
        }
    }
};

// ============================================================================
// FANET MESSAGE HANDLING
// ============================================================================

class FANETNetwork {
public:
    FANETMessage messageQueue[MESSAGE_QUEUE_SIZE];
    uint8_t queueHead;
    uint8_t queueTail;
    uint8_t queueCount;

    // Swarm coordination
    SwarmState swarmState;
    uint8_t currentLeaderId;
    unsigned long lastLeaderAnnounce;
    bool leaderElectionInProgress;

    FANETNetwork() {
        queueHead = 0;
        queueTail = 0;
        queueCount = 0;
        currentLeaderId = 0xFF;
        lastLeaderAnnounce = 0;
        leaderElectionInProgress = false;

        swarmState.leaderId = 0xFF;
        swarmState.formation = FORM_SEARCH_PATTERN;
        swarmState.activeDrones = 0;
        swarmState.coverageArea = 0;
        swarmState.missionTime = 0;
        swarmState.targetFound = false;
    }

    // Queue a message for transmission
    bool queueMessage(FANETMessage& msg) {
        if (queueCount >= MESSAGE_QUEUE_SIZE) {
            return false;
        }
        messageQueue[queueTail] = msg;
        queueTail = (queueTail + 1) % MESSAGE_QUEUE_SIZE;
        queueCount++;
        return true;
    }

    // Dequeue next message
    bool dequeueMessage(FANETMessage& msg) {
        if (queueCount == 0) {
            return false;
        }
        msg = messageQueue[queueHead];
        queueHead = (queueHead + 1) % MESSAGE_QUEUE_SIZE;
        queueCount--;
        return true;
    }

    // Create beacon message
    FANETMessage createBeacon(VirtualDrone& drone) {
        FANETMessage msg;
        msg.sourceId = drone.id;
        msg.destId = BROADCAST_ID;
        msg.type = MSG_BEACON;
        msg.hopCount = 0;
        msg.ttl = 1;  // Single hop for beacons
        msg.timestamp = millis();

        // Pack position data
        memcpy(msg.data, &drone.position, sizeof(GPSCoordinate));
        msg.data[sizeof(GPSCoordinate)] = (uint8_t)drone.state;
        msg.data[sizeof(GPSCoordinate) + 1] = (uint8_t)drone.telemetry.batteryLevel;
        msg.dataLength = sizeof(GPSCoordinate) + 2;

        return msg;
    }

    // Create telemetry message for ground station
    FANETMessage createTelemetry(VirtualDrone& drone) {
        FANETMessage msg;
        msg.sourceId = drone.id;
        msg.destId = GROUND_STATION_ID;
        msg.type = MSG_TELEMETRY;
        msg.hopCount = 0;
        msg.ttl = MAX_HOPS;
        msg.timestamp = millis();

        memcpy(msg.data, &drone.telemetry, sizeof(DroneTelemetry));
        msg.dataLength = sizeof(DroneTelemetry);

        return msg;
    }

    // Create emergency message
    FANETMessage createEmergency(VirtualDrone& drone, const char* reason) {
        FANETMessage msg;
        msg.sourceId = drone.id;
        msg.destId = GROUND_STATION_ID;
        msg.type = MSG_EMERGENCY;
        msg.hopCount = 0;
        msg.ttl = MAX_HOPS;
        msg.timestamp = millis();

        // Pack emergency data
        memcpy(msg.data, &drone.position, sizeof(GPSCoordinate));
        strncpy((char*)(msg.data + sizeof(GPSCoordinate)), reason, 20);
        msg.dataLength = sizeof(GPSCoordinate) + 20;

        return msg;
    }

    // Create leader announcement
    FANETMessage createLeaderAnnounce(VirtualDrone& drone) {
        FANETMessage msg;
        msg.sourceId = drone.id;
        msg.destId = BROADCAST_ID;
        msg.type = MSG_LEADER_ANNOUNCE;
        msg.hopCount = 0;
        msg.ttl = MAX_HOPS;
        msg.timestamp = millis();

        msg.data[0] = drone.id;
        msg.data[1] = (uint8_t)drone.telemetry.batteryLevel;
        msg.dataLength = 2;

        return msg;
    }

    // Process received message
    void processMessage(FANETMessage& msg, VirtualDrone drones[], int numDrones) {
        switch (msg.type) {
            case MSG_BEACON:
                processBeacon(msg, drones, numDrones);
                break;
            case MSG_TELEMETRY:
                processTelemetry(msg);
                break;
            case MSG_EMERGENCY:
                processEmergency(msg);
                break;
            case MSG_LEADER_ANNOUNCE:
                processLeaderAnnounce(msg, drones, numDrones);
                break;
            case MSG_RELAY:
                processRelay(msg, drones, numDrones);
                break;
            default:
                break;
        }
    }

    // Process beacon - update neighbor tables
    void processBeacon(FANETMessage& msg, VirtualDrone drones[], int numDrones) {
        GPSCoordinate beaconPos;
        memcpy(&beaconPos, msg.data, sizeof(GPSCoordinate));
        DroneState beaconState = (DroneState)msg.data[sizeof(GPSCoordinate)];
        float beaconBattery = msg.data[sizeof(GPSCoordinate) + 1];

        // Update all drones' neighbor tables
        for (int i = 0; i < numDrones; i++) {
            if (drones[i].id != msg.sourceId) {
                if (drones[i].isInRange(beaconPos)) {
                    drones[i].updateNeighbor(msg.sourceId, beaconPos,
                                             beaconState, beaconBattery);
                }
            }
        }
    }

    // Process telemetry at ground station
    void processTelemetry(FANETMessage& msg) {
        DroneTelemetry telem;
        memcpy(&telem, msg.data, sizeof(DroneTelemetry));

        Serial.printf("[GCS] Telemetry from Drone-%d: ", telem.droneId);
        Serial.printf("Pos(%.6f,%.6f,%.1fm) ",
                      telem.position.latitude,
                      telem.position.longitude,
                      telem.position.altitude);
        Serial.printf("Batt=%.1f%% ", telem.batteryLevel);
        Serial.printf("Neighbors=%d ", telem.neighborCount);
        Serial.printf("State=%d ", (int)telem.state);
        if (telem.hasTarget) {
            Serial.printf("TARGET@%.0fdeg", telem.targetBearing);
        }
        Serial.println();

        // Update swarm state
        swarmState.activeDrones++;
        if (telem.hasTarget && !swarmState.targetFound) {
            swarmState.targetFound = true;
            swarmState.targetLocation = telem.position;
            Serial.println("\n[GCS] *** SWARM TARGET ACQUISITION CONFIRMED ***\n");
        }
    }

    // Process emergency message
    void processEmergency(FANETMessage& msg) {
        GPSCoordinate emergPos;
        memcpy(&emergPos, msg.data, sizeof(GPSCoordinate));
        char reason[21];
        strncpy(reason, (char*)(msg.data + sizeof(GPSCoordinate)), 20);
        reason[20] = '\0';

        Serial.printf("\n[GCS] !!! EMERGENCY from Drone-%d !!!\n", msg.sourceId);
        Serial.printf("[GCS] Location: (%.6f, %.6f, %.1fm)\n",
                      emergPos.latitude, emergPos.longitude, emergPos.altitude);
        Serial.printf("[GCS] Reason: %s\n\n", reason);
    }

    // Process leader announcement
    void processLeaderAnnounce(FANETMessage& msg, VirtualDrone drones[], int numDrones) {
        uint8_t announcedLeader = msg.data[0];
        uint8_t leaderBattery = msg.data[1];

        // Simple leader election: highest ID with sufficient battery wins
        if (announcedLeader > currentLeaderId ||
            (currentLeaderId == 0xFF && leaderBattery > 30)) {
            currentLeaderId = announcedLeader;
            lastLeaderAnnounce = millis();
            swarmState.leaderId = currentLeaderId;

            // Update drone states
            for (int i = 0; i < numDrones; i++) {
                if (drones[i].id == currentLeaderId) {
                    drones[i].isLeader = true;
                    Serial.printf("[SWARM] Drone-%d elected as SWARM LEADER\n",
                                  currentLeaderId);
                } else {
                    drones[i].isLeader = false;
                }
            }
        }
    }

    // Process relay message - multi-hop forwarding
    void processRelay(FANETMessage& msg, VirtualDrone drones[], int numDrones) {
        if (msg.hopCount >= msg.ttl) {
            Serial.printf("[RELAY] Message from Drone-%d expired (TTL=%d)\n",
                          msg.sourceId, msg.ttl);
            return;
        }

        // Forward to destination or next hop
        msg.hopCount++;

        // Find next hop toward destination
        if (msg.destId == GROUND_STATION_ID) {
            // Ground station - forward toward lowest ID drone (closest to GCS)
            Serial.printf("[RELAY] Forwarding msg from Drone-%d to GCS (hop %d/%d)\n",
                          msg.sourceId, msg.hopCount, msg.ttl);
        }

        // Re-queue for transmission
        queueMessage(msg);
    }

    // Check for leader timeout and trigger election
    void checkLeaderStatus(VirtualDrone drones[], int numDrones) {
        unsigned long now = millis();

        if (currentLeaderId != 0xFF &&
            now - lastLeaderAnnounce > LEADER_TIMEOUT) {
            Serial.printf("[SWARM] Leader Drone-%d timeout - starting election\n",
                          currentLeaderId);
            currentLeaderId = 0xFF;
            leaderElectionInProgress = true;

            // Find best candidate
            uint8_t bestCandidate = 0xFF;
            float bestBattery = 0;
            for (int i = 0; i < numDrones; i++) {
                if (drones[i].telemetry.batteryLevel > bestBattery &&
                    drones[i].state != STATE_RETURNING) {
                    bestBattery = drones[i].telemetry.batteryLevel;
                    bestCandidate = drones[i].id;
                }
            }

            if (bestCandidate != 0xFF) {
                FANETMessage announce = createLeaderAnnounce(drones[bestCandidate]);
                queueMessage(announce);
            }
        }
    }

    // Calculate swarm metrics
    void updateSwarmState(VirtualDrone drones[], int numDrones) {
        swarmState.activeDrones = 0;
        double sumLat = 0, sumLon = 0, sumAlt = 0;

        for (int i = 0; i < numDrones; i++) {
            if (drones[i].state != STATE_RETURNING) {
                swarmState.activeDrones++;
                sumLat += drones[i].position.latitude;
                sumLon += drones[i].position.longitude;
                sumAlt += drones[i].position.altitude;
            }
        }

        if (swarmState.activeDrones > 0) {
            swarmState.centroid.latitude = sumLat / swarmState.activeDrones;
            swarmState.centroid.longitude = sumLon / swarmState.activeDrones;
            swarmState.centroid.altitude = sumAlt / swarmState.activeDrones;
        }

        swarmState.missionTime = millis() / 1000;
    }
};

// ============================================================================
// FORMATION CONTROL
// ============================================================================

class FormationController {
public:
    FormationType currentFormation;
    float spacing;
    GPSCoordinate leaderPosition;

    FormationController() {
        currentFormation = FORM_SEARCH_PATTERN;
        spacing = FORMATION_SPACING;
    }

    // Calculate formation position for each drone
    GPSCoordinate getFormationPosition(uint8_t droneIndex, GPSCoordinate& center,
                                        float heading) {
        GPSCoordinate pos = center;
        float offsetX = 0, offsetY = 0;

        // Convert heading to radians
        float headRad = heading * PI / 180.0;

        switch (currentFormation) {
            case FORM_LINE:
                // Line formation perpendicular to heading
                offsetX = (droneIndex - 1.5) * spacing;
                break;

            case FORM_WEDGE:
                // V-formation behind leader
                if (droneIndex == 0) {
                    // Leader at front
                } else {
                    int side = (droneIndex % 2 == 0) ? 1 : -1;
                    int depth = (droneIndex + 1) / 2;
                    offsetX = side * depth * spacing * 0.7;
                    offsetY = -depth * spacing;
                }
                break;

            case FORM_CIRCLE:
                // Circle formation
                {
                    float angle = (2 * PI * droneIndex) / NUM_DRONES;
                    offsetX = spacing * cos(angle);
                    offsetY = spacing * sin(angle);
                }
                break;

            case FORM_GRID:
                // 2x2 grid
                offsetX = ((droneIndex % 2) - 0.5) * spacing;
                offsetY = ((droneIndex / 2) - 0.5) * spacing;
                break;

            case FORM_SEARCH_PATTERN:
                // Spread out for maximum coverage
                {
                    float angle = (2 * PI * droneIndex) / NUM_DRONES;
                    float radius = spacing * 2;  // Wider spread
                    offsetX = radius * cos(angle);
                    offsetY = radius * sin(angle);
                }
                break;
        }

        // Rotate offsets by heading
        float rotX = offsetX * cos(headRad) - offsetY * sin(headRad);
        float rotY = offsetX * sin(headRad) + offsetY * cos(headRad);

        // Convert meter offsets to GPS coordinates
        // Approximate: 1 degree latitude = 111 km
        // 1 degree longitude = 111 km * cos(latitude)
        pos.latitude += rotY / 111000.0;
        pos.longitude += rotX / (111000.0 * cos(pos.latitude * PI / 180.0));

        return pos;
    }

    // Print formation status
    void printFormationStatus(VirtualDrone drones[], int numDrones) {
        Serial.println("\n[FORMATION] Current Swarm Positions:");
        Serial.println("+---------+-------------+---------------+--------+----------+");
        Serial.println("| Drone   | Latitude    | Longitude     | Alt(m) | State    |");
        Serial.println("+---------+-------------+---------------+--------+----------+");

        for (int i = 0; i < numDrones; i++) {
            const char* stateStr;
            switch (drones[i].state) {
                case STATE_INITIALIZING: stateStr = "INIT"; break;
                case STATE_SEARCHING: stateStr = "SEARCH"; break;
                case STATE_HOVERING: stateStr = "HOVER"; break;
                case STATE_RETURNING: stateStr = "RETURN"; break;
                case STATE_RELAYING: stateStr = "RELAY"; break;
                case STATE_EMERGENCY: stateStr = "EMERG"; break;
                case STATE_LEADER: stateStr = "LEADER"; break;
                default: stateStr = "UNKNOWN"; break;
            }

            Serial.printf("| Drone-%d | %11.6f | %13.6f | %6.1f | %-8s |\n",
                          drones[i].id,
                          drones[i].position.latitude,
                          drones[i].position.longitude,
                          drones[i].position.altitude,
                          stateStr);
        }
        Serial.println("+---------+-------------+---------------+--------+----------+\n");
    }
};

// ============================================================================
// ROUTING PROTOCOL
// ============================================================================

class FANETRouter {
public:
    // Find best next hop toward destination
    uint8_t findNextHop(VirtualDrone& source, uint8_t destId,
                         VirtualDrone drones[], int numDrones) {
        if (destId == BROADCAST_ID) {
            return BROADCAST_ID;
        }

        // For ground station, route toward drone 0 (designated relay)
        if (destId == GROUND_STATION_ID) {
            // Find neighbor closest to drone 0
            float minDist = 999999;
            uint8_t bestHop = GROUND_STATION_ID;

            for (int i = 0; i < source.neighborCount; i++) {
                if (source.neighbors[i].droneId == 0) {
                    // Direct path to relay drone
                    return 0;
                }

                // Find neighbor closest to drone 0
                for (int j = 0; j < numDrones; j++) {
                    if (drones[j].id == source.neighbors[i].droneId) {
                        float dist = drones[j].distanceTo(drones[0].position);
                        if (dist < minDist) {
                            minDist = dist;
                            bestHop = source.neighbors[i].droneId;
                        }
                        break;
                    }
                }
            }
            return bestHop;
        }

        // Direct destination check
        for (int i = 0; i < source.neighborCount; i++) {
            if (source.neighbors[i].droneId == destId) {
                return destId;
            }
        }

        // No route found
        return 0xFF;
    }

    // Calculate route quality metric
    float calculateLinkQuality(Neighbor& neighbor) {
        // Combine signal strength and battery level
        float signalFactor = (neighbor.signalStrength + 100) / 50.0;  // Normalize
        float batteryFactor = neighbor.batteryLevel / 100.0;
        float ageFactor = 1.0 - ((millis() - neighbor.lastSeen) /
                                 (float)NEIGHBOR_TIMEOUT);

        return signalFactor * 0.4 + batteryFactor * 0.3 + ageFactor * 0.3;
    }
};

// ============================================================================
// GLOBAL INSTANCES
// ============================================================================

VirtualDrone drones[NUM_DRONES];
FANETNetwork network;
FormationController formation;
FANETRouter router;

// Timing
unsigned long lastStatusPrint = 0;
unsigned long simulationStart = 0;
const unsigned long STATUS_PRINT_INTERVAL = 5000;

// ============================================================================
// MAIN SETUP
// ============================================================================

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

    Serial.println("\n");
    Serial.println("================================================================");
    Serial.println("     UAV/FANET COMMUNICATION LAB - ESP32 DRONE SIMULATION       ");
    Serial.println("================================================================");
    Serial.println("  This lab demonstrates Flying Ad-hoc Network (FANET) concepts: ");
    Serial.println("  - Mesh network formation and neighbor discovery               ");
    Serial.println("  - Multi-hop message relay to ground station                   ");
    Serial.println("  - Swarm coordination and leader election                      ");
    Serial.println("  - GPS position tracking and sharing                           ");
    Serial.println("  - Search and rescue mission simulation                        ");
    Serial.println("================================================================\n");

    // Initialize random seed
    randomSeed(analogRead(0));

    // Initialize drones
    Serial.println("[SYSTEM] Initializing UAV Swarm...\n");
    for (int i = 0; i < NUM_DRONES; i++) {
        drones[i].initialize(i, i);
        delay(100);
    }

    // Drone 0 is initially the leader (closest to ground station)
    drones[0].isLeader = true;
    network.currentLeaderId = 0;
    network.lastLeaderAnnounce = millis();

    Serial.println("\n[SYSTEM] Swarm initialized. Starting FANET operations...\n");
    Serial.println("========================================================\n");

    simulationStart = millis();
}

// ============================================================================
// MAIN LOOP
// ============================================================================

void loop() {
    unsigned long currentTime = millis();

    // ===========================================
    // UPDATE EACH DRONE
    // ===========================================
    for (int i = 0; i < NUM_DRONES; i++) {
        VirtualDrone& drone = drones[i];

        // Update position (simulated GPS movement)
        drone.updatePosition();

        // Check for target (search mission)
        drone.checkForTarget();

        // Prune stale neighbors
        drone.pruneNeighbors();

        // Send beacon
        if (currentTime - drone.lastBeacon >= BEACON_INTERVAL) {
            FANETMessage beacon = network.createBeacon(drone);
            network.queueMessage(beacon);
            drone.lastBeacon = currentTime;

            // Process beacon immediately for simulation
            network.processBeacon(beacon, drones, NUM_DRONES);
        }

        // Send telemetry to ground station
        if (currentTime - drone.lastTelemetry >= TELEMETRY_INTERVAL) {
            FANETMessage telemetry = network.createTelemetry(drone);

            // Find route to ground station
            uint8_t nextHop = router.findNextHop(drone, GROUND_STATION_ID,
                                                  drones, NUM_DRONES);

            if (nextHop != 0xFF) {
                if (drone.id == 0) {
                    // Drone 0 sends directly to ground station
                    network.processTelemetry(telemetry);
                } else {
                    // Other drones relay through mesh
                    Serial.printf("[DRONE-%d] Relaying telemetry via Drone-%d\n",
                                  drone.id, nextHop);
                    telemetry.type = MSG_RELAY;
                    network.queueMessage(telemetry);
                    // Simulate successful relay
                    network.processTelemetry(telemetry);
                }
            }

            drone.lastTelemetry = currentTime;
        }

        // Leader announces periodically
        if (drone.isLeader &&
            currentTime - network.lastLeaderAnnounce >= HEARTBEAT_INTERVAL * 2) {
            FANETMessage announce = network.createLeaderAnnounce(drone);
            network.queueMessage(announce);
            network.processLeaderAnnounce(announce, drones, NUM_DRONES);
        }

        // Check for low battery emergency
        if (drone.telemetry.batteryLevel < 15.0 &&
            drone.state != STATE_RETURNING) {
            Serial.printf("[DRONE-%d] LOW BATTERY WARNING: %.1f%%\n",
                          drone.id, drone.telemetry.batteryLevel);
            drone.state = STATE_RETURNING;

            FANETMessage emergency = network.createEmergency(drone, "LOW BATTERY RTH");
            network.queueMessage(emergency);
            network.processEmergency(emergency);
        }
    }

    // ===========================================
    // NETWORK MANAGEMENT
    // ===========================================

    // Check leader status
    network.checkLeaderStatus(drones, NUM_DRONES);

    // Update swarm state
    network.updateSwarmState(drones, NUM_DRONES);

    // ===========================================
    // PERIODIC STATUS OUTPUT
    // ===========================================

    if (currentTime - lastStatusPrint >= STATUS_PRINT_INTERVAL) {
        Serial.println("\n========== FANET STATUS UPDATE ==========");

        // Print swarm summary
        Serial.printf("[SWARM] Mission Time: %lu sec\n",
                      network.swarmState.missionTime);
        Serial.printf("[SWARM] Active Drones: %d/%d\n",
                      network.swarmState.activeDrones, NUM_DRONES);
        Serial.printf("[SWARM] Leader: Drone-%d\n", network.currentLeaderId);
        Serial.printf("[SWARM] Centroid: (%.6f, %.6f)\n",
                      network.swarmState.centroid.latitude,
                      network.swarmState.centroid.longitude);

        if (network.swarmState.targetFound) {
            Serial.println("[SWARM] *** TARGET LOCATED ***");
        }

        // Print neighbor tables
        Serial.println("\n[MESH] Neighbor Tables:");
        for (int i = 0; i < NUM_DRONES; i++) {
            Serial.printf("  Drone-%d neighbors: ", drones[i].id);
            if (drones[i].neighborCount == 0) {
                Serial.print("(none)");
            } else {
                for (int j = 0; j < drones[i].neighborCount; j++) {
                    Serial.printf("D%d(%.0fm) ",
                                  drones[i].neighbors[j].droneId,
                                  drones[i].neighbors[j].distance);
                }
            }
            Serial.println();
        }

        // Print formation
        formation.printFormationStatus(drones, NUM_DRONES);

        lastStatusPrint = currentTime;
    }

    // Small delay to prevent watchdog timeout
    delay(50);
}

449.3.3 Lab Exercises

TipExercise 1: Mesh Topology Analysis

Objective: Understand how neighbor discovery works in FANETs

Tasks: 1. Run the simulation and observe the neighbor tables printed every 5 seconds 2. Note which drones can communicate directly (neighbors) vs which require multi-hop relay 3. Modify COMM_RANGE_METERS to 500 meters and observe how the mesh topology changes 4. Answer: With reduced range, which drones become isolated? How does this affect telemetry delivery?

Expected Observation: Reducing communication range fragments the mesh network, requiring more relay hops or causing some drones to lose connectivity with the ground station.

TipExercise 2: Leader Election Simulation

Objective: Explore distributed leader election in drone swarms

Tasks: 1. Find the LEADER_TIMEOUT constant and reduce it to 2000 ms 2. Observe what happens when leader elections occur more frequently 3. Modify the election criteria in processLeaderAnnounce() to prioritize: - Highest battery level instead of highest ID - Most neighbors (best connectivity) 4. Answer: What are the trade-offs between different leader election criteria?

Code Modification (change in processLeaderAnnounce):

// Original: highest ID wins
if (announcedLeader > currentLeaderId)

// Modified: highest battery wins
if (leaderBattery > bestLeaderBattery)

TipExercise 3: Multi-Hop Routing Enhancement

Objective: Implement and test different routing strategies

Tasks: 1. Locate the findNextHop() function in the FANETRouter class 2. The current strategy routes toward Drone-0 (ground station relay) 3. Modify the routing to consider: - Link quality (signal strength + battery) - Hop count minimization - Load balancing across multiple paths 4. Test your modifications and compare telemetry delivery reliability

Challenge: Implement a “geographic routing” strategy that forwards messages toward the GPS coordinates of the ground station.

TipExercise 4: Swarm Formation Control

Objective: Experiment with different swarm formations

Tasks: 1. Find the FormationType enum and getFormationPosition() function 2. The simulation defaults to FORM_SEARCH_PATTERN 3. Modify setup() to use different formations: - FORM_LINE - for corridor search - FORM_WEDGE - for leader-following - FORM_CIRCLE - for area encirclement 4. Observe how formation affects coverage and connectivity

Questions to Answer: - Which formation provides the best mesh connectivity? - Which formation covers the most area? - How does formation choice affect battery consumption?

TipExercise 5: Emergency Response Protocol

Objective: Design robust emergency handling for UAV networks

Tasks: 1. Find the emergency message handling in processEmergency() 2. Currently, emergencies are only logged - enhance the response: - When a drone sends LOW BATTERY, have nearby drones adjust formation - Implement a “buddy system” where healthy drones escort returning drones - Add altitude collision avoidance for returning drones 3. Test by reducing initial battery levels to trigger emergencies sooner

Code Addition:

// In processEmergency() - add swarm response
void respondToEmergency(uint8_t emergDroneId, VirtualDrone drones[], int numDrones) {
    // Find nearest healthy drone
    // Reassign as escort
    // Update formation to close gap
}

449.3.4 Expected Learning Outcomes

After completing this lab, students should be able to:

Outcome	Demonstrated Skill
Mesh Networking	Understand how drones discover neighbors and form ad-hoc networks without infrastructure
Multi-Hop Routing	Explain how messages traverse multiple drone relays to reach the ground station
Leader Election	Implement and compare distributed coordinator selection algorithms
GPS Handling	Parse, validate, and share geographic coordinates between network nodes
Swarm Coordination	Design formation patterns that balance coverage, connectivity, and energy efficiency
Fault Tolerance	Build resilient systems that handle drone failures, low batteries, and link disruptions
Real-Time Telemetry	Aggregate and process sensor data from multiple mobile nodes

InlineKnowledgeCheck_kc_uav_lab_1 = ({
  question: "A package delivery company operates a fleet of 20 delivery drones in a city with a 10 km radius coverage area. FAA regulations require that all drones maintain communication with the ground control station and broadcast Remote ID (identification and location) every 1 second. Each drone sends 100 bytes of telemetry per second. What is the minimum uplink bandwidth required at the ground control station?",
  options: [
    "16 kbps (kilobits per second)",
    "160 kbps (kilobits per second)",
    "1.6 Mbps (megabits per second)",
    "16 Mbps (megabits per second)"
  ],
  correctAnswer: 0,
  feedback: {
    correct: "Perfect calculation! 16 kbps is correct. Data per drone per second: 100 bytes/s times 8 bits/byte = 800 bits/s per drone. Total for 20 drones: 20 drones times 800 bits/s = 16,000 bits/s = 16 kbps. Real-world systems add protocol overhead (headers, acknowledgments) increasing requirement to approximately 20-22 kbps - still well within typical 4G/5G cellular uplink capacity.",
    incorrect: [
      "If you calculated 160 kbps, check your unit conversion. 100 bytes/s times 20 drones = 2,000 bytes/s. Converting to bits: 2,000 times 8 = 16,000 bits/s = 16 kbps, not 160 kbps.",
      "Calculating 1.6 Mbps suggests incorrect unit conversion or adding unrelated data streams. The calculation should be: 20 drones times 100 bytes/s = 2,000 bytes/s times 8 bits/byte = 16,000 bits/s = 16 kbps.",
      "If you arrived at 16 Mbps, there's a major unit conversion error. You may have confused bytes with kilobytes. 100 bytes = 0.1 kilobytes, not 100 kilobytes."
    ]
  },
  difficulty: "easy",
  learningObjective: "Calculate bandwidth requirements for UAV telemetry and regulatory compliance data"
})

NoteConnecting to Real UAV Systems

The concepts demonstrated in this simulation directly map to real FANET implementations:

Simulation Concept	Real-World Implementation
FANETMessage struct	MAVLink protocol messages used by PX4/ArduPilot
GPSCoordinate handling	NMEA sentence parsing from GPS modules
Neighbor discovery	802.11s mesh networking or custom radio protocols
Leader election	Raft/Paxos consensus adapted for mobile networks
Multi-hop routing	AODV, OLSR, or geographic routing (GPSR, GOAFR)
Swarm formations	Distributed formation control algorithms (consensus-based)

For production UAV systems, consider using: - PX4 or ArduPilot for flight control - MAVLink for ground-to-air communication - ROS 2 for multi-robot coordination - ESP-NOW or LoRa for air-to-air mesh networking

449.4 Summary

This hands-on lab demonstrated key FANET concepts through practical ESP32 simulation:

• Mesh Networking: Drones discover neighbors via beacon broadcasts and maintain neighbor tables with distance and signal strength
• Multi-Hop Routing: Messages traverse multiple relay nodes using geographic-based routing toward the ground station
• Leader Election: Distributed algorithm selects swarm coordinator based on battery level and connectivity
• Formation Control: Various patterns (line, wedge, circle, grid, search) balance coverage, connectivity, and energy efficiency
• Emergency Handling: Low battery triggers return-to-home with mesh-relayed emergency alerts

449.5 What’s Next

Continue exploring UAV networks with related chapters:

• FANET and Integration - Advanced routing protocols and ground IoT integration
• UAV Production Review - Comprehensive UAV network summary and review

Return to UAV Fundamentals Index