diff --git a/aerpaw/client/uav_runner.py b/aerpaw/client/uav_runner.py
index d24a57d..dd4e1e5 100644
--- a/aerpaw/client/uav_runner.py
+++ b/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)
- sock.connect((self.server_ip, self.server_port))
- sock.settimeout(None)
+
+ # Retry connection up to 10 times (~30 seconds total)
+ sock = 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
- data = sock.recv(1024).decode().strip()
- if data == "FINISHED":
- print("[UAV] Received FINISHED - mission complete")
- else:
- print(f"[UAV] Unexpected: {data}")
+ # Command loop - handle RTL, LAND, FINISHED from controller
+ while True:
+ data = sock.recv(1024).decode().strip()
+ print(f"[UAV] Received: {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
\ No newline at end of file
+ print("[UAV] Socket closed")
\ No newline at end of file
diff --git a/aerpaw/controller.coderprj b/aerpaw/controller.coderprj
index 1d09349..a9f8900 100644
--- a/aerpaw/controller.coderprj
+++ b/aerpaw/controller.coderprj
@@ -43,6 +43,11 @@
+
+ int32
+
+
+
@@ -140,7 +145,7 @@
true
- 2026-01-29T19:44:44
+ 2026-01-30T17:33:44
diff --git a/aerpaw/controller.m b/aerpaw/controller.m
index 3b39ca7..eea06da 100644
--- a/aerpaw/controller.m
+++ b/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
\ No newline at end of file
diff --git a/aerpaw/impl/controller_impl.cpp b/aerpaw/impl/controller_impl.cpp
index c686efa..414f5bb 100644
--- a/aerpaw/impl/controller_impl.cpp
+++ b/aerpaw/impl/controller_impl.cpp
@@ -1,9 +1,12 @@
#include "controller_impl.h"
#include
#include
+#include
#include
#include
+#include
#include
+#include
#include
#include
@@ -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 accumulated(numClients);
+ std::vector 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::max(), '\n');
+}
diff --git a/aerpaw/impl/controller_impl.h b/aerpaw/impl/controller_impl.h
index 4781321..ab9aefd 100644
--- a/aerpaw/impl/controller_impl.h
+++ b/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