stagger takeoff/landing for increased spacing

aerpaw/MESSAGE_TYPE.m

@@ -8,5 +8,6 @@ classdef MESSAGE_TYPE < uint8
         GUIDANCE_TOGGLE  (6)  % Server->Client: toggle guidance mode on/off
         REQUEST_POSITION (7)  % Server->Client: respond with current ENU position
         POSITION         (8)  % Client->Server: current ENU position (3 doubles)
+        TAKEOFF          (9)  % Server->Client: take off to 25 m, ACK then READY on arrival
     end
 end

aerpaw/client/uav_runner.py

@@ -44,6 +44,7 @@ class MessageType(IntEnum):
     GUIDANCE_TOGGLE  = 6
     REQUEST_POSITION = 7
     POSITION         = 8
+    TAKEOFF          = 9
 
 
 AERPAW_DIR = Path('/root/miSim/aerpaw')
@@ -184,13 +185,11 @@ class UAVRunner(BasicRunner):
         log_task = None
         nav_task = None
         try:
-            # Takeoff to above AERPAW minimum altitude
-            print("[UAV] Taking off...")
-            await drone.takeoff(25)
-            print("[UAV] Takeoff complete, waiting for commands...")
-
-            # Start GPS logging in background
+            # Takeoff is now driven by a TAKEOFF command from the controller
+            # (staggered in time between UAVs), not performed at startup. Start
+            # GPS logging immediately so the ground/idle period is captured too.
             log_task = asyncio.create_task(_gps_log_loop(drone))
+            print("[UAV] Connected; waiting for commands...")
 
             # Command loop - handle all messages from controller
             waypoint_num = 0
@@ -213,6 +212,27 @@ class UAVRunner(BasicRunner):
                         await send_message_type(writer, MessageType.ACK)
                         print("[UAV] Sent ACK (guidance mode exited, ready for sequential commands)")
 
+                elif msg_type == MessageType.TAKEOFF:
+                    # aerpawlib only switches the vehicle to GUIDED inside
+                    # _initialize_postarm(), which await_ready_to_move() runs only
+                    # when the vehicle is DISARMED at the first movement command.
+                    # The safety pilot arms all UAVs at experiment start, so a UAV
+                    # whose takeoff is deliberately delayed (for time separation)
+                    # is already armed by now and would skip that path -> no GUIDED
+                    # switch -> simple_takeoff is ignored in ALT_HOLD and it never
+                    # leaves the ground. Run the post-arm init explicitly to
+                    # guarantee GUIDED mode (and home capture) before taking off;
+                    # takeoff()'s own gate then sees the vehicle armed and does not
+                    # re-initialize.
+                    await send_message_type(writer, MessageType.ACK)
+                    print("[UAV] Sent ACK (takeoff)")
+                    print("[UAV] Initializing (GUIDED) and taking off to 25 m...")
+                    await drone._initialize_postarm()
+                    await drone.takeoff(25)
+                    print("[UAV] Takeoff complete")
+                    await send_message_type(writer, MessageType.READY)
+                    print("[UAV] Sent READY")
+
                 elif msg_type == MessageType.REQUEST_POSITION:
                     # Respond immediately with current ENU position relative to origin
                     pos = drone.position
@@ -288,6 +308,11 @@ class UAVRunner(BasicRunner):
         except (ValueError, asyncio.IncompleteReadError, ConnectionError) as e:
             print(f"[UAV] Error: {e}")
 
+        except Exception as e:
+            import traceback
+            print(f"[UAV] Unhandled exception: {type(e).__name__}: {e}")
+            traceback.print_exc()
+
         finally:
             if nav_task is not None and not nav_task.done():
                 nav_task.cancel()

aerpaw/controller.m

@@ -127,37 +127,72 @@ if ~coder.target('MATLAB')
 end
 % -------------------------------------------------------------------------------
 
-% Waypoint loop: send each waypoint to all clients, wait for all to arrive
+% Waypoint loop
+if coder.target('MATLAB')
+    % Simulation: simple lockstep (collisions are not modelled here).
     for w = 1:numWaypoints
-    % Send TARGET for waypoint w to each client
         for i = 1:numClients
-        % Targets are grouped by client: client i's waypoints are at rows
-        % (i-1)*numWaypoints+1 through i*numWaypoints
             targetIdx = (i - 1) * numWaypoints + w;
             target = targets(targetIdx, :);
-
-        if coder.target('MATLAB')
             disp(['Sending TARGET to client ', num2str(i), ' (waypoint ', num2str(w), '): ', ...
                   num2str(target(1)), ',', num2str(target(2)), ',', num2str(target(3))]);
