kdee/miSim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added RTL and LAND capabilities

Browse Source
This commit is contained in:
Kevin Dee
2026-01-30 18:16:33 -08:00
parent dd82cb3956
commit 448db1e0e3
5 changed files with 215 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
aerpaw/client/uav_runner.py
View File

@@ -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

7
aerpaw/controller.coderprj
View File

@@ -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>

49
aerpaw/controller.m
View File

@@ -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

88
aerpaw/impl/controller_impl.cpp
View File

@@ -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');
}

7
aerpaw/impl/controller_impl.h
View File

@@ -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