From 4cdcb16ee353ffedb2c6b973c63c4ecd7d7b1cd3 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Fri, 30 Jan 2026 18:16:33 -0800 Subject: [PATCH] added RTL and LAND capabilities --- aerpaw/client/uav_runner.py | 84 +++++++++++++++++++++++++------ aerpaw/controller.coderprj | 7 ++- aerpaw/controller.m | 49 ++++++++++++++++-- aerpaw/impl/controller_impl.cpp | 88 +++++++++++++++++++++++++++++++++ aerpaw/impl/controller_impl.h | 7 +++ 5 files changed, 215 insertions(+), 20 deletions(-) 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