-        else
-            coder.ceval('sendTarget', int32(i), coder.ref(target));
         end
-    end
-
-    % Wait for ACK from all clients
-    if coder.target('MATLAB')
         disp('Waiting for ACK from all clients...');
+        disp(['All UAVs arrived at waypoint ', num2str(w)]);
+    end
 else
-        coder.ceval('waitForAllMessageType', int32(numClients), ...
-                    int32(MESSAGE_TYPE.ACK));
+    % ---- Staggered flyout -----------------------------------------------------
+    % Separate the two UAVs in TIME so they are never doing the same vertical
+    % climb at the same moment over their ~10 m-separated pads (which can trip
+    % the proximity guard on a spurious GPS sample). The UAV that transits
+    % highest leads by exactly one event.
+    %
+    % Events per UAV: 1 = TAKEOFF (25 m), 2 = WP1 (climb at pad), 3 = WP2
+    % (traverse), 4 = WP3 (descend). At each step the higher UAV does event
+    % `step` while the lower UAV does event `step-1`; both are dispatched, then
+    % we collect ACK and then READY (arrival) from each active UAV.
+    numEvents = double(numWaypoints) + 1;   % 3 waypoints + 1 takeoff
+    for step = 1:(numEvents + 1)
+        hEvent = step;
+        lEvent = step - 1;
+        hActive = (hEvent >= 1) && (hEvent <= numEvents);
+        lActive = (lEvent >= 1) && (lEvent <= numEvents);
+
+        % --- Dispatch this step's event to each active UAV ---
+        if hActive
+            if hEvent == 1
+                coder.ceval('sendMessageType', higherIdx, int32(MESSAGE_TYPE.TAKEOFF));
+            else
+                hRow = (double(higherIdx) - 1) * double(numWaypoints) + (hEvent - 1);
+                hTarget = targets(hRow, :);
+                coder.ceval('sendTarget', higherIdx, coder.ref(hTarget));
+            end
+        end
+        if lActive
+            if lEvent == 1
+                coder.ceval('sendMessageType', lowerIdx, int32(MESSAGE_TYPE.TAKEOFF));
+            else
+                lRow = (double(lowerIdx) - 1) * double(numWaypoints) + (lEvent - 1);
+                lTarget = targets(lRow, :);
+                coder.ceval('sendTarget', lowerIdx, coder.ref(lTarget));
+            end
         end
 
-    % Wait for READY from all clients (all arrived at waypoint w)
-    if coder.target('MATLAB')
-        disp(['All UAVs arrived at waypoint ', num2str(w)]);
-    else
-        coder.ceval('waitForAllMessageType', int32(numClients), ...
-                    int32(MESSAGE_TYPE.READY));
+        % --- Collect ACK (command received) from each active UAV ---
+        if hActive
+            coder.ceval('waitForClientMessageType', higherIdx, int32(MESSAGE_TYPE.ACK));
+        end
+        if lActive
+            coder.ceval('waitForClientMessageType', lowerIdx, int32(MESSAGE_TYPE.ACK));
+        end
+
+        % --- Collect READY (arrived at event) from each active UAV ---
+        if hActive
+            coder.ceval('waitForClientMessageType', higherIdx, int32(MESSAGE_TYPE.READY));
+        end
+        if lActive
+            coder.ceval('waitForClientMessageType', lowerIdx, int32(MESSAGE_TYPE.READY));
+        end
     end
 end
 
@@ -262,17 +297,48 @@ if ~coder.target('MATLAB')
         retBase = double(iUAV - 1) * double(NUM_RETURN_WP);
         returnTargets(retBase + 1, :) = [positions(iUAV,1),         positions(iUAV,2),         returnTransitAlt(iUAV)];
         returnTargets(retBase + 2, :) = [initialPositions(iUAV,1),  initialPositions(iUAV,2),  returnTransitAlt(iUAV)];
-        returnTargets(retBase + 3, :) =  initialPositions(iUAV, :);
+        % Final return waypoint: pad XY from the pre-takeoff GPS query, but a fixed
+        % 25 m up. That query ran while the UAV was on the ground, so its ENU
+        % up-component is ~0 (often slightly negative) — using it directly commands
+        % a negative altitude. RTL/LAND perform the actual touchdown from here.
+        returnTargets(retBase + 3, :) = [initialPositions(iUAV,1), initialPositions(iUAV,2), 25.0];
     end
 
