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

config cleanup

Browse Source
This commit is contained in:
Kevin Dee
2026-02-13 18:06:06 -08:00
parent cde86065e9
commit dbb4ba178a
6 changed files with 131 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

100
aerpaw/client/uav_runner.py
View File

@@ -9,15 +9,13 @@ Or use the auto-detecting wrapper:
./run_uav.sh
Binary Protocol:
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)
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)
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
@@ -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('<ddd', data)
return x, y, z
class UAVRunner(BasicRunner):
def initialize_args(self, extra_args):
"""Load configuration from YAML config file."""
@@ -126,6 +108,11 @@ class UAVRunner(BasicRunner):
@entrypoint
async def run_mission(self, drone: Drone):
"""Main mission entry point."""
# Enable built-in telemetry logging
drone._verbose_logging = True
drone._verbose_logging_file_prefix = "uav_telemetry"
drone._verbose_logging_delay = 1.0 # 1 Hz
print(f"[UAV] Connecting to controller at {self.server_ip}:{self.server_port}")
# Retry connection up to 10 times (~30 seconds total)
@@ -153,58 +140,59 @@ class UAVRunner(BasicRunner):
# Takeoff to above AERPAW minimum altitude
print("[UAV] Taking off...")
await drone.takeoff(25)
print("[UAV] Takeoff complete, waiting for target...")
print("[UAV] Takeoff complete, waiting for commands...")
# 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
# NED: north, east, down
target = self.origin + VectorNED(north=enu_y, east=enu_x, down=-enu_z)
print(f"[UAV] Target coord: {target.lat:.6f}, {target.lon:.6f}, {target.alt:.1f}")
# Acknowledge
send_message_type(sock, MessageType.ACK)
print(f"[UAV] Sent {MessageType.ACK.name}")
# Move to target
print("[UAV] Moving to target...")
await drone.goto_coordinates(target)
print("[UAV] Arrived at target")
# Signal ready
send_message_type(sock, MessageType.READY)
print(f"[UAV] Sent {MessageType.READY.name}, waiting for commands...")
# Command loop - handle RTL, LAND, READY (mission complete) from controller
# Command loop - handle TARGET, RTL, LAND, READY from controller
waypoint_num = 0
while True:
msg_type = recv_message_type(sock)
print(f"[UAV] Received: {msg_type.name}")
if msg_type == MessageType.RTL:
if msg_type == MessageType.TARGET:
# Read 24 bytes of coordinates (3 little-endian doubles)
data = recv_exactly(sock, 24)
enu_x, enu_y, enu_z = struct.unpack('<ddd', data)
waypoint_num += 1
print(f"[UAV] TARGET (waypoint {waypoint_num}): x={enu_x}, y={enu_y}, z={enu_z}")
# Convert ENU to lat/lon (ENU: x=East, y=North, z=Up)
target = self.origin + VectorNED(north=enu_y, east=enu_x, down=-enu_z)
print(f"[UAV] Target coord: {target.lat:.6f}, {target.lon:.6f}, {target.alt:.1f}")
send_message_type(sock, MessageType.ACK)
print(f"[UAV] Sent {MessageType.ACK.name}")
print(f"[UAV] Sent ACK")
print(f"[UAV] Moving to waypoint {waypoint_num}...")
await drone.goto_coordinates(target)
print(f"[UAV] Arrived at waypoint {waypoint_num}")
send_message_type(sock, MessageType.READY)
print(f"[UAV] Sent READY")
elif msg_type == MessageType.RTL:
send_message_type(sock, MessageType.ACK)
print(f"[UAV] Sent ACK")
print("[UAV] Returning to home...")
# Navigate to home lat/lon at safe altitude
home = drone.home_coords
safe_alt = 25 # Set to 25m alt for RTL motion
safe_alt = 25
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")
send_message_type(sock, MessageType.READY)
print(f"[UAV] Sent {MessageType.READY.name}")
print(f"[UAV] Sent READY")
elif msg_type == MessageType.LAND:
send_message_type(sock, MessageType.ACK)
print(f"[UAV] Sent {MessageType.ACK.name}")
print(f"[UAV] Sent ACK")
print("[UAV] Landing...")
await drone.land()
print("[UAV] Landed and disarmed")
# Switch out of LAND mode so the drone is re-armable
from dronekit import VehicleMode
drone._vehicle.mode = VehicleMode("ALT_HOLD")
print("[UAV] Landed and disarmed (ALT_HOLD)")
send_message_type(sock, MessageType.READY)
print(f"[UAV] Sent {MessageType.READY.name}")
print(f"[UAV] Sent READY")
elif msg_type == MessageType.READY:
print("[UAV] Mission complete")

