added RTL and LAND capabilities

aerpaw/client/uav_runner.py

@@ -12,9 +12,16 @@ Protocol:
     Server sends: TARGET:x,y,z (ENU coordinates in meters)
     Client sends: ACK:TARGET
     Client (after moving): READY
+    Server sends: RTL
+    Client sends: ACK:RTL
+    Client (after returning home): RTL_COMPLETE
+    Server sends: LAND
+    Client sends: ACK:LAND
+    Client (after landing): LAND_COMPLETE
     Server sends: FINISHED
 """
 from pathlib import Path
+import asyncio
 import socket
 
 from aerpawlib.runner import BasicRunner, entrypoint
@@ -89,10 +96,27 @@ class UAVRunner(BasicRunner):
     async def run_mission(self, drone: Drone):
         """Main mission entry point."""
         print(f"[UAV] Connecting to controller at {self.server_ip}:{self.server_port}")
-        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+
-        sock.settimeout(30)
+        # Retry connection up to 10 times (~30 seconds total)
-        sock.connect((self.server_ip, self.server_port))
+        sock = None
-        sock.settimeout(None)
+        for attempt in range(10):
+            try:
+                sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+                sock.settimeout(5)
+                sock.connect((self.server_ip, self.server_port))
+                sock.settimeout(None)
+                print(f"[UAV] Connected to controller")
+                break
+            except (ConnectionRefusedError, socket.timeout, OSError) as e:
+                if sock:
+                    sock.close()
+                print(f"[UAV] Connection attempt {attempt + 1}/10 failed: {e}")
+                if attempt < 9:
+                    await asyncio.sleep(3)
+
+        if sock is None or sock.fileno() == -1:
+            print("[UAV] Failed to connect to controller after 10 attempts")
+            return
 
         try:
             # Takeoff to above AERPAW minimum altitude
@@ -124,23 +148,51 @@ class UAVRunner(BasicRunner):
 
             # Signal ready
             sock.sendall(b"READY")
-            print("[UAV] Sent READY")
+            print("[UAV] Sent READY, waiting for commands...")
 
-            # Wait for FINISHED
+            # Command loop - handle RTL, LAND, FINISHED from controller
-            data = sock.recv(1024).decode().strip()
+            while True:
-            if data == "FINISHED":
+                data = sock.recv(1024).decode().strip()
-                print("[UAV] Received FINISHED - mission complete")
+                print(f"[UAV] Received: {data}")
-            else:
+
-                print(f"[UAV] Unexpected: {data}")
+                if data == "RTL":
+                    sock.sendall(b"ACK:RTL")
+                    print("[UAV] Returning to home...")
+                    # Navigate to home lat/lon but stay at current altitude
+                    # (AERPAW blocks goto_coordinates below 20m, but land() mode is allowed)
+                    home = drone.home_coords
+                    current_alt = drone.position.alt
+                    safe_alt = 25  # Set to 25m alt for RTL motion
+                    rtl_target = Coordinate(home.lat, home.lon, safe_alt)
+                    print(f"[UAV] RTL to {home.lat:.6f}, {home.lon:.6f} at {safe_alt:.1f}m")
+                    await drone.goto_coordinates(rtl_target)
+                    print("[UAV] Arrived at home position")
+                    sock.sendall(b"RTL_COMPLETE")
+                    print("[UAV] Sent RTL_COMPLETE")
+
+                elif data == "LAND":
+                    sock.sendall(b"ACK:LAND")
+                    print("[UAV] Landing...")
+                    await drone.land()
+                    print("[UAV] Landed and disarmed")
+                    sock.sendall(b"LAND_COMPLETE")
+                    print("[UAV] Sent LAND_COMPLETE")
+
+                elif data == "FINISHED":
+                    print("[UAV] Received FINISHED - mission complete")
+                    break
+
+                else:
+                    print(f"[UAV] Unknown command: {data}")
 
         except ValueError as e:
             error_msg = f"ERROR:{str(e)}"
-            sock.sendall(error_msg.encode())
+            try:
+                sock.sendall(error_msg.encode())
+            except Exception:
+                pass
             print(f"[UAV] Error: {e}")
-            raise
 
         finally:
             sock.close()
             print("[UAV] Socket closed")
-
-        # aerpawlib handles landing automatically

aerpaw/controller.coderprj

@@ -43,6 +43,11 @@
                   <Size type="coderapp.internal.codertype.Dimension"/>
                   <Size type="coderapp.internal.codertype.Dimension"/>
                 </Types>
+                <Types id="9" type="coderapp.internal.codertype.PrimitiveType">
+                  <ClassName>int32</ClassName>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                  <Size type="coderapp.internal.codertype.Dimension"/>
+                </Types>
               </Types>
             </coderapp.internal.interface.project.Interface>
           </MF0>
@@ -140,7 +145,7 @@
                 </Artifacts>
                 <BuildFolder type="coderapp.internal.util.mfz.FileSpec"/>
                 <Success>true</Success>
-                <Timestamp>2026-01-29T19:44:44</Timestamp>
+                <Timestamp>2026-01-30T17:33:44</Timestamp>
               </MainBuildResult>
             </coderapp.internal.mlc.mfz.MatlabCoderProjectState>
           </MF0>

aerpaw/controller.m

@@ -14,7 +14,7 @@ targets = zeros(MAX_CLIENTS, 3);
 % Load targets from file
 if coder.target('MATLAB')
     disp('Loading targets from file (simulation)...');
-    targetsLoaded = readmatrix('config/targets.txt');
+    targetsLoaded = readmatrix('aerpaw/config/targets.txt');
     numTargets = min(size(targetsLoaded, 1), numClients);
     targets(1:numTargets, :) = targetsLoaded(1:numTargets, :);
     disp(['Loaded ', num2str(numTargets), ' targets']);
@@ -89,10 +89,51 @@ if coder.target('MATLAB')
     disp('All UAVs at target positions.');
 end
 
-% Send COMPLETE to all clients before closing
+% Wait for user input before closing experiment
+if coder.target('MATLAB')
+    input('Press Enter to close experiment (RTL + LAND): ', 's');
+else
+    coder.ceval('waitForUserInput');
+end
+
+% Send RTL command to all clients
 for i = 1:numClients
     if coder.target('MATLAB')
-        disp(['Sending COMPLETE to client ', num2str(i)]);
+        disp(['Sending RTL to client ', num2str(i)]);
+    else
+        coder.ceval('sendRTL', int32(i));
+    end
+end
+
+% Wait for RTL_COMPLETE from all clients (simultaneously using select())
+if coder.target('MATLAB')
+    disp('Waiting for RTL_COMPLETE from all clients...');
+    disp('All UAVs returned to home.');
+else
+    coder.ceval('waitForAllRTLComplete', int32(numClients));
+end
+
+% Send LAND command to all clients
+for i = 1:numClients
+    if coder.target('MATLAB')
+        disp(['Sending LAND to client ', num2str(i)]);
+    else
+        coder.ceval('sendLAND', int32(i));
+    end
+end
+
+% Wait for LAND_COMPLETE from all clients (simultaneously using select())
+if coder.target('MATLAB')
+    disp('Waiting for LAND_COMPLETE from all clients...');
+    disp('All UAVs landed and disarmed.');
+else
+    coder.ceval('waitForAllLANDComplete', int32(numClients));
+end
+
+% Send FINISHED to all clients before closing
+for i = 1:numClients
+    if coder.target('MATLAB')
+        disp(['Sending FINISHED to client ', num2str(i)]);
     else
         coder.ceval('sendFinished', int32(i));
     end
@@ -103,4 +144,6 @@ if ~coder.target('MATLAB')
     coder.ceval('closeServer');
 end
 
+disp('Experiment complete.');
+
 end

aerpaw/impl/controller_impl.cpp

@@ -1,9 +1,12 @@
 #include "controller_impl.h"
 #include <iostream>
 #include <vector>
+#include <string>
 #include <cstring>
 #include <cstdio>
+#include <limits>
 #include <sys/socket.h>
+#include <sys/select.h>
 #include <arpa/inet.h>
 #include <unistd.h>
 
@@ -158,3 +161,88 @@ void sendFinished(int clientId) {
     send(clientSockets[clientId - 1], msg, strlen(msg), 0);
     std::cout << "Sent FINISHED to client " << clientId << "\n";
 }
+
+// Send RTL (Return-To-Launch) command to a client
+void sendRTL(int clientId) {
+    if (clientId <= 0 || clientId > (int)clientSockets.size()) return;
+
+    const char* msg = "RTL";
+    send(clientSockets[clientId - 1], msg, strlen(msg), 0);
+    std::cout << "Sent RTL to client " << clientId << "\n";
+}
+
+// Send LAND command to a client
+void sendLAND(int clientId) {
+    if (clientId <= 0 || clientId > (int)clientSockets.size()) return;
+
+    const char* msg = "LAND";
+    send(clientSockets[clientId - 1], msg, strlen(msg), 0);
+    std::cout << "Sent LAND to client " << clientId << "\n";
+}
+
+// Wait for a specific message from ALL clients simultaneously using select()
+// Returns 1 if all clients sent the expected message, 0 otherwise
+static int waitForAllMessage(int numClients, const char* expectedMessage) {
+    if (numClients <= 0 || numClients > (int)clientSockets.size()) return 0;
+
+    std::vector<std::string> accumulated(numClients);
+    std::vector<bool> completed(numClients, false);
+    int completedCount = 0;
+    char buffer[512];
+
+    while (completedCount < numClients) {
+        // Build fd_set for select()
+        fd_set readfds;
+        FD_ZERO(&readfds);
+        int maxfd = -1;
+
+        for (int i = 0; i < numClients; i++) {
+            if (!completed[i]) {
+                FD_SET(clientSockets[i], &readfds);
+                if (clientSockets[i] > maxfd) maxfd = clientSockets[i];
+            }
+        }
+
+        // Wait for any socket to have data
+        int ready = select(maxfd + 1, &readfds, nullptr, nullptr, nullptr);
+        if (ready <= 0) return 0;
+
+        // Check each socket
+        for (int i = 0; i < numClients; i++) {
+            if (!completed[i] && FD_ISSET(clientSockets[i], &readfds)) {
+                int len = recv(clientSockets[i], buffer, sizeof(buffer) - 1, 0);
+                if (len <= 0) return 0;
+                buffer[len] = '\0';
+                std::cout << "Received from client " << (i + 1) << ": " << buffer << "\n";
+                accumulated[i] += buffer;
+
+                // Check if expected message received
+                if (accumulated[i].find(expectedMessage) != std::string::npos) {
+                    completed[i] = true;
+                    completedCount++;
+                    std::cout << "Client " << (i + 1) << " completed " << expectedMessage << "\n";
+                }
+            }
+        }
+    }
+
+    return 1;
+}
+
+// Wait for RTL_COMPLETE from ALL clients simultaneously
+// Returns 1 if all clients completed RTL, 0 otherwise
+int waitForAllRTLComplete(int numClients) {
+    return waitForAllMessage(numClients, "RTL_COMPLETE");
+}
+
+// Wait for LAND_COMPLETE from ALL clients simultaneously
+// Returns 1 if all clients completed LAND, 0 otherwise
+int waitForAllLANDComplete(int numClients) {
+    return waitForAllMessage(numClients, "LAND_COMPLETE");
+}
+
+// Wait for user to press Enter
+void waitForUserInput() {
+    std::cout << "Press Enter to close experiment (RTL + LAND)...\n";
+    std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
+}

aerpaw/impl/controller_impl.h

@@ -18,6 +18,13 @@ int receiveTargetAck(int clientId);
 int waitForReady(int clientId);
 void sendFinished(int clientId);
 
+// RTL and LAND protocol functions
+void sendRTL(int clientId);
+void sendLAND(int clientId);
+int waitForAllRTLComplete(int numClients);
+int waitForAllLANDComplete(int numClients);
+void waitForUserInput();
+
 #ifdef __cplusplus
 }
 #endif