-    for w = 1:NUM_RETURN_WP
-        for i = 1:numClients
-            retIdx = double(i - 1) * double(NUM_RETURN_WP) + w;
-            retTarget = returnTargets(retIdx, :);
-            coder.ceval('sendTarget', int32(i), coder.ref(retTarget));
+    % Staggered flyback: same higher-leads-by-one pattern as the flyout, but
+    % over the 3 return waypoints only (the UAVs are already airborne, so there
+    % is no TAKEOFF event). The higher UAV climbs/traverses/descends one event
+    % ahead of the lower, so they never share the same descent over their pads.
+    numReturnEvents = double(NUM_RETURN_WP);   % 3
+    for step = 1:(numReturnEvents + 1)
+        hEvent = step;
+        lEvent = step - 1;
+        hActive = (hEvent >= 1) && (hEvent <= numReturnEvents);
+        lActive = (lEvent >= 1) && (lEvent <= numReturnEvents);
+
+        if hActive
+            hRow = (double(higherRetIdx) - 1) * double(NUM_RETURN_WP) + hEvent;
+            hTarget = returnTargets(hRow, :);
+            coder.ceval('sendTarget', higherRetIdx, coder.ref(hTarget));
+        end
+        if lActive
+            lRow = (double(lowerRetIdx) - 1) * double(NUM_RETURN_WP) + lEvent;
+            lTarget = returnTargets(lRow, :);
+            coder.ceval('sendTarget', lowerRetIdx, coder.ref(lTarget));
+        end
+
+        if hActive
+            coder.ceval('waitForClientMessageType', higherRetIdx, int32(MESSAGE_TYPE.ACK));
+        end
+        if lActive
+            coder.ceval('waitForClientMessageType', lowerRetIdx, int32(MESSAGE_TYPE.ACK));
+        end
+
+        if hActive
+            coder.ceval('waitForClientMessageType', higherRetIdx, int32(MESSAGE_TYPE.READY));
+        end
+        if lActive
+            coder.ceval('waitForClientMessageType', lowerRetIdx, int32(MESSAGE_TYPE.READY));
         end
-        coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.ACK));
-        coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.READY));
     end
 else
     disp('Altitude-staggered return (simulation): UAVs commanded back to takeoff positions.');

aerpaw/impl/controller_impl.cpp

@@ -512,6 +512,7 @@ static const char* messageTypeName(uint8_t msgType) {
         case 6: return "GUIDANCE_TOGGLE";
         case 7: return "REQUEST_POSITION";
         case 8: return "POSITION";
+        case 9: return "TAKEOFF";
         default: return "UNKNOWN";
     }
 }
@@ -614,6 +615,33 @@ int waitForAllMessageType(int numClients, int expectedType) {
     return 1;
 }
 
+// Wait for a specific message type from a SINGLE client (blocking).
+// Used by the staggered flyout/flyback, where the controller waits on one UAV
+// while the other is still flying its own (offset) event. Skips past any
+// unexpected byte and returns 0 on disconnect/error, 1 on success.
+int waitForClientMessageType(int clientId, int expectedType) {
+    if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0;
+    int sock = clientSockets[clientId - 1];
+    uint8_t expected = (uint8_t)expectedType;
+
+    while (true) {
+        uint8_t msg;
+        int len = recv(sock, &msg, 1, MSG_WAITALL);
+        if (len <= 0) {
+            std::cerr << logPrefix() << "waitForClientMessageType: client " << clientId
+                      << " disconnected while waiting for " << messageTypeName(expected) << "\n";
+            return 0;
+        }
+        if (msg == expected) {
+            std::cout << logPrefix() << "Received " << messageTypeName(expected)
+                      << " from client " << clientId << "\n";
+            return 1;
+        }
+        std::cerr << logPrefix() << "Unexpected " << messageTypeName(msg) << " from client "
+                  << clientId << " (expected " << messageTypeName(expected) << ")\n";
+    }
+}
+
 // Broadcast GUIDANCE_TOGGLE to all clients
 void sendGuidanceToggle(int numClients) {
     for (int i = 1; i <= numClients; i++) {

aerpaw/impl/controller_impl.h

@@ -61,6 +61,9 @@ int loadObstacles(const char* filename,
 int sendMessageType(int clientId, int msgType);
 int sendTarget(int clientId, const double* coords);
 int waitForAllMessageType(int numClients, int expectedType);
+// Block until a SINGLE client sends expectedType (used by the staggered
+// flyout/flyback to wait on one UAV while the other is mid-flight).
+int waitForClientMessageType(int clientId, int expectedType);
 
 // Guidance loop operations
 void setGuidanceStep(int step, int totalSteps);  // call at the top of each guidance iteration