From dbb4ba178ac4b2cc1894d3b5a0a06be36f5ea163 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Fri, 13 Feb 2026 18:06:06 -0800 Subject: [PATCH] config cleanup --- aerpaw/client/uav_runner.py | 108 +++++++++------------ aerpaw/config/{config.yaml => client.yaml} | 12 +-- aerpaw/config/server.yaml | 24 +++++ aerpaw/controller.coderprj | 7 +- aerpaw/controller.m | 93 ++++++++++-------- aerpaw/run_uav.sh | 2 +- 6 files changed, 131 insertions(+), 115 deletions(-) rename aerpaw/config/{config.yaml => client.yaml} (64%) create mode 100644 aerpaw/config/server.yaml diff --git a/aerpaw/client/uav_runner.py b/aerpaw/client/uav_runner.py index 51b4408..553ad1d 100644 --- a/aerpaw/client/uav_runner.py +++ b/aerpaw/client/uav_runner.py @@ -9,16 +9,14 @@ Or use the auto-detecting wrapper: ./run_uav.sh Binary Protocol: - Server sends: TARGET (1 byte) + x,y,z (24 bytes as 3 doubles) - Client sends: ACK (1 byte) - Client (after moving): READY (1 byte) - Server sends: RTL (1 byte) - Client sends: ACK (1 byte) - Client (after returning home): READY (1 byte) - Server sends: LAND (1 byte) - Client sends: ACK (1 byte) - Client (after landing): READY (1 byte) - Server sends: READY (1 byte) - mission complete, disconnect + For each waypoint: + Server sends: TARGET (1 byte) + x,y,z (24 bytes as 3 doubles) + Client sends: ACK (1 byte) + Client (after moving): READY (1 byte) + After all waypoints: + Server sends: RTL (1 byte) → Client: ACK, return home, READY + Server sends: LAND (1 byte) → Client: ACK, land, READY + Server sends: READY (1 byte) - mission complete, disconnect """ from enum import IntEnum from pathlib import Path @@ -43,7 +41,7 @@ class MessageType(IntEnum): AERPAW_DIR = Path(__file__).parent.parent -CONFIG_FILE = AERPAW_DIR / "config" / "config.yaml" +CONFIG_FILE = AERPAW_DIR / "config" / "client.yaml" def load_config(): @@ -89,22 +87,6 @@ def send_message_type(sock: socket.socket, msg_type: MessageType): sock.sendall(bytes([msg_type])) -def recv_target(sock: socket.socket) -> tuple[float, float, float]: - """Receive TARGET message (1 byte type + 3 doubles). - - Returns (x, y, z) ENU coordinates. - """ - # First byte is message type (already consumed or check it) - msg_type = recv_message_type(sock) - if msg_type != MessageType.TARGET: - raise ValueError(f"Expected TARGET, got {msg_type.name}") - - # Next 24 bytes are 3 doubles (little-endian) - data = recv_exactly(sock, 24) - x, y, z = struct.unpack(' + + int32 + + + @@ -155,7 +160,7 @@ true - 2026-02-01T15:39:22 + 2026-02-13T18:04:24 diff --git a/aerpaw/controller.m b/aerpaw/controller.m index c839ca7..922937f 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -5,26 +5,32 @@ end coder.extrinsic('disp', 'loadTargetsFromYaml'); -% Maximum clients supported +% Maximum clients and waypoints supported MAX_CLIENTS = 4; +MAX_WAYPOINTS = 10; +MAX_TARGETS = MAX_CLIENTS * MAX_WAYPOINTS; -% Allocate targets array (MAX_CLIENTS x 3) -targets = zeros(MAX_CLIENTS, 3); +% Allocate targets array (MAX_TARGETS x 3) +targets = zeros(MAX_TARGETS, 3); +numWaypoints = int32(0); % Load targets from YAML config file if coder.target('MATLAB') - disp('Loading targets from config.yaml (simulation)...'); - targetsLoaded = loadTargetsFromYaml('aerpaw/config/config.yaml'); - numTargets = min(size(targetsLoaded, 1), numClients); - targets(1:numTargets, :) = targetsLoaded(1:numTargets, :); - disp(['Loaded ', num2str(numTargets), ' targets']); + disp('Loading targets from server.yaml (simulation)...'); + targetsLoaded = loadTargetsFromYaml('aerpaw/config/server.yaml'); + totalLoaded = size(targetsLoaded, 1); + targets(1:totalLoaded, :) = targetsLoaded(1:totalLoaded, :); + numWaypoints = int32(totalLoaded / numClients); + disp(['Loaded ', num2str(numWaypoints), ' waypoints per client']); else coder.cinclude('controller_impl.h'); % Define filename as null-terminated character array for C compatibility - filename = ['config/config.yaml', char(0)]; + filename = ['config/server.yaml', char(0)]; % loadTargets fills targets array (column-major for MATLAB compatibility) - coder.ceval('loadTargets', coder.ref(filename), ... - coder.ref(targets), int32(MAX_CLIENTS)); + totalLoaded = int32(0); + totalLoaded = coder.ceval('loadTargets', coder.ref(filename), ... + coder.ref(targets), int32(MAX_TARGETS)); + numWaypoints = totalLoaded / int32(numClients); end % Initialize server @@ -43,35 +49,38 @@ for i = 1:numClients end end -% Send target coordinates to each client -for i = 1:numClients - % Get target for this client (1x3 array) - target = targets(i, :); +% Waypoint loop: send each waypoint to all clients, wait for all to arrive +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 ', char(MESSAGE_TYPE.TARGET), ' to client ', num2str(i), ': ', ... - num2str(target(1)), ',', num2str(target(2)), ',', num2str(target(3))]); - else - coder.ceval('sendTarget', int32(i), coder.ref(target)); + 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 -end -% Wait for ACK from all clients -if coder.target('MATLAB') - disp(['Waiting for ', char(MESSAGE_TYPE.ACK), ' from all clients...']); - disp('All acknowledgments received.'); -else - coder.ceval('waitForAllMessageType', int32(numClients), ... - int32(MESSAGE_TYPE.ACK)); -end + % Wait for ACK from all clients + if coder.target('MATLAB') + disp('Waiting for ACK from all clients...'); + else + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.ACK)); + end -% Wait for READY signals from all clients -if coder.target('MATLAB') - disp(['Waiting for ', char(MESSAGE_TYPE.READY), ' from all clients...']); - disp('All UAVs at target positions.'); -else - coder.ceval('waitForAllMessageType', int32(numClients), ... - int32(MESSAGE_TYPE.READY)); + % 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)); + end end % Wait for user input before closing experiment @@ -84,7 +93,7 @@ end % Send RTL command to all clients for i = 1:numClients if coder.target('MATLAB') - disp(['Sending ', char(MESSAGE_TYPE.RTL), ' to client ', num2str(i)]); + disp(['Sending RTL to client ', num2str(i)]); else coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.RTL)); end @@ -92,7 +101,7 @@ end % Wait for ACK from all clients if coder.target('MATLAB') - disp(['Waiting for ', char(MESSAGE_TYPE.ACK), ' from all clients...']); + disp('Waiting for ACK from all clients...'); else coder.ceval('waitForAllMessageType', int32(numClients), ... int32(MESSAGE_TYPE.ACK)); @@ -100,7 +109,6 @@ end % Wait for READY from all clients (returned to home) if coder.target('MATLAB') - disp(['Waiting for ', char(MESSAGE_TYPE.READY), ' from all clients...']); disp('All UAVs returned to home.'); else coder.ceval('waitForAllMessageType', int32(numClients), ... @@ -110,7 +118,7 @@ end % Send LAND command to all clients for i = 1:numClients if coder.target('MATLAB') - disp(['Sending ', char(MESSAGE_TYPE.LAND), ' to client ', num2str(i)]); + disp(['Sending LAND to client ', num2str(i)]); else coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.LAND)); end @@ -118,7 +126,7 @@ end % Wait for ACK from all clients if coder.target('MATLAB') - disp(['Waiting for ', char(MESSAGE_TYPE.ACK), ' from all clients...']); + disp('Waiting for ACK from all clients...'); else coder.ceval('waitForAllMessageType', int32(numClients), ... int32(MESSAGE_TYPE.ACK)); @@ -126,7 +134,6 @@ end % Wait for READY from all clients (landed and disarmed) if coder.target('MATLAB') - disp(['Waiting for ', char(MESSAGE_TYPE.READY), ' from all clients...']); disp('All UAVs landed and disarmed.'); else coder.ceval('waitForAllMessageType', int32(numClients), ... @@ -136,7 +143,7 @@ end % Send READY to all clients to signal mission complete for i = 1:numClients if coder.target('MATLAB') - disp(['Sending ', char(MESSAGE_TYPE.READY), ' to client ', num2str(i)]); + disp(['Sending READY to client ', num2str(i)]); else coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.READY)); end diff --git a/aerpaw/run_uav.sh b/aerpaw/run_uav.sh index c501282..62d2275 100755 --- a/aerpaw/run_uav.sh +++ b/aerpaw/run_uav.sh @@ -38,7 +38,7 @@ export AERPAW_ENV="$ENV" # Read MAVLink connection from config.yaml using Python CONN=$(python3 -c " import yaml -with open('config/config.yaml') as f: +with open('config/client.yaml') as f: cfg = yaml.safe_load(f) env = cfg['environments']['$ENV']['mavlink'] print(f\"udp:{env['ip']}:{env['port']}\")