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

codegen fixes, bug fixes, gets running on testbed environment

Browse Source
This commit is contained in:
Kevin Dee
2026-02-24 19:05:54 -08:00
parent 58d87cd16f
commit 1ada914384
38 changed files with 1732 additions and 263 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
aerpaw/MESSAGE_TYPE.m
View File

@@ -3,7 +3,10 @@ classdef MESSAGE_TYPE < uint8
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
RTL (4) % Server->Client: return to launch
LAND (5) % Server->Client: land now
GUIDANCE_TOGGLE (6) % Server->Client: toggle guidance mode on/off
REQUEST_POSITION (7) % Server->Client: respond with current ENU position
POSITION (8) % Client->Server: current ENU position (3 doubles)
end
end

77
aerpaw/client/uav_runner.py
View File

@@ -36,11 +36,14 @@ 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
TARGET = 1
ACK = 2
READY = 3
RTL = 4
LAND = 5
GUIDANCE_TOGGLE = 6
REQUEST_POSITION = 7
POSITION = 8
AERPAW_DIR = Path(__file__).parent.parent
@@ -177,6 +180,7 @@ class UAVRunner(BasicRunner):
return
log_task = None
nav_task = None
try:
# Takeoff to above AERPAW minimum altitude
print("[UAV] Taking off...")
@@ -186,32 +190,64 @@ class UAVRunner(BasicRunner):
# Start GPS logging in background
log_task = asyncio.create_task(_gps_log_loop(drone))
# Command loop - handle TARGET, RTL, LAND, READY from controller
# Command loop - handle all messages from controller
waypoint_num = 0
in_guidance = False
while True:
msg_type = await recv_message_type(reader)
print(f"[UAV] Received: {msg_type.name}")
if msg_type == MessageType.TARGET:
if msg_type == MessageType.GUIDANCE_TOGGLE:
in_guidance = not in_guidance
print(f"[UAV] Guidance mode: {'ON' if in_guidance else 'OFF'}")
if not in_guidance:
# Exiting guidance: wait for current navigation to finish
# before resuming sequential (ACK/READY) mode
if nav_task and not nav_task.done():
print("[UAV] Waiting for current navigation to complete...")
await nav_task
nav_task = None
# Acknowledge that we are ready for sequential commands
await send_message_type(writer, MessageType.ACK)
print("[UAV] Sent ACK (guidance mode exited, ready for sequential commands)")
elif msg_type == MessageType.REQUEST_POSITION:
# Respond immediately with current ENU position relative to origin
pos = drone.position
enu = pos - self.origin # VectorNED(north, east, down)
await send_message_type(writer, MessageType.POSITION)
writer.write(struct.pack('<ddd', enu.east, enu.north, -enu.down))
await writer.drain()
print(f"[UAV] Sent POSITION: E={enu.east:.1f} N={enu.north:.1f} U={-enu.down:.1f}")
elif msg_type == MessageType.TARGET:
# Read 24 bytes of coordinates (3 little-endian doubles)
data = await recv_exactly(reader, 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}")
await send_message_type(writer, MessageType.ACK)
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}")
await send_message_type(writer, MessageType.READY)
print(f"[UAV] Sent READY")
if in_guidance:
# Guidance mode: non-blocking — cancel previous nav and start new
print(f"[UAV] Guidance TARGET: E={enu_x:.1f} N={enu_y:.1f} U={enu_z:.1f}")
if nav_task and not nav_task.done():
nav_task.cancel()
await asyncio.gather(nav_task, return_exceptions=True)
nav_task = asyncio.create_task(drone.goto_coordinates(target))
# No ACK/READY in guidance mode
else:
# Sequential mode: ACK → navigate → READY
waypoint_num += 1
print(f"[UAV] TARGET (waypoint {waypoint_num}): x={enu_x:.1f}, y={enu_y:.1f}, z={enu_z:.1f}")
print(f"[UAV] Target coord: {target.lat:.6f}, {target.lon:.6f}, {target.alt:.1f}")
await send_message_type(writer, MessageType.ACK)
print("[UAV] Sent ACK")
print(f"[UAV] Moving to waypoint {waypoint_num}...")
await drone.goto_coordinates(target)
print(f"[UAV] Arrived at waypoint {waypoint_num}")
await send_message_type(writer, MessageType.READY)
print("[UAV] Sent READY")
elif msg_type == MessageType.RTL:
await send_message_type(writer, MessageType.ACK)
@@ -246,6 +282,9 @@ class UAVRunner(BasicRunner):
print(f"[UAV] Error: {e}")
finally:
if nav_task is not None and not nav_task.done():
nav_task.cancel()
await asyncio.gather(nav_task, return_exceptions=True)
if log_task is not None:
log_task.cancel()
await asyncio.gather(log_task, return_exceptions=True)

3
aerpaw/compile.sh
View File

@@ -8,6 +8,8 @@ BUILD="$AERPAW_DIR/build"
mkdir -p "$BUILD"
echo "Compiling controller executable..."
# Compile all codegen sources (handles any new generated files)
g++ -I/home/kdee/matlab/R2025a/extern/include \
-I"$CODEGEN" \
@@ -16,6 +18,7 @@ g++ -I/home/kdee/matlab/R2025a/extern/include \
"$IMPL/controller_impl.cpp" \
"$CODEGEN"/*.cpp \
-o "$BUILD/controller_app" \
-fopenmp \
-lpthread
echo "Built: $BUILD/controller_app"

16
aerpaw/config/server.yaml
View File

@@ -17,15 +17,7 @@
# Max distance from origin: ~22m (all waypoints within geofence)
#
targets:
# UAV 1: straight line along x-axis at y=5, alt=45m
- [0.0, 5.0, 45.0]
- [4.0, 5.0, 45.0]
- [8.0, 5.0, 45.0]
- [12.0, 5.0, 45.0]
- [16.0, 5.0, 45.0]
# UAV 2: triangular path diverging/converging, alt=30m
- [0.0, -5.0, 30.0]
- [4.0, -15.0, 30.0]
- [8.0, -20.0, 30.0]
- [12.0, -15.0, 30.0]
- [16.0, -5.0, 30.0]
# UAV 1 Initial Position — west side of guidance domain
- [5.0, 10.0, 45.0]
# UAV 2 Initial Position — east side of guidance domain
- [15.0, 10.0, 30.0]

938
aerpaw/controller.coderprj
View File

File diff suppressed because it is too large Load Diff

64
aerpaw/controller.m
View File

@@ -83,7 +83,69 @@ for w = 1:numWaypoints
end
end
% Wait for user input before closing experiment
% ---- Phase 2: miSim guidance loop ----------------------------------------
% Guidance parameters (adjust here and recompile as needed)
MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations
GUIDANCE_RATE_MS = int32(5000); % ms between iterations (0.2 Hz default)
% Wait for user input to start guidance loop
if coder.target('MATLAB')
input('Press Enter to start guidance loop: ', 's');
else
coder.ceval('waitForUserInput');
end
% Enter guidance mode on all clients
if ~coder.target('MATLAB')
coder.ceval('sendGuidanceToggle', int32(numClients));
end
% Request initial GPS positions and initialise guidance algorithm
positions = zeros(MAX_CLIENTS, 3);
if ~coder.target('MATLAB')
coder.ceval('sendRequestPositions', int32(numClients));
coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS));
end
guidance_step(positions(1:numClients, :), true);
% Main guidance loop
for step = 1:MAX_GUIDANCE_STEPS
% Query current GPS positions from all clients
if ~coder.target('MATLAB')
coder.ceval('sendRequestPositions', int32(numClients));
coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS));
end
% Run one guidance step: feed GPS positions in, get targets out
nextPositions = guidance_step(positions(1:numClients, :), false);
% Send target to each client (no ACK/READY expected in guidance mode)
for i = 1:numClients
target = nextPositions(i, :);
if ~coder.target('MATLAB')
coder.ceval('sendTarget', int32(i), coder.ref(target));
else
disp(['[guidance] target UAV ', num2str(i), ': ', num2str(target)]);
end
end
% Wait for the guidance rate interval before the next iteration
if ~coder.target('MATLAB')
coder.ceval('sleepMs', int32(GUIDANCE_RATE_MS));
end
end
% Exit guidance mode on all clients (second toggle)
if ~coder.target('MATLAB')
coder.ceval('sendGuidanceToggle', int32(numClients));
% Wait for ACK from all clients: confirms each client has finished its
% last guidance navigation and is back in sequential (ACK/READY) mode.
coder.ceval('waitForAllMessageType', int32(numClients), ...
int32(MESSAGE_TYPE.ACK));
end
% --------------------------------------------------------------------------
% Wait for user input before closing experiment (RTL + LAND)
if coder.target('MATLAB')
input('Press Enter to close experiment (RTL + LAND): ', 's');
else

179
aerpaw/guidance_step.m Normal file
View File

@@ -0,0 +1,179 @@
function nextPositions = guidance_step(currentPositions, isInit)
% guidance_step One step of the miSim sensing coverage guidance algorithm.
%
% Wraps the miSim gradient-ascent + CBF motion algorithm for AERPAW.
% Holds full simulation state in a persistent variable between calls.
%
% Usage (from controller.m):
% guidance_step(initPositions, true) % first call: initialise state
% nextPos = guidance_step(gpsPos, false) % subsequent calls: run one step
%
% Inputs:
% currentPositions (MAX_CLIENTS × 3) double ENU [east north up] metres
% isInit (1,1) logical true on first call only
%
% Output:
% nextPositions (MAX_CLIENTS × 3) double guidance targets, ENU metres
%
% Codegen notes:
% - Persistent variable 'sim' holds the miSim object between calls.
% - Plotting/video are disabled (makePlots=false, makeVideo=false) for
% deployment. The coder.target('MATLAB') guards in the miSim/agent
% class files must be in place before codegen will succeed.
% - objectiveFunctionWrapper returns a function handle which is not
% directly codegen-compatible; the MATLAB path uses it normally. The
% compiled path requires an equivalent C impl (see TODO below).
coder.extrinsic('disp', 'objectiveFunctionWrapper');
% =========================================================================
% Tunable guidance parameters — adjust here and recompile as needed.
% =========================================================================
MAX_CLIENTS = int32(4); % must match MAX_CLIENTS in controller.m
% Domain bounds in ENU metres [east, north, up]
DOMAIN_MIN = [ 0.0, 0.0, 25.0];
DOMAIN_MAX = [ 20.0, 20.0, 55.0];
MIN_ALT = 25.0; % hard altitude floor (m)
% Sensing objective: bivariate Gaussian centred at [east, north]
OBJECTIVE_GROUND_POS = [10.0, 10.0];
DISCRETIZATION_STEP = 0.1; % objective grid step (m) — coarser = faster
PROTECTED_RANGE = 1.0; % objective centre must be >this from domain edge
% Agent safety geometry
COLLISION_RADIUS = 3.0; % spherical collision radius (m)
COMMS_RANGE = 60.0; % communications range (m)
% Gradient-ascent parameters
INITIAL_STEP_SIZE = 1; % step size at iteration 0 (decays to 0 at MAX_ITER)
MAX_ITER = 100; % guidance steps (sets decay rate)
% Sensor model (sigmoidSensor)
ALPHA_DIST = 60.0; % effective sensing distance (m) — set beyond max domain slant range (~53 m)
BETA_DIST = 0.2; % distance sigmoid steepness — gentle, tilt drives the coverage gradient
ALPHA_TILT = 10.0; % max useful tilt angle (degrees)
BETA_TILT = 1.0; % tilt sigmoid steepness
% Safety filter — Control Barrier Function (CBF/QP)
BARRIER_GAIN = 100;
BARRIER_EXPONENT = 3;
% Simulation timestep (s) — should match GUIDANCE_RATE_MS / 1000 in controller.m
TIMESTEP = 5.0;
% =========================================================================
persistent sim;
if isempty(sim)
sim = miSim;
end
% Pre-allocate output with known static size (required for codegen)
nextPositions = zeros(MAX_CLIENTS, 3);
numAgents = int32(size(currentPositions, 1));
if isInit
if coder.target('MATLAB')
disp('[guidance_step] Initialising simulation...');
end
% --- Build domain geometry ---
dom = rectangularPrism;
dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
% --- Build sensing objective ---
dom.objective = sensingObjective;
if coder.target('MATLAB')
% objectiveFunctionWrapper returns a function handle — MATLAB only.
objFcn = objectiveFunctionWrapper(OBJECTIVE_GROUND_POS, 3*eye(2));
dom.objective = dom.objective.initialize(objFcn, dom, ...
DISCRETIZATION_STEP, PROTECTED_RANGE);
else
% Evaluate bivariate Gaussian inline (codegen-compatible; no function handle).
% Must build the same grid that initializeWithValues uses internally.
xGrid = unique([DOMAIN_MIN(1):DISCRETIZATION_STEP:DOMAIN_MAX(1), DOMAIN_MAX(1)]);
yGrid = unique([DOMAIN_MIN(2):DISCRETIZATION_STEP:DOMAIN_MAX(2), DOMAIN_MAX(2)]);
[gridX, gridY] = meshgrid(xGrid, yGrid);
dx = gridX - OBJECTIVE_GROUND_POS(1);
dy = gridY - OBJECTIVE_GROUND_POS(2);
objValues = exp(-0.5 * (dx .* dx + dy .* dy));
dom.objective = dom.objective.initializeWithValues(objValues, dom, ...
DISCRETIZATION_STEP, PROTECTED_RANGE);
end
% --- Build shared sensor model ---
sensor = sigmoidSensor;
sensor = sensor.initialize(ALPHA_DIST, BETA_DIST, ALPHA_TILT, BETA_TILT);
% --- Initialise agents from GPS positions ---
agentList = cell(numAgents, 1);
for ii = 1:numAgents
pos = currentPositions(ii, :);
geom = spherical;
geom = geom.initialize(pos, COLLISION_RADIUS, REGION_TYPE.COLLISION, ...
sprintf("UAV %d Collision", ii));
ag = agent;
ag = ag.initialize(pos, geom, sensor, COMMS_RANGE, MAX_ITER, ...
INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
agentList{ii} = ag;
end
% --- Initialise simulation (plots and video disabled) ---
sim = miSim;
sim = sim.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
MIN_ALT, TIMESTEP, MAX_ITER, cell(0, 1), false, false);
if coder.target('MATLAB')
disp('[guidance_step] Initialisation complete.');
end
% On the init call return current positions unchanged
for ii = 1:numAgents
nextPositions(ii, :) = currentPositions(ii, :);
end
else
% =====================================================================
% One guidance step
% =====================================================================
% 1. Inject actual GPS positions (closed-loop feedback)
for ii = 1:size(sim.agents, 1)
sim.agents{ii}.lastPos = sim.agents{ii}.pos;
sim.agents{ii}.pos = currentPositions(ii, :);
% Re-centre collision geometry at new position
d = currentPositions(ii, :) - sim.agents{ii}.collisionGeometry.center;
sim.agents{ii}.collisionGeometry = sim.agents{ii}.collisionGeometry.initialize( ...
sim.agents{ii}.collisionGeometry.center + d, ...
sim.agents{ii}.collisionGeometry.radius, ...
REGION_TYPE.COLLISION);
end
% 2. Advance timestep counter
sim.timestepIndex = sim.timestepIndex + 1;
% 3. Update communications topology (Lesser Neighbour Assignment)
sim = sim.lesserNeighbor();
% 4. Compute Voronoi partitioning
sim.partitioning = sim.agents{1}.partition(sim.agents, sim.domain.objective);
% 5. Unconstrained gradient-ascent step for each agent
for ii = 1:size(sim.agents, 1)
sim.agents{ii} = sim.agents{ii}.run(sim.domain, sim.partitioning, ...
sim.timestepIndex, ii, sim.agents);
end
% 6. Apply CBF safety filter (collision / comms / domain constraints via QP)
sim = sim.constrainMotion();
% 7. Return constrained next positions
for ii = 1:size(sim.agents, 1)
nextPositions(ii, :) = sim.agents{ii}.pos;
end
end
end

78
aerpaw/impl/controller_impl.cpp
View File

@@ -9,6 +9,7 @@
#include <sys/select.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <time.h>
#define SERVER_PORT 5000
#define SERVER_IP "127.0.0.1"
@@ -120,6 +121,9 @@ static const char* messageTypeName(uint8_t msgType) {
case 3: return "READY";
case 4: return "RTL";
case 5: return "LAND";
case 6: return "GUIDANCE_TOGGLE";
case 7: return "REQUEST_POSITION";
case 8: return "POSITION";
default: return "UNKNOWN";
}
}
@@ -190,7 +194,17 @@ int waitForAllMessageType(int numClients, int expectedType) {
if (!completed[i] && FD_ISSET(clientSockets[i], &readfds)) {
uint8_t msgType;
int len = recv(clientSockets[i], &msgType, 1, 0);
if (len != 1) return 0;
if (len == 0) {
std::cerr << "waitForAllMessageType: client " << (i + 1)
<< " disconnected while waiting for "
<< messageTypeName(expected) << "\n";
return 0;
}
if (len < 0) {
std::cerr << "waitForAllMessageType: recv error for client " << (i + 1)
<< " while waiting for " << messageTypeName(expected) << "\n";
return 0;
}
std::cout << "Received " << messageTypeName(msgType) << " from client " << (i + 1) << "\n";
@@ -207,6 +221,66 @@ int waitForAllMessageType(int numClients, int expectedType) {
// Wait for user to press Enter
void waitForUserInput() {
std::cout << "Press Enter to close experiment (RTL + LAND)...\n";
std::cout << "Press Enter to continue...\n";
std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
}
// Broadcast GUIDANCE_TOGGLE to all clients
void sendGuidanceToggle(int numClients) {
for (int i = 1; i <= numClients; i++) {
sendMessageType(i, 6); // GUIDANCE_TOGGLE = 6
}
}
// Send REQUEST_POSITION to all clients
int sendRequestPositions(int numClients) {
for (int i = 1; i <= numClients; i++) {
if (!sendMessageType(i, 7)) return 0; // REQUEST_POSITION = 7
}
return 1;
}
// Receive POSITION response (1 byte type + 24 bytes ENU) from all clients.
// Stores results in column-major order: positions[client + 0*maxClients] = x (east),
// positions[client + 1*maxClients] = y (north),
// positions[client + 2*maxClients] = z (up)
int recvPositions(int numClients, double* positions, int maxClients) {
if (numClients <= 0 || numClients > (int)clientSockets.size()) return 0;
for (int i = 0; i < numClients; i++) {
// Expect: POSITION byte (1) + 3 doubles (24)
uint8_t msgType;
if (recv(clientSockets[i], &msgType, 1, MSG_WAITALL) != 1) {
std::cerr << "recvPositions: failed to read msg type from client " << (i + 1) << "\n";
return 0;
}
if (msgType != 8) { // POSITION = 8
std::cerr << "recvPositions: expected POSITION(8), got " << (int)msgType
<< " from client " << (i + 1) << "\n";
return 0;
}
double coords[3];
if (recv(clientSockets[i], coords, sizeof(coords), MSG_WAITALL) != (ssize_t)sizeof(coords)) {
std::cerr << "recvPositions: failed to read coords from client " << (i + 1) << "\n";
return 0;
}
// Store column-major (MATLAB layout): col 0 = east, col 1 = north, col 2 = up
positions[i + 0 * maxClients] = coords[0]; // east (x)
positions[i + 1 * maxClients] = coords[1]; // north (y)
positions[i + 2 * maxClients] = coords[2]; // up (z)
std::cout << "Position from client " << (i + 1) << ": "
<< coords[0] << "," << coords[1] << "," << coords[2] << "\n";
}
return 1;
}
// Sleep for the given number of milliseconds
void sleepMs(int ms) {
struct timespec ts;
ts.tv_sec = ms / 1000;
ts.tv_nsec = (ms % 1000) * 1000000L;
nanosleep(&ts, nullptr);
}

6
aerpaw/impl/controller_impl.h
View File

@@ -23,6 +23,12 @@ int sendMessageType(int clientId, int msgType);
int sendTarget(int clientId, const double* coords);
int waitForAllMessageType(int numClients, int expectedType);
// Guidance loop operations
void sendGuidanceToggle(int numClients);
int sendRequestPositions(int numClients);
int recvPositions(int numClients, double* positions, int maxClients); // column-major maxClients x 3
void sleepMs(int ms);
#ifdef __cplusplus
}
#endif

9
aerpaw/results/plotGpsCsvs.m
View File

@@ -6,9 +6,10 @@ c = ["r", "g", "b", "m", "c", "y", "k"]; % plotting colors
seaToGroundLevel = 110; % meters, measured approximately from USGS national map viewer
% Paths to logs
gpsCsvs = ["GPS_DATA_0c8d904aa159_2026-02-16_13:26:33.csv"; ...
"GPS_DATA_8e4f52dac04d_2026-02-16_13:26:33.csv"; ...
];
gpsCsvs = fullfile ("sandbox", "test1", ...
["GPS_DATA_0c8d904aa159_2026-02-24_21:33:25.csv"; ...
"GPS_DATA_8e4f52dac04d_2026-02-24_21:33:25.csv"; ...
]);
G = cell(size(gpsCsvs));
for ii = 1:size(gpsCsvs, 1)
@@ -16,7 +17,7 @@ for ii = 1:size(gpsCsvs, 1)
G{ii} = readGpsCsv(gpsCsvs(ii));
% Plot recorded trajectory
geoplot3(gf, G{ii}.Latitude, G{ii}.Longitude, G{ii}.Altitude + seaToGroundLevel, c(mod(ii, length(c))), 'LineWidth', 3, "MarkerSize", 5);
geoplot3(gf, G{ii}.Latitude, G{ii}.Longitude, G{ii}.Altitude + seaToGroundLevel, c(mod(ii, length(c))), 'LineWidth', 2, "MarkerSize", 5);
end
hold(gf, "off");