From f40d2bfd84da806d6b3a964133663f0b1b845fc3 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Tue, 3 Mar 2026 15:53:22 -0800 Subject: [PATCH] moved reader out of miSim, went to event-based guidance --- @miSim/miSim.m | 1 - aerpaw/client/uav_runner.py | 11 ++++-- aerpaw/controller.coderprj | 2 +- aerpaw/controller.m | 34 +++++++++---------- aerpaw/impl/controller_impl.cpp | 8 ++++- .../KW9h3Gs8ZTtGq070jCUxWZpaFvYd.xml} | 0 .../KW9h3Gs8ZTtGq070jCUxWZpaFvYp.xml} | 0 {@miSim => util}/readScenarioCsv.m | 3 +- 8 files changed, 34 insertions(+), 25 deletions(-) rename resources/project/{SIL3u_W39LwE7HHYsarfFmr9gVQ/J2y3iIxdkUJ49I60U1-dbvlUZyUd.xml => E2mMq2X73DyjKhlQAouGqrsyLgg/KW9h3Gs8ZTtGq070jCUxWZpaFvYd.xml} (100%) rename resources/project/{SIL3u_W39LwE7HHYsarfFmr9gVQ/J2y3iIxdkUJ49I60U1-dbvlUZyUp.xml => E2mMq2X73DyjKhlQAouGqrsyLgg/KW9h3Gs8ZTtGq070jCUxWZpaFvYp.xml} (100%) rename {@miSim => util}/readScenarioCsv.m (95%) diff --git a/@miSim/miSim.m b/@miSim/miSim.m index 8009121..8f15aa7 100644 --- a/@miSim/miSim.m +++ b/@miSim/miSim.m @@ -79,7 +79,6 @@ classdef miSim [obj] = plotH(obj); [obj] = updatePlots(obj); [obj] = teardown(obj); - inits = readScenarioCsv(csvPath); writeInits(obj); validate(obj); end diff --git a/aerpaw/client/uav_runner.py b/aerpaw/client/uav_runner.py index 9204bb1..716f0ae 100644 --- a/aerpaw/client/uav_runner.py +++ b/aerpaw/client/uav_runner.py @@ -229,13 +229,18 @@ class UAVRunner(BasicRunner): target = self.origin + VectorNED(north=enu_y, east=enu_x, down=-enu_z) if in_guidance: - # Guidance mode: non-blocking — cancel previous nav and start new + # Guidance mode (event-triggered): navigate to target, + # then send ACK once arrived so the controller knows + # all UAVs have reached their targets before it + # requests positions and computes the next step. 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 + await drone.goto_coordinates(target) + await send_message_type(writer, MessageType.ACK) + print("[UAV] Sent ACK (arrived at guidance target)") + nav_task = None else: # Sequential mode: ACK → navigate → READY waypoint_num += 1 diff --git a/aerpaw/controller.coderprj b/aerpaw/controller.coderprj index afc8cb8..3089563 100644 --- a/aerpaw/controller.coderprj +++ b/aerpaw/controller.coderprj @@ -1095,7 +1095,7 @@ true - 2026-03-03T15:05:52 + 2026-03-03T15:32:50 diff --git a/aerpaw/controller.m b/aerpaw/controller.m index 2f65572..957e5c5 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -78,7 +78,7 @@ for w = 1:numWaypoints target = targets(targetIdx, :); if coder.target('MATLAB') - disp(['Sending TARGET to client ', num2str(i), ' (waypoint ', num2str(w), '): ', ... + disp([datestr(now, 'HH:MM:SS'), ' 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)); @@ -105,7 +105,6 @@ end % ---- 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) % Enter guidance mode on all clients if ~coder.target('MATLAB') @@ -124,36 +123,37 @@ end guidance_step(positions(1:numClients, :), true, ... scenarioParams, obstacleMin, obstacleMax, numObstacles); -% Main guidance loop +% Main guidance loop (event-triggered) 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 + % Run one guidance step: feed current GPS positions in, get targets out nextPositions = guidance_step(positions(1:numClients, :), false, ... scenarioParams, obstacleMin, obstacleMax, numObstacles); - % Send target to each client (no ACK/READY expected in guidance mode) + % Send target to each client 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)]); + disp([datestr(now, 'HH:MM:SS'), ' [guidance] target UAV ', num2str(i), ': ', num2str(target)]); end end - % Simulation: advance positions to guidance outputs for closed-loop feedback - if coder.target('MATLAB') - positions(1:numClients, :) = nextPositions(1:numClients, :); + % Wait for ACK from all clients (each UAV ACKs when it arrives at its target) + if ~coder.target('MATLAB') + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.ACK)); + else + disp(['[guidance] step ', num2str(step), ': all UAVs arrived']); end - % Wait for the guidance rate interval before the next iteration + % Request current GPS positions from all clients if ~coder.target('MATLAB') - coder.ceval('sleepMs', int32(GUIDANCE_RATE_MS)); + coder.ceval('sendRequestPositions', int32(numClients)); + coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS)); + else + % Simulation: advance positions to guidance outputs for closed-loop feedback + positions(1:numClients, :) = nextPositions(1:numClients, :); end end diff --git a/aerpaw/impl/controller_impl.cpp b/aerpaw/impl/controller_impl.cpp index f1071e7..2cb5b9f 100644 --- a/aerpaw/impl/controller_impl.cpp +++ b/aerpaw/impl/controller_impl.cpp @@ -419,7 +419,13 @@ int sendTarget(int clientId, const double* coords) { return 0; } - std::cout << "Sent TARGET to client " << clientId << ": " + // 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 << ": " << coords[0] << "," << coords[1] << "," << coords[2] << "\n"; return 1; } diff --git a/resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/J2y3iIxdkUJ49I60U1-dbvlUZyUd.xml b/resources/project/E2mMq2X73DyjKhlQAouGqrsyLgg/KW9h3Gs8ZTtGq070jCUxWZpaFvYd.xml similarity index 100% rename from resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/J2y3iIxdkUJ49I60U1-dbvlUZyUd.xml rename to resources/project/E2mMq2X73DyjKhlQAouGqrsyLgg/KW9h3Gs8ZTtGq070jCUxWZpaFvYd.xml diff --git a/resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/J2y3iIxdkUJ49I60U1-dbvlUZyUp.xml b/resources/project/E2mMq2X73DyjKhlQAouGqrsyLgg/KW9h3Gs8ZTtGq070jCUxWZpaFvYp.xml similarity index 100% rename from resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/J2y3iIxdkUJ49I60U1-dbvlUZyUp.xml rename to resources/project/E2mMq2X73DyjKhlQAouGqrsyLgg/KW9h3Gs8ZTtGq070jCUxWZpaFvYp.xml diff --git a/@miSim/readScenarioCsv.m b/util/readScenarioCsv.m similarity index 95% rename from @miSim/readScenarioCsv.m rename to util/readScenarioCsv.m index 4f81ba1..e71e69b 100644 --- a/@miSim/readScenarioCsv.m +++ b/util/readScenarioCsv.m @@ -1,6 +1,5 @@ -function scenario = readScenarioCsv(obj, csvPath) +function scenario = readScenarioCsv(csvPath) arguments (Input) - obj (1, 1) {mustBeA(obj, 'miSim')}; %#ok csvPath (1, 1) string; end arguments (Output)