moved reader out of miSim, went to event-based guidance

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

aerpaw/controller.coderprj

@@ -1095,7 +1095,7 @@
                 </Artifacts>
                 <BuildFolder type="coderapp.internal.util.mfz.FileSpec"/>
                 <Success>true</Success>
-                <Timestamp>2026-03-03T15:05:52</Timestamp>
+                <Timestamp>2026-03-03T15:32:50</Timestamp>
               </MainBuildResult>
             </coderapp.internal.mlc.mfz.MatlabCoderProjectState>
           </MF0>

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
 

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;
 }