10
aerpaw/config/config.yaml → aerpaw/config/client.yaml
View File

@@ -1,18 +1,10 @@
# AERPAW UAV Guidance System Configuration
# This file contains all settings for both simulation and testbed environments
# AERPAW UAV (Client) Configuration
# ENU coordinate system origin (AERPAW Lake Wheeler Road Field)
origin:
lat: 35.727436
lon: -78.696471
alt: 0.0 # Alt=0 means ENU z directly becomes target altitude above home
# Target positions in ENU coordinates (meters)
# Each target is assigned to a client in order: client 1 gets targets[0], etc.
targets:
- [10.5, 20.3, 45.0] # Client 1: east, north, up
- [25.0, 30.0, 30.0] # Client 2: east, north, up
# Environment-specific settings
environments:
local:

24
aerpaw/config/server.yaml Normal file
View File

@@ -0,0 +1,24 @@
# AERPAW Controller (Server) Configuration
# ENU coordinate system origin (AERPAW Lake Wheeler Road Field)
origin:
lat: 35.727436
lon: -78.696471
alt: 0.0 # Alt=0 means ENU z directly becomes target altitude above home
# Target waypoints in ENU coordinates (meters: east, north, up)
# Grouped by client: first N entries = Client 1, next N = Client 2, etc.
# Each client must have the same number of waypoints.
targets:
# Client 1 waypoints
- [10.5, 20.3, 45.0]
- [30.0, 40.0, 45.0]
- [50.0, 60.0, 45.0]
- [70.0, 80.0, 45.0]
- [90.0, 100.0, 45.0]
# Client 2 waypoints
- [25.0, 30.0, 30.0]
- [35.0, 40.0, 30.0]
- [45.0, 50.0, 30.0]
- [55.0, 60.0, 30.0]
- [65.0, 70.0, 30.0]

7
aerpaw/controller.coderprj
View File

@@ -58,6 +58,11 @@
<Size type="coderapp.internal.codertype.Dimension"/>
<Size type="coderapp.internal.codertype.Dimension"/>
</Types>
<Types id="12" 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>
@@ -155,7 +160,7 @@
</Artifacts>
<BuildFolder type="coderapp.internal.util.mfz.FileSpec"/>
<Success>true</Success>
<Timestamp>2026-02-01T15:39:22</Timestamp>
<Timestamp>2026-02-13T18:04:24</Timestamp>
</MainBuildResult>
</coderapp.internal.mlc.mfz.MatlabCoderProjectState>
</MF0>

61
aerpaw/controller.m
View File

@@ -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,13 +49,17 @@ for i = 1:numClients
end
end
% Send target coordinates to each client
% 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
% Get target for this client (1x3 array)
target = targets(i, :);
% 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), ': ', ...
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));
@@ -58,21 +68,20 @@ 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.');
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
% Wait for READY from all clients (all arrived at waypoint w)
if coder.target('MATLAB')
disp(['Waiting for ', char(MESSAGE_TYPE.READY), ' from all clients...']);
disp('All UAVs at target positions.');
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
if coder.target('MATLAB')
@@ -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

2
aerpaw/run_uav.sh
View File

@@ -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']}\")