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

controller logging and analysis improvements

Browse Source
This commit is contained in:
Kevin Dee
2026-04-08 00:02:54 -07:00
parent 87060ca123
commit 09a10abfd5
6 changed files with 107 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
aerpaw/controller.coderprj
+6 -1
View File
@@ -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
+8 -2
View File
@@ -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
+57 -16
View File
@@ -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
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;
aerpaw/impl/controller_impl.h
+1
View File
@@ -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
+23 -10
View File
@@ -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
aerpaw/results/resultsAnalysis.m
+12 -11
View File
@@ -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)