diff --git a/aerpaw/MESSAGE_TYPE.m b/aerpaw/MESSAGE_TYPE.m new file mode 100644 index 0000000..338c5ee --- /dev/null +++ b/aerpaw/MESSAGE_TYPE.m @@ -0,0 +1,9 @@ +classdef MESSAGE_TYPE < uint8 + enumeration + TARGET (1) % Server->Client: target coordinates follow (3 doubles) + ACK (2) % Client->Server: command received + READY (3) % Both: ready for next command / mission complete + RTL (4) % Server->Client: return to launch + LAND (5) % Server->Client: land now + end +end diff --git a/aerpaw/build/controller_app b/aerpaw/build/controller_app index 56ed54e..48b5e4e 100755 Binary files a/aerpaw/build/controller_app and b/aerpaw/build/controller_app differ diff --git a/aerpaw/client/uav.py b/aerpaw/client/uav.py deleted file mode 100755 index 61f07da..0000000 --- a/aerpaw/client/uav.py +++ /dev/null @@ -1,282 +0,0 @@ -#!/usr/bin/env python3 -""" -UAV Client for AERPAW Target Location Protocol - -Protocol: - Server sends: TARGET:x,y,z (ENU coordinates in meters) - Client sends: ACK:TARGET - Client (after moving): READY - Server sends: FINISHED - -Coordinates: - - Server sends local ENU (East, North, Up) coordinates in meters - - Client converts to lat/lon using shared origin from config/origin.txt - - Origin must match between controller and UAV - -Auto-detection: - - If flight controller is available, uses aerpawlib to move drone - - If no flight controller, runs in test mode with placeholder movement -""" - -import asyncio -import socket -import sys -import time -from pathlib import Path -from typing import Optional, Tuple - -# Get the aerpaw directory (parent of client/) -AERPAW_DIR = Path(__file__).parent.parent -CONFIG_DIR = AERPAW_DIR / "config" - - -def load_origin(path: Path) -> Tuple[float, float, float]: - """ - Load ENU origin from config file. - Returns (lat, lon, alt) tuple. - """ - with open(path, 'r') as f: - for line in f: - line = line.strip() - if not line or line.startswith('#'): - continue - parts = line.split(',') - if len(parts) == 3: - return float(parts[0]), float(parts[1]), float(parts[2]) - raise ValueError(f"No valid origin found in {path}") - - -def load_connection(path: Path) -> str: - """ - Load drone connection string from config file. - Returns connection string (e.g., 'udp:127.0.0.1:14550'). - """ - with open(path, 'r') as f: - for line in f: - line = line.strip() - if not line or line.startswith('#'): - continue - return line - raise ValueError(f"No valid connection string found in {path}") - - -def load_server(path: Path) -> Tuple[str, int]: - """ - Load controller server address from config file. - Returns (ip, port) tuple. - """ - with open(path, 'r') as f: - for line in f: - line = line.strip() - if not line or line.startswith('#'): - continue - parts = line.split(',') - if len(parts) == 2: - return parts[0].strip(), int(parts[1].strip()) - raise ValueError(f"No valid server address found in {path}") - - -def parse_target(message: str) -> Tuple[float, float, float]: - """Parse TARGET:x,y,z message and return (x, y, z) ENU coordinates.""" - if not message.startswith("TARGET:"): - raise ValueError(f"Invalid message format: {message}") - - coords_str = message[7:] # Remove "TARGET:" prefix - parts = coords_str.split(",") - if len(parts) != 3: - raise ValueError(f"Expected 3 coordinates, got {len(parts)}") - - x, y, z = float(parts[0]), float(parts[1]), float(parts[2]) - return (x, y, z) - - -def enu_to_coordinate(origin_lat: float, origin_lon: float, origin_alt: float, - enu_x: float, enu_y: float, enu_z: float): - """ - Convert ENU (East, North, Up) coordinates to lat/lon/alt. - - Args: - origin_lat, origin_lon, origin_alt: Origin in degrees/meters - enu_x: East offset in meters - enu_y: North offset in meters - enu_z: Up offset in meters (altitude above origin) - - Returns: - aerpawlib Coordinate object - """ - from aerpawlib.util import Coordinate, VectorNED - - origin = Coordinate(origin_lat, origin_lon, origin_alt) - # ENU to NED: north=enu_y, east=enu_x, down=-enu_z - offset = VectorNED(north=enu_y, east=enu_x, down=-enu_z) - return origin + offset - - -def try_connect_drone(conn_str: str, timeout: float = 10.0): - """ - Try to connect to drone flight controller. - Returns Drone object if successful, None if not available. - """ - try: - from aerpawlib.vehicle import Drone - print(f" Attempting to connect to flight controller: {conn_str}") - drone = Drone(conn_str) - print(f" Connected to flight controller") - return drone - except Exception as e: - print(f" Could not connect to flight controller: {e}") - return None - - -def try_connect_server(ip: str, port: int, timeout: float = 2.0) -> Optional[socket.socket]: - """ - Try to connect to controller server with timeout. - Returns socket if successful, None if connection fails. - """ - try: - s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - s.settimeout(timeout) - s.connect((ip, port)) - s.settimeout(None) # Reset to blocking mode after connect - return s - except (socket.timeout, ConnectionRefusedError, OSError): - return None - - -def move_to_target_test(enu_x: float, enu_y: float, enu_z: float, - lat: float, lon: float, alt: float): - """ - Placeholder movement for test mode. - """ - print(f" [TEST MODE] Target ENU: ({enu_x}, {enu_y}, {enu_z}) meters") - print(f" [TEST MODE] Target lat/lon: ({lat:.6f}, {lon:.6f}, {alt:.1f})") - print(f" [TEST MODE] Simulating movement...") - time.sleep(1.0) - print(f" [TEST MODE] Arrived at target") - - -async def move_to_target_real(drone, target_coord, tolerance: float = 2.0): - """ - Move drone to target using aerpawlib. - """ - print(f" [REAL MODE] Moving to: ({target_coord.lat:.6f}, {target_coord.lon:.6f}, {target_coord.alt:.1f})") - await drone.goto_coordinates(target_coord, tolerance=tolerance) - print(f" [REAL MODE] Arrived at target") - - -async def run_uav_client(client_id: int): - """Run a single UAV client.""" - print(f"UAV {client_id}: Starting...") - - # Load configuration - origin_path = CONFIG_DIR / "origin.txt" - connection_testbed_path = CONFIG_DIR / "connection_testbed.txt" - connection_local_path = CONFIG_DIR / "connection.txt" - server_testbed_path = CONFIG_DIR / "server_testbed.txt" - server_local_path = CONFIG_DIR / "server.txt" - - print(f"UAV {client_id}: Loading origin from {origin_path}") - origin_lat, origin_lon, origin_alt = load_origin(origin_path) - print(f"UAV {client_id}: Origin: lat={origin_lat}, lon={origin_lon}, alt={origin_alt}") - - # Load both connection configs - testbed_conn = load_connection(connection_testbed_path) - local_conn = load_connection(connection_local_path) - - # Try testbed flight controller first (AERPAW MAVLink filter) - print(f"UAV {client_id}: Trying testbed flight controller: {testbed_conn}...") - drone = try_connect_drone(testbed_conn) - - if not drone: - print(f"UAV {client_id}: Testbed not available, trying local: {local_conn}...") - drone = try_connect_drone(local_conn) - - real_mode = drone is not None - - if real_mode: - print(f"UAV {client_id}: Running in REAL mode") - else: - print(f"UAV {client_id}: Running in TEST mode (no flight controller)") - - # Try testbed server first, fall back to local - testbed_ip, testbed_port = load_server(server_testbed_path) - local_ip, local_port = load_server(server_local_path) - - print(f"UAV {client_id}: Trying testbed server at {testbed_ip}:{testbed_port}...") - s = try_connect_server(testbed_ip, testbed_port) - - if s: - print(f"UAV {client_id}: Connected to testbed server") - else: - print(f"UAV {client_id}: Testbed server not available, trying local server at {local_ip}:{local_port}...") - s = try_connect_server(local_ip, local_port) - if s: - print(f"UAV {client_id}: Connected to local server") - else: - print(f"UAV {client_id}: ERROR - Could not connect to any server") - if drone: - drone.close() - return - - try: - # Receive TARGET command - data = s.recv(1024).decode().strip() - print(f"UAV {client_id}: Received: {data}") - - # Parse target coordinates (ENU) - enu_x, enu_y, enu_z = parse_target(data) - print(f"UAV {client_id}: Parsed ENU target: x={enu_x}, y={enu_y}, z={enu_z}") - - # Convert ENU to lat/lon - target_coord = enu_to_coordinate(origin_lat, origin_lon, origin_alt, - enu_x, enu_y, enu_z) - print(f"UAV {client_id}: Target coordinate: lat={target_coord.lat:.6f}, lon={target_coord.lon:.6f}, alt={target_coord.alt:.1f}") - - # Send acknowledgment - s.sendall(b"ACK:TARGET") - print(f"UAV {client_id}: Sent ACK:TARGET") - - # Move to target - if real_mode: - await move_to_target_real(drone, target_coord) - else: - move_to_target_test(enu_x, enu_y, enu_z, - target_coord.lat, target_coord.lon, target_coord.alt) - - # Signal ready - s.sendall(b"READY") - print(f"UAV {client_id}: Sent READY") - - # Wait for FINISHED signal from server - data = s.recv(1024).decode().strip() - if data == "FINISHED": - print(f"UAV {client_id}: Received FINISHED - session ended normally") - else: - print(f"UAV {client_id}: Unexpected message: {data}") - - except ValueError as e: - error_msg = f"ERROR:{str(e)}" - s.sendall(error_msg.encode()) - print(f"UAV {client_id}: Error - {e}") - - finally: - s.close() - if drone: - drone.close() - print(f"UAV {client_id}: Connection closed") - - -def main(): - if len(sys.argv) != 2: - print(f"Usage: {sys.argv[0]} ") - print(" Run one instance per UAV, e.g.:") - print(" Terminal 1: ./uav.py 1") - print(" Terminal 2: ./uav.py 2") - sys.exit(1) - - client_id = int(sys.argv[1]) - asyncio.run(run_uav_client(client_id)) - - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/aerpaw/client/uav_runner.py b/aerpaw/client/uav_runner.py index d29ea05..10ed12b 100644 --- a/aerpaw/client/uav_runner.py +++ b/aerpaw/client/uav_runner.py @@ -8,29 +8,40 @@ Run via: Or use the auto-detecting wrapper: ./run_uav.sh -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 +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 """ +from enum import IntEnum from pathlib import Path import asyncio import os import socket - +import struct import yaml from aerpawlib.runner import BasicRunner, entrypoint from aerpawlib.util import Coordinate, VectorNED from aerpawlib.vehicle import Drone + +# Message types - must match MESSAGE_TYPE.m enum +class MessageType(IntEnum): + TARGET = 1 + ACK = 2 + READY = 3 + RTL = 4 + LAND = 5 + + AERPAW_DIR = Path(__file__).parent.parent CONFIG_FILE = AERPAW_DIR / "config" / "config.yaml" @@ -56,15 +67,42 @@ def get_environment(): return env -def parse_target(message: str): - """Parse TARGET:x,y,z message. Returns (x, y, z) ENU coordinates.""" - if not message.startswith("TARGET:"): - raise ValueError(f"Invalid message format: {message}") - coords_str = message[7:] - parts = coords_str.split(",") - if len(parts) != 3: - raise ValueError(f"Expected 3 coordinates, got {len(parts)}") - return float(parts[0]), float(parts[1]), float(parts[2]) +def recv_exactly(sock: socket.socket, n: int) -> bytes: + """Receive exactly n bytes from socket.""" + data = b'' + while len(data) < n: + chunk = sock.recv(n - len(data)) + if not chunk: + raise ConnectionError("Connection closed while receiving data") + data += chunk + return data + + +def recv_message_type(sock: socket.socket) -> MessageType: + """Receive a single-byte message type.""" + data = recv_exactly(sock, 1) + return MessageType(data[0]) + + +def send_message_type(sock: socket.socket, msg_type: MessageType): + """Send a single-byte message type.""" + 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 + + + @@ -145,7 +150,7 @@ true - 2026-01-30T18:21:40 + 2026-02-01T15:39:22 diff --git a/aerpaw/controller.m b/aerpaw/controller.m index e7f51c9..c839ca7 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -49,27 +49,29 @@ for i = 1:numClients target = targets(i, :); if coder.target('MATLAB') - disp(['Sending TARGET to client ', num2str(i), ': ', ... + 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)); end end -% Wait for TARGET acknowledgments from all clients (simultaneously using select()) +% Wait for ACK from all clients if coder.target('MATLAB') - disp('Waiting for ACK:TARGET from all clients...'); - disp('All TARGET acknowledgments received.'); + disp(['Waiting for ', char(MESSAGE_TYPE.ACK), ' from all clients...']); + disp('All acknowledgments received.'); else - coder.ceval('waitForAllTargetAck', int32(numClients)); + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.ACK)); end -% Wait for READY signals from all clients (simultaneously using select()) +% Wait for READY signals from all clients if coder.target('MATLAB') - disp('Waiting for READY from all clients...'); + disp(['Waiting for ', char(MESSAGE_TYPE.READY), ' from all clients...']); disp('All UAVs at target positions.'); else - coder.ceval('waitForAllReady', int32(numClients)); + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.READY)); end % Wait for user input before closing experiment @@ -82,43 +84,61 @@ end % Send RTL command to all clients for i = 1:numClients if coder.target('MATLAB') - disp(['Sending RTL to client ', num2str(i)]); + disp(['Sending ', char(MESSAGE_TYPE.RTL), ' to client ', num2str(i)]); else - coder.ceval('sendRTL', int32(i)); + coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.RTL)); end end -% Wait for RTL_COMPLETE from all clients (simultaneously using select()) +% Wait for ACK from all clients if coder.target('MATLAB') - disp('Waiting for RTL_COMPLETE from all clients...'); + disp(['Waiting for ', char(MESSAGE_TYPE.ACK), ' from all clients...']); +else + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.ACK)); +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('waitForAllRTLComplete', int32(numClients)); + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.READY)); end % Send LAND command to all clients for i = 1:numClients if coder.target('MATLAB') - disp(['Sending LAND to client ', num2str(i)]); + disp(['Sending ', char(MESSAGE_TYPE.LAND), ' to client ', num2str(i)]); else - coder.ceval('sendLAND', int32(i)); + coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.LAND)); end end -% Wait for LAND_COMPLETE from all clients (simultaneously using select()) +% Wait for ACK from all clients if coder.target('MATLAB') - disp('Waiting for LAND_COMPLETE from all clients...'); - disp('All UAVs landed and disarmed.'); + disp(['Waiting for ', char(MESSAGE_TYPE.ACK), ' from all clients...']); else - coder.ceval('waitForAllLANDComplete', int32(numClients)); + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.ACK)); end -% Send FINISHED to all clients before closing +% 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), ... + int32(MESSAGE_TYPE.READY)); +end + +% Send READY to all clients to signal mission complete for i = 1:numClients if coder.target('MATLAB') - disp(['Sending FINISHED to client ', num2str(i)]); + disp(['Sending ', char(MESSAGE_TYPE.READY), ' to client ', num2str(i)]); else - coder.ceval('sendFinished', int32(i)); + coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.READY)); end end @@ -129,4 +149,4 @@ end disp('Experiment complete.'); -end \ No newline at end of file +end diff --git a/aerpaw/impl/controller_impl.cpp b/aerpaw/impl/controller_impl.cpp index aba545c..1d90798 100644 --- a/aerpaw/impl/controller_impl.cpp +++ b/aerpaw/impl/controller_impl.cpp @@ -51,23 +51,6 @@ void acceptClient(int clientId) { std::cout << "Client " << clientId << " connected\n"; } -void sendMessage(int clientId) { - if(clientId <= 0 || clientId > clientSockets.size()) return; - const char* msg = "Hello from server"; - send(clientSockets[clientId-1], msg, strlen(msg), 0); - std::cout << "Sent message to client " << clientId << "\n"; -} - -int receiveAck(int clientId) { - if(clientId <= 0 || clientId > clientSockets.size()) return 0; - char buffer[1024]; - int len = recv(clientSockets[clientId-1], buffer, sizeof(buffer)-1, 0); - if(len <= 0) return 0; - buffer[len] = '\0'; - std::cout << "Received ACK from client " << clientId << ": " << buffer << "\n"; - return 1; -} - void closeServer() { for(auto sock : clientSockets) { close(sock); @@ -129,91 +112,61 @@ int loadTargets(const char* filename, double* targets, int maxClients) { return count; } -// Send target coordinates to a client -// target points to 3 doubles: [x, y, z] -void sendTarget(int clientId, const double* target) { - if (clientId <= 0 || clientId > (int)clientSockets.size()) return; - - char buffer[256]; - snprintf(buffer, sizeof(buffer), "TARGET:%.6f,%.6f,%.6f", - target[0], target[1], target[2]); - - send(clientSockets[clientId - 1], buffer, strlen(buffer), 0); - std::cout << "Sent target to client " << clientId << ": " << buffer << "\n"; +// Message type names for logging +static const char* messageTypeName(uint8_t msgType) { + switch (msgType) { + case 1: return "TARGET"; + case 2: return "ACK"; + case 3: return "READY"; + case 4: return "RTL"; + case 5: return "LAND"; + default: return "UNKNOWN"; + } } -// Receive and validate ACK:TARGET response -// Returns 1 if ACK:TARGET received, 0 otherwise -int receiveTargetAck(int clientId) { +// Send a single-byte message type to a client +int sendMessageType(int clientId, int msgType) { if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0; - char buffer[256]; - int len = recv(clientSockets[clientId - 1], buffer, sizeof(buffer) - 1, 0); - if (len <= 0) return 0; - buffer[len] = '\0'; - - std::cout << "Received from client " << clientId << ": " << buffer << "\n"; - - if (strncmp(buffer, "ACK:TARGET", 10) == 0) { - return 1; + uint8_t msg = (uint8_t)msgType; + ssize_t sent = send(clientSockets[clientId - 1], &msg, 1, 0); + if (sent != 1) { + std::cerr << "Send failed for client " << clientId << "\n"; + return 0; } - return 0; + + std::cout << "Sent to client " << clientId << ": " << messageTypeName(msg) << " (" << (int)msg << ")\n"; + return 1; } -// Wait for READY signal from client -// Returns 1 if READY received, 0 otherwise -int waitForReady(int clientId) { +// Send TARGET message with coordinates (1 byte type + 24 bytes coords) +int sendTarget(int clientId, const double* coords) { if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0; - char buffer[256]; - int len = recv(clientSockets[clientId - 1], buffer, sizeof(buffer) - 1, 0); - if (len <= 0) return 0; - buffer[len] = '\0'; + // Build message: 1 byte type + 3 doubles (little-endian) + uint8_t buffer[1 + 3 * sizeof(double)]; + buffer[0] = 1; // TARGET = 1 + memcpy(buffer + 1, coords, 3 * sizeof(double)); - std::cout << "Received from client " << clientId << ": " << buffer << "\n"; - - if (strncmp(buffer, "READY", 5) == 0) { - return 1; + ssize_t sent = send(clientSockets[clientId - 1], buffer, sizeof(buffer), 0); + if (sent != sizeof(buffer)) { + std::cerr << "Send target failed for client " << clientId << "\n"; + return 0; } - return 0; + + std::cout << "Sent TARGET to client " << clientId << ": " + << coords[0] << "," << coords[1] << "," << coords[2] << "\n"; + return 1; } -// Send COMPLETE message to signal graceful shutdown -void sendFinished(int clientId) { - if (clientId <= 0 || clientId > (int)clientSockets.size()) return; - - const char* msg = "FINISHED"; - 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) { +// Wait for a specific message type from ALL clients simultaneously using select() +// Returns 1 if all clients responded with expected message type, 0 on failure +int waitForAllMessageType(int numClients, int expectedType) { if (numClients <= 0 || numClients > (int)clientSockets.size()) return 0; - std::vector accumulated(numClients); + uint8_t expected = (uint8_t)expectedType; std::vector completed(numClients, false); int completedCount = 0; - char buffer[512]; while (completedCount < numClients) { // Build fd_set for select() @@ -235,17 +188,17 @@ static int waitForAllMessage(int numClients, const char* expectedMessage) { // 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; + uint8_t msgType; + int len = recv(clientSockets[i], &msgType, 1, 0); + if (len != 1) return 0; - // Check if expected message received - if (accumulated[i].find(expectedMessage) != std::string::npos) { + std::cout << "Received from client " << (i + 1) << ": " + << messageTypeName(msgType) << " (" << (int)msgType << ")\n"; + + if (msgType == expected) { completed[i] = true; completedCount++; - std::cout << "Client " << (i + 1) << " completed " << expectedMessage << "\n"; + std::cout << "Client " << (i + 1) << " completed: " << messageTypeName(expected) << "\n"; } } } @@ -254,30 +207,6 @@ static int waitForAllMessage(int numClients, const char* expectedMessage) { return 1; } -// Wait for ACK:TARGET from ALL clients simultaneously -// Returns 1 if all clients acknowledged, 0 otherwise -int waitForAllTargetAck(int numClients) { - return waitForAllMessage(numClients, "ACK:TARGET"); -} - -// Wait for READY from ALL clients simultaneously -// Returns 1 if all clients are ready, 0 otherwise -int waitForAllReady(int numClients) { - return waitForAllMessage(numClients, "READY"); -} - -// 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"; diff --git a/aerpaw/impl/controller_impl.h b/aerpaw/impl/controller_impl.h index c0939ca..5f6ac05 100644 --- a/aerpaw/impl/controller_impl.h +++ b/aerpaw/impl/controller_impl.h @@ -1,34 +1,28 @@ #ifndef CONTROLLER_IMPL_H #define CONTROLLER_IMPL_H +#include + #ifdef __cplusplus extern "C" { #endif +// Server lifecycle void initServer(); void acceptClient(int clientId); -void sendMessage(int clientId); -int receiveAck(int clientId); void closeServer(); -// Target location protocol functions +// Configuration int loadTargets(const char* filename, double* targets, int maxClients); -void sendTarget(int clientId, const double* target); -int receiveTargetAck(int clientId); -int waitForReady(int clientId); -void sendFinished(int clientId); -// Parallel wait functions (using select() for simultaneous processing) -int waitForAllTargetAck(int numClients); -int waitForAllReady(int numClients); -int waitForAllRTLComplete(int numClients); -int waitForAllLANDComplete(int numClients); - -// RTL and LAND protocol functions -void sendRTL(int clientId); -void sendLAND(int clientId); +// User interaction void waitForUserInput(); +// Binary protocol operations +int sendMessageType(int clientId, int msgType); +int sendTarget(int clientId, const double* coords); +int waitForAllMessageType(int numClients, int expectedType); + #ifdef __cplusplus } #endif diff --git a/geometries/REGION_TYPE.m b/geometries/REGION_TYPE.m index d271234..8a15dd1 100644 --- a/geometries/REGION_TYPE.m +++ b/geometries/REGION_TYPE.m @@ -1,4 +1,4 @@ -classdef REGION_TYPE +classdef REGION_TYPE < uint8 properties id color