message type updates
This commit is contained in:
9
aerpaw/MESSAGE_TYPE.m
Normal file
9
aerpaw/MESSAGE_TYPE.m
Normal file
@@ -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
|
||||
Binary file not shown.
@@ -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]} <client_id>")
|
||||
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()
|
||||
@@ -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('<ddd', data)
|
||||
return x, y, z
|
||||
|
||||
|
||||
class UAVRunner(BasicRunner):
|
||||
@@ -117,12 +155,9 @@ class UAVRunner(BasicRunner):
|
||||
await drone.takeoff(25)
|
||||
print("[UAV] Takeoff complete, waiting for target...")
|
||||
|
||||
# Receive TARGET command
|
||||
data = sock.recv(1024).decode().strip()
|
||||
print(f"[UAV] Received: {data}")
|
||||
|
||||
enu_x, enu_y, enu_z = parse_target(data)
|
||||
print(f"[UAV] Target ENU: x={enu_x}, y={enu_y}, z={enu_z}")
|
||||
# Receive TARGET command (1 byte type + 24 bytes coords)
|
||||
enu_x, enu_y, enu_z = recv_target(sock)
|
||||
print(f"[UAV] Received TARGET: x={enu_x}, y={enu_y}, z={enu_z}")
|
||||
|
||||
# Convert ENU to lat/lon
|
||||
# ENU: x=East, y=North, z=Up
|
||||
@@ -131,8 +166,8 @@ class UAVRunner(BasicRunner):
|
||||
print(f"[UAV] Target coord: {target.lat:.6f}, {target.lon:.6f}, {target.alt:.1f}")
|
||||
|
||||
# Acknowledge
|
||||
sock.sendall(b"ACK:TARGET")
|
||||
print("[UAV] Sent ACK:TARGET")
|
||||
send_message_type(sock, MessageType.ACK)
|
||||
print(f"[UAV] Sent {MessageType.ACK.name}")
|
||||
|
||||
# Move to target
|
||||
print("[UAV] Moving to target...")
|
||||
@@ -140,52 +175,47 @@ class UAVRunner(BasicRunner):
|
||||
print("[UAV] Arrived at target")
|
||||
|
||||
# Signal ready
|
||||
sock.sendall(b"READY")
|
||||
print("[UAV] Sent READY, waiting for commands...")
|
||||
send_message_type(sock, MessageType.READY)
|
||||
print(f"[UAV] Sent {MessageType.READY.name}, waiting for commands...")
|
||||
|
||||
# Command loop - handle RTL, LAND, FINISHED from controller
|
||||
# Command loop - handle RTL, LAND, READY (mission complete) from controller
|
||||
while True:
|
||||
data = sock.recv(1024).decode().strip()
|
||||
print(f"[UAV] Received: {data}")
|
||||
msg_type = recv_message_type(sock)
|
||||
print(f"[UAV] Received: {msg_type.name}")
|
||||
|
||||
if data == "RTL":
|
||||
sock.sendall(b"ACK:RTL")
|
||||
if msg_type == MessageType.RTL:
|
||||
send_message_type(sock, MessageType.ACK)
|
||||
print(f"[UAV] Sent {MessageType.ACK.name}")
|
||||
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)
|
||||
# Navigate to home lat/lon at safe altitude
|
||||
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")
|
||||
send_message_type(sock, MessageType.READY)
|
||||
print(f"[UAV] Sent {MessageType.READY.name}")
|
||||
|
||||
elif data == "LAND":
|
||||
sock.sendall(b"ACK:LAND")
|
||||
elif msg_type == MessageType.LAND:
|
||||
send_message_type(sock, MessageType.ACK)
|
||||
print(f"[UAV] Sent {MessageType.ACK.name}")
|
||||
print("[UAV] Landing...")
|
||||
await drone.land()
|
||||
print("[UAV] Landed and disarmed")
|
||||
sock.sendall(b"LAND_COMPLETE")
|
||||
print("[UAV] Sent LAND_COMPLETE")
|
||||
send_message_type(sock, MessageType.READY)
|
||||
print(f"[UAV] Sent {MessageType.READY.name}")
|
||||
|
||||
elif data == "FINISHED":
|
||||
print("[UAV] Received FINISHED - mission complete")
|
||||
elif msg_type == MessageType.READY:
|
||||
print("[UAV] Received READY from server - mission complete")
|
||||
break
|
||||
|
||||
else:
|
||||
print(f"[UAV] Unknown command: {data}")
|
||||
print(f"[UAV] Unknown command: {msg_type}")
|
||||
|
||||
except ValueError as e:
|
||||
error_msg = f"ERROR:{str(e)}"
|
||||
try:
|
||||
sock.sendall(error_msg.encode())
|
||||
except Exception:
|
||||
pass
|
||||
except (ValueError, ConnectionError) as e:
|
||||
print(f"[UAV] Error: {e}")
|
||||
|
||||
finally:
|
||||
sock.close()
|
||||
print("[UAV] Socket closed")
|
||||
print("[UAV] Socket closed")
|
||||
|
||||
@@ -8,14 +8,13 @@ BUILD="$AERPAW_DIR/build"
|
||||
|
||||
mkdir -p "$BUILD"
|
||||
|
||||
# Compile all codegen sources (handles any new generated files)
|
||||
g++ -I/home/kdee/matlab/R2025a/extern/include \
|
||||
-I"$CODEGEN" \
|
||||
-I"$IMPL" \
|
||||
"$IMPL/controller_main.cpp" \
|
||||
"$CODEGEN/controller.cpp" \
|
||||
"$IMPL/controller_impl.cpp" \
|
||||
"$CODEGEN/controller_initialize.cpp" \
|
||||
"$CODEGEN/controller_terminate.cpp" \
|
||||
"$CODEGEN"/*.cpp \
|
||||
-o "$BUILD/controller_app" \
|
||||
-lpthread
|
||||
|
||||
|
||||
@@ -48,6 +48,11 @@
|
||||
<Size type="coderapp.internal.codertype.Dimension"/>
|
||||
<Size type="coderapp.internal.codertype.Dimension"/>
|
||||
</Types>
|
||||
<Types id="10" 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>
|
||||
@@ -145,7 +150,7 @@
|
||||
</Artifacts>
|
||||
<BuildFolder type="coderapp.internal.util.mfz.FileSpec"/>
|
||||
<Success>true</Success>
|
||||
<Timestamp>2026-01-30T18:21:40</Timestamp>
|
||||
<Timestamp>2026-02-01T15:39:22</Timestamp>
|
||||
</MainBuildResult>
|
||||
</coderapp.internal.mlc.mfz.MatlabCoderProjectState>
|
||||
</MF0>
|
||||
|
||||
@@ -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
|
||||
end
|
||||
|
||||
@@ -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<std::string> accumulated(numClients);
|
||||
uint8_t expected = (uint8_t)expectedType;
|
||||
std::vector<bool> 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";
|
||||
|
||||
@@ -1,34 +1,28 @@
|
||||
#ifndef CONTROLLER_IMPL_H
|
||||
#define CONTROLLER_IMPL_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#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
|
||||
|
||||
Reference in New Issue
Block a user