controller logging and analysis improvements

aerpaw/controller.coderprj

@@ -168,6 +168,11 @@
                   <Size type="coderapp.internal.codertype.Dimension"/>
                   <Size type="coderapp.internal.codertype.Dimension"/>
                 </Types>
+                <Types id="34" 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>
@@ -1183,7 +1188,7 @@
                 </Artifacts>
                 <BuildFolder type="coderapp.internal.util.mfz.FileSpec"/>
                 <Success>true</Success>
-                <Timestamp>2026-04-06T20:46:55</Timestamp>
+                <Timestamp>2026-04-07T22:43:01</Timestamp>
               </MainBuildResult>
             </coderapp.internal.mlc.mfz.MatlabCoderProjectState>
           </MF0>

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
 % --------------------------------------------------------------------------
 

aerpaw/impl/controller_impl.cpp

@@ -16,6 +16,44 @@
 
 static int serverSocket = -1;
 static std::vector<int> 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
+// Send a single-byte message type to a client (no logging)
-int sendMessageType(int clientId, int msgType) {
+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
+    std::cout << logPrefix() << "Sent TARGET to client " << clientId << ": "
-    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 << ": "
               << 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;

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

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 = readtable(filepath, 'VariableNamingRule', 'preserve');
-    T.Var1 = datetime(strip(strip(append(T.Var1, " ", T.Var2), 'left', '['), 'right', ']'), "InputFormat", "yyyy-MM-dd HH:mm:ss.SSSSSS");
+    s = split(T.(T.Properties.VariableNames{1}), ']');
-    T.Var2 = [];
+    s2 = strip(s(startsWith(s(:, 2), " ("), 1), 'left', '[');
-    T.Var3 = strip(append(T.Var3, " ", T.Var4, " ", T.Var5, " ", T.Var6, " ", string(T.Var7), " ", T.Var8, " ", T.Var9));
+    d = datetime(s2, "InputFormat", "yyyy-MM-dd HH:mm:ss.SSSSSS")';
-    T.Var4 = []; T.Var5 = []; T.Var6 = []; T.Var7 = []; T.Var8 = []; T.Var9 = [];
+    it = s(startsWith(s(:, 2), " ("), 2);
-    T.Properties.VariableNames{1} = 'timestamp';
+    it = str2double(strip(strip(it, 'left'), 'left', '('));
-    T.Properties.VariableNames{2} = 'message';
+    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

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");
+rpIdx = startsWith(controller.message, "Iteration duration: ");
-rpTimes = controller.timestamp(rpIdx);
+s = split(controller.message(rpIdx), "Iteration duration: ");
-dt = diff(rpTimes);
+s = split(s(:, 2), ' s');
-dt.Format = "mm:ss.SSS";
+s = duration(strcat("00:", s(:, 1)), "InputFormat", "mm:ss.SSS");
-fprintf("Minimum command spacing: %2.3f seconds\n", seconds(min(dt)));
+s.Format = "mm:ss.SSS";
-fprintf("Maximum command spacing: %2.3f seconds\n", seconds(max(dt)));
+fprintf("Minimum command spacing: %2.3f seconds\n", seconds(min(s)));
-fprintf("Mean command spacing: %2.3f seconds\n", seconds(mean(dt)));
+fprintf("Maximum command spacing: %2.3f seconds\n", seconds(max(s)));
-fprintf("Median command spacing: %2.3f seconds\n", seconds(median(dt)));
+fprintf("Mean command spacing: %2.3f seconds\n", seconds(mean(s)));
-if seconds(min(dt)) < 4 
+fprintf("Median command spacing: %2.3f seconds\n", seconds(median(s)));
-    warning("Minimum command spacing %2.3f questionably short for AERPAW", seconds(min(dt)));
+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)