diff --git a/aerpaw/controller.coderprj b/aerpaw/controller.coderprj
index 7b72992..ee5af41 100644
--- a/aerpaw/controller.coderprj
+++ b/aerpaw/controller.coderprj
@@ -168,6 +168,11 @@
+
+ int32
+
+
+
@@ -1183,7 +1188,7 @@
true
- 2026-04-06T20:46:55
+ 2026-04-07T22:43:01
diff --git a/aerpaw/controller.m b/aerpaw/controller.m
index 1b00cf9..9d83aa5 100644
--- a/aerpaw/controller.m
+++ b/aerpaw/controller.m
@@ -102,7 +102,7 @@ for w = 1:numWaypoints
target = targets(targetIdx, :);
if coder.target('MATLAB')
- disp([datestr(now, 'HH:MM:SS'), ' Sending TARGET to client ', num2str(i), ' (waypoint ', num2str(w), '): ', ...
+ 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));
@@ -149,6 +149,10 @@ guidance_step(positions(1:numClients, :), true, ...
% Main guidance loop (event-triggered)
for step = 1:MAX_GUIDANCE_STEPS
+ if ~coder.target('MATLAB')
+ coder.ceval('setGuidanceStep', int32(step), int32(MAX_GUIDANCE_STEPS));
+ end
+
% Run one guidance step: feed current GPS positions in, get targets out
nextPositions = guidance_step(positions(1:numClients, :), false, ...
scenarioParams, obstacleMin, obstacleMax, numObstacles);
@@ -159,7 +163,7 @@ for step = 1:MAX_GUIDANCE_STEPS
if ~coder.target('MATLAB')
coder.ceval('sendTarget', int32(i), coder.ref(target));
else
- disp([datestr(now, 'HH:MM:SS'), ' [guidance] target UAV ', num2str(i), ': ', num2str(target)]);
+ disp(['[step ', num2str(step), '] target UAV ', num2str(i), ': ', num2str(target)]);
end
end
@@ -188,6 +192,8 @@ if ~coder.target('MATLAB')
% last guidance navigation and is back in sequential (ACK/READY) mode.
coder.ceval('waitForAllMessageType', int32(numClients), ...
int32(MESSAGE_TYPE.ACK));
+ % Reset step counter so post-guidance logging carries no step prefix.
+ coder.ceval('setGuidanceStep', int32(0), int32(MAX_GUIDANCE_STEPS));
end
% --------------------------------------------------------------------------
diff --git a/aerpaw/impl/controller_impl.cpp b/aerpaw/impl/controller_impl.cpp
index e8992fa..ded16d9 100644
--- a/aerpaw/impl/controller_impl.cpp
+++ b/aerpaw/impl/controller_impl.cpp
@@ -16,6 +16,44 @@
static int serverSocket = -1;
static std::vector clientSockets;
+static int guidanceStep = 0;
+static int guidanceTotalSteps = 0;
+static struct timespec lastStepTime = {0, 0};
+
+// During guidance returns "(%d/%d) "; outside guidance returns "HH:MM:SS ".
+static std::string logPrefix() {
+ if (guidanceStep > 0) {
+ char buf[32];
+ snprintf(buf, sizeof(buf), "(%d/%d) ", guidanceStep, guidanceTotalSteps);
+ return std::string(buf);
+ }
+ time_t now = time(nullptr);
+ struct tm* lt = localtime(&now);
+ char ts[16];
+ strftime(ts, sizeof(ts), "%H:%M:%S", lt);
+ return std::string(ts) + " ";
+}
+
+void setGuidanceStep(int step, int totalSteps) {
+ struct timespec now;
+ clock_gettime(CLOCK_MONOTONIC, &now);
+
+ // From step 2 onward, elapsed = setGuidanceStep(N-1) → setGuidanceStep(N),
+ // which spans the full prior iteration: guidance computation + target send
+ // + flight + position request/receive.
+ if (step > 1 && lastStepTime.tv_sec != 0) {
+ double elapsed = (now.tv_sec - lastStepTime.tv_sec)
+ + (now.tv_nsec - lastStepTime.tv_nsec) * 1e-9;
+ guidanceStep = step;
+ guidanceTotalSteps = totalSteps;
+ std::cout << logPrefix() << "Iteration duration: " << elapsed << " s\n";
+ } else {
+ guidanceStep = step;
+ guidanceTotalSteps = totalSteps;
+ }
+
+ lastStepTime = now;
+}
void initSockets() {}
void cleanupSockets() {}
@@ -451,18 +489,22 @@ static const char* messageTypeName(uint8_t msgType) {
}
}
-// Send a single-byte message type to a client
-int sendMessageType(int clientId, int msgType) {
+// Send a single-byte message type to a client (no logging)
+static int sendMessageTypeRaw(int clientId, int msgType) {
if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0;
-
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 1;
+}
- std::cout << "Sent " << messageTypeName(msg) << " to client " << clientId << "\n";
+// Send a single-byte message type to a client
+int sendMessageType(int clientId, int msgType) {
+ if (!sendMessageTypeRaw(clientId, msgType)) return 0;
+ std::cout << logPrefix() << "Sent " << messageTypeName((uint8_t)msgType) << " to client " << clientId << "\n";
return 1;
}
@@ -481,13 +523,7 @@ int sendTarget(int clientId, const double* coords) {
return 0;
}
- // Timestamp
- time_t now = time(nullptr);
- struct tm* lt = localtime(&now);
- char ts[16];
- strftime(ts, sizeof(ts), "%H:%M:%S", lt);
-
- std::cout << ts << " Sent TARGET to client " << clientId << ": "
+ std::cout << logPrefix() << "Sent TARGET to client " << clientId << ": "
<< coords[0] << "," << coords[1] << "," << coords[2] << "\n";
return 1;
}
@@ -535,31 +571,36 @@ int waitForAllMessageType(int numClients, int expectedType) {
return 0;
}
- std::cout << "Received " << messageTypeName(msgType) << " from client " << (i + 1) << "\n";
-
if (msgType == expected) {
completed[i] = true;
completedCount++;
+ } else {
+ std::cerr << logPrefix() << "Unexpected " << messageTypeName(msgType)
+ << " from client " << (i + 1)
+ << " (expected " << messageTypeName(expected) << ")\n";
}
}
}
}
+ std::cout << logPrefix() << "Received " << messageTypeName(expected) << " from all clients\n";
return 1;
}
// Broadcast GUIDANCE_TOGGLE to all clients
void sendGuidanceToggle(int numClients) {
for (int i = 1; i <= numClients; i++) {
- sendMessageType(i, 6); // GUIDANCE_TOGGLE = 6
+ sendMessageTypeRaw(i, 6); // GUIDANCE_TOGGLE = 6
}
+ std::cout << logPrefix() << "Sent GUIDANCE_TOGGLE to clients\n";
}
// 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
+ if (!sendMessageTypeRaw(i, 7)) return 0; // REQUEST_POSITION = 7
}
+ std::cout << logPrefix() << "Sent REQUEST_POSITION to clients\n";
return 1;
}
@@ -594,7 +635,7 @@ int recvPositions(int numClients, double* positions, int maxClients) {
positions[i + 1 * maxClients] = coords[1]; // north (y)
positions[i + 2 * maxClients] = coords[2]; // up (z)
- std::cout << "Position from client " << (i + 1) << ": "
+ std::cout << logPrefix() << "Position from client " << (i + 1) << ": "
<< coords[0] << "," << coords[1] << "," << coords[2] << "\n";
}
return 1;
diff --git a/aerpaw/impl/controller_impl.h b/aerpaw/impl/controller_impl.h
index a43f0fd..f751f80 100644
--- a/aerpaw/impl/controller_impl.h
+++ b/aerpaw/impl/controller_impl.h
@@ -62,6 +62,7 @@ int sendTarget(int clientId, const double* coords);
int waitForAllMessageType(int numClients, int expectedType);
// Guidance loop operations
+void setGuidanceStep(int step, int totalSteps); // call at the top of each guidance iteration
void sendGuidanceToggle(int numClients);
int sendRequestPositions(int numClients);
int recvPositions(int numClients, double* positions, int maxClients); // column-major maxClients x 3
diff --git a/aerpaw/results/readControllerLogs.m b/aerpaw/results/readControllerLogs.m
index cedbde2..2d5a3f1 100644
--- a/aerpaw/results/readControllerLogs.m
+++ b/aerpaw/results/readControllerLogs.m
@@ -1,19 +1,32 @@
-function T = readControllerLogs(filepath)
+function T2 = readControllerLogs(filepath)
arguments (Input)
filepath (1, 1) string;
end
arguments (Output)
- T table;
+ T2 table;
end
assert(isfile(filepath), "File not found at %s", filepath);
- T = readtable(filepath);
- T.Var1 = datetime(strip(strip(append(T.Var1, " ", T.Var2), 'left', '['), 'right', ']'), "InputFormat", "yyyy-MM-dd HH:mm:ss.SSSSSS");
- T.Var2 = [];
- T.Var3 = strip(append(T.Var3, " ", T.Var4, " ", T.Var5, " ", T.Var6, " ", string(T.Var7), " ", T.Var8, " ", T.Var9));
- T.Var4 = []; T.Var5 = []; T.Var6 = []; T.Var7 = []; T.Var8 = []; T.Var9 = [];
- T.Properties.VariableNames{1} = 'timestamp';
- T.Properties.VariableNames{2} = 'message';
+ T = readtable(filepath, 'VariableNamingRule', 'preserve');
+ s = split(T.(T.Properties.VariableNames{1}), ']');
+ s2 = strip(s(startsWith(s(:, 2), " ("), 1), 'left', '[');
+ d = datetime(s2, "InputFormat", "yyyy-MM-dd HH:mm:ss.SSSSSS")';
+ it = s(startsWith(s(:, 2), " ("), 2);
+ it = str2double(strip(strip(it, 'left'), 'left', '('));
+ T.Var3 = strip(append(T.Var3, " ", T.Var4, " ", T.Var5, " ", T.Var6, " ", T.Var7));
+ T.Var4 = []; T.Var5 = []; T.Var6 = []; T.Var7 = [];
+ msg = T.(T.Properties.VariableNames{2});
+ msg = msg(startsWith(s(:, 2), " ("), :);
+ s3 = split(msg, ') ');
+ s3 = s3(:, 2);
+ msg = append(s3, T.Var3(startsWith(s(:, 2), " (")));
+ T2 = table(it, d', msg, 'VariableNames', ["iteration", "timestamp", "message"]);
+ % T.Var1 = datetime(strip(strip(append(T.Var1, " ", T.Var2), 'left', '['), 'right', ']'), "InputFormat", "yyyy-MM-dd HH:mm:ss.SSSSSS");
+ % T.Var2 = [];
+ % T.Var3 = strip(append(T.Var3, " ", T.Var4, " ", T.Var5, " ", T.Var6, " ", string(T.Var7), " ", T.Var8, " ", T.Var9));
+ % T.Var4 = []; T.Var5 = []; T.Var6 = []; T.Var7 = []; T.Var8 = []; T.Var9 = [];
+ % T.Properties.VariableNames{1} = 'timestamp';
+ % T.Properties.VariableNames{2} = 'message';
- T(ismissing(T.message), :) = [];
+ % T(ismissing(T.message), :) = [];
end
\ No newline at end of file
diff --git a/aerpaw/results/resultsAnalysis.m b/aerpaw/results/resultsAnalysis.m
index 2d13f69..2c3617d 100644
--- a/aerpaw/results/resultsAnalysis.m
+++ b/aerpaw/results/resultsAnalysis.m
@@ -1,5 +1,5 @@
%% Plot AERPAW logs (trajectory, radio)
-resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "test2"); % Define path to results copied from AERPAW platform
+resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "two_around_wall"); % Define path to results copied from AERPAW platform
% Measure intervals between issuing commands from the controller
% (make sure this is ~4-5 seconds at minimum to avoid overwhelming the UAV autopilot)
@@ -8,16 +8,17 @@ controllerPath = fullfile(r(startsWith({r.name}, 'controller_')).folder, r(start
controllerPath = dir(controllerPath);
controllerPath = fullfile(controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).folder, controllerPath(endsWith({controllerPath.name}, '_controller_log.txt')).name);
controller = readControllerLogs(controllerPath);
-rpIdx = startsWith(controller.message, "Sent REQUEST_POSITION to client 1");
-rpTimes = controller.timestamp(rpIdx);
-dt = diff(rpTimes);
-dt.Format = "mm:ss.SSS";
-fprintf("Minimum command spacing: %2.3f seconds\n", seconds(min(dt)));
-fprintf("Maximum command spacing: %2.3f seconds\n", seconds(max(dt)));
-fprintf("Mean command spacing: %2.3f seconds\n", seconds(mean(dt)));
-fprintf("Median command spacing: %2.3f seconds\n", seconds(median(dt)));
-if seconds(min(dt)) < 4
- warning("Minimum command spacing %2.3f questionably short for AERPAW", seconds(min(dt)));
+rpIdx = startsWith(controller.message, "Iteration duration: ");
+s = split(controller.message(rpIdx), "Iteration duration: ");
+s = split(s(:, 2), ' s');
+s = duration(strcat("00:", s(:, 1)), "InputFormat", "mm:ss.SSS");
+s.Format = "mm:ss.SSS";
+fprintf("Minimum command spacing: %2.3f seconds\n", seconds(min(s)));
+fprintf("Maximum command spacing: %2.3f seconds\n", seconds(max(s)));
+fprintf("Mean command spacing: %2.3f seconds\n", seconds(mean(s)));
+fprintf("Median command spacing: %2.3f seconds\n", seconds(median(s)));
+if seconds(min(s)) < 4
+ warning("Minimum command spacing %2.3f questionably short for AERPAW", seconds(min(s)));
end
% Plot GPS logged data and scenario information (domain, objective, obstacles)