plot script fixes

.gitignore

@@ -48,3 +48,21 @@ sandbox/*
 
 # Figures
 *.fig
+
+# Python Virtual Environment
+aerpaw/venv/
+aerpaw/venv/*
+
+# Pycache
+__pycache__
+
+# aerpaw stuff
+aerpaw/build/
+aerpaw/build/*
+aerpaw/client/__pycache__/
+aerpaw/client/__pycache__/*
+aerpaw/codegen/
+aerpaw/codegen/*
+
+# results
+*.csv

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "aerpaw/aerpawlib"]
+	path = aerpaw/aerpawlib
+	url = https://github.com/morzack/aerpawlib-vehicle-control.git

@agent/agent.m

@@ -17,8 +17,8 @@ classdef agent
         fovGeometry;
 
         % Communication
-        commsGeometry = spherical;
+        commsGeometry;
-        lesserNeighbors = [];
+        lesserNeighbors = zeros(1, 0);
 
         % Performance
         performance = 0;
@@ -34,6 +34,17 @@ classdef agent
     end
 
     methods (Access = public)
+        function obj = agent()
+            arguments (Input)
+            end
+            arguments (Output)
+                obj (1, 1) agent
+            end
+            obj.collisionGeometry = spherical;
+            obj.sensorModel = sigmoidSensor;
+            obj.fovGeometry = cone;
+            obj.commsGeometry = spherical;
+        end
         [obj] = initialize(obj, pos, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
         [obj] = run(obj, domain, partitioning, t, index, agents);
         [partitioning] = partition(obj, agents, objective)

@agent/initialize.m

@@ -1,6 +1,6 @@
 function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, label, plotCommsGeometry)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'agent')};
+        obj (1, 1) {mustBeA(obj, "agent")};
         pos (1, 3) double;
         collisionGeometry (1, 1) {mustBeGeometry};
         sensorModel (1, 1) {mustBeSensor};
@@ -11,7 +11,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
         plotCommsGeometry (1, 1) logical = false;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'agent')};
+        obj (1, 1) {mustBeA(obj, "agent")};
     end
 
     obj.pos = pos;
@@ -23,7 +23,9 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
     obj.stepDecayRate = obj.initialStepSize / maxIter;
 
     % Initialize performance vector
-    obj.performance = [0, NaN(1, maxIter), 0];
+    if coder.target('MATLAB')
+        obj.performance = [0, NaN(1, maxIter), 0];
+    end
 
     % Add spherical geometry based on com range
     obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label));

@agent/partition.m

@@ -1,35 +1,39 @@
 function [partitioning] = partition(obj, agents, objective)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'agent')};
+        obj (1, 1) {mustBeA(obj, "agent")};
-        agents (:, 1) {mustBeA(agents, 'cell')};
+        agents (:, 1) {mustBeA(agents, "cell")};
-        objective (1, 1) {mustBeA(objective, 'sensingObjective')};
+        objective (1, 1) {mustBeA(objective, "sensingObjective")};
     end
     arguments (Output)
         partitioning (:, :) double;
     end
 
-    % Assess sensing performance of each agent at each sample point
+    nAgents = size(agents, 1);
-    % in the domain
+    gridM   = size(objective.X, 1);
-    agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, [objective.X(:), objective.Y(:), zeros(size(objective.X(:)))]), size(objective.X)), agents, 'UniformOutput', false);
+    gridN   = size(objective.X, 2);
-    agentPerformances{end + 1} = objective.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
+    nPoints = gridM * gridN;
-    agentPerformances = cat(3, agentPerformances{:});
 
-    % Get highest performance value at each point
+    % Assess sensing performance of each agent at each sample point.
-    [~, idx] = max(agentPerformances, [], 3);
+    % agentPerf is (nPoints x nAgents+1): the extra column is the
-
+    % minimum threshold that must be exceeded for any assignment.
-    % Collect agent indices in the same way as performance
+    agentPerf = zeros(nPoints, nAgents + 1);
-    indices = 1:size(agents, 1);
+    for aa = 1:nAgents
-    agentInds = squeeze(tensorprod(indices, ones(size(objective.X))));
+        p = agents{aa}.sensorModel.sensorPerformance(agents{aa}.pos, ...
-    if size(agentInds, 1) ~= size(agents, 1)
+            [objective.X(:), objective.Y(:), zeros(nPoints, 1)]);
-        agentInds = reshape(agentInds, [size(agents, 1), size(agentInds)]); % needed for cases with 1 agent where prior squeeze is too agressive
+        agentPerf(:, aa) = p(:);
     end
-    agentInds = num2cell(agentInds, 2:3);
+    agentPerf(:, nAgents + 1) = objective.sensorPerformanceMinimum;
-    agentInds = cellfun(@(x) squeeze(x), agentInds, 'UniformOutput', false);
-    agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
-    agentInds = cat(3, agentInds{:});
 
-    % Use highest performing agent's index to form partitions
+    % Find which agent has highest performance at each point.
-    [m, n, ~] = size(agentInds);
+    % If the threshold column wins (idx == nAgents+1) the point is unassigned (0).
-    [jj, kk] = ndgrid(1:m, 1:n);
+    [~, idx] = max(agentPerf, [], 2);
-    partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
+
+    assignedAgent = zeros(nPoints, 1);
+    for pp = 1:nPoints
+        if idx(pp) <= nAgents
+            assignedAgent(pp) = idx(pp);
+        end
+    end
+
+    partitioning = reshape(assignedAgent, gridM, gridN);
 end

@agent/plot.m

@@ -1,12 +1,12 @@
 function [obj, f] = plot(obj, ind, f)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'agent')};
+        obj (1, 1) {mustBeA(obj, "agent")};
         ind (1, :) double = NaN;
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'agent')};
+        obj (1, 1) {mustBeA(obj, "agent")};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
     end
 
     % Create axes if they don't already exist
@@ -14,11 +14,11 @@ function [obj, f] = plot(obj, ind, f)
 
     % Plot points representing the agent position
     hold(f.Children(1).Children(end), "on");
-    o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
+    o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), "filled", "ko", "SizeData", 25);
     hold(f.Children(1).Children(end), "off");
 
     % Check if this is a tiled layout figure
-    if strcmp(f.Children(1).Type, 'tiledlayout')
+    if strcmp(f.Children(1).Type, "tiledlayout")
         % Add to other perspectives
         o = [o; copyobj(o(1), f.Children(1).Children(2))];
         o = [o; copyobj(o(1), f.Children(1).Children(3))];

@agent/run.m

@@ -1,19 +1,19 @@
 function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'agent')};
+        obj (1, 1) {mustBeA(obj, "agent")};
         domain (1, 1) {mustBeGeometry};
         partitioning (:, :) double;
         timestepIndex (1, 1) double;
         index (1, 1) double;
-        agents (:, 1) {mustBeA(agents, 'cell')};
+        agents (:, 1) {mustBeA(agents, "cell")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'agent')};
+        obj (1, 1) {mustBeA(obj, "agent")};
     end
 
     % Collect objective function values across partition
     partitionMask = partitioning == index;
-    if ~unique(partitionMask)
+    if ~any(partitionMask(:))
         % This agent has no partition, maintain current state
         return;
     end
@@ -32,10 +32,10 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
         pos = obj.pos + delta * deltaApplicator(ii, 1:3);
         
         % Compute performance values on partition
-        if ii < 5
+        if ii < 6
             % Compute sensing performance
             sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
-            % Objective performance does not change for 0, +/- X, Y steps.
+            % Objective performance does not change for 0, +/- X, +/- Y steps.
             % Those values are computed once before the loop and are only
             % recomputed when +/- Z steps are applied
         else
@@ -64,17 +64,26 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
     end
 
     % Store agent performance at current time and place
-    obj.performance(timestepIndex + 1) = C_delta(1);
+    if coder.target('MATLAB')
+        obj.performance(timestepIndex + 1) = C_delta(1);
+    end
 
     % Compute gradient by finite central differences
     gradC = [(C_delta(2)-C_delta(3))/(2*delta), (C_delta(4)-C_delta(5))/(2*delta), (C_delta(6)-C_delta(7))/(2*delta)];
 
     % Compute scaling factor
     targetRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
-    rateFactor = targetRate / norm(gradC);
+    gradNorm = norm(gradC);
 
-    % Compute unconstrained next position
+    % Compute unconstrained next position.
-    pNext = obj.pos + rateFactor * gradC;
+    % Guard against near-zero gradient: when sensor performance is saturated
+    % or near-zero across the whole partition, rateFactor -> Inf and pNext
+    % explodes. Stay put instead.
+    if gradNorm < 1e-100
+        pNext = obj.pos;
+    else
+        pNext = obj.pos + (targetRate / gradNorm) * gradC;
+    end
 
     % Move to next position
     obj.lastPos = obj.pos;
@@ -82,9 +91,9 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
 
     % Reinitialize collision geometry in the new position
     d = obj.pos - obj.collisionGeometry.center;
-    if isa(obj.collisionGeometry, 'rectangularPrism')
+    if isa(obj.collisionGeometry, "rectangularPrism")
         obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label);
-    elseif isa(obj.collisionGeometry, 'spherical')
+    elseif isa(obj.collisionGeometry, "spherical")
         obj.collisionGeometry = obj.collisionGeometry.initialize(obj.collisionGeometry.center + d, obj.collisionGeometry.radius, obj.collisionGeometry.tag, obj.collisionGeometry.label);
     else
         error("?");

@agent/updatePlots.m

@@ -1,6 +1,6 @@
 function updatePlots(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'agent')};
+        obj (1, 1) {mustBeA(obj, "agent")};
     end
     arguments (Output)
     end

@miSim/constrainMotion.m

@@ -1,60 +1,80 @@
 function [obj] = constrainMotion(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
-    if size(obj.agents, 1) < 2
+    nAgents = size(obj.agents, 1);
+
+    if nAgents < 2
         nAAPairs = 0;
     else
-        nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs
+        nAAPairs = nchoosek(nAgents, 2); % unique agent/agent pairs
     end
 
-    agents = [obj.agents{:}];
+    % Compute velocity matrix from unconstrained gradient-ascent step
-    v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
+    v = zeros(nAgents, 3);
-    if all(isnan(v), 'all') || all(v == zeros(size(obj.agents, 1), 3), 'all')
+    for ii = 1:nAgents
+        v(ii, :) = (obj.agents{ii}.pos - obj.agents{ii}.lastPos) ./ obj.timestep;
+    end
+    if all(isnan(v), "all") || all(v == zeros(nAgents, 3), "all")
         % Agents are not attempting to move, so there is no motion to be
         % constrained
         return;
     end
 
     % Initialize QP based on number of agents and obstacles
-    nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
+    nAOPairs = nAgents * size(obj.obstacles, 1); % unique agent/obstacle pairs
-    nADPairs = size(obj.agents, 1) * 5; % agents x (4 walls + 1 ceiling)
+    nADPairs = nAgents * 6; % agents x (4 walls + 1 floor + 1 ceiling)
-    nLNAPairs = sum(obj.constraintAdjacencyMatrix, 'all') - size(obj.agents, 1);
+    nLNAPairs = sum(obj.constraintAdjacencyMatrix, "all") - nAgents;
     total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
     kk = 1;
-    A = zeros(total, 3 * size(obj.agents, 1));
+    A = zeros(total, 3 * nAgents);
     b = zeros(total, 1);
 
     % Set up collision avoidance constraints
-    h = NaN(size(obj.agents, 1));
+    h = NaN(nAgents, nAgents);
-    h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
+    h(logical(eye(nAgents))) = 0; % self value is 0
-    for ii = 1:(size(obj.agents, 1) - 1)
+    for ii = 1:(nAgents - 1)
-        for jj = (ii + 1):size(obj.agents, 1)
+        for jj = (ii + 1):nAgents
-            h(ii, jj) = norm(agents(ii).pos - agents(jj).pos)^2 - (agents(ii).collisionGeometry.radius + agents(jj).collisionGeometry.radius)^2;
+            h(ii, jj) = norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2;
             h(jj, ii) = h(ii, jj);
 
-            A(kk, (3 * ii - 2):(3 * ii)) =  -2 * (agents(ii).pos - agents(jj).pos);
+            A(kk, (3 * ii - 2):(3 * ii)) =  -2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
             A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
-            b(kk) = obj.barrierGain * h(ii, jj)^obj.barrierExponent;
+            % Slack derived from existing params: recovery velocity = max gradient approach velocity.
+            % Correction splits between 2 agents, so |A| = 2*r_sum
+            r_sum_ij = obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius;
+            v_max_ij = max(obj.agents{ii}.initialStepSize, obj.agents{jj}.initialStepSize) / obj.timestep;
+            hMin = -(4 * r_sum_ij * v_max_ij / obj.barrierGain)^(1 / obj.barrierExponent);
+            if norm(A(kk, :)) < 1e-9
+                % Agents are coincident: A-row is zero, so b < 0 would make
+                % 0 ≤ b unsatisfiable. Fall back to b = 0 (no correction possible).
+                b(kk) = 0;
+            else
+                b(kk) = obj.barrierGain * max(hMin, h(ii, jj))^obj.barrierExponent;
+            end
             kk = kk + 1;
         end
     end
 
-    hObs = NaN(size(obj.agents, 1), size(obj.obstacles, 1));
+    hObs = NaN(nAgents, size(obj.obstacles, 1));
     % Set up obstacle avoidance constraints
-    for ii = 1:size(obj.agents, 1)
+    for ii = 1:nAgents
         for jj = 1:size(obj.obstacles, 1)
             % find closest position to agent on/in obstacle
-            cPos = obj.obstacles{jj}.closestToPoint(agents(ii).pos);
+            cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.pos);
 
-            hObs(ii, jj) = dot(agents(ii).pos - cPos, agents(ii).pos - cPos) - agents(ii).collisionGeometry.radius^2;
+            hObs(ii, jj) = dot(obj.agents{ii}.pos - cPos, obj.agents{ii}.pos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
 
-            A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - cPos);
+            A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - cPos);
-            b(kk) = obj.barrierGain * hObs(ii, jj)^obj.barrierExponent;
+            % Floor for single-agent constraint: full correction on one agent, |A| = 2*r_i
+            r_i = obj.agents{ii}.collisionGeometry.radius;
+            v_max_i = obj.agents{ii}.initialStepSize / obj.timestep;
+            hMin = -(2 * r_i * v_max_i / obj.barrierGain)^(1 / obj.barrierExponent);
+            b(kk) = obj.barrierGain * max(hMin, hObs(ii, jj))^obj.barrierExponent;
 
             kk = kk + 1;
         end
@@ -63,58 +83,61 @@ function [obj] = constrainMotion(obj)
     % Set up domain constraints (walls and ceiling only)
     % Floor constraint is implicit with an obstacle corresponding to the
     % minimum allowed altitude, but I included it anyways
-    for ii = 1:size(obj.agents, 1)
+    h_xMin = 0.0; h_xMax = 0.0; h_yMin = 0.0; h_yMax = 0.0; h_zMin = 0.0; h_zMax = 0.0;
+    for ii = 1:nAgents
         % X minimum
-        h_xMin = (agents(ii).pos(1) - obj.domain.minCorner(1)) - agents(ii).collisionGeometry.radius;
+        h_xMin = (obj.agents{ii}.pos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0];
-        b(kk) = obj.barrierGain * h_xMin^obj.barrierExponent;
+        b(kk) = obj.barrierGain * max(0, h_xMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % X maximum
-        h_xMax = (obj.domain.maxCorner(1) - agents(ii).pos(1)) - agents(ii).collisionGeometry.radius;
+        h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.pos(1)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0];
-        b(kk) = obj.barrierGain * h_xMax^obj.barrierExponent;
+        b(kk) = obj.barrierGain * max(0, h_xMax)^obj.barrierExponent;
         kk = kk + 1;
 
         % Y minimum
-        h_yMin = (agents(ii).pos(2) - obj.domain.minCorner(2)) - agents(ii).collisionGeometry.radius;
+        h_yMin = (obj.agents{ii}.pos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0];
-        b(kk) = obj.barrierGain * h_yMin^obj.barrierExponent;
+        b(kk) = obj.barrierGain * max(0, h_yMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % Y maximum
-        h_yMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
+        h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.pos(2)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0];
-        b(kk) = obj.barrierGain * h_yMax^obj.barrierExponent;
+        b(kk) = obj.barrierGain * max(0, h_yMax)^obj.barrierExponent;
         kk = kk + 1;
 
-        % Z minimum
+        % Z minimum — enforce z >= minAlt + radius (not just z >= domain floor + radius)
-        h_zMin = (agents(ii).pos(3) - obj.domain.minCorner(3)) - agents(ii).collisionGeometry.radius;
+        h_zMin = (obj.agents{ii}.pos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1];
-        b(kk) = obj.barrierGain * h_zMin^obj.barrierExponent;
+        b(kk) = obj.barrierGain * max(0, h_zMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % Z maximum
-        h_zMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
+        h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.pos(3)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1];
-        b(kk) = obj.barrierGain * h_zMax^obj.barrierExponent;
+        b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent;
         kk = kk + 1;
     end
 
-    % Save off h function values (ignoring network constraints which may evolve in time)
+    if coder.target('MATLAB')
-    obj.h(:, obj.timestepIndex) = [h(triu(true(size(obj.agents, 1)), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;]; 
+        % Save off h function values (logging only — not needed in compiled mode)
+        obj.h(:, obj.timestepIndex) = [h(triu(true(nAgents), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;];
+    end
 
     % Add communication network constraints
-    hComms = NaN(size(obj.agents, 1));
+    hComms = NaN(nAgents, nAgents);
-    hComms(logical(eye(size(obj.agents, 1)))) = 0;
+    hComms(logical(eye(nAgents))) = 0;
-    for ii = 1:(size(obj.agents, 1) - 1)
+    for ii = 1:(nAgents - 1)
-        for jj = (ii + 1):size(obj.agents, 1)
+        for jj = (ii + 1):nAgents
             if obj.constraintAdjacencyMatrix(ii, jj)
-                hComms(ii, jj) = min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])^2 - norm(agents(ii).pos - agents(jj).pos)^2;
+                hComms(ii, jj) = min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])^2 - norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2;
 
-                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (agents(ii).pos - agents(jj).pos);
+                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
                 A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
-                b(kk) = obj.barrierGain * hComms(ii, jj)^obj.barrierExponent;
+                b(kk) = obj.barrierGain * max(0, hComms(ii, jj))^obj.barrierExponent;
 
                 kk = kk + 1;
             end
@@ -122,22 +145,36 @@ function [obj] = constrainMotion(obj)
     end
 
     % Solve QP program generated earlier
-    vhat = reshape(v', 3 * size(obj.agents, 1), 1);
+    vhat = reshape(v', 3 * nAgents, 1);
-    H = 2 * eye(3 * size(obj.agents, 1));
+    H = 2 * eye(3 * nAgents);
     f = -2 * vhat;
 
     % Update solution based on constraints
-    assert(size(A,2) == size(H,1))
+    if coder.target('MATLAB')
-    assert(size(A,1) == size(b,1))
+        assert(size(A,2) == size(H,1))
-    assert(size(H,1) == length(f))
+        assert(size(A,1) == size(b,1))
-    opt = optimoptions('quadprog', 'Display', 'off');
+        assert(size(H,1) == length(f))
-    [vNew, ~, exitflag, m] = quadprog(sparse(H), double(f), A, b, [],[], [], [], [], opt);
+    end
-    assert(exitflag == 1, sprintf('quadprog failure... %s%s', newline, m.message));
+    opt = optimoptions("quadprog", "Display", "off", "Algorithm", "active-set", "UseCodegenSolver", true);
-    vNew = reshape(vNew, 3, size(obj.agents, 1))';
+    x0 = zeros(size(H, 1), 1);
+    [vNew, ~, exitflag] = quadprog(H, double(f), A, b, [], [], [], [], x0, opt);
+    vNew = reshape(vNew, 3, nAgents)';
 
-    if exitflag <= 0
+    if exitflag < 0
-        warning("QP failed, continuing with unconstrained solution...")
+        % Infeasible or other hard failure: hold all agents at current positions
-        vNew = v;
+        if coder.target('MATLAB')
+            warning("QP infeasible (exitflag=%d), holding positions.", int16(exitflag));
+        else
+            fprintf("[constrainMotion] QP infeasible (exitflag=%d), holding positions\n", int16(exitflag));
+        end
+        vNew = zeros(nAgents, 3);
+    elseif exitflag == 0
+        % Max iterations exceeded: use suboptimal solution already in vNew
+        if coder.target('MATLAB')
+            warning("QP max iterations exceeded, using suboptimal solution.");
+        else
+            fprintf("[constrainMotion] QP max iterations exceeded, using suboptimal solution\n");
+        end
     end
 
     % Update the "next position" that was previously set by unconstrained

@miSim/initialize.m

@@ -1,6 +1,6 @@
 function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
         domain (1, 1) {mustBeGeometry};
         agents (:, 1) cell;
         barrierGain (1, 1) double = 100;
@@ -13,21 +13,27 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         makeVideo (1, 1) logical = true;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     % enable/disable plotting and video writer
     obj.makePlots = makePlots;
     if ~obj.makePlots
         if makeVideo
-            warning("makeVideo set to true, but makePlots set to false. Setting makeVideo to false.");
+            if coder.target('MATLAB')
+                warning("makeVideo set to true, but makePlots set to false. Setting makeVideo to false.");
+            end
             makeVideo = false;
         end
     end
     obj.makeVideo = makeVideo;
 
     % Generate artifact(s) name
-    obj.artifactName = strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'));
+    if coder.target('MATLAB')
+        obj.artifactName = strcat(string(datetime("now"), "yyyy_MM_dd_HH_mm_ss"));
+    else
+        obj.artifactName = ""; % Generate no artifacts from simulation in codegen
+    end
 
     % Define simulation time parameters
     obj.timestep = timestep;
@@ -37,14 +43,24 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     % Define domain
     obj.domain = domain;
 
-    % Add geometries representing obstacles within the domain
+    % Add geometries representing obstacles within the domain, pre-allocating
-    obj.obstacles = obstacles;
+    % one extra slot for the minimum altitude floor obstacle if needed
+    numInputObs = size(obstacles, 1);
+    if minAlt > 0
+        obj.obstacles = repmat({rectangularPrism}, numInputObs + 1, 1);
+    else
+        obj.obstacles = repmat({rectangularPrism}, numInputObs, 1);
+    end
+    for kk = 1:numInputObs
+        obj.obstacles{kk} = obstacles{kk};
+    end
 
     % Add an additional obstacle spanning the domain's footprint to
     % represent the minimum allowable altitude
     if minAlt > 0
-        obj.obstacles{end + 1, 1} = rectangularPrism;
+        minAltObstacle = rectangularPrism;
-        obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
+        minAltObstacle = minAltObstacle.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
+        obj.obstacles{numInputObs + 1} = minAltObstacle;
     end
 
     % Define agents
@@ -56,18 +72,19 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     for ii = 1:size(obj.agents, 1)
         % Agent
         if isempty(char(obj.agents{ii}.label))
-            obj.agents{ii}.label = sprintf("Agent %d", ii);
+            obj.agents{ii}.label = sprintf("Agent %d", int8(ii));
         end
 
         % Collision geometry
         if isempty(char(obj.agents{ii}.collisionGeometry.label))
-            obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", ii);
+            obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii));
         end
     end
 
     % Set CBF parameters
     obj.barrierGain = barrierGain;
     obj.barrierExponent = barrierExponent;
+    obj.minAlt = minAlt;
 
     % Compute adjacency matrix and lesser neighbors
     obj = obj.updateAdjacency();
@@ -76,22 +93,26 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     % Set up times to iterate over
     obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
 
-    % Prepare performance data store (at t = 0, all have 0 performance)
+    if coder.target('MATLAB')
-    obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
+        % Prepare performance data store (at t = 0, all have 0 performance)
+        obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
 
-    % Prepare h function data store
+        % Prepare h function data store
-    obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1));
+        obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1));
+    end
 
     % Create initial partitioning
     obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
 
-    % Initialize variable that will store agent positions for trail plots
+    if coder.target('MATLAB')
-    obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);
+        % Initialize variable that will store agent positions for trail plots
-    obj.posHist(1:size(obj.agents, 1), 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false)), size(obj.agents, 1), 1, 3);
+        obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);
+        obj.posHist(1:size(obj.agents, 1), 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
 
-    % Set up plots showing initialized state
+        % Set up plots showing initialized state
-    obj = obj.plot();
+        obj = obj.plot();
 
-    % Run validations
+        % Run validations
-    obj.validate();
+        obj.validate();
+    end
 end

@miSim/initializeFromCsv.m

@@ -0,0 +1,129 @@
+function obj = initializeFromCsv(obj, csvPath)
+% INITIALIZEFROMCSV  Initialize miSim from an AERPAW scenario CSV file.
+%
+% Reads all guidance parameters, domain geometry, initial UAV positions,
+% and obstacle definitions from the CSV, then builds and initialises the
+% simulation. Ends by calling the standard obj.initialize(...) method.
+%
+% This is the MATLAB-path counterpart to the compiled path that unpacks a
+% flat scenarioParams array in guidance_step.m.  It is only ever called
+% from within a coder.target('MATLAB') guard and is never compiled.
+%
+% Usage (inside guidance_step.m on MATLAB path):
+%   sim = sim.initializeFromCsv('aerpaw/config/scenario.csv');
+%
+% Expected CSV columns (see scenario.csv):
+%   timestep, maxIter, minAlt, discretizationStep, protectedRange,
+%   initialStepSize, barrierGain, barrierExponent,
+%   collisionRadius (per-UAV), comRange (per-UAV),
+%   alphaDist (per-UAV), betaDist (per-UAV),
+%   alphaTilt (per-UAV), betaTilt (per-UAV),
+%   domainMin ("x,y,z"), domainMax ("x,y,z"), objectivePos ("x,y"),
+%   objectiveVar ("v11,v12,v21,v22"), sensorPerformanceMinimum,
+%   initialPositions (flat "x1,y1,z1, x2,y2,z2,..."),
+%   numObstacles, obstacleMin (flat), obstacleMax (flat)
+
+arguments (Input)
+    obj     (1, 1) {mustBeA(obj, 'miSim')};
+    csvPath (1, 1) string;
+end
+arguments (Output)
+    obj (1, 1) {mustBeA(obj, 'miSim')};
+end
+
+% ---- Parse CSV via readScenarioCsv ---------------------------------------
+scenario = obj.readScenarioCsv(csvPath);
+
+TIMESTEP            = scenario.timestep;
+MAX_ITER            = scenario.maxIter;
+MIN_ALT             = scenario.minAlt;
+DISCRETIZATION_STEP = scenario.discretizationStep;
+PROTECTED_RANGE     = scenario.protectedRange;
+INITIAL_STEP_SIZE   = scenario.initialStepSize;
+BARRIER_GAIN        = scenario.barrierGain;
+BARRIER_EXPONENT    = scenario.barrierExponent;
+% Per-UAV parameters (vectors with one element per UAV)
+COLLISION_RADIUS_VEC = scenario.collisionRadius;   % 1×N
+COMMS_RANGE_VEC      = scenario.comRange;           % 1×N
+ALPHA_DIST_VEC       = scenario.alphaDist;          % 1×N
+BETA_DIST_VEC        = scenario.betaDist;           % 1×N
+ALPHA_TILT_VEC       = scenario.alphaTilt;          % 1×N
+BETA_TILT_VEC        = scenario.betaTilt;           % 1×N
+
+DOMAIN_MIN                 = scenario.domainMin;                % 1×3
+DOMAIN_MAX                 = scenario.domainMax;                % 1×3
+OBJECTIVE_GROUND_POS       = scenario.objectivePos;             % 1×2
+OBJECTIVE_VAR              = reshape(scenario.objectiveVar, 2, 2); % 2×2 covariance matrix
+SENSOR_PERFORMANCE_MINIMUM = scenario.sensorPerformanceMinimum; % scalar
+
+% Initial UAV positions: flat vector reshaped to N×3
+flatPos = scenario.initialPositions;          % 1×(3*N)
+assert(mod(numel(flatPos), 3) == 0, ...
+    "initialPositions must have a multiple of 3 values; got %d", numel(flatPos));
+positions  = reshape(flatPos, 3, [])';        % N×3
+numAgents  = size(positions, 1);
+
+% Validate per-UAV parameter lengths match numAgents
+assert(numel(COLLISION_RADIUS_VEC) == numAgents, ...
+    "collisionRadius has %d values but expected %d (one per UAV)", numel(COLLISION_RADIUS_VEC), numAgents);
+assert(numel(COMMS_RANGE_VEC) == numAgents, ...
+    "comRange has %d values but expected %d (one per UAV)", numel(COMMS_RANGE_VEC), numAgents);
+assert(numel(ALPHA_DIST_VEC) == numAgents, ...
+    "alphaDist has %d values but expected %d (one per UAV)", numel(ALPHA_DIST_VEC), numAgents);
+assert(numel(BETA_DIST_VEC) == numAgents, ...
+    "betaDist has %d values but expected %d (one per UAV)", numel(BETA_DIST_VEC), numAgents);
+assert(numel(ALPHA_TILT_VEC) == numAgents, ...
+    "alphaTilt has %d values but expected %d (one per UAV)", numel(ALPHA_TILT_VEC), numAgents);
+assert(numel(BETA_TILT_VEC) == numAgents, ...
+    "betaTilt has %d values but expected %d (one per UAV)", numel(BETA_TILT_VEC), numAgents);
+
+numObstacles = scenario.numObstacles;
+
+% ---- Build domain --------------------------------------------------------
+dom = rectangularPrism;
+dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
+
+% ---- Build sensing objective (MATLAB path: objectiveFunctionWrapper) -----
+dom.objective = sensingObjective;
+objFcn = objectiveFunctionWrapper(OBJECTIVE_GROUND_POS, OBJECTIVE_VAR);
+dom.objective = dom.objective.initialize(objFcn, dom, DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM);
+
+% ---- Initialise agents from scenario positions ---------------------------
+% Each agent gets its own sensor model and collision/comms radii from the
+% per-UAV parameter vectors.
+agentList = cell(numAgents, 1);
+for ii = 1:numAgents
+    pos  = positions(ii, :);
+
+    % Per-UAV sensor model
+    sensor = sigmoidSensor;
+    sensor = sensor.initialize(ALPHA_DIST_VEC(ii), BETA_DIST_VEC(ii), ...
+                               ALPHA_TILT_VEC(ii), BETA_TILT_VEC(ii));
+
+    geom = spherical;
+    geom = geom.initialize(pos, COLLISION_RADIUS_VEC(ii), REGION_TYPE.COLLISION, ...
+                           sprintf("UAV %d Collision", ii));
+    ag = agent;
+    ag = ag.initialize(pos, geom, sensor, COMMS_RANGE_VEC(ii), MAX_ITER, ...
+                       INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
+    agentList{ii} = ag;
+end
+
+% ---- Build obstacles from CSV --------------------------------------------
+obstacleList = cell(numObstacles, 1);
+if numObstacles > 0
+    obsMin = reshape(scenario.obstacleMin, 3, numObstacles)';  % N×3
+    obsMax = reshape(scenario.obstacleMax, 3, numObstacles)';
+    for ii = 1:numObstacles
+        obs = rectangularPrism;
+        obs = obs.initialize([obsMin(ii, :); obsMax(ii, :)], ...
+                             REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii));
+        obstacleList{ii} = obs;
+    end
+end
+
+% ---- Initialise simulation (plots and video disabled) --------------------
+obj = obj.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
+                     MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false);
+
+end

@miSim/lesserNeighbor.m

@@ -1,50 +1,56 @@
 function obj = lesserNeighbor(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     % initialize solution with self-connections only
     constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1)));
 
-    for ii = 1:size(obj.agents, 1)
+    nAgents = size(obj.agents, 1);
+    for ii = 1:nAgents
         % Find lesser neighbors of each agent
         % Lesser neighbors of ii are jj < ii in range of ii
-        lesserNeighbors = [];
+        lnBuf = zeros(1, nAgents);
+        lnCount = 0;
         for jj = 1:(ii - 1)
             if obj.adjacency(ii, jj)
-                lesserNeighbors = [lesserNeighbors, jj];
+                lnCount = lnCount + 1;
+                lnBuf(lnCount) = jj;
             end
         end
-        obj.agents{ii}.lesserNeighbors = lesserNeighbors;
+        obj.agents{ii}.lesserNeighbors = lnBuf(1:lnCount);
 
         % Early exit for isolated agents
-        if isempty(obj.agents{ii}.lesserNeighbors)
+        if lnCount == 0
             continue
         end
 
         % Focus on subgraph defined by lesser neighbors
         subgraphAdjacency = obj.adjacency(obj.agents{ii}.lesserNeighbors, obj.agents{ii}.lesserNeighbors);
 
-        % Find connected components in each agent's subgraph
+        % Find connected components; store only the max global index per
-        % TODO: rewrite this using matlab "conncomp" function?
+        % component (the only value needed below) to avoid a cell array
-        visited = false(size(subgraphAdjacency, 1), 1);
+        visited = false(1, lnCount);
-        components = {};
+        maxInComponent = zeros(1, lnCount);
-        for jj = 1:size(subgraphAdjacency, 1)
+        numComponents = 0;
+        for jj = 1:lnCount
             if ~visited(jj)
                 reachable = bfs(subgraphAdjacency, jj);
                 visited(reachable) = true;
-                components{end+1} = obj.agents{ii}.lesserNeighbors(reachable);
+                numComponents = numComponents + 1;
+                maxInComponent(numComponents) = max(obj.agents{ii}.lesserNeighbors(reachable));
             end
         end
 
         % Connect to the greatest index in each connected component in the
         % lesser neighborhood of this agent
-        for jj = 1:size(components, 2)
+        for jj = 1:numComponents
-            constraintAdjacencyMatrix(ii, max(components{jj})) = true;
+            maxIdx = maxInComponent(jj);
-            constraintAdjacencyMatrix(max(components{jj}), ii) = true;
+            constraintAdjacencyMatrix(ii, maxIdx) = true;
+            constraintAdjacencyMatrix(maxIdx, ii) = true;
         end
     end
     obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix | constraintAdjacencyMatrix';
@@ -53,24 +59,34 @@ end
 function cComp = bfs(subgraphAdjacency, startIdx)
     n = size(subgraphAdjacency, 1);
     visited = false(1, n);
-    queue = startIdx;
+
-    cComp = startIdx;
+    % Pre-allocated queue and component buffer with head/tail pointers
+    % to avoid element deletion and dynamic array growth
+    queue    = zeros(1, n);
+    cCompBuf = zeros(1, n);
+    qHead = 1;
+    qTail = 2;
+    queue(1)    = startIdx;
+    cCompBuf(1) = startIdx;
+    cSize = 1;
     visited(startIdx) = true;
 
-    while ~isempty(queue)
+    while qHead < qTail
-        current = queue(1);
+        current = queue(qHead);
-        queue(1) = [];
+        qHead = qHead + 1;
 
         % Find all neighbors of current node in the subgraph
         neighbors = find(subgraphAdjacency(current, :));
-        
+        for kk = 1:numel(neighbors)
-        for neighbor = neighbors
+            neighbor = neighbors(kk);
             if ~visited(neighbor)
                 visited(neighbor) = true;
-                cComp = [cComp, neighbor];
+                cCompBuf(cSize + 1) = neighbor;
-                queue = [queue, neighbor];
+                cSize = cSize + 1;
+                queue(qTail) = neighbor;
+                qTail = qTail + 1;
             end
         end
     end
-    cComp = sort(cComp);
+    cComp = sort(cCompBuf(1:cSize));
 end

@miSim/miSim.m

@@ -2,23 +2,27 @@ classdef miSim
     % multiagent interconnection simulation
 
     % Simulation parameters
-    properties (SetAccess = private, GetAccess = public)
+    properties (SetAccess = public, GetAccess = public)
         timestep = NaN; % delta time interval for simulation iterations
         timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
         maxIter = NaN; % maximum number of simulation iterations
-        domain = rectangularPrism;
+        domain;
-        objective = sensingObjective;
+        objective;
-        obstacles = cell(0, 1); % geometries that define obstacles within the domain
+        obstacles; % geometries that define obstacles within the domain
-        agents = cell(0, 1); % agents that move within the domain
+        agents; % agents that move within the domain
-        adjacency = NaN; % Adjacency matrix representing communications network graph
+        adjacency = false(0, 0); % Adjacency matrix representing communications network graph
-        constraintAdjacencyMatrix = NaN; % Adjacency matrix representing desired lesser neighbor connections
+        constraintAdjacencyMatrix = false(0, 0); % Adjacency matrix representing desired lesser neighbor connections
-        partitioning = NaN;
+        partitioning = zeros(0, 0);
         perf; % sensor performance timeseries array
         performance = 0; % simulation performance timeseries vector
-        barrierGain = 100; % CBF gain parameter
+        barrierGain = NaN; % CBF gain parameter
-        barrierExponent = 3; % CBF exponent parameter
+        barrierExponent = NaN; % CBF exponent parameter
+        minAlt = 0; % minimum allowable altitude (m)
         artifactName = "";
+        f; % main plotting tiled layout figure
         fPerf; % performance plot figure
+        % Indicies for various plot types in the main tiled layout figure
+        spatialPlotIndices = [6, 4, 3, 2];
     end
 
     properties (Access = private)
@@ -30,7 +34,6 @@ classdef miSim
         % Plot objects
         makePlots = true; % enable/disable simulation plotting (performance implications)
         makeVideo = true; % enable/disable VideoWriter (performance implications)
-        f; % main plotting tiled layout figure
         connectionsPlot; % objects for lines connecting agents in spatial plots
         graphPlot; % objects for abstract network graph plot
         partitionPlot; % objects for partition plot
@@ -40,7 +43,6 @@ classdef miSim
         trailPlot; % objects for agent trail plot
 
         % Indicies for various plot types in the main tiled layout figure
-        spatialPlotIndices = [6, 4, 3, 2];
         objectivePlotIndices = [6, 4];
         networkGraphIndex = 5;
         partitionGraphIndex = 1;
@@ -54,7 +56,17 @@ classdef miSim
     end
 
     methods (Access = public)
+        function obj = miSim()
+            arguments (Output)
+                obj (1, 1) miSim
+            end
+            obj.domain = rectangularPrism;
+            obj.objective = sensingObjective;
+            obj.obstacles = {rectangularPrism};
+            obj.agents = {agent};
+        end
         [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo);
+        [obj] = initializeFromCsv(obj, csvPath);
         [obj] = run(obj);
         [obj] = lesserNeighbor(obj);
         [obj] = constrainMotion(obj);
@@ -67,8 +79,9 @@ classdef miSim
         [obj] = plotTrails(obj);
         [obj] = plotH(obj);
         [obj] = updatePlots(obj);
+        [obj] = teardown(obj);
+        writeInits(obj);
         validate(obj);
-        teardown(obj);
     end
     methods (Access = private)
         [v] = setupVideoWriter(obj);

@miSim/plot.m

@@ -1,9 +1,9 @@
 function obj = plot(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     
     % fast exit when plotting is disabled

@miSim/plotConnections.m

@@ -1,9 +1,9 @@
 function obj = plotConnections(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     % Iterate over lower triangle off-diagonal region of the 
@@ -23,19 +23,17 @@ function obj = plotConnections(obj)
     % Plot the connections
     if isnan(obj.spatialPlotIndices)
         hold(obj.f.CurrentAxes, "on");
-        o = plot3(obj.f.CurrentAxes, X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+        o = plot3(obj.f.CurrentAxes, X, Y, Z, "Color", "g", "LineWidth", 2, "LineStyle", "--");
         hold(obj.f.CurrentAxes, "off");
     else
         hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "on");
-        o = plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+        o = plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), X, Y, Z, "Color", "g", "LineWidth", 2, "LineStyle", "--");
         hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "off");
     end
 
     % Copy to other plots
-    if size(obj.spatialPlotIndices, 2) > 1
+    for ii = 2:size(obj.spatialPlotIndices, 2)
-        for ii = 2:size(obj.spatialPlotIndices, 2)
+        o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
-            o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
-        end 
     end
 
     obj.connectionsPlot = o;

@miSim/plotGraph.m

@@ -1,23 +1,23 @@
 function obj = plotGraph(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     % Form graph from adjacency matrix
-    G = graph(obj.constraintAdjacencyMatrix, 'omitselfloops');
+    G = graph(obj.constraintAdjacencyMatrix, "omitselfloops");
 
     % Plot graph object
     if isnan(obj.networkGraphIndex)
-        hold(obj.f.CurrentAxes, 'on');
+        hold(obj.f.CurrentAxes, "on");
-        o = plot(obj.f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
+        o = plot(obj.f.CurrentAxes, G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
-        hold(obj.f.CurrentAxes, 'off');
+        hold(obj.f.CurrentAxes, "off");
     else
-        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'on');
+        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), "on");
-        o = plot(obj.f.Children(1).Children(obj.networkGraphIndex(1)), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
+        o = plot(obj.f.Children(1).Children(obj.networkGraphIndex(1)), G, "LineStyle", "--", "EdgeColor", "g", "NodeColor", "k", "LineWidth", 2);
-        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'off');
+        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), "off");
         if size(obj.networkGraphIndex, 2) > 1
             for ii = 2:size(ind, 2)
                 o = [o; copyobj(o(1), obj.f.Children(1).Children(obj.networkGraphIndex(ii)))];

@miSim/plotH.m

@@ -1,9 +1,9 @@
 function obj = plotH(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     obj.hf = figure;
@@ -12,7 +12,7 @@ function obj = plotH(obj)
     nexttile(obj.hf.Children(1));
     axes(obj.hf.Children(1).Children(1));
     grid(obj.hf.Children(1).Children(1), "on");
-    xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
+    xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Collision Avoidance");
     hold(obj.hf.Children(1).Children(1), "on");
     obj.caPlot = plot(obj.h(1:(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2), :)');
@@ -22,13 +22,13 @@ function obj = plotH(obj)
             legendStrings = [legendStrings; sprintf("A%d A%d", jj, ii)];
         end
     end
-    legend(obj.hf.Children(1).Children(1), legendStrings, 'Location', 'bestoutside');
+    legend(obj.hf.Children(1).Children(1), legendStrings, "Location", "bestoutside");
     hold(obj.hf.Children(1).Children(2), "off");
 
     nexttile(obj.hf.Children(1));
     axes(obj.hf.Children(1).Children(1));
     grid(obj.hf.Children(1).Children(1), "on");
-    xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(2), "");
+    xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Obstacles");
     hold(obj.hf.Children(1).Children(1), "on");
     obj.obsPlot = plot(obj.h((1 + (size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)):(((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1)), :)');
@@ -38,23 +38,23 @@ function obj = plotH(obj)
             legendStrings = [legendStrings; sprintf("A%d O%d", jj, ii)];
         end
     end
-    legend(obj.hf.Children(1).Children(1), legendStrings, 'Location', 'bestoutside');
+    legend(obj.hf.Children(1).Children(1), legendStrings, "Location", "bestoutside");
     hold(obj.hf.Children(1).Children(2), "off");
 
     nexttile(obj.hf.Children(1));
     axes(obj.hf.Children(1).Children(1));
     grid(obj.hf.Children(1).Children(1), "on");
-    xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
+    xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Domain");
     hold(obj.hf.Children(1).Children(1), "on");
     obj.domPlot = plot(obj.h((1 + (((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1))):size(obj.h, 1), 1:end)');
-    legend(obj.hf.Children(1).Children(1), ["X Min"; "X Max"; "Y Min"; "Y Max"; "Z Min"; "Z Max";], 'Location', 'bestoutside');
+    legend(obj.hf.Children(1).Children(1), ["X Min"; "X Max"; "Y Min"; "Y Max"; "Z Min"; "Z Max";], "Location", "bestoutside");
     hold(obj.hf.Children(1).Children(2), "off");
 
     nexttile(obj.hf.Children(1));
     axes(obj.hf.Children(1).Children(1));
     grid(obj.hf.Children(1).Children(1), "on");
-    xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
+    xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Communications");
     % skipped this for now because it is very complicated
 

@miSim/plotPartitions.m

@@ -1,19 +1,19 @@
 function obj = plotPartitions(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     if isnan(obj.partitionGraphIndex)
-        hold(obj.f.CurrentAxes, 'on');
+        hold(obj.f.CurrentAxes, "on");
         o = imagesc(obj.f.CurrentAxes, obj.partitioning);
-        hold(obj.f.CurrentAxes, 'off');
+        hold(obj.f.CurrentAxes, "off");
     else
-        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
+        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), "on");
         o = imagesc(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), obj.partitioning);
-        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
+        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), "off");
         if size(obj.partitionGraphIndex, 2) > 1
             for ii = 2:size(ind, 2)
                 o = [o, copyobj(o(1), obj.f.Children(1).Children(obj.partitionGraphIndex(ii)))];

@miSim/plotPerformance.m

@@ -1,9 +1,9 @@
 function obj = plotPerformance(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     % fast exit when plotting is disabled
@@ -15,31 +15,31 @@ function obj = plotPerformance(obj)
 
     axes(obj.fPerf);
     title(obj.fPerf.Children(1), "Sensor Performance");
-    xlabel(obj.fPerf.Children(1), 'Time (s)');
+    xlabel(obj.fPerf.Children(1), "Time (s)");
-    ylabel(obj.fPerf.Children(1), 'Sensor Performance');
+    ylabel(obj.fPerf.Children(1), "Sensor Performance");
-    grid(obj.fPerf.Children(1), 'on');
+    grid(obj.fPerf.Children(1), "on");
 
     % Plot current cumulative performance
-    hold(obj.fPerf.Children(1), 'on');
+    hold(obj.fPerf.Children(1), "on");
     o = plot(obj.fPerf.Children(1), obj.perf(end, :));
-    warning('off', 'MATLAB:gui:array:InvalidArrayShape'); % suppress this warning to avoid polluting output
+    warning("off", "MATLAB:gui:array:InvalidArrayShape"); % suppress this warning to avoid polluting output
     o.XData = NaN(1, obj.maxIter); % correct time will be set at runtime
     o.YData = [0, NaN(1, obj.maxIter - 1)];
-    hold(obj.fPerf.Children(1), 'off');
+    hold(obj.fPerf.Children(1), "off");
 
     % Plot current agent performance
     for ii = 1:(size(obj.perf, 1) - 1)
-        hold(obj.fPerf.Children(1), 'on');
+        hold(obj.fPerf.Children(1), "on");
         o = [o; plot(obj.fPerf.Children(1), obj.perf(ii, :))];
         o(end).XData = NaN(1, obj.maxIter); % correct time will be set at runtime
         o(end).YData = [0, NaN(1, obj.maxIter - 1)];
-        hold(obj.fPerf.Children(1), 'off');
+        hold(obj.fPerf.Children(1), "off");
     end
 
     % Add legend
-    agentStrings = string(cellfun(@(x) x.label, obj.agents, 'UniformOutput', false));
+    agentStrings = string(cellfun(@(x) x.label, obj.agents, "UniformOutput", false));
     agentStrings = ["Total"; agentStrings];
-    legend(obj.fPerf.Children(1), agentStrings, 'Location', 'northwest');
+    legend(obj.fPerf.Children(1), agentStrings, "Location", "northwest");
 
     obj.performancePlot = o;
 end

@miSim/plotTrails.m

@@ -1,9 +1,9 @@
 function obj = plotTrails(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')}
+        obj (1, 1) {mustBeA(obj, "miSim")}
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')}
+        obj (1, 1) {mustBeA(obj, "miSim")}
     end
     
     % fast exit when plotting is disabled
@@ -14,13 +14,17 @@ function obj = plotTrails(obj)
     % Plot full range of position history on each spatial plot axes
     o = [];
     for ii = 1:(size(obj.posHist, 1))
-        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), 'on');
+        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "on");
-        o = [o; plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), obj.posHist(ii, 1:obj.maxIter, 1), obj.posHist(ii, 1:obj.maxIter, 2), obj.posHist(ii, 1:obj.maxIter, 3), 'Color', 'k', 'LineWidth', 1)];
+        o = [o; plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), obj.posHist(ii, 1:obj.maxIter, 1), obj.posHist(ii, 1:obj.maxIter, 2), obj.posHist(ii, 1:obj.maxIter, 3), "Color", "k", "LineWidth", 1)];
-        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), 'off');
+        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "off");
     end
 
-    % Copy trails to other figures?
+    % Copy to other plots
-    obj.trailPlot = o;
+    for ii = 2:size(obj.spatialPlotIndices, 2)
+        o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
+    end 
 
     % Add legend?
+
+    obj.trailPlot = o;
 end

@miSim/run.m

@@ -1,26 +1,29 @@
 function [obj] = run(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
-    % Start video writer
+    if coder.target('MATLAB')
-    if obj.makeVideo
+        % Start video writer
-        v = obj.setupVideoWriter();
+        if obj.makeVideo
-        v.open();
+            v = obj.setupVideoWriter();
+            v.open();
+        end
     end
 
     for ii = 1:size(obj.times, 1)
         % Display current sim time
         obj.t = obj.times(ii);
         obj.timestepIndex = ii;
-        fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
+        if coder.target('MATLAB')
+            fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
 
-        % Before moving
+            % Validate current simulation configuration
-        % Validate current simulation configuration
+            obj.validate();
-        obj.validate();
+        end
 
         % Update partitioning before moving (this one is strictly for
         % plotting purposes, the real partitioning is done by the agents)
@@ -39,28 +42,31 @@ function [obj] = run(obj)
         % CBF constraints solved by QP
         obj = constrainMotion(obj);
 
-        % After moving
+        if coder.target('MATLAB')
-        % Update agent position history array
+            % Update agent position history array
-        obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false)), size(obj.agents, 1), 1, 3);
+            obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
 
-        % Update total performance
+            % Update total performance
-        obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))];
+            obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))];
 
-        % Update adjacency matrix
+            % Update adjacency matrix
-        obj = obj.updateAdjacency();
+            obj = obj.updateAdjacency();
 
-        % Update plots
+            % Update plots
-        obj = obj.updatePlots();
+            obj = obj.updatePlots();
 
-        % Write frame in to video
+            % Write frame in to video
-        if obj.makeVideo
+            if obj.makeVideo
-            I = getframe(obj.f);
+                I = getframe(obj.f);
-            v.writeVideo(I);
+                v.writeVideo(I);
+            end
         end
     end
 
-    if obj.makeVideo
+    if coder.target('MATLAB')
-        % Close video file
+        if obj.makeVideo
-        v.close();
+            % Close video file
+            v.close();
+        end
     end
 end

@miSim/setupVideoWriter.m

@@ -1,15 +1,15 @@
 function v = setupVideoWriter(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        v (1, 1) {mustBeA(v, 'VideoWriter')};
+        v (1, 1) {mustBeA(v, "VideoWriter")};
     end
 
     if ispc || ismac
-        v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'MPEG-4');
+        v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, "sandbox", strcat(obj.artifactName, "_miSimHist")), "MPEG-4");
     elseif isunix
-        v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'Motion JPEG AVI');
+        v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, "sandbox", strcat(obj.artifactName, "_miSimHist")), "Motion JPEG AVI");
     end
     
     v.FrameRate = 1 / obj.timestep;

@miSim/teardown.m

@@ -1,8 +1,9 @@
-function teardown(obj)
+function obj = teardown(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     % Close plots
@@ -10,4 +11,20 @@ function teardown(obj)
     close(obj.fPerf);
     close(obj.f);
 
+    % reset parameters
+    obj.timestep = NaN;
+    obj.timestepIndex = NaN;
+    obj.maxIter = NaN;
+    obj.domain = rectangularPrism;
+    obj.objective = sensingObjective;
+    obj.obstacles = cell(0, 1);
+    obj.agents = cell(0, 1);
+    obj.adjacency = NaN;
+    obj.constraintAdjacencyMatrix = NaN;
+    obj.partitioning = NaN;
+    obj.performance = 0;
+    obj.barrierGain = NaN;
+    obj.barrierExponent = NaN;
+    obj.artifactName = "";
+
 end

@miSim/updateAdjacency.m

@@ -1,9 +1,9 @@
 function obj = updateAdjacency(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     % Initialize assuming only self-connections

@miSim/updatePlots.m

@@ -1,9 +1,9 @@
 function [obj] = updatePlots(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
     % Fast exit when plotting is disabled
@@ -40,10 +40,12 @@ function [obj] = updatePlots(obj)
     end
 
     % Update agent trails
-    for ii = 1:size(obj.agents, 1)
+    for jj = 1:size(obj.spatialPlotIndices, 2)
-        obj.trailPlot(ii).XData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 1);
+        for ii = 1:size(obj.agents, 1)
-        obj.trailPlot(ii).YData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 2);
+            obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).XData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 1);
-        obj.trailPlot(ii).ZData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 3);
+            obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).YData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 2);
+            obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).ZData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 3);
+        end
     end
 
     drawnow;

@miSim/validate.m

@@ -1,6 +1,6 @@
 function validate(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
+        obj (1, 1) {mustBeA(obj, "miSim")};
     end
     arguments (Output)
     end
@@ -10,17 +10,19 @@ function validate(obj)
         warning("Network is not connected");
     end
 
-    if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, 'all')
+    if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, "all")
         warning("Eliminated network connections that were necessary");
     end
 
     %% Obstacle Validators
-    AO_collisions = cellfun(@(a) cellfun(@(o) o.contains(a.pos), obj.obstacles), obj.agents, 'UniformOutput', false);
+    % Agent-Obstacle Collision Detection
-    AO_collisions = vertcat(AO_collisions{:});
+    for jj = 1:size(obj.obstacles, 1)
-    if any(AO_collisions)
+        for kk = 1:size(obj.agents, 1)
-        [idx, idy] = find(AO_collisions);
+            P = min(max(obj.agents{kk}.pos, obj.obstacles{jj}.minCorner), obj.obstacles{jj}.maxCorner);
-        for ii = 1:size(idx, 1)
+            d = obj.agents{kk}.pos - P;
-            error("Agent(s) %d colliding with obstacle(s) %d", idx(ii), idy(ii));
+            if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2
+                warning("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, dot(d, d) - obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail
+            end
         end
     end
 

@miSim/writeInits.m

@@ -0,0 +1,33 @@
+function writeInits(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, "miSim")};
+    end
+    arguments (Output)
+    end
+
+    % Collect agent parameters
+    collisionRadii = cellfun(@(x) x.collisionGeometry.radius, obj.agents);
+    alphaDist = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
+    betaDist = cellfun(@(x) x.sensorModel.betaDist, obj.agents);
+    alphaTilt = cellfun(@(x) x.sensorModel.alphaTilt, obj.agents);
+    betaTilt = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
+    comRanges = cellfun(@(x) x.commsGeometry.radius, obj.agents);
+    initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
+    pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false));
+
+
+    % Combine with simulation parameters
+    inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ...
+                    "discretizationStep", obj.domain.objective.discretizationStep, "protectedRange", obj.domain.objective.protectedRange, ...
+                    "sensorPerformanceMinimum", obj.domain.objective.sensorPerformanceMinimum, "initialStepSize", initialStepSize, ...
+                    "barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent, "numObstacles", size(obj.obstacles, 1), ...
+                    "numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, "alphaDist", alphaDist, ...
+                    "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
+                    ... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
+                    "pos", pos);
+
+    % Save all parameters to output file
+    initsFile = strcat(obj.artifactName, "_miSimInits");
+    initsFile = fullfile(matlab.project.rootProject().RootFolder, "sandbox", initsFile);
+    save(initsFile, "-struct", "inits");
+end

@miSim/writeParams.m

@@ -1,25 +0,0 @@
-function writeParams(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-    arguments (Output)
-    end
-
-    % Collect agent parameters
-    collisionRadii = cellfun(@(x) x.collisionGeometry.radius, obj.agents);
-    alphaDist = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
-    betaDist = cellfun(@(x) x.sensorModel.betaDist, obj.agents);
-    alphaTilt = cellfun(@(x) x.sensorModel.alphaTilt, obj.agents);
-    betaTilt = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
-    comRange = cellfun(@(x) x.commsGeometry.radius, obj.agents);
-
-    % Combine with simulation parameters
-    params = struct('timestep', obj.timestep, 'maxIter', obj.maxIter, 'minAlt', obj.obstacles{end}.maxCorner(3), 'discretizationStep', obj.domain.objective.discretizationStep, ...
-                    'collisionRadius', collisionRadii, 'alphaDist', alphaDist, 'betaDist', betaDist, ...
-                    'alphaTilt', alphaTilt, 'betaTilt', betaTilt, 'comRange', comRange);
-
-    % Save all parameters to output file
-    paramsFile = strcat(obj.artifactName, "_miSimParams");
-    paramsFile = fullfile(matlab.project.rootProject().RootFolder, 'sandbox', paramsFile);
-    save(paramsFile, "-struct", "params");
-end

@sensingObjective/initialize.m

@@ -1,21 +1,20 @@
 function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
     arguments (Input)
-        obj (1,1) {mustBeA(obj, 'sensingObjective')};
+        obj (1,1) {mustBeA(obj, "sensingObjective")};
-        objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
+        objectiveFunction (1, 1) {mustBeA(objectiveFunction, "function_handle")};
         domain (1, 1) {mustBeGeometry};
         discretizationStep (1, 1) double = 1;
         protectedRange (1, 1) double = 1;
         sensorPerformanceMinimum (1, 1) double = 1e-6;
     end
     arguments (Output)
-        obj (1,1) {mustBeA(obj, 'sensingObjective')};
+        obj (1,1) {mustBeA(obj, "sensingObjective")};
     end
 
     obj.discretizationStep = discretizationStep;
 
     obj.sensorPerformanceMinimum = sensorPerformanceMinimum;
     
-    obj.groundAlt = domain.minCorner(3);
     obj.protectedRange = protectedRange;
 
     % Extract footprint limits
@@ -31,8 +30,7 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     [obj.X, obj.Y] = meshgrid(xGrid, yGrid);
 
     % Evaluate function over grid points
-    obj.objectiveFunction = objectiveFunction;
+    obj.values = reshape(objectiveFunction(obj.X, obj.Y), size(obj.X));
-    obj.values = reshape(obj.objectiveFunction(obj.X, obj.Y), size(obj.X));
     
     % Normalize
     obj.values = obj.values ./ max(obj.values, [], "all");

@sensingObjective/initializeRandomMvnpdf.m

@@ -1,12 +1,12 @@
 function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protectedRange)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sensingObjective')};
+        obj (1, 1) {mustBeA(obj, "sensingObjective")};
         domain (1, 1) {mustBeGeometry};
         discretizationStep (1, 1) double = 1;
         protectedRange (1, 1) double = 1;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'sensingObjective')};
+        obj (1, 1) {mustBeA(obj, "sensingObjective")};
     end
 
     % Set random objective position
@@ -14,13 +14,12 @@ function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protected
     while domain.distance(mu) < protectedRange
         mu = domain.random();
     end
-    mu = mu(1:2);
 
     % Set random distribution parameters
     sig = [2 + rand * 2, 1; 1, 2 + rand * 2];
 
     % Set up random bivariate normal distribution function
-    objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu, sig);
+    objectiveFunction = objectiveFunctionWrapper(mu(1:2), sig);
 
     % Regular initialization
     obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange);

@sensingObjective/initializeWithValues.m

@@ -0,0 +1,42 @@
+function obj = initializeWithValues(obj, values, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
+    arguments (Input)
+        obj (1,1) {mustBeA(obj, "sensingObjective")};
+        values (:,:) double;
+        domain (1, 1) {mustBeGeometry};
+        discretizationStep (1, 1) double = 1;
+        protectedRange (1, 1) double = 1;
+        sensorPerformanceMinimum (1, 1) double = 1e-6;
+    end
+    arguments (Output)
+        obj (1,1) {mustBeA(obj, "sensingObjective")};
+    end
+
+    obj.discretizationStep = discretizationStep;
+    obj.sensorPerformanceMinimum = sensorPerformanceMinimum;
+    obj.protectedRange = protectedRange;
+
+    % Extract footprint limits
+    xMin = min(domain.footprint(:, 1));
+    xMax = max(domain.footprint(:, 1));
+    yMin = min(domain.footprint(:, 2));
+    yMax = max(domain.footprint(:, 2));
+
+    xGrid = unique([xMin:obj.discretizationStep:xMax, xMax]);
+    yGrid = unique([yMin:obj.discretizationStep:yMax, yMax]);
+
+    % Store grid points
+    [obj.X, obj.Y] = meshgrid(xGrid, yGrid);
+
+    % Use pre-computed values (caller must evaluate on same grid)
+    obj.values = reshape(values, size(obj.X));
+
+    % Normalize
+    obj.values = obj.values ./ max(obj.values, [], "all");
+
+    % Store ground position (peak of objective)
+    idx = obj.values == 1;
+    obj.groundPos = [obj.X(idx), obj.Y(idx)];
+    obj.groundPos = obj.groundPos(1, 1:2);
+
+    assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
+end

@sensingObjective/plot.m

@@ -1,11 +1,11 @@
 function f = plot(obj, ind, f)
     arguments (Input)
-        obj (1,1) {mustBeA(obj, 'sensingObjective')};
+        obj (1,1) {mustBeA(obj, "sensingObjective")};
         ind (1, :) double = NaN;
-        f (1,1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+        f (1,1) {mustBeA(f, "matlab.ui.Figure")} = figure;
     end
     arguments (Output)
-        f (1,1) {mustBeA(f, 'matlab.ui.Figure')};
+        f (1,1) {mustBeA(f, "matlab.ui.Figure")};
     end
 
     % Create axes if they don't already exist
@@ -14,15 +14,15 @@ function f = plot(obj, ind, f)
     % Plot gradient on the "floor" of the domain
     if isnan(ind)
         hold(f.CurrentAxes, "on");
-        o = surf(f.CurrentAxes, obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
+        o = surf(f.CurrentAxes, obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
-        o.HitTest = 'off';
+        o.HitTest = "off";
-        o.PickableParts = 'none';
+        o.PickableParts = "none";
         hold(f.CurrentAxes, "off");
     else
         hold(f.Children(1).Children(ind(1)), "on");
-        o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
+        o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, zeros(size(obj.X)), obj.values ./ max(obj.values, [], "all"), "EdgeColor", "none");
-        o.HitTest = 'off';
+        o.HitTest = "off";
-        o.PickableParts = 'none';
+        o.PickableParts = "none";
         hold(f.Children(1).Children(ind(1)), "off");
     end
 

@sensingObjective/sensingObjective.m

@@ -2,10 +2,8 @@ classdef sensingObjective
     % Sensing objective definition parent class
     properties (SetAccess = private, GetAccess = public)
         label = "";
-        groundAlt = NaN;
         groundPos = [NaN, NaN];
         discretizationStep = NaN;
-        objectiveFunction = @(x, y) NaN; % define objective functions over a grid in this manner
         X = [];
         Y = [];
         values = [];
@@ -15,7 +13,8 @@ classdef sensingObjective
 
     methods (Access = public)
         [obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum);
-        [obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep, protectedRange);
+        [obj] = initializeWithValues(obj, values, domain, discretizationStep, protectedRange, sensorPerformanceMinimum);
+        [obj] = initializeRandomMvnpdf(obj, domain, discretizationStep, protectedRange);
         [f  ] = plot(obj, ind, f);
     end
 end

@sigmoidSensor/distanceMembership.m

@@ -1,6 +1,6 @@
 function x = distanceMembership(obj, d)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+        obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
         d (:, 1) double;
     end
     arguments (Output)

@sigmoidSensor/initialize.m

@@ -1,13 +1,13 @@
 function obj = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
+        obj (1, 1) {mustBeA(obj, "sigmoidSensor")}
         alphaDist (1, 1) double;
         betaDist (1, 1) double;
         alphaTilt (1, 1) double;
         betaTilt (1, 1) double;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
+        obj (1, 1) {mustBeA(obj, "sigmoidSensor")}
     end
     
     obj.alphaDist = alphaDist;

@sigmoidSensor/plotParameters.m

@@ -1,9 +1,9 @@
 function f = plotParameters(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+        obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
     end
     arguments (Output)
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
     end
 
     % Distance and tilt sample points
@@ -24,9 +24,9 @@ function f = plotParameters(obj)
     title("Distance Membership Sigmoid");
     xlabel("Distance (m)");
     ylabel("Membership");
-    hold('on');
+    hold("on");
-    plot(d, d_x, 'LineWidth', 2);
+    plot(d, d_x, "LineWidth", 2);
-    hold('off');
+    hold("off");
     ylim([0, 1]);
 
     % Tilt
@@ -35,8 +35,8 @@ function f = plotParameters(obj)
     title("Tilt Membership Sigmoid");
     xlabel("Tilt (deg)");
     ylabel("Membership");
-    hold('on');
+    hold("on");
-    plot(t, t_x, 'LineWidth', 2);
+    plot(t, t_x, "LineWidth", 2);
-    hold('off');
+    hold("off");
     ylim([0, 1]);
 end

@sigmoidSensor/sensorPerformance.m

@@ -1,6 +1,6 @@
 function value = sensorPerformance(obj, agentPos, targetPos)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+        obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
         agentPos (1, 3) double;
         targetPos (:, 3) double;
     end

@sigmoidSensor/tiltMembership.m

@@ -1,6 +1,6 @@
 function x = tiltMembership(obj, t)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+        obj (1, 1) {mustBeA(obj, "sigmoidSensor")};
         t (:, 1) double; % degrees
     end
     arguments (Output)

aerpaw/MESSAGE_TYPE.m

@@ -0,0 +1,12 @@
+classdef MESSAGE_TYPE < uint8
+    enumeration
+        TARGET  (1)  % Server->Client: target coordinates follow (3 doubles)
+        ACK     (2)  % Client->Server: command received
+        READY   (3)  % Both: ready for next command / mission complete
+        RTL              (4)  % Server->Client: return to launch
+        LAND             (5)  % Server->Client: land now
+        GUIDANCE_TOGGLE  (6)  % Server->Client: toggle guidance mode on/off
+        REQUEST_POSITION (7)  % Server->Client: respond with current ENU position
+        POSITION         (8)  % Client->Server: current ENU position (3 doubles)
+    end
+end

aerpaw/client/uav_runner.py

@@ -0,0 +1,300 @@
+#!/usr/bin/env python3
+"""
+UAV Runner for AERPAW - aerpawlib BasicRunner implementation.
+
+Run via:
+    python -m aerpawlib --script client.uav_runner --conn <conn> --vehicle drone
+
+Or use the auto-detecting wrapper:
+    ./run_uav.sh
+
+Binary Protocol:
+    For each waypoint:
+        Server sends: TARGET (1 byte) + x,y,z (24 bytes as 3 doubles)
+        Client sends: ACK (1 byte)
+        Client (after moving): READY (1 byte)
+    After all waypoints:
+        Server sends: RTL (1 byte) → Client: ACK, return home, READY
+        Server sends: LAND (1 byte) → Client: ACK, land, READY
+        Server sends: READY (1 byte) - mission complete, disconnect
+"""
+from enum import IntEnum
+from pathlib import Path
+import asyncio
+import csv
+import datetime
+import os
+import platform
+import struct
+import time
+import yaml
+
+from aerpawlib.runner import BasicRunner, entrypoint
+from aerpawlib.util import Coordinate, VectorNED
+from aerpawlib.vehicle import Drone
+
+
+# Message types - must match MESSAGE_TYPE.m enum
+class MessageType(IntEnum):
+    TARGET           = 1
+    ACK              = 2
+    READY            = 3
+    RTL              = 4
+    LAND             = 5
+    GUIDANCE_TOGGLE  = 6
+    REQUEST_POSITION = 7
+    POSITION         = 8
+
+
+AERPAW_DIR = Path('/root/miSim/aerpaw')
+CONFIG_FILE = Path(os.environ.get('AERPAW_CLIENT_CONFIG',
+                                  AERPAW_DIR / "config" / "client.yaml"))
+
+
+def load_config():
+    """Load configuration from YAML file."""
+    with open(CONFIG_FILE, 'r') as f:
+        return yaml.safe_load(f)
+
+
+def get_environment():
+    """Get environment from AERPAW_ENV variable. Fails if not set."""
+    env = os.environ.get('AERPAW_ENV')
+    if env is None:
+        raise RuntimeError(
+            "AERPAW_ENV environment variable not set. "
+            "Set to 'local' or 'testbed', or use: ./run_uav.sh [local|testbed]"
+        )
+    if env not in ('local', 'testbed'):
+        raise RuntimeError(
+            f"Invalid AERPAW_ENV '{env}'. Must be 'local' or 'testbed'."
+        )
+    return env
+
+
+async def recv_exactly(reader: asyncio.StreamReader, n: int) -> bytes:
+    """Receive exactly n bytes from async stream."""
+    data = await reader.readexactly(n)
+    return data
+
+
+async def recv_message_type(reader: asyncio.StreamReader) -> MessageType:
+    """Receive a single-byte message type."""
+    data = await recv_exactly(reader, 1)
+    return MessageType(data[0])
+
+
+async def send_message_type(writer: asyncio.StreamWriter, msg_type: MessageType):
+    """Send a single-byte message type."""
+    writer.write(bytes([msg_type]))
+    await writer.drain()
+
+
+def _gps_log_row(vehicle, line_num, writer):
+    """Sample vehicle state and write one CSV row (matches gps_logger.py format)."""
+    pos = vehicle.position
+    lat, lon = pos.lat, pos.lon
+    alt = round(float(str(pos.alt)), 3)
+
+    batt = str(vehicle.battery)
+    volt = float(batt[16:batt.find(",")])
+
+    timestamp = datetime.datetime.now()
+    fix, num_sat = vehicle.gps.fix_type, vehicle.gps.satellites_visible
+
+    if fix < 2:
+        lat, lon, alt = -999, -999, -999
+
+    vel = vehicle.velocity
+    attitude = vehicle.attitude
+    attitude_str = (
+        "(" + ",".join(map(str, [attitude.pitch, attitude.yaw, attitude.roll])) + ")"
+    )
+
+    writer.writerow([line_num, lon, lat, alt, attitude_str, vel, volt, timestamp, fix, num_sat])
+
+
+async def _gps_log_loop(drone):
+    """Background async task that logs GPS data at 1Hz."""
+    results_dir = os.environ.get('RESULTS_DIR', '/root/Results')
+    log_prefix = os.environ.get('LOG_PREFIX', datetime.datetime.now().strftime('%Y-%m-%d_%H_%M_%S'))
+    filename = os.path.join(results_dir, f"{log_prefix}_gps_log.csv")
+    print(f"[UAV] GPS logging to {filename}")
+    line_num = 1
+    try:
+        with open(filename, "w+") as f:
+            writer = csv.writer(f)
+            while True:
+                if drone.connected:
+                    _gps_log_row(drone, line_num, writer)
+                    f.flush()
+                    os.fsync(f)
+                    line_num += 1
+                else:
+                    print("[UAV] [GPS] No vehicle heartbeat")
+                await asyncio.sleep(1.0)
+    except asyncio.CancelledError:
+        print(f"[UAV] GPS logging stopped ({line_num - 1} rows written)")
+    except Exception as e:
+        print(f"[UAV] GPS logging error: {e}")
+
+
+class UAVRunner(BasicRunner):
+    def initialize_args(self, extra_args):
+        """Load configuration from YAML config file."""
+        config = load_config()
+        env = get_environment()
+        print(f"[UAV] Environment: {env}")
+
+        # Load origin
+        origin = config['origin']
+        self.origin = Coordinate(origin['lat'], origin['lon'], origin['alt'])
+        print(f"[UAV] Origin: {origin['lat']}, {origin['lon']}, {origin['alt']}")
+
+        # Load controller address for this environment
+        env_config = config['environments'][env]
+        self.server_ip = env_config['controller']['ip']
+        self.server_port = env_config['controller']['port']
+        print(f"[UAV] Controller: {self.server_ip}:{self.server_port}")
+
+    @entrypoint
+    async def run_mission(self, drone: Drone):
+        """Main mission entry point."""
+        print(f"[UAV] Connecting to controller at {self.server_ip}:{self.server_port}")
+
+        # Retry connection up to 10 times (~30 seconds total)
+        reader, writer = None, None
+        for attempt in range(10):
+            try:
+                reader, writer = await asyncio.wait_for(
+                    asyncio.open_connection(self.server_ip, self.server_port),
+                    timeout=5,
+                )
+                print(f"[UAV] Connected to controller")
+                break
+            except (ConnectionRefusedError, asyncio.TimeoutError, OSError) as e:
+                print(f"[UAV] Connection attempt {attempt + 1}/10 failed: {e}")
+                if attempt < 9:
+                    await asyncio.sleep(3)
+
+        if reader is None:
+            print("[UAV] Failed to connect to controller after 10 attempts")
+            return
+
+        log_task = None
+        nav_task = None
+        try:
+            # Takeoff to above AERPAW minimum altitude
+            print("[UAV] Taking off...")
+            await drone.takeoff(25)
+            print("[UAV] Takeoff complete, waiting for commands...")
+
+            # Start GPS logging in background
+            log_task = asyncio.create_task(_gps_log_loop(drone))
+
+            # Command loop - handle all messages from controller
+            waypoint_num = 0
+            in_guidance = False
+            while True:
+                msg_type = await recv_message_type(reader)
+                print(f"[UAV] Received: {msg_type.name}")
+
+                if msg_type == MessageType.GUIDANCE_TOGGLE:
+                    in_guidance = not in_guidance
+                    print(f"[UAV] Guidance mode: {'ON' if in_guidance else 'OFF'}")
+                    if not in_guidance:
+                        # Exiting guidance: wait for current navigation to finish
+                        # before resuming sequential (ACK/READY) mode
+                        if nav_task and not nav_task.done():
+                            print("[UAV] Waiting for current navigation to complete...")
+                            await nav_task
+                            nav_task = None
+                        # Acknowledge that we are ready for sequential commands
+                        await send_message_type(writer, MessageType.ACK)
+                        print("[UAV] Sent ACK (guidance mode exited, ready for sequential commands)")
+
+                elif msg_type == MessageType.REQUEST_POSITION:
+                    # Respond immediately with current ENU position relative to origin
+                    pos = drone.position
+                    enu = pos - self.origin  # VectorNED(north, east, down)
+                    await send_message_type(writer, MessageType.POSITION)
+                    writer.write(struct.pack('<ddd', enu.east, enu.north, -enu.down))
+                    await writer.drain()
+                    print(f"[UAV] Sent POSITION: E={enu.east:.1f} N={enu.north:.1f} U={-enu.down:.1f}")
+
+                elif msg_type == MessageType.TARGET:
+                    # Read 24 bytes of coordinates (3 little-endian doubles)
+                    data = await recv_exactly(reader, 24)
+                    enu_x, enu_y, enu_z = struct.unpack('<ddd', data)
+
+                    # Convert ENU to lat/lon (ENU: x=East, y=North, z=Up)
+                    target = self.origin + VectorNED(north=enu_y, east=enu_x, down=-enu_z)
+
+                    if in_guidance:
+                        # 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)
+                        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
+                        print(f"[UAV] TARGET (waypoint {waypoint_num}): x={enu_x:.1f}, y={enu_y:.1f}, z={enu_z:.1f}")
+                        print(f"[UAV] Target coord: {target.lat:.6f}, {target.lon:.6f}, {target.alt:.1f}")
+                        await send_message_type(writer, MessageType.ACK)
+                        print("[UAV] Sent ACK")
+                        print(f"[UAV] Moving to waypoint {waypoint_num}...")
+                        await drone.goto_coordinates(target)
+                        print(f"[UAV] Arrived at waypoint {waypoint_num}")
+                        await send_message_type(writer, MessageType.READY)
+                        print("[UAV] Sent READY")
+
+                elif msg_type == MessageType.RTL:
+                    await send_message_type(writer, MessageType.ACK)
+                    print(f"[UAV] Sent ACK")
+                    print("[UAV] Returning to home...")
+                    home = drone.home_coords
+                    safe_alt = 25
+                    rtl_target = Coordinate(home.lat, home.lon, safe_alt)
+                    print(f"[UAV] RTL to {home.lat:.6f}, {home.lon:.6f} at {safe_alt:.1f}m")
+                    await drone.goto_coordinates(rtl_target)
+                    print("[UAV] Arrived at home position")
+                    await send_message_type(writer, MessageType.READY)
+                    print(f"[UAV] Sent READY")
+
+                elif msg_type == MessageType.LAND:
+                    await send_message_type(writer, MessageType.ACK)
+                    print(f"[UAV] Sent ACK")
+                    print("[UAV] Landing...")
+                    await drone.land()
+                    print("[UAV] Landed and disarmed")
+                    await send_message_type(writer, MessageType.READY)
+                    print(f"[UAV] Sent READY")
+
+                elif msg_type == MessageType.READY:
+                    print("[UAV] Mission complete")
+                    break
+
+                else:
+                    print(f"[UAV] Unknown command: {msg_type}")
+
+        except (ValueError, asyncio.IncompleteReadError, ConnectionError) as e:
+            print(f"[UAV] Error: {e}")
+
+        finally:
+            if nav_task is not None and not nav_task.done():
+                nav_task.cancel()
+                await asyncio.gather(nav_task, return_exceptions=True)
+            if log_task is not None:
+                log_task.cancel()
+                await asyncio.gather(log_task, return_exceptions=True)
+            writer.close()
+            await writer.wait_closed()
+            print("[UAV] Connection closed")

aerpaw/compile.sh

@@ -0,0 +1,24 @@
+#!/bin/bash
+# Build controller_app from codegen + impl sources
+
+AERPAW_DIR="$(cd "$(dirname "$0")" && pwd)"
+CODEGEN="$AERPAW_DIR/codegen"
+IMPL="$AERPAW_DIR/impl"
+BUILD="$AERPAW_DIR/build"
+
+mkdir -p "$BUILD"
+
+echo "Compiling controller executable..."
+
+# Compile all codegen sources (handles any new generated files)
+g++ -I/home/kdee/matlab/R2025a/extern/include \
+    -I"$CODEGEN" \
+    -I"$IMPL" \
+    "$IMPL/controller_main.cpp" \
+    "$IMPL/controller_impl.cpp" \
+    "$CODEGEN"/*.cpp \
+    -o "$BUILD/controller_app" \
+    -fopenmp \
+    -lpthread
+
+echo "Built: $BUILD/controller_app"

aerpaw/config/client1.yaml

@@ -0,0 +1,38 @@
+# AERPAW UAV (Client) Configuration
+
+# Unique 0-indexed UAV identifier (each UAV must have a distinct value)
+uav_id: 0
+
+# TDM (Time-Division Multiplexing) radio settings
+# All UAVs share a common frequency; each transmits only during its time slot.
+# Slot assignment uses wall-clock modular arithmetic (GPS/NTP synced).
+tdm:
+  slot_duration: 0.5      # seconds per slot
+  guard_interval: 0.05    # seconds of silence at slot boundaries
+
+# ENU coordinate system origin (AERPAW Lake Wheeler Road Field)
+origin:
+  lat: 35.72550610629396
+  lon: -78.70019657805574
+  alt: 0.0  # Alt=0 means ENU z directly becomes target altitude above home
+# Environment-specific settings
+environments:
+  local:
+    # MAVLink connection for SITL simulation (UDP)
+    mavlink:
+      ip: "127.0.0.1"
+      port: 14550
+    # Controller server address
+    controller:
+      ip: "127.0.0.1"
+      port: 5000
+
+  testbed:
+    # AERPAW testbed: E-VM listens, MAVLink Filter connects TO us (UDP)
+    mavlink:
+      ip: "192.168.32.26"
+      port: 14550
+    # Controller runs on host machine (192.168.122.1 from E-VM perspective)
+    controller:
+      ip: "192.168.122.1"
+      port: 5000

aerpaw/config/client2.yaml

@@ -0,0 +1,38 @@
+# AERPAW UAV (Client) Configuration
+
+# Unique 0-indexed UAV identifier (each UAV must have a distinct value)
+uav_id: 1
+
+# TDM (Time-Division Multiplexing) radio settings
+# All UAVs share a common frequency; each transmits only during its time slot.
+# Slot assignment uses wall-clock modular arithmetic (GPS/NTP synced).
+tdm:
+  slot_duration: 0.5      # seconds per slot
+  guard_interval: 0.05    # seconds of silence at slot boundaries
+
+# ENU coordinate system origin (AERPAW Lake Wheeler Road Field)
+origin:
+  lat: 35.72550610629396
+  lon: -78.70019657805574
+  alt: 0.0  # Alt=0 means ENU z directly becomes target altitude above home
+# Environment-specific settings
+environments:
+  local:
+    # MAVLink connection for SITL simulation (UDP)
+    mavlink:
+      ip: "127.0.0.1"
+      port: 14550
+    # Controller server address
+    controller:
+      ip: "127.0.0.1"
+      port: 5000
+
+  testbed:
+    # AERPAW testbed: E-VM listens, MAVLink Filter connects TO us (UDP)
+    mavlink:
+      ip: "192.168.32.26"
+      port: 14550
+    # Controller runs on host machine (192.168.122.1 from E-VM perspective)
+    controller:
+      ip: "192.168.122.1"
+      port: 5000

aerpaw/config/scenario.csv

@@ -0,0 +1,2 @@
+timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax
+5, 120, 30.0, 0.1, 1.0, 2.0, 100, 3, "3.0, 3.0", "30.0, 30.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "50.0, 50.0, 80.0", "35.0, 35.0", "10, 5, 5, 10", 0.15, "5.0, 10.0, 45.0, 15.0, 10.0, 35.0", 1, "2.0, 15.0, 0.0", "25.0, 25.0, 50.0"

aerpaw/controller.m

@@ -0,0 +1,236 @@
+function controller(numClients)
+arguments (Input)
+    numClients (1, 1) int32;
+end
+
+coder.extrinsic('disp', 'readScenarioCsv');
+
+% Maximum clients supported (one initial position per UAV)
+MAX_CLIENTS = 4;
+MAX_TARGETS = MAX_CLIENTS;
+
+% Allocate targets array (MAX_TARGETS x 3)
+targets = zeros(MAX_TARGETS, 3);
+numWaypoints = int32(0);
+totalLoaded  = int32(0);  % pre-declare type for coder.ceval %#ok<NASGU>
+
+% Load initial UAV positions from scenario CSV
+if coder.target('MATLAB')
+    disp('Loading initial positions from scenario.csv (simulation)...');
+    tmpSim = miSim;
+    sc = tmpSim.readScenarioCsv('aerpaw/config/scenario.csv');
+    flatPos = double(sc.initialPositions);  % 1×(3*N) flat vector
+    posMatrix = reshape(flatPos, 3, [])';   % N×3, same layout as initializeFromCsv
+    totalLoaded = int32(size(posMatrix, 1));
+    targets(1:totalLoaded, :) = posMatrix;
+    numWaypoints = int32(1);
+    disp(['Loaded ', num2str(double(totalLoaded)), ' initial positions']);
+else
+    coder.cinclude('controller_impl.h');
+    filename = ['config/scenario.csv', char(0)];
+    totalLoaded = coder.ceval('loadInitialPositions', coder.ref(filename), ...
+                coder.ref(targets), int32(MAX_TARGETS));
+    numWaypoints = totalLoaded / int32(numClients);
+end
+
+% Load guidance scenario from CSV (parameters for guidance_step)
+NUM_SCENARIO_PARAMS = 45;
+MAX_OBSTACLES_CTRL  = int32(8);
+scenarioParams = zeros(1, NUM_SCENARIO_PARAMS);
+obstacleMin    = zeros(MAX_OBSTACLES_CTRL, 3);
+obstacleMax    = zeros(MAX_OBSTACLES_CTRL, 3);
+numObstacles   = int32(0);
+if ~coder.target('MATLAB')
+    coder.cinclude('controller_impl.h');
+    scenarioFilename = ['config/scenario.csv', char(0)];
+    coder.ceval('loadScenario', coder.ref(scenarioFilename), coder.ref(scenarioParams));
+    numObstacles = coder.ceval('loadObstacles', coder.ref(scenarioFilename), ...
+                         coder.ref(obstacleMin), coder.ref(obstacleMax), ...
+                         int32(MAX_OBSTACLES_CTRL));
+end
+% On MATLAB path, scenarioParams and obstacle arrays are left as zeros.
+% guidance_step's MATLAB path loads parameters directly from scenario.csv
+% via sim.initializeFromCsv and does not use these arrays.
+
+% Initialize server
+if coder.target('MATLAB')
+    disp('Initializing server (simulation)...');
+else
+    coder.ceval('initServer');
+end
+
+% Accept clients
+for i = 1:numClients
+    if coder.target('MATLAB')
+        disp(['Accepting client ', num2str(i)]);
+    else
+        coder.ceval('acceptClient', int32(i));
+    end
+end
+
+% Waypoint loop: send each waypoint to all clients, wait for all to arrive
+for w = 1:numWaypoints
+    % Send TARGET for waypoint w to each client
+    for i = 1:numClients
+        % Targets are grouped by client: client i's waypoints are at rows
+        % (i-1)*numWaypoints+1 through i*numWaypoints
+        targetIdx = (i - 1) * numWaypoints + w;
+        target = targets(targetIdx, :);
+
+        if coder.target('MATLAB')
+            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));
+        end
+    end
+
+    % Wait for ACK from all clients
+    if coder.target('MATLAB')
+        disp('Waiting for ACK from all clients...');
+    else
+        coder.ceval('waitForAllMessageType', int32(numClients), ...
+                    int32(MESSAGE_TYPE.ACK));
+    end
+
+    % Wait for READY from all clients (all arrived at waypoint w)
+    if coder.target('MATLAB')
+        disp(['All UAVs arrived at waypoint ', num2str(w)]);
+    else
+        coder.ceval('waitForAllMessageType', int32(numClients), ...
+                    int32(MESSAGE_TYPE.READY));
+    end
+end
+
+% ---- Phase 2: miSim guidance loop ----------------------------------------
+% Guidance parameters (adjust here and recompile as needed)
+MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations
+
+% Enter guidance mode on all clients
+if ~coder.target('MATLAB')
+    coder.ceval('sendGuidanceToggle', int32(numClients));
+end
+
+% Request initial GPS positions and initialise guidance algorithm
+positions = zeros(MAX_CLIENTS, 3);
+if ~coder.target('MATLAB')
+    coder.ceval('sendRequestPositions', int32(numClients));
+    coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS));
+else
+    % Simulation: seed positions from CSV waypoints so agents don't start at origin
+    positions(1:totalLoaded, :) = targets(1:totalLoaded, :);
+end
+guidance_step(positions(1:numClients, :), true, ...
+              scenarioParams, obstacleMin, obstacleMax, numObstacles);
+
+% Main guidance loop (event-triggered)
+for step = 1:MAX_GUIDANCE_STEPS
+    % 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
+    for i = 1:numClients
+        target = nextPositions(i, :);
+        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)]);
+        end
+    end
+
+    % 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
+
+    % Request 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));
+    else
+        % Simulation: advance positions to guidance outputs for closed-loop feedback
+        positions(1:numClients, :) = nextPositions(1:numClients, :);
+    end
+end
+
+% Exit guidance mode on all clients (second toggle)
+if ~coder.target('MATLAB')
+    coder.ceval('sendGuidanceToggle', int32(numClients));
+    % Wait for ACK from all clients: confirms each client has finished its
+    % last guidance navigation and is back in sequential (ACK/READY) mode.
+    coder.ceval('waitForAllMessageType', int32(numClients), ...
+                int32(MESSAGE_TYPE.ACK));
+end
+% --------------------------------------------------------------------------
+
+% Send RTL command to all clients
+for i = 1:numClients
+    if coder.target('MATLAB')
+        disp(['Sending RTL to client ', num2str(i)]);
+    else
+        coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.RTL));
+    end
+end
+
+% Wait for ACK from all clients
+if coder.target('MATLAB')
+    disp('Waiting for ACK from all clients...');
+else
+    coder.ceval('waitForAllMessageType', int32(numClients), ...
+                int32(MESSAGE_TYPE.ACK));
+end
+
+% Wait for READY from all clients (returned to home)
+if coder.target('MATLAB')
+    disp('All UAVs returned to home.');
+else
+    coder.ceval('waitForAllMessageType', int32(numClients), ...
+                int32(MESSAGE_TYPE.READY));
+end
+
+% Send LAND command to all clients
+for i = 1:numClients
+    if coder.target('MATLAB')
+        disp(['Sending LAND to client ', num2str(i)]);
+    else
+        coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.LAND));
+    end
+end
+
+% Wait for ACK from all clients
+if coder.target('MATLAB')
+    disp('Waiting for ACK from all clients...');
+else
+    coder.ceval('waitForAllMessageType', int32(numClients), ...
+                int32(MESSAGE_TYPE.ACK));
+end
+
+% Wait for READY from all clients (landed and disarmed)
+if coder.target('MATLAB')
+    disp('All UAVs landed and disarmed.');
+else
+    coder.ceval('waitForAllMessageType', int32(numClients), ...
+                int32(MESSAGE_TYPE.READY));
+end
+
+% Send READY to all clients to signal mission complete
+for i = 1:numClients
+    if coder.target('MATLAB')
+        disp(['Sending READY to client ', num2str(i)]);
+    else
+        coder.ceval('sendMessageType', int32(i), int32(MESSAGE_TYPE.READY));
+    end
+end
+
+% Close server
+if ~coder.target('MATLAB')
+    coder.ceval('closeServer');
+end
+
+disp('Experiment complete.');
+
+end

aerpaw/guidance_step.m

@@ -0,0 +1,200 @@
+function nextPositions = guidance_step(currentPositions, isInit, ...
+                                       scenarioParams, ...
+                                       obstacleMin, obstacleMax, numObstacles)
+% guidance_step  One step of the miSim sensing coverage guidance algorithm.
+%
+% Wraps the miSim gradient-ascent + CBF motion algorithm for AERPAW.
+% Holds full simulation state in a persistent variable between calls.
+%
+% Usage (from controller.m):
+%   guidance_step(initPositions, true,  scenarioParams, obstacleMin, obstacleMax, numObstacles)
+%   nextPos = guidance_step(gpsPos, false, scenarioParams, obstacleMin, obstacleMax, numObstacles)
+%
+% Inputs:
+%   currentPositions  (MAX_CLIENTS × 3) double  ENU [east north up] metres
+%   isInit            (1,1)             logical  true on first call only
+%   scenarioParams    (1 × NUM_SCENARIO_PARAMS) double
+%                       Flat array of guidance parameters (compiled path).
+%                       On MATLAB path this is ignored; parameters are loaded
+%                       from scenario.csv via initializeFromCsv instead.
+%                       Index mapping (1-based):
+%                         1  timestep            9-12  collisionRadius[1:4]
+%                         2  maxIter            13-16  comRange[1:4]
+%                         3  minAlt             17-20  alphaDist[1:4]
+%                         4  discretizationStep 21-24  betaDist[1:4]
+%                         5  protectedRange     25-28  alphaTilt[1:4]
+%                         6  initialStepSize    29-32  betaTilt[1:4]
+%                         7  barrierGain        33-35  domainMin
+%                         8  barrierExponent    36-38  domainMax
+%                                               39-40  objectivePos
+%                                               41-44  objectiveVar (2x2, col-major)
+%                                               45     sensorPerformanceMinimum
+%   obstacleMin       (MAX_OBSTACLES × 3) double  column-major obstacle corners (compiled path)
+%   obstacleMax       (MAX_OBSTACLES × 3) double
+%   numObstacles      (1,1) int32                 actual obstacle count
+%
+% Output:
+%   nextPositions     (MAX_CLIENTS × 3) double  guidance targets, ENU metres
+%
+% Codegen notes:
+%   - Persistent variable 'sim' holds the miSim object between calls.
+%   - On MATLAB path isInit uses sim.initializeFromCsv (file I/O, not compiled).
+%   - On compiled path isInit uses scenarioParams + obstacle arrays directly.
+%   - Plotting/video are disabled (makePlots=false, makeVideo=false).
+
+coder.extrinsic('disp', 'objectiveFunctionWrapper', 'initializeFromCsv');
+
+MAX_CLIENTS = int32(4);      % must match MAX_CLIENTS in controller.m
+
+% Path to scenario CSV — used on MATLAB path only (not compiled)
+SCENARIO_CSV = 'aerpaw/config/scenario.csv';
+
+persistent sim;
+if isempty(sim)
+    sim = miSim;
+end
+
+% Pre-allocate output with known static size (required for codegen)
+nextPositions = zeros(MAX_CLIENTS, 3);
+
+numAgents = int32(size(currentPositions, 1));
+
+if isInit
+    if coder.target('MATLAB')
+        disp('[guidance_step] Initialising simulation...');
+
+        % MATLAB path: load all parameters and obstacles from scenario CSV.
+        % initializeFromCsv reads the file, builds domain/agents/obstacles,
+        % and calls sim.initialize internally.
+        sim = sim.initializeFromCsv(SCENARIO_CSV);
+
+        disp('[guidance_step] Initialisation complete.');
+    else
+        % ================================================================
+        % Compiled path: unpack scenarioParams array and obstacle arrays.
+        % Per-UAV parameters are stored as MAX_CLIENTS-wide blocks; only
+        % the first numAgents entries of each block are used.
+        % ================================================================
+        TIMESTEP            = scenarioParams(1);
+        MAX_ITER            = int32(scenarioParams(2));
+        MIN_ALT             = scenarioParams(3);
+        DISCRETIZATION_STEP = scenarioParams(4);
+        PROTECTED_RANGE     = scenarioParams(5);
+        INITIAL_STEP_SIZE   = scenarioParams(6);
+        BARRIER_GAIN        = scenarioParams(7);
+        BARRIER_EXPONENT    = scenarioParams(8);
+        COLLISION_RADIUS_VEC = scenarioParams(9:12);   % per-UAV [1:MAX_CLIENTS]
+        COMMS_RANGE_VEC      = scenarioParams(13:16);
+        ALPHA_DIST_VEC       = scenarioParams(17:20);
+        BETA_DIST_VEC        = scenarioParams(21:24);
+        ALPHA_TILT_VEC       = scenarioParams(25:28);
+        BETA_TILT_VEC        = scenarioParams(29:32);
+        DOMAIN_MIN                  = scenarioParams(33:35);
+        DOMAIN_MAX                  = scenarioParams(36:38);
+        OBJECTIVE_GROUND_POS        = scenarioParams(39:40);
+        OBJECTIVE_VAR               = reshape(scenarioParams(41:44), 2, 2);
+        SENSOR_PERFORMANCE_MINIMUM  = scenarioParams(45);
+
+        % --- Build domain geometry ---
+        dom = rectangularPrism;
+        dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
+
+        % --- Build sensing objective (inline Gaussian; codegen-compatible) ---
+        dom.objective = sensingObjective;
+        xGrid = unique([DOMAIN_MIN(1):DISCRETIZATION_STEP:DOMAIN_MAX(1), DOMAIN_MAX(1)]);
+        yGrid = unique([DOMAIN_MIN(2):DISCRETIZATION_STEP:DOMAIN_MAX(2), DOMAIN_MAX(2)]);
+        [gridX, gridY] = meshgrid(xGrid, yGrid);
+        dx = gridX - OBJECTIVE_GROUND_POS(1);
+        dy = gridY - OBJECTIVE_GROUND_POS(2);
+        % Bivariate Gaussian using objectiveVar covariance matrix (avoids inv())
+        ov_a = OBJECTIVE_VAR(1,1); ov_b = OBJECTIVE_VAR(1,2);
+        ov_c = OBJECTIVE_VAR(2,1); ov_d = OBJECTIVE_VAR(2,2);
+        ov_det = ov_a * ov_d - ov_b * ov_c;
+        objValues = exp((-0.5 / ov_det) .* (ov_d .* dx.*dx - (ov_b + ov_c) .* dx.*dy + ov_a .* dy.*dy));
+        dom.objective = dom.objective.initializeWithValues(objValues, dom, ...
+            DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM);
+
+        % --- Initialise agents from GPS positions (per-UAV parameters) ----
+        agentList = cell(numAgents, 1);
+        for ii = 1:numAgents
+            pos  = currentPositions(ii, :);
+
+            % Per-UAV sensor model
+            sensor = sigmoidSensor;
+            sensor = sensor.initialize(ALPHA_DIST_VEC(ii), BETA_DIST_VEC(ii), ...
+                                       ALPHA_TILT_VEC(ii), BETA_TILT_VEC(ii));
+
+            geom = spherical;
+            geom = geom.initialize(pos, COLLISION_RADIUS_VEC(ii), REGION_TYPE.COLLISION, ...
+                                   sprintf("UAV %d Collision", ii));
+            ag = agent;
+            ag = ag.initialize(pos, geom, sensor, COMMS_RANGE_VEC(ii), MAX_ITER, ...
+                               INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
+            agentList{ii} = ag;
+        end
+
+        % --- Build obstacle list from flat arrays ---
+        coder.varsize('obstacleList', [8, 1], [1, 0]);
+        obstacleList = repmat({rectangularPrism}, numObstacles, 1);
+        for ii = 1:numObstacles
+            obs = rectangularPrism;
+            obs = obs.initialize([obstacleMin(ii, :); obstacleMax(ii, :)], ...
+                                 REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii));
+            obstacleList{ii} = obs;
+        end
+
+        % --- Initialise simulation (plots and video disabled) ---
+        sim = miSim;
+        sim = sim.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
+                             MIN_ALT, TIMESTEP, MAX_ITER, obstacleList, false, false);
+    end
+
+    % On the init call return current positions unchanged
+    for ii = 1:numAgents
+        nextPositions(ii, :) = currentPositions(ii, :);
+    end
+
+else
+    % =====================================================================
+    % One guidance step
+    % =====================================================================
+
+    % 1. Inject actual GPS positions (closed-loop feedback)
+    for ii = 1:size(sim.agents, 1)
+        sim.agents{ii}.lastPos = sim.agents{ii}.pos;
+        sim.agents{ii}.pos     = currentPositions(ii, :);
+
+        % Re-centre collision geometry at new position
+        d = currentPositions(ii, :) - sim.agents{ii}.collisionGeometry.center;
+        sim.agents{ii}.collisionGeometry = sim.agents{ii}.collisionGeometry.initialize( ...
+            sim.agents{ii}.collisionGeometry.center + d, ...
+            sim.agents{ii}.collisionGeometry.radius, ...
+            REGION_TYPE.COLLISION);
+    end
+
+    % 2. Advance timestep counter
+    sim.timestepIndex = sim.timestepIndex + 1;
+
+    % 3. Update communications topology (Lesser Neighbour Assignment)
+    sim = sim.lesserNeighbor();
+
+    % 4. Compute Voronoi partitioning
+    sim.partitioning = sim.agents{1}.partition(sim.agents, sim.domain.objective);
+
+    % 5. Unconstrained gradient-ascent step for each agent
+    for ii = 1:size(sim.agents, 1)
+        sim.agents{ii} = sim.agents{ii}.run(sim.domain, sim.partitioning, ...
+                                             sim.timestepIndex, ii, sim.agents);
+    end
+
+    % 6. Apply CBF safety filter (collision / comms / domain constraints via QP)
+    sim = sim.constrainMotion();
+
+    % 7. Return constrained next positions
+    for ii = 1:size(sim.agents, 1)
+        nextPositions(ii, :) = sim.agents{ii}.pos;
+    end
+
+end
+
+end

aerpaw/impl/controller_impl.cpp

@@ -0,0 +1,588 @@
+#include "controller_impl.h"
+#include <iostream>
+#include <vector>
+#include <string>
+#include <cstring>
+#include <cstdio>
+#include <limits>
+#include <sys/socket.h>
+#include <sys/select.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <time.h>
+
+#define SERVER_PORT 5000
+#define SERVER_IP "127.0.0.1"
+
+static int serverSocket = -1;
+static std::vector<int> clientSockets;
+
+void initSockets() {}
+void cleanupSockets() {}
+
+void initServer() {
+    initSockets();
+    serverSocket = socket(AF_INET, SOCK_STREAM, 0);
+    if(serverSocket < 0) { std::cerr << "Socket creation failed\n"; return; }
+
+    sockaddr_in serverAddr;
+    serverAddr.sin_family = AF_INET;
+    serverAddr.sin_addr.s_addr = INADDR_ANY;
+    serverAddr.sin_port = htons(SERVER_PORT);
+
+    int opt = 1;
+    setsockopt(serverSocket, SOL_SOCKET, SO_REUSEADDR, (char*)&opt, sizeof(opt));
+
+    if(bind(serverSocket, (sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) {
+        std::cerr << "Bind failed\n"; return;
+    }
+    if(listen(serverSocket, 5) < 0) {
+        std::cerr << "Listen failed\n"; return;
+    }
+
+    std::cout << "Server initialized\n";
+}
+
+void acceptClient(int clientId) {
+    sockaddr_in clientAddr;
+    socklen_t addrLen = sizeof(clientAddr);
+    int clientSock = accept(serverSocket, (sockaddr*)&clientAddr, &addrLen);
+    if(clientSock < 0) { std::cerr << "Accept failed for client " << clientId << "\n"; return; }
+    clientSockets.push_back(clientSock);
+    std::cout << "Client " << clientId << " connected\n";
+}
+
+void closeServer() {
+    for(auto sock : clientSockets) {
+        close(sock);
+    }
+    close(serverSocket);
+    cleanupSockets();
+}
+
+// Load target coordinates from file
+// File format: one line per UAV with "x,y,z" coordinates
+// Returns number of targets loaded
+int loadTargets(const char* filename, double* targets, int maxClients) {
+    FILE* file = fopen(filename, "r");
+    if (!file) {
+        std::cerr << "Failed to open config file: " << filename << "\n";
+        return 0;
+    }
+
+    int count = 0;
+    char line[256];
+    bool inTargets = false;
+
+    // Simple YAML parser for targets section
+    // Expects format:
+    //   targets:
+    //     - [x, y, z]
+    //     - [x, y, z]
+    while (fgets(line, sizeof(line), file) && count < maxClients) {
+        // Check if we've entered the targets section
+        if (strstr(line, "targets:") != nullptr) {
+            inTargets = true;
+            continue;
+        }
+
+        // If we hit another top-level key (no leading whitespace), exit targets section
+        if (inTargets && line[0] != ' ' && line[0] != '\t' && line[0] != '\n' && line[0] != '#') {
+            break;
+        }
+
+        // Parse target entries: "  - [x, y, z]"
+        if (inTargets) {
+            double x, y, z;
+            // Try to match the array format
+            if (sscanf(line, " - [%lf, %lf, %lf]", &x, &y, &z) == 3) {
+                // MATLAB uses column-major order, so for a maxClients x 3 matrix:
+                // Column 1 (x): indices 0, 1, 2, ...
+                // Column 2 (y): indices maxClients, maxClients+1, ...
+                // Column 3 (z): indices 2*maxClients, 2*maxClients+1, ...
+                targets[count + 0 * maxClients] = x;
+                targets[count + 1 * maxClients] = y;
+                targets[count + 2 * maxClients] = z;
+                std::cout << "Loaded target " << (count + 1) << ": " << x << "," << y << "," << z << "\n";
+                count++;
+            }
+        }
+    }
+
+    fclose(file);
+    return count;
+}
+
+// ---------------------------------------------------------------------------
+// Scenario CSV loading
+// ---------------------------------------------------------------------------
+
+// Split a CSV data row into fields, respecting quoted commas.
+// Inserts NUL terminators into `line` in place.
+// Returns the number of fields found.
+static int splitCSVRow(char* line, char** fields, int maxFields) {
+    int n = 0;
+    bool inQuote = false;
+    if (n < maxFields) fields[n++] = line;
+    for (char* p = line; *p && *p != '\n' && *p != '\r'; ++p) {
+        if (*p == '"') {
+            inQuote = !inQuote;
+        } else if (*p == ',' && !inQuote && n < maxFields) {
+            *p = '\0';
+            fields[n++] = p + 1;
+        }
+    }
+    // NUL-terminate the last field (strip trailing newline)
+    for (char* p = fields[n - 1]; *p; ++p) {
+        if (*p == '\n' || *p == '\r') { *p = '\0'; break; }
+    }
+    return n;
+}
+
+// Trim leading/trailing ASCII whitespace and remove enclosing double-quotes.
+// Modifies the string in place and returns the start of the trimmed content.
+static char* trimField(char* s) {
+    // Trim leading whitespace
+    while (*s == ' ' || *s == '\t') ++s;
+    // Remove enclosing quotes
+    size_t len = strlen(s);
+    if (len >= 2 && s[0] == '"' && s[len - 1] == '"') {
+        s[len - 1] = '\0';
+        ++s;
+    }
+    // Trim trailing whitespace
+    char* end = s + strlen(s) - 1;
+    while (end > s && (*end == ' ' || *end == '\t')) { *end-- = '\0'; }
+    return s;
+}
+
+// Open scenario.csv, skip the header row, return the data row in `line`.
+// Returns 1 on success, 0 on failure.
+static int readScenarioDataRow(const char* filename, char* line, int lineSize) {
+    FILE* f = fopen(filename, "r");
+    if (!f) {
+        std::cerr << "loadScenario: cannot open " << filename << "\n";
+        return 0;
+    }
+    // Skip header row
+    if (!fgets(line, lineSize, f)) { fclose(f); return 0; }
+    // Read data row
+    int ok = (fgets(line, lineSize, f) != NULL);
+    fclose(f);
+    return ok ? 1 : 0;
+}
+
+// Load guidance parameters from scenario.csv into flat params[NUM_SCENARIO_PARAMS].
+// Index mapping (0-based):
+//   0-7  : timestep, maxIter, minAlt, discretizationStep, protectedRange,
+//           initialStepSize, barrierGain, barrierExponent
+//   8-11 : collisionRadius[1:4]  (per-UAV, MAX_CLIENTS_PER_PARAM slots)
+//   12-15: comRange[1:4]
+//   16-19: alphaDist[1:4]
+//   20-23: betaDist[1:4]
+//   24-27: alphaTilt[1:4]
+//   28-31: betaTilt[1:4]
+//   32-34: domainMin  (east, north, up)
+//   35-37: domainMax  (east, north, up)
+//   38-39: objectivePos (east, north)
+//   40-43: objectiveVar (2x2 col-major: v11, v12, v21, v22)
+//   44   : sensorPerformanceMinimum
+// Returns 1 on success, 0 on failure.
+int loadScenario(const char* filename, double* params) {
+    char line[4096];
+    if (!readScenarioDataRow(filename, line, sizeof(line))) return 0;
+
+    char copy[4096];
+    strncpy(copy, line, sizeof(copy) - 1);
+    copy[sizeof(copy) - 1] = '\0';
+
+    char* fields[32];
+    int nf = splitCSVRow(copy, fields, 32);
+    if (nf < 19) {
+        fprintf(stderr, "loadScenario: expected >=19 columns, got %d\n", nf);
+        return 0;
+    }
+
+    // Zero-initialise all params so unused per-UAV slots default to 0
+    for (int i = 0; i < NUM_SCENARIO_PARAMS; i++) params[i] = 0.0;
+
+    // Scalar fields (columns 0–7): timestep..barrierExponent
+    for (int i = 0; i < 8; i++) {
+        params[i] = atof(trimField(fields[i]));
+    }
+
+    // Per-UAV fields (columns 8–13): collisionRadius, comRange,
+    // alphaDist, betaDist, alphaTilt, betaTilt.
+    // Each is a quoted comma-separated list with up to MAX_CLIENTS_PER_PARAM values.
+    // Stored in params as consecutive blocks of MAX_CLIENTS_PER_PARAM (4) slots.
+    {
+        const int perUavCols[] = {8, 9, 10, 11, 12, 13};
+        const int perUavOffsets[] = {8, 12, 16, 20, 24, 28};  // 0-based param indices
+        const char* perUavNames[] = {"collisionRadius", "comRange",
+                                     "alphaDist", "betaDist",
+                                     "alphaTilt", "betaTilt"};
+        for (int p = 0; p < 6; p++) {
+            char tmp[256];
+            strncpy(tmp, fields[perUavCols[p]], sizeof(tmp) - 1);
+            tmp[sizeof(tmp) - 1] = '\0';
+            char* t = trimField(tmp);
+            // Parse comma-separated values
+            double vals[4] = {0, 0, 0, 0};
+            int count = 0;
+            char* tok = strtok(t, ",");
+            while (tok && count < MAX_CLIENTS_PER_PARAM) {
+                vals[count++] = atof(tok);
+                tok = strtok(nullptr, ",");
+            }
+            if (count == 0) {
+                fprintf(stderr, "loadScenario: failed to parse %s\n", perUavNames[p]);
+                return 0;
+            }
+            for (int k = 0; k < MAX_CLIENTS_PER_PARAM; k++) {
+                params[perUavOffsets[p] + k] = vals[k];
+            }
+        }
+    }
+
+    // domainMin: column 14, format "east, north, up"
+    {
+        char tmp[256]; strncpy(tmp, fields[14], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char* t = trimField(tmp);
+        if (sscanf(t, "%lf , %lf , %lf", &params[32], &params[33], &params[34]) != 3) {
+            fprintf(stderr, "loadScenario: failed to parse domainMin: %s\n", t);
+            return 0;
+        }
+    }
+
+    // domainMax: column 15
+    {
+        char tmp[256]; strncpy(tmp, fields[15], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char* t = trimField(tmp);
+        if (sscanf(t, "%lf , %lf , %lf", &params[35], &params[36], &params[37]) != 3) {
+            fprintf(stderr, "loadScenario: failed to parse domainMax: %s\n", t);
+            return 0;
+        }
+    }
+
+    // objectivePos: column 16
+    {
+        char tmp[256]; strncpy(tmp, fields[16], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char* t = trimField(tmp);
+        if (sscanf(t, "%lf , %lf", &params[38], &params[39]) != 2) {
+            fprintf(stderr, "loadScenario: failed to parse objectivePos: %s\n", t);
+            return 0;
+        }
+    }
+
+    // objectiveVar: column 17, format "v11, v12, v21, v22"
+    {
+        char tmp[256]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char* t = trimField(tmp);
+        if (sscanf(t, "%lf , %lf , %lf , %lf", &params[40], &params[41], &params[42], &params[43]) != 4) {
+            fprintf(stderr, "loadScenario: failed to parse objectiveVar: %s\n", t);
+            return 0;
+        }
+    }
+
+    // sensorPerformanceMinimum: column 18
+    {
+        char tmp[64]; strncpy(tmp, fields[18], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        params[44] = atof(trimField(tmp));
+    }
+
+    printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g]\n",
+           params[32], params[33], params[34], params[35], params[36], params[37]);
+    return 1;
+}
+
+// Load initial UAV positions from scenario.csv (column 19: initialPositions).
+// targets is a column-major [maxClients x 3] array (same layout as loadTargets):
+//   targets[i + 0*maxClients] = east
+//   targets[i + 1*maxClients] = north
+//   targets[i + 2*maxClients] = up
+// Returns number of positions loaded.
+int loadInitialPositions(const char* filename, double* targets, int maxClients) {
+    char line[4096];
+    if (!readScenarioDataRow(filename, line, sizeof(line))) return 0;
+
+    char copy[4096];
+    strncpy(copy, line, sizeof(copy) - 1);
+    copy[sizeof(copy) - 1] = '\0';
+
+    char* fields[32];
+    int nf = splitCSVRow(copy, fields, 32);
+    if (nf < 20) {
+        fprintf(stderr, "loadInitialPositions: expected >=20 columns, got %d\n", nf);
+        return 0;
+    }
+
+    // Column 19: initialPositions flat "x1,y1,z1, x2,y2,z2, ..."
+    char tmp[1024]; strncpy(tmp, fields[19], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+    char* t = trimField(tmp);
+
+    double vals[3 * 4];  // up to MAX_CLIENTS triples
+    int parsed = 0;
+    char* tok = strtok(t, ",");
+    while (tok && parsed < 3 * maxClients) {
+        vals[parsed++] = atof(tok);
+        tok = strtok(nullptr, ",");
+    }
+
+    int count = parsed / 3;
+    for (int i = 0; i < count; i++) {
+        targets[i + 0 * maxClients] = vals[i * 3 + 0];  // east
+        targets[i + 1 * maxClients] = vals[i * 3 + 1];  // north
+        targets[i + 2 * maxClients] = vals[i * 3 + 2];  // up
+    }
+
+    printf("Loaded %d initial position(s) from scenario\n", count);
+    return count;
+}
+
+// Load obstacle bounding-box corners from scenario.csv.
+// Columns used: 20 (numObstacles), 21 (obstacleMin flat), 22 (obstacleMax flat).
+// obstacleMin and obstacleMax are column-major [maxObstacles x 3] arrays:
+//   obstacleMin[obs + 0*maxObstacles] = east_min
+//   obstacleMin[obs + 1*maxObstacles] = north_min
+//   obstacleMin[obs + 2*maxObstacles] = up_min
+// Returns number of obstacles loaded (0 if none or on error).
+int loadObstacles(const char* filename, double* obstacleMin, double* obstacleMax,
+                  int maxObstacles) {
+    char line[4096];
+    if (!readScenarioDataRow(filename, line, sizeof(line))) return 0;
+
+    char copy[4096];
+    strncpy(copy, line, sizeof(copy) - 1);
+    copy[sizeof(copy) - 1] = '\0';
+
+    char* fields[32];
+    int nf = splitCSVRow(copy, fields, 32);
+    if (nf < 23) return 0;
+
+    // Column 20: numObstacles
+    int n = (int)atof(trimField(fields[20]));
+    if (n <= 0) return 0;
+    if (n > maxObstacles) {
+        fprintf(stderr, "loadObstacles: %d obstacles exceeds MAX_OBSTACLES=%d\n", n, maxObstacles);
+        n = maxObstacles;
+    }
+
+    // Column 21: obstacleMin flat "x0,y0,z0, x1,y1,z1, ..."
+    {
+        char tmp[1024]; strncpy(tmp, fields[21], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char* t = trimField(tmp);
+        double vals[3 * 8];  // up to MAX_OBSTACLES triples
+        int parsed = 0;
+        char* tok = strtok(t, ",");
+        while (tok && parsed < 3 * n) {
+            vals[parsed++] = atof(tok);
+            tok = strtok(nullptr, ",");
+        }
+        if (parsed < 3 * n) {
+            fprintf(stderr, "loadObstacles: obstacleMin has fewer values than expected\n");
+            return 0;
+        }
+        for (int obs = 0; obs < n; obs++) {
+            obstacleMin[obs + 0 * maxObstacles] = vals[obs * 3 + 0];  // east
+            obstacleMin[obs + 1 * maxObstacles] = vals[obs * 3 + 1];  // north
+            obstacleMin[obs + 2 * maxObstacles] = vals[obs * 3 + 2];  // up
+        }
+    }
+
+    // Column 22: obstacleMax flat
+    {
+        char tmp[1024]; strncpy(tmp, fields[22], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char* t = trimField(tmp);
+        double vals[3 * 8];
+        int parsed = 0;
+        char* tok = strtok(t, ",");
+        while (tok && parsed < 3 * n) {
+            vals[parsed++] = atof(tok);
+            tok = strtok(nullptr, ",");
+        }
+        if (parsed < 3 * n) {
+            fprintf(stderr, "loadObstacles: obstacleMax has fewer values than expected\n");
+            return 0;
+        }
+        for (int obs = 0; obs < n; obs++) {
+            obstacleMax[obs + 0 * maxObstacles] = vals[obs * 3 + 0];
+            obstacleMax[obs + 1 * maxObstacles] = vals[obs * 3 + 1];
+            obstacleMax[obs + 2 * maxObstacles] = vals[obs * 3 + 2];
+        }
+    }
+
+    printf("Loaded %d obstacle(s) from scenario\n", n);
+    return n;
+}
+
+// Message type names for logging
+static const char* messageTypeName(uint8_t msgType) {
+    switch (msgType) {
+        case 1: return "TARGET";
+        case 2: return "ACK";
+        case 3: return "READY";
+        case 4: return "RTL";
+        case 5: return "LAND";
+        case 6: return "GUIDANCE_TOGGLE";
+        case 7: return "REQUEST_POSITION";
+        case 8: return "POSITION";
+        default: return "UNKNOWN";
+    }
+}
+
+// Send a single-byte message type to a client
+int sendMessageType(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;
+    }
+
+    std::cout << "Sent " << messageTypeName(msg) << " to client " << clientId << "\n";
+    return 1;
+}
+
+// Send TARGET message with coordinates (1 byte type + 24 bytes coords)
+int sendTarget(int clientId, const double* coords) {
+    if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0;
+
+    // Build message: 1 byte type + 3 doubles (little-endian)
+    uint8_t buffer[1 + 3 * sizeof(double)];
+    buffer[0] = 1;  // TARGET = 1
+    memcpy(buffer + 1, coords, 3 * sizeof(double));
+
+    ssize_t sent = send(clientSockets[clientId - 1], buffer, sizeof(buffer), 0);
+    if (sent != sizeof(buffer)) {
+        std::cerr << "Send target failed for client " << clientId << "\n";
+        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 << ": "
+              << coords[0] << "," << coords[1] << "," << coords[2] << "\n";
+    return 1;
+}
+
+// Wait for a specific message type from ALL clients simultaneously using select()
+// Returns 1 if all clients responded with expected message type, 0 on failure
+int waitForAllMessageType(int numClients, int expectedType) {
+    if (numClients <= 0 || numClients > (int)clientSockets.size()) return 0;
+
+    uint8_t expected = (uint8_t)expectedType;
+    std::vector<bool> completed(numClients, false);
+    int completedCount = 0;
+
+    while (completedCount < numClients) {
+        // Build fd_set for select()
+        fd_set readfds;
+        FD_ZERO(&readfds);
+        int maxfd = -1;
+
+        for (int i = 0; i < numClients; i++) {
+            if (!completed[i]) {
+                FD_SET(clientSockets[i], &readfds);
+                if (clientSockets[i] > maxfd) maxfd = clientSockets[i];
+            }
+        }
+
+        // Wait for any socket to have data
+        int ready = select(maxfd + 1, &readfds, nullptr, nullptr, nullptr);
+        if (ready <= 0) return 0;
+
+        // Check each socket
+        for (int i = 0; i < numClients; i++) {
+            if (!completed[i] && FD_ISSET(clientSockets[i], &readfds)) {
+                uint8_t msgType;
+                int len = recv(clientSockets[i], &msgType, 1, 0);
+                if (len == 0) {
+                    std::cerr << "waitForAllMessageType: client " << (i + 1)
+                              << " disconnected while waiting for "
+                              << messageTypeName(expected) << "\n";
+                    return 0;
+                }
+                if (len < 0) {
+                    std::cerr << "waitForAllMessageType: recv error for client " << (i + 1)
+                              << " while waiting for " << messageTypeName(expected) << "\n";
+                    return 0;
+                }
+
+                std::cout << "Received " << messageTypeName(msgType) << " from client " << (i + 1) << "\n";
+
+                if (msgType == expected) {
+                    completed[i] = true;
+                    completedCount++;
+                }
+            }
+        }
+    }
+
+    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
+    }
+}
+
+// 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
+    }
+    return 1;
+}
+
+// Receive POSITION response (1 byte type + 24 bytes ENU) from all clients.
+// Stores results in column-major order: positions[client + 0*maxClients] = x (east),
+//                                       positions[client + 1*maxClients] = y (north),
+//                                       positions[client + 2*maxClients] = z (up)
+int recvPositions(int numClients, double* positions, int maxClients) {
+    if (numClients <= 0 || numClients > (int)clientSockets.size()) return 0;
+
+    for (int i = 0; i < numClients; i++) {
+        // Expect: POSITION byte (1) + 3 doubles (24)
+        uint8_t msgType;
+        if (recv(clientSockets[i], &msgType, 1, MSG_WAITALL) != 1) {
+            std::cerr << "recvPositions: failed to read msg type from client " << (i + 1) << "\n";
+            return 0;
+        }
+        if (msgType != 8) {  // POSITION = 8
+            std::cerr << "recvPositions: expected POSITION(8), got " << (int)msgType
+                      << " from client " << (i + 1) << "\n";
+            return 0;
+        }
+
+        double coords[3];
+        if (recv(clientSockets[i], coords, sizeof(coords), MSG_WAITALL) != (ssize_t)sizeof(coords)) {
+            std::cerr << "recvPositions: failed to read coords from client " << (i + 1) << "\n";
+            return 0;
+        }
+
+        // Store column-major (MATLAB layout): col 0 = east, col 1 = north, col 2 = up
+        positions[i + 0 * maxClients] = coords[0];  // east  (x)
+        positions[i + 1 * maxClients] = coords[1];  // north (y)
+        positions[i + 2 * maxClients] = coords[2];  // up    (z)
+
+        std::cout << "Position from client " << (i + 1) << ": "
+                  << coords[0] << "," << coords[1] << "," << coords[2] << "\n";
+    }
+    return 1;
+}
+
+// Sleep for the given number of milliseconds
+void sleepMs(int ms) {
+    struct timespec ts;
+    ts.tv_sec  = ms / 1000;
+    ts.tv_nsec = (ms % 1000) * 1000000L;
+    nanosleep(&ts, nullptr);
+}

aerpaw/impl/controller_impl.h

@@ -0,0 +1,71 @@
+#ifndef CONTROLLER_IMPL_H
+#define CONTROLLER_IMPL_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Server lifecycle
+void initServer();
+void acceptClient(int clientId);
+void closeServer();
+
+// Configuration
+int loadTargets(const char* filename, double* targets, int maxClients);
+
+// Scenario loading (scenario.csv)
+// Number of elements in the flat params array passed to guidance_step.
+// Indices (0-based):
+//   0-7   scalars (timestep..barrierExponent)
+//   8-11  collisionRadius[1:4]  (per-UAV, MAX_CLIENTS slots)
+//   12-15 comRange[1:4]
+//   16-19 alphaDist[1:4]
+//   20-23 betaDist[1:4]
+//   24-27 alphaTilt[1:4]
+//   28-31 betaTilt[1:4]
+//   32-34 domainMin
+//   35-37 domainMax
+//   38-39 objectivePos
+//   40-43 objectiveVar (2x2 col-major)
+//   44    sensorPerformanceMinimum
+#define NUM_SCENARIO_PARAMS 45
+#define MAX_CLIENTS_PER_PARAM 4
+// Maximum number of obstacles (upper bound for pre-allocated arrays).
+#define MAX_OBSTACLES 8
+
+// Load guidance parameters from scenario.csv into flat params[NUM_SCENARIO_PARAMS].
+// Returns 1 on success, 0 on failure.
+int loadScenario(const char* filename, double* params);
+
+// Load initial UAV positions from scenario.csv (initialPositions column).
+// targets is a column-major [maxClients x 3] array (same layout as loadTargets).
+// Returns number of positions loaded.
+int loadInitialPositions(const char* filename, double* targets, int maxClients);
+
+// Load obstacle bounding-box corners from scenario.csv.
+// obstacleMin and obstacleMax are column-major [maxObstacles x 3] arrays
+// (same layout convention as loadTargets / recvPositions).
+// Returns the number of obstacles loaded.
+int loadObstacles(const char* filename,
+                  double* obstacleMin,
+                  double* obstacleMax,
+                  int maxObstacles);
+
+// Binary protocol operations
+int sendMessageType(int clientId, int msgType);
+int sendTarget(int clientId, const double* coords);
+int waitForAllMessageType(int numClients, int expectedType);
+
+// Guidance loop operations
+void sendGuidanceToggle(int numClients);
+int  sendRequestPositions(int numClients);
+int  recvPositions(int numClients, double* positions, int maxClients); // column-major maxClients x 3
+void sleepMs(int ms);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // CONTROLLER_IMPL_H

aerpaw/impl/controller_main.cpp

@@ -0,0 +1,16 @@
+#include <iostream>
+#include "controller.h" 
+#include "controller_impl.h" // TCP implementation header
+
+int main() {
+    // Number of clients to handle
+    int numClients = 2;  // for now
+
+    std::cout << "Initializing TCP server...\n";
+
+    // Call MATLAB-generated server function
+    controller(numClients);
+
+    std::cout << "Server finished.\n";
+    return 0;
+}

aerpaw/impl/tmwtypes.h

@@ -0,0 +1,888 @@
+/*
+ * Copyright 1984-2023 The MathWorks, Inc.
+ */
+
+#if defined(_MSC_VER)
+# pragma once
+#endif
+#if defined(__GNUC__) && (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ > 3))
+# pragma once
+#endif
+
+#ifndef tmwtypes_h
+#define tmwtypes_h
+
+#ifndef __TMWTYPES__
+#define __TMWTYPES__
+/*
+ * File    : tmwtypes.h
+ * Abstract:
+ *      Data types for use with MATLAB/SIMULINK and the Real-Time Workshop.
+ *
+ *      When compiling stand-alone model code, data types can be overridden
+ *      via compiler switches.
+ *
+ *      Define NO_FLOATS to eliminate reference to real_T, etc.
+ */
+
+#ifdef MW_LIBTOOLING
+#include "mwstdint.h"
+#endif
+
+#include <limits.h>
+
+/* __STDC_VERSION__ version check below means "check for a C99 compiler".
+
+   Visual Studio (checked on versions 2015 and 2017) does
+   not define __STDC_VERSION__, however  it has stdbool.h available,
+   thus a separate check for _MSC_VER below.
+ */
+#if defined(__APPLE_CC__) \
+    || (defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)) \
+    || (defined(_MSC_VER) && (_MSC_VER >= 1900))
+#ifndef tmwtypes_do_not_include_stdbool
+#include <stdbool.h>
+#endif
+#endif
+
+#define LOGICAL_IS_A_TYPE
+#define SPARSE_GENERALIZATION
+
+#ifdef NO_FLOATS
+# define double double_not_allowed
+# define float  float_not_allowed
+#endif /*NO_FLOATS*/
+
+#ifndef NO_FLOATS
+
+#ifndef __MWERKS__
+# ifdef __STDC__
+#  include <float.h>
+# else
+#  ifndef FLT_MANT_DIG
+#   define FLT_MANT_DIG 24
+#  endif
+#  ifndef DBL_MANT_DIG
+#   define DBL_MANT_DIG 53
+#  endif
+# endif
+#endif
+
+#endif /*NO_FLOATS*/
+
+/*
+ *      The following data types cannot be overridden when building MEX files.
+ */
+#ifdef MATLAB_MEX_FILE
+# undef CHARACTER_T
+# undef INTEGER_T
+# undef BOOLEAN_T
+# undef REAL_T
+# undef TIME_T
+#endif
+
+/*
+ * The uchar_T, ushort_T and ulong_T types are needed for compilers which do 
+ * not allow defines to be specified, at the command line, with spaces in them.
+ */
+
+typedef unsigned char       uchar_T;
+typedef unsigned short      ushort_T;
+typedef unsigned long       ulong_T;
+
+#if (defined(_MSC_VER) && _MSC_VER >= 1500)      \
+    || defined(__x86_64__) || defined(__LP64__)  \
+    || defined(__LCC64__)
+    
+typedef unsigned long long  ulonglong_T;
+#endif
+
+
+
+/*=======================================================================*
+ * Fixed width word size data types:                                     *
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+ *   real32_T, real64_T           - 32 and 64 bit floating point numbers *
+ *=======================================================================*/
+
+/* When used with Real Time Workshop generated code, this
+ * header file can be used with a variety of compilers.
+ *
+ * The compiler could be for an 8 bit embedded processor that
+ * only had 8 bits per integer and 16 bits per long.
+ * In that example, a 32 bit integer size is not even available.
+ * This header file should be robust to that.   
+ *
+ * For the case of an 8 bit processor, the preprocessor
+ * may be limited to 16 bit math like its target.  That limitation 
+ * would mean that 32 bit comparisons can't be done accurately.  
+ * To increase robustness to this, comparisons are done against
+ * smaller values first.  An inaccurate 32 bit comparison isn't
+ * attempted if the 16 bit comparison has already succeeded.
+ *
+ * Limitations on preprocessor math can also be stricter than
+ * for the target.  There are known cases where a compiler
+ * targeting processors with 64 bit longs can't do accurate
+ * preprocessor comparisons on more than 32 bits.  
+ */
+
+/* Determine the number of bits for int, long, short, and char.
+ * If one fails to be determined, set the number of bits to -1
+ */
+
+#ifndef TMW_BITS_PER_INT
+# if   INT_MAX  == 0x7FL
+#  define TMW_BITS_PER_INT 8
+# elif INT_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_INT 16
+# elif INT_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_INT 32
+# else
+#  define TMW_BITS_PER_INT -1
+# endif
+#endif
+
+#ifndef TMW_BITS_PER_LONG
+# if   LONG_MAX  == 0x7FL
+#  define TMW_BITS_PER_LONG 8
+# elif LONG_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_LONG 16
+# elif LONG_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_LONG 32
+# else
+#  define TMW_BITS_PER_LONG -1
+# endif
+#endif
+
+#ifndef TMW_BITS_PER_SHRT
+# if   SHRT_MAX  == 0x7FL
+#  define TMW_BITS_PER_SHRT 8
+# elif SHRT_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_SHRT 16
+# elif SHRT_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_SHRT 32
+# else
+#  define TMW_BITS_PER_SHRT -1
+# endif
+#endif
+
+#ifndef TMW_BITS_PER_SCHAR
+# if   SCHAR_MAX  == 0x7FL
+#  define TMW_BITS_PER_SCHAR 8
+# elif SCHAR_MAX  == 0x7FFFL
+#  define TMW_BITS_PER_SCHAR 16
+# elif SCHAR_MAX  == 0x7FFFFFFFL
+#  define TMW_BITS_PER_SCHAR 32
+# else
+#  define TMW_BITS_PER_SCHAR -1
+# endif
+#endif
+
+#ifndef TMW_CHAR_SIGNED
+# if SCHAR_MAX == CHAR_MAX
+#  define TMW_CHAR_SIGNED 1
+# else
+#  define TMW_CHAR_SIGNED 0
+# endif
+#endif
+
+/* It is common for one or more of the integer types
+ * to be the same size.  For example, on many embedded
+ * processors, both shorts and ints are 16 bits.  On
+ * processors used for workstations, it is quite common
+ * for both int and long to be 32 bits.  
+ *   When there is more than one choice for typdef'ing
+ * a portable type like int16_T or uint32_T, in
+ * concept, it should not matter which choice is made.
+ * However, some style guides and some code checking
+ * tools do identify and complain about seemingly
+ * irrelevant differences.  For example, a code
+ * checking tool may complain about an implicit
+ * conversion from int to short even though both
+ * are 16 bits.  To reduce these types of
+ * complaints, it is best to make int the
+ * preferred choice when more than one is available.
+ */
+
+#ifndef INT8_T
+# if   defined(MW_LIBTOOLING)
+#  define INT8_T  int8_t
+# elif TMW_BITS_PER_INT   == 8
+#  define  INT8_T int
+# elif TMW_BITS_PER_LONG  == 8
+#  define  INT8_T long
+# elif TMW_BITS_PER_SCHAR == 8
+#  define  INT8_T signed char
+# elif TMW_BITS_PER_SHRT  == 8
+#  define  INT8_T short
+# endif
+#endif
+#ifdef INT8_T
+ typedef INT8_T int8_T;
+#endif
+
+#ifndef UINT8_T
+# if   defined(MW_LIBTOOLING)
+#  define  UINT8_T uint8_t
+# elif   TMW_BITS_PER_INT   == 8
+#  define  UINT8_T unsigned int
+# elif TMW_BITS_PER_LONG  == 8
+#  define  UINT8_T unsigned long
+# elif TMW_BITS_PER_SCHAR == 8
+#  define  UINT8_T unsigned char
+# elif TMW_BITS_PER_SHRT  == 8
+#  define  UINT8_T unsigned short
+# endif
+#endif
+#ifdef UINT8_T
+ typedef UINT8_T uint8_T;
+#endif
+
+
+#ifndef INT16_T
+# if   defined(MW_LIBTOOLING)
+#  define  INT16_T int16_t
+# elif   TMW_BITS_PER_INT   == 16
+#  define  INT16_T int
+# elif TMW_BITS_PER_LONG  == 16
+#  define  INT16_T long
+# elif TMW_BITS_PER_SCHAR == 16
+#  define  INT16_T signed char
+# elif TMW_BITS_PER_SHRT  == 16
+#  define  INT16_T short
+# endif
+#endif
+#ifdef INT16_T
+ typedef INT16_T int16_T;
+#endif
+
+
+#ifndef UINT16_T
+# if   defined(MW_LIBTOOLING)
+#  define  UINT16_T uint16_t
+# elif TMW_BITS_PER_INT   == 16
+#  define  UINT16_T unsigned int
+# elif TMW_BITS_PER_LONG  == 16
+#  define  UINT16_T unsigned long
+# elif TMW_BITS_PER_SCHAR == 16
+#  define  UINT16_T unsigned char
+# elif TMW_BITS_PER_SHRT  == 16
+#  define  UINT16_T unsigned short
+# endif
+#endif
+#ifdef UINT16_T
+ typedef UINT16_T uint16_T;
+#endif
+
+
+#ifndef INT32_T
+# if   defined(MW_LIBTOOLING)
+#  define  INT32_T int32_t
+# elif TMW_BITS_PER_INT   == 32
+#  define  INT32_T int
+# elif TMW_BITS_PER_LONG  == 32
+#  define  INT32_T long
+# elif TMW_BITS_PER_SCHAR == 32
+#  define  INT32_T signed char
+# elif TMW_BITS_PER_SHRT  == 32
+#  define  INT32_T short
+# endif
+#endif
+#ifdef INT32_T
+ typedef INT32_T int32_T;
+#endif
+
+
+#ifndef UINT32_T
+# if   defined(MW_LIBTOOLING)
+#  define  UINT32_T uint32_t
+# elif TMW_BITS_PER_INT   == 32
+#  define  UINT32_T unsigned int
+# elif TMW_BITS_PER_LONG  == 32
+#  define  UINT32_T unsigned long
+# elif TMW_BITS_PER_SCHAR == 32
+#   define UINT32_T unsigned char
+# elif TMW_BITS_PER_SHRT  == 32
+#  define  UINT32_T unsigned short
+# endif
+#endif
+#ifdef UINT32_T
+ typedef UINT32_T uint32_T;
+#endif
+
+/* The following is used to emulate smaller integer types when only
+ * larger types are available.  For example, compilers for TI C3x/C4x DSPs 
+ * define char and short to be 32 bits, so 8 and 16 bits are not directly
+ * available.  This target is commonly used with RTW rapid prototyping.
+ * Other DSPs define char to be 16 bits, so 8 bits is not directly 
+ * available.
+ */
+#ifndef INT8_T
+# ifdef INT16_T
+#  define INT8_T INT16_T
+   typedef INT8_T int8_T;
+# else
+#  ifdef INT32_T
+#   define INT8_T INT32_T
+    typedef INT8_T int8_T;
+#  endif
+# endif
+#endif
+
+#ifndef UINT8_T
+# ifdef UINT16_T
+#   define UINT8_T UINT16_T
+    typedef UINT8_T uint8_T;
+# else
+#  ifdef UINT32_T
+#   define UINT8_T UINT32_T
+    typedef UINT8_T uint8_T;
+#  endif
+# endif
+#endif
+
+#ifndef INT16_T
+# ifdef INT32_T
+#   define INT16_T INT32_T
+    typedef INT16_T int16_T;
+# endif
+#endif
+
+#ifndef UINT16_T
+# ifdef UINT32_T
+#   define UINT16_T UINT32_T
+    typedef UINT16_T uint16_T;
+# endif
+#endif
+
+
+#ifndef NO_FLOATS
+
+#ifndef REAL32_T
+# ifndef __MWERKS__
+#  if FLT_MANT_DIG >= 23
+#   define REAL32_T float
+#  endif
+# else
+#  define REAL32_T float
+# endif
+#endif
+#ifdef REAL32_T
+ typedef REAL32_T real32_T;
+#endif
+
+
+#ifndef REAL64_T
+# ifndef __MWERKS__
+#  if DBL_MANT_DIG >= 52
+#   define REAL64_T double
+#  endif
+# else
+#  define REAL64_T double
+# endif
+#endif
+#ifdef REAL64_T
+ typedef REAL64_T real64_T;
+#endif
+
+#endif /* NO_FLOATS*/
+
+/*=======================================================================*
+ * Fixed width word size data types:                                     *
+ *   int64_T                      - signed 64 bit integers               *
+ *   uint64_T                     - unsigned 64 bit integers             *
+ *=======================================================================*/
+
+# if   defined(MW_LIBTOOLING)
+#  ifdef INT64_T
+#    undef  INT64_T
+#  endif
+#  define  INT64_T int64_t
+#  ifdef UINT64_T
+#    undef  UINT64_T
+#  endif
+#  define  UINT64_T uint64_t
+# endif
+#if !defined(INT64_T) || !defined(UINT64_T) || !defined(FMT64)
+# if defined(__APPLE__) || defined(__clang__)
+#  ifndef INT64_T
+#   define INT64_T long long
+#  endif
+#  ifndef UINT64_T
+#   define UINT64_T unsigned long long
+#  endif
+#  ifndef FMT64
+#   define FMT64 "ll"
+#  endif
+#  if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# elif (defined(__x86_64__) || defined(__LP64__))&& !defined(__MINGW64__) 
+#  ifndef INT64_T
+#   define INT64_T long
+#  endif
+#  ifndef UINT64_T
+#   define UINT64_T unsigned long
+#  endif
+#  ifndef FMT64
+#   define FMT64 "l"
+#  endif
+#  if !defined(INT_TYPE_64_IS_LONG)
+#    define INT_TYPE_64_IS_LONG
+#  endif
+# elif defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
+                         || (defined(__WATCOMC__)  && __WATCOMC__  >= 1100)
+#  ifndef INT64_T
+#   define INT64_T __int64
+#  endif
+#  ifndef UINT64_T
+#   define UINT64_T unsigned __int64
+#  endif
+#  ifndef FMT64
+#   define FMT64 "I64"
+#  endif
+# elif defined(__GNUC__) || defined(TMW_ENABLE_INT64) \
+                         || defined(__LCC64__)
+#  ifndef INT64_T
+#   define INT64_T long long
+#  endif
+#  ifndef UINT64_T
+#   define UINT64_T unsigned long long
+#  endif
+#  ifndef FMT64
+#   define FMT64 "ll"
+#  endif
+# endif
+
+#endif
+
+#if defined(INT64_T)
+# if defined(__GNUC__) && \
+    ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9)))
+  __extension__
+# endif
+ typedef INT64_T int64_T;
+#endif
+
+#if defined(_WIN64) || (defined(__APPLE__) && defined(__LP64__)) \
+                    || defined(__x86_64__) \
+                    || defined(__LP64__)
+#  define INT_TYPE_64_IS_SUPPORTED
+#endif
+
+#if defined(UINT64_T)
+# if defined(__GNUC__) && \
+    ((__GNUC__ > 2) || ((__GNUC__ == 2) && (__GNUC_MINOR__ >=9)))
+  __extension__
+# endif
+ typedef UINT64_T uint64_T;
+#endif
+
+/*===========================================================================*
+ * Format string modifiers for using size_t variables in printf statements.  *
+ *===========================================================================*/
+
+#ifndef FMT_SIZE_T
+#  if (defined( __GNUC__ ) || defined(_STDC_C99))&& !defined(__MINGW64__)
+#    define FMT_SIZE_T "z"
+#  elif defined (__WATCOMC__)
+#    define FMT_SIZE_T "l"
+#  elif defined (_WIN32 )
+#    define FMT_SIZE_T "I"
+#  else
+#    define FMT_SIZE_T "l"
+#  endif
+#endif
+
+#ifndef FMT_PTRDIFF_T
+#  if defined(__APPLE__)
+#    define FMT_PTRDIFF_T "l"
+#  elif defined( __GNUC__ ) || defined(_STDC_C99)
+#    define FMT_PTRDIFF_T "t"
+#  elif defined (__WATCOMC__)
+#    define FMT_PTRDIFF_T "l"
+#  elif defined (_WIN32 )
+#    define FMT_PTRDIFF_T "I"
+#  else
+#    define FMT_PTRDIFF_T "l"
+#  endif
+#endif
+
+/*===========================================================================*
+ * General or logical data types where the word size is not guaranteed.      *
+ *  real_T     - possible settings include real32_T or real64_T              *
+ *  time_T     - possible settings include real32_T or real64_T              *
+ *  boolean_T                                                                *
+ *  char_T                                                                   *
+ *  int_T                                                                    *
+ *  uint_T                                                                   *
+ *  byte_T                                                                   *
+ *===========================================================================*/
+
+#ifndef NO_FLOATS
+
+#ifndef REAL_T
+# ifdef REAL64_T
+#  define REAL_T real64_T
+# else
+#  ifdef REAL32_T
+#   define REAL_T real32_T
+#  endif
+# endif
+#endif
+#ifdef REAL_T
+ typedef REAL_T real_T;
+#endif
+
+#ifndef TIME_T
+# ifdef REAL_T
+#  define TIME_T real_T
+# endif
+#endif
+#ifdef TIME_T
+ typedef TIME_T time_T;
+#endif
+
+#endif /* NO_FLOATS */
+
+#ifndef BOOLEAN_T
+# if defined(UINT8_T)
+#  define BOOLEAN_T UINT8_T
+# else
+#  define BOOLEAN_T unsigned int
+# endif
+#endif
+typedef BOOLEAN_T boolean_T;
+
+
+#ifndef CHARACTER_T
+# define CHARACTER_T char
+#endif
+typedef CHARACTER_T char_T;
+
+
+#ifndef INTEGER_T
+# define INTEGER_T int
+#endif
+typedef INTEGER_T int_T;
+
+
+#ifndef UINTEGER_T
+# define UINTEGER_T unsigned
+#endif
+typedef UINTEGER_T uint_T;
+
+
+#ifndef BYTE_T
+# define BYTE_T unsigned char
+#endif
+typedef BYTE_T byte_T;
+
+
+/*===========================================================================*
+ * Define Complex Structures                                                 *
+ *===========================================================================*/
+#ifndef NO_FLOATS
+
+#ifndef CREAL32_T
+#  ifdef REAL32_T
+    typedef struct {
+      real32_T re, im;
+    } creal32_T;
+#    define CREAL32_T creal32_T
+#  endif
+#endif
+
+#ifndef CREAL64_T
+#  ifdef REAL64_T
+    typedef struct {
+      real64_T re, im;
+    } creal64_T;
+#    define CREAL64_T creal64_T
+#  endif
+#endif
+
+#ifndef CREAL_T
+#  ifdef REAL_T
+    typedef struct {
+      real_T re, im;
+    } creal_T;
+#    define CREAL_T creal_T
+#  endif
+#endif
+
+#endif /* NO_FLOATS */
+
+#ifndef CINT8_T
+#  ifdef INT8_T
+    typedef struct {
+      int8_T re, im;
+    } cint8_T;
+#    define CINT8_T cint8_T
+#  endif
+#endif
+
+#ifndef CUINT8_T
+#  ifdef UINT8_T
+    typedef struct {
+      uint8_T re, im;
+    } cuint8_T;
+#    define CUINT8_T cuint8_T
+#  endif
+#endif
+
+#ifndef CINT16_T
+#  ifdef INT16_T
+    typedef struct {
+      int16_T re, im;
+    } cint16_T;
+#    define CINT16_T cint16_T
+#  endif
+#endif
+
+#ifndef CUINT16_T
+#  ifdef UINT16_T
+    typedef struct {
+      uint16_T re, im;
+    } cuint16_T;
+#    define CUINT16_T cuint16_T
+#  endif
+#endif
+
+#ifndef CINT32_T
+#  ifdef INT32_T
+    typedef struct {
+      int32_T re, im;
+    } cint32_T;
+#    define CINT32_T cint32_T
+#  endif
+#endif
+
+#ifndef CUINT32_T
+#  ifdef UINT32_T
+    typedef struct {
+      uint32_T re, im;
+    } cuint32_T;
+#    define CUINT32_T cuint32_T
+#  endif
+#endif
+
+#ifndef CINT64_T
+#  ifdef INT64_T
+    typedef struct {
+      int64_T re, im;
+    } cint64_T;
+#    define CINT64_T cint64_T
+#  endif
+#endif
+
+#ifndef CUINT64_T
+#  ifdef UINT64_T
+    typedef struct {
+      uint64_T re, im;
+    } cuint64_T;
+#    define CUINT64_T cuint64_T
+#  endif
+#endif
+
+/*=======================================================================*
+ * Min and Max:                                                          *
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+ *=======================================================================*/
+
+#define  MAX_int8_T      ((int8_T)(127))            /* 127  */
+#define  MIN_int8_T      ((int8_T)(-128))           /* -128 */
+#define  MAX_uint8_T     ((uint8_T)(255))           /* 255  */
+#define  MIN_uint8_T     ((uint8_T)(0))
+                           
+#define  MAX_int16_T     ((int16_T)(32767))         /* 32767 */
+#define  MIN_int16_T     ((int16_T)(-32768))        /* -32768 */
+#define  MAX_uint16_T    ((uint16_T)(65535))        /* 65535 */
+#define  MIN_uint16_T    ((uint16_T)(0))
+                           
+#define  MAX_int32_T     ((int32_T)(2147483647))    /* 2147483647  */
+#define  MIN_int32_T     ((int32_T)(-2147483647-1)) /* -2147483648 */
+#define  MAX_uint32_T    ((uint32_T)(0xFFFFFFFFU))  /* 4294967295  */
+#define  MIN_uint32_T    ((uint32_T)(0))
+
+#if defined(_MSC_VER) || (defined(__BORLANDC__) && __BORLANDC__ >= 0x530) \
+                      || (defined(__WATCOMC__)  && __WATCOMC__  >= 1100) \
+                      || defined(__LCC64__)
+#  ifdef INT64_T
+#    define  MAX_int64_T     ((int64_T)(9223372036854775807LL))
+#    define  MIN_int64_T     ((int64_T)(-9223372036854775807LL-1LL))
+#  endif
+#  ifdef UINT64_T
+#    define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFULL))
+#    define  MIN_uint64_T    ((uint64_T)(0))
+#  endif
+#else
+#  ifdef INT64_T
+#    ifdef INT_TYPE_64_IS_LONG
+#      define  MAX_int64_T     ((int64_T)(9223372036854775807L))
+#      define  MIN_int64_T     ((int64_T)(-9223372036854775807L-1L))
+#    else
+#      define  MAX_int64_T     ((int64_T)(9223372036854775807LL))
+#      define  MIN_int64_T     ((int64_T)(-9223372036854775807LL-1LL))
+#    endif
+#  endif
+#  ifdef UINT64_T
+#    ifdef INT_TYPE_64_IS_LONG
+#      define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFUL))
+#      define  MIN_uint64_T    ((uint64_T)(0))
+#    else
+#      define  MAX_uint64_T    ((uint64_T)(0xFFFFFFFFFFFFFFFFULL))
+#      define  MIN_uint64_T    ((uint64_T)(0))
+#    endif
+#  endif
+#endif
+
+#if (defined(_MSC_VER) && !defined(__clang__))
+
+/* Conversion from unsigned __int64 to double is not implemented in Visual Studio
+ * and results in a compile error, thus the value must first be cast to
+ * signed __int64, and then to double.
+ *
+ * If the 64 bit int value is greater than 2^63-1, which is the signed int64 max,
+ * the macro below provides a workaround for casting a uint64 value to a double
+ * in windows.
+ */
+#  define uint64_to_double(u) ( ((u) > _I64_MAX) ? \
+            (double)(__int64)((u) - _I64_MAX - 1) + (double)_I64_MAX + 1: \
+            (double)(__int64)(u) )
+
+/* The following inline function should only be used in the macro double_to_uint64,
+ * as it only handles the specfic range of double between 2^63 and 2^64-1 */
+__forceinline
+uint64_T double_to_uint64_helper(double d) {
+  union double_to_uint64_union_type {
+    double dd;
+    uint64_T i64;
+  } di;
+  di.dd = d;
+  return (((di.i64 & 0x000fffffffffffff) | 0x0010000000000000) << 11);
+}
+
+/* The largest double value that can be cast to uint64 in windows is the
+ * signed int64 max, which is 2^63-1. The macro below provides
+ * a workaround for casting large double values to uint64 in windows.
+ */
+/* The magic number 18446744073709551616.0 is 2^64 */
+/* The magic number 9223372036854775808.0 is 2^63 */
+#  define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \
+            0xffffffffffffffffULL : \
+            ((d) >= 0.0) ? \
+            ((d) >= 9223372036854775808.0) ? \
+            double_to_uint64_helper(d) : \
+            (unsigned __int64)(d) : \
+            0ULL )
+#else
+#  define uint64_to_double(u) ((double)(u))
+#  if defined(__BORLANDC__) || defined(__WATCOMC__) || defined(__TICCSC__)
+/* double_to_uint64 defined only for MSVC and UNIX */
+#  else
+#  define double_to_uint64(d) ( ((d) >= 18446744073709551616.0) ? \
+            (unsigned long long) 0xffffffffffffffffULL : \
+            ((d) >= 0) ? (unsigned long long)(d) : (unsigned long long) 0 )
+#  endif
+#endif
+
+#if !defined(__cplusplus) && !defined(__bool_true_false_are_defined)
+
+#ifndef _bool_T
+#define _bool_T
+
+typedef boolean_T bool;
+
+#ifndef false
+#define false (0)
+#endif
+#ifndef true 
+#define true (1)
+#endif
+
+#endif /* _bool_T */
+
+#endif /* !__cplusplus */
+
+/* 
+ * This software assumes that the code is being compiled on a target using a 
+ * 2's complement representation for signed integer values.
+ */
+#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
+#error "This code must be compiled using a 2's complement representation for signed integer values"
+#endif
+
+/*
+ * Maximum length of a MATLAB identifier (function/variable/model)
+ * including the null-termination character.
+ */
+#define TMW_NAME_LENGTH_MAX 2049
+
+/*
+ * Maximum values for indices and dimensions
+ */
+#include <stddef.h>
+
+#ifdef MX_COMPAT_32
+typedef int mwSize;
+typedef int mwIndex;
+typedef int mwSignedIndex;
+#else
+typedef size_t    mwSize;         /* unsigned pointer-width integer */
+typedef size_t    mwIndex;        /* unsigned pointer-width integer */
+typedef ptrdiff_t mwSignedIndex;  /* a signed pointer-width integer */
+#endif
+
+                                  /* for the individual dim */
+/* If updating SLSize or SLIndex, update defintions in sl_types_def.h
+   as well. */
+#ifndef SLSIZE_SLINDEX
+  #define SLSIZE_SLINDEX
+  #ifdef INT_TYPE_64_IS_SUPPORTED
+    typedef int64_T SLIndex;
+    typedef int64_T SLSize;
+  #else
+    typedef int SLIndex;
+    typedef int SLSize;
+  #endif
+#endif
+
+/* for the total size */
+#define SLIndexType size_t
+#define INVALID_SIZET_VALUE   (std::numeric_limits<SLIndexType>::max())
+#define MAX_VALID_SIZET_VALUE   (std::numeric_limits<SLIndexType>::max() -1)
+
+
+#if (defined(_LP64) || defined(_WIN64)) && !defined(MX_COMPAT_32)
+/* Currently 2^48 based on hardware limitations */
+# define MWSIZE_MAX    281474976710655UL
+# define MWINDEX_MAX   281474976710655UL
+# define MWSINDEX_MAX  281474976710655L
+# define MWSINDEX_MIN -281474976710655L
+#else
+# define MWSIZE_MAX    2147483647UL
+# define MWINDEX_MAX   2147483647UL
+# define MWSINDEX_MAX  2147483647L
+# define MWSINDEX_MIN -2147483647L
+#endif
+#define MWSIZE_MIN    0UL
+#define MWINDEX_MIN   0UL
+
+/** UTF-16 character type */
+
+#if (defined(__cplusplus) && (__cplusplus >= 201103L)) || (defined(_HAS_CHAR16_T_LANGUAGE_SUPPORT) && _HAS_CHAR16_T_LANGUAGE_SUPPORT)
+typedef char16_t CHAR16_T;
+#define U16_STRING_LITERAL_PREFIX u
+#elif defined(_MSC_VER)
+typedef wchar_t CHAR16_T;
+#define U16_STRING_LITERAL_PREFIX L
+#else
+typedef UINT16_T CHAR16_T;
+#endif
+
+#endif  /* __TMWTYPES__ */
+
+#endif /* tmwtypes_h */

aerpaw/loadTargetsFromYaml.m

@@ -0,0 +1,42 @@
+function targets = loadTargetsFromYaml(filename)
+%LOADTARGETSFROMYAML Load target coordinates from YAML config file
+%   targets = loadTargetsFromYaml(filename) reads the targets section
+%   from a YAML config file and returns an Nx3 matrix of [x, y, z] coordinates.
+
+targets = [];
+fid = fopen(filename, 'r');
+if fid == -1
+    error('Could not open file: %s', filename);
+end
+
+inTargets = false;
+while ~feof(fid)
+    line = fgetl(fid);
+    if ~ischar(line)
+        break;
+    end
+
+    % Check if we've entered the targets section
+    if contains(line, 'targets:')
+        inTargets = true;
+        continue;
+    end
+
+    % If we hit another top-level key, exit targets section
+    if inTargets && ~isempty(line) && line(1) ~= ' ' && line(1) ~= '#'
+        break;
+    end
+
+    % Parse target entries: "  - [x, y, z]"
+    if inTargets
+        % Extract numbers from array format
+        match = regexp(line, '\[\s*([-\d.]+)\s*,\s*([-\d.]+)\s*,\s*([-\d.]+)\s*\]', 'tokens');
+        if ~isempty(match)
+            coords = str2double(match{1});
+            targets = [targets; coords]; %#ok<AGROW>
+        end
+    end
+end
+
+fclose(fid);
+end

aerpaw/radio/CSwSNRTX.py

@@ -0,0 +1,336 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+#
+# SPDX-License-Identifier: GPL-3.0
+#
+# GNU Radio Python Flow Graph
+# Title: CSwSNRTX
+# Author: Ozgur Ozdemir
+# Description: Channel Sounder Transmitter with offset freq
+# GNU Radio version: v3.8.5.0-6-g57bd109d
+
+from gnuradio import analog
+from gnuradio import blocks
+from gnuradio import digital
+from gnuradio import filter
+from gnuradio.filter import firdes
+from gnuradio import gr
+import sys
+import signal
+import threading
+from argparse import ArgumentParser
+from gnuradio.eng_arg import eng_float, intx
+from gnuradio import eng_notation
+from gnuradio import uhd
+import time
+
+
+def derive_num_uavs_from_csv(csv_path=None):
+    """Derive number of UAVs from scenario.csv by counting initial positions.
+
+    The initialPositions column contains a quoted comma-separated list of
+    x,y,z triples.  num_uavs = len(values) / 3.
+    """
+    import os, csv
+    if csv_path is None:
+        csv_path = '/root/miSim/aerpaw/config/scenario.csv'
+    with open(csv_path, 'r') as f:
+        reader = csv.reader(f, skipinitialspace=True)
+        header = [h.strip() for h in next(reader)]
+        row = next(reader)
+    col = header.index('initialPositions')
+    init_pos = row[col].strip()
+    n_vals = len([v.strip() for v in init_pos.split(',') if v.strip()])
+    if n_vals % 3 != 0:
+        raise ValueError(f"initialPositions has {n_vals} values; expected a multiple of 3")
+    return n_vals // 3
+
+
+class TdmScheduler(threading.Thread):
+    """Daemon thread that mutes/unmutes a GNU Radio mute_cc block on a
+    wall-clock TDM schedule.
+
+    Slot assignment:  current_slot = floor(utc_time / slot_duration) % num_uavs
+    Guard interval:   the first *guard_interval* seconds of every slot are
+                      always muted to avoid TX overlap due to clock skew.
+    """
+
+    def __init__(self, mute_block, uav_id, num_uavs,
+                 slot_duration=0.5, guard_interval=0.05):
+        super().__init__(daemon=True)
+        self.mute_block = mute_block
+        self.uav_id = uav_id
+        self.num_uavs = num_uavs
+        self.slot_duration = slot_duration
+        self.guard_interval = guard_interval
+        self._stop_event = threading.Event()
+
+    def run(self):
+        print(f"[TDM] Scheduler started: uav_id={self.uav_id}, "
+              f"num_uavs={self.num_uavs}, slot={self.slot_duration}s, "
+              f"guard={self.guard_interval}s")
+        while not self._stop_event.is_set():
+            now = time.time()
+            slot_time = now % (self.slot_duration * self.num_uavs)
+            current_slot = int(slot_time / self.slot_duration)
+            time_into_slot = slot_time - current_slot * self.slot_duration
+
+            my_slot = (current_slot == self.uav_id)
+            in_guard = (time_into_slot < self.guard_interval)
+
+            should_mute = (not my_slot) or in_guard
+            self.mute_block.set_mute(should_mute)
+
+            # Sleep ~1 ms for responsive timing without busy-waiting
+            self._stop_event.wait(0.001)
+
+    def stop(self):
+        self._stop_event.set()
+
+
+class CSwSNRTX(gr.top_block):
+
+    def __init__(self, args='', freq=3.32e9, gaintx=76, offset=250e3, samp_rate=2e6, sps=16,
+                 uav_id=0, num_uavs=1, slot_duration=0.5, guard_interval=0.05):
+        gr.top_block.__init__(self, "CSwSNRTX")
+
+        ##################################################
+        # Parameters
+        ##################################################
+        self.args = args
+        self.freq = freq
+        self.gaintx = gaintx
+        self.offset = offset
+        self.samp_rate = samp_rate
+        self.sps = sps
+        self.uav_id = uav_id
+        self.num_uavs = num_uavs
+        self.slot_duration = slot_duration
+        self.guard_interval = guard_interval
+
+        ##################################################
+        # Variables
+        ##################################################
+        self.alpha = alpha = 0.99
+
+        ##################################################
+        # Blocks
+        ##################################################
+        self.uhd_usrp_sink_0 = uhd.usrp_sink(
+            ",".join(("", args)),
+            uhd.stream_args(
+                cpu_format="fc32",
+                args='',
+                channels=list(range(0,1)),
+            ),
+            '',
+        )
+        self.uhd_usrp_sink_0.set_center_freq(freq, 0)
+        self.uhd_usrp_sink_0.set_gain(gaintx, 0)
+        self.uhd_usrp_sink_0.set_antenna('TX/RX', 0)
+        self.uhd_usrp_sink_0.set_samp_rate(samp_rate)
+        self.uhd_usrp_sink_0.set_time_unknown_pps(uhd.time_spec())
+        self.root_raised_cosine_filter_0 = filter.fir_filter_ccf(
+            1,
+            firdes.root_raised_cosine(
+                sps,
+                samp_rate,
+                samp_rate/sps,
+                alpha,
+                10*sps+1))
+        self.interp_fir_filter_xxx_0 = filter.interp_fir_filter_ccc(sps, [1]+[0]*(sps-1))
+        self.interp_fir_filter_xxx_0.declare_sample_delay(0)
+        self.digital_glfsr_source_x_0 = digital.glfsr_source_b(12, True, 0, 1)
+        self.digital_chunks_to_symbols_xx_0 = digital.chunks_to_symbols_bc((-1,1), 1)
+        self.blocks_multiply_xx_0 = blocks.multiply_vcc(1)
+        self.blocks_multiply_const_vxx_0 = blocks.multiply_const_cc(1/1.58)
+        self.blocks_mute_0 = blocks.mute_cc(True)  # TDM: start muted
+        self.analog_sig_source_x_0 = analog.sig_source_c(samp_rate, analog.GR_COS_WAVE, offset, 1, 0, 0)
+
+
+        ##################################################
+        # Connections
+        ##################################################
+        self.connect((self.analog_sig_source_x_0, 0), (self.blocks_multiply_xx_0, 1))
+        self.connect((self.blocks_multiply_const_vxx_0, 0), (self.blocks_mute_0, 0))
+        self.connect((self.blocks_mute_0, 0), (self.uhd_usrp_sink_0, 0))
+        self.connect((self.blocks_multiply_xx_0, 0), (self.blocks_multiply_const_vxx_0, 0))
+        self.connect((self.digital_chunks_to_symbols_xx_0, 0), (self.interp_fir_filter_xxx_0, 0))
+        self.connect((self.digital_glfsr_source_x_0, 0), (self.digital_chunks_to_symbols_xx_0, 0))
+        self.connect((self.interp_fir_filter_xxx_0, 0), (self.root_raised_cosine_filter_0, 0))
+        self.connect((self.root_raised_cosine_filter_0, 0), (self.blocks_multiply_xx_0, 0))
+
+
+    def get_args(self):
+        return self.args
+
+    def set_args(self, args):
+        self.args = args
+
+    def get_freq(self):
+        return self.freq
+
+    def set_freq(self, freq):
+        self.freq = freq
+        self.uhd_usrp_sink_0.set_center_freq(self.freq, 0)
+
+    def get_gaintx(self):
+        return self.gaintx
+
+    def set_gaintx(self, gaintx):
+        self.gaintx = gaintx
+        self.uhd_usrp_sink_0.set_gain(self.gaintx, 0)
+
+    def get_offset(self):
+        return self.offset
+
+    def set_offset(self, offset):
+        self.offset = offset
+        self.analog_sig_source_x_0.set_frequency(self.offset)
+
+    def get_samp_rate(self):
+        return self.samp_rate
+
+    def set_samp_rate(self, samp_rate):
+        self.samp_rate = samp_rate
+        self.analog_sig_source_x_0.set_sampling_freq(self.samp_rate)
+        self.root_raised_cosine_filter_0.set_taps(firdes.root_raised_cosine(self.sps, self.samp_rate, self.samp_rate/self.sps, self.alpha, 10*self.sps+1))
+        self.uhd_usrp_sink_0.set_samp_rate(self.samp_rate)
+
+    def get_sps(self):
+        return self.sps
+
+    def set_sps(self, sps):
+        self.sps = sps
+        self.interp_fir_filter_xxx_0.set_taps([1]+[0]*(self.sps-1))
+        self.root_raised_cosine_filter_0.set_taps(firdes.root_raised_cosine(self.sps, self.samp_rate, self.samp_rate/self.sps, self.alpha, 10*self.sps+1))
+
+    def get_alpha(self):
+        return self.alpha
+
+    def set_alpha(self, alpha):
+        self.alpha = alpha
+        self.root_raised_cosine_filter_0.set_taps(firdes.root_raised_cosine(self.sps, self.samp_rate, self.samp_rate/self.sps, self.alpha, 10*self.sps+1))
+
+
+def argument_parser():
+    description = 'Channel Sounder Transmitter with offset freq'
+    parser = ArgumentParser(description=description)
+    parser.add_argument(
+        "--args", dest="args", type=str, default='',
+        help="Set args [default=%(default)r]")
+    parser.add_argument(
+        "--freq", dest="freq", type=eng_float, default="3.32G",
+        help="Set freq [default=%(default)r]")
+    parser.add_argument(
+        "--gaintx", dest="gaintx", type=eng_float, default="76.0",
+        help="Set gaintx [default=%(default)r]")
+    parser.add_argument(
+        "--offset", dest="offset", type=eng_float, default="250.0k",
+        help="Set offset [default=%(default)r]")
+    parser.add_argument(
+        "--samp-rate", dest="samp_rate", type=eng_float, default="2.0M",
+        help="Set samp_rate [default=%(default)r]")
+    parser.add_argument(
+        "--sps", dest="sps", type=intx, default=16,
+        help="Set sps [default=%(default)r]")
+    parser.add_argument(
+        "--uav-id", dest="uav_id", type=int, default=None,
+        help="TDM slot index for this UAV (0-indexed). "
+             "If omitted, read from config/client.yaml.")
+    parser.add_argument(
+        "--num-uavs", dest="num_uavs", type=int, default=None,
+        help="Total number of UAVs (TDM slots). "
+             "If omitted, derived from config/scenario.csv.")
+    parser.add_argument(
+        "--slot-duration", dest="slot_duration", type=float, default=None,
+        help="TDM slot duration in seconds [default: 0.5 or from client.yaml]")
+    parser.add_argument(
+        "--guard-interval", dest="guard_interval", type=float, default=None,
+        help="TDM guard interval in seconds [default: 0.05 or from client.yaml]")
+    return parser
+
+
+def _resolve_tdm_options(options):
+    """Fill in TDM parameters from client.yaml / scenario.csv when not
+    provided on the command line."""
+    import os, yaml
+    cfg_dir = '/root/miSim/aerpaw/config'
+    env_cfg = os.environ.get('AERPAW_CLIENT_CONFIG', '')
+    if env_cfg:
+        yaml_path = env_cfg if os.path.isabs(env_cfg) else os.path.join('/root/miSim/aerpaw', env_cfg)
+    else:
+        yaml_path = os.path.join(cfg_dir, 'client.yaml')
+
+    cfg = {}
+    if os.path.isfile(yaml_path):
+        with open(yaml_path, 'r') as f:
+            cfg = yaml.safe_load(f) or {}
+
+    tdm_cfg = cfg.get('tdm', {})
+
+    if options.uav_id is None:
+        options.uav_id = int(cfg.get('uav_id', 0))
+    if options.slot_duration is None:
+        options.slot_duration = float(tdm_cfg.get('slot_duration', 0.5))
+    if options.guard_interval is None:
+        options.guard_interval = float(tdm_cfg.get('guard_interval', 0.05))
+    if options.num_uavs is None:
+        try:
+            options.num_uavs = derive_num_uavs_from_csv(
+                '/root/miSim/aerpaw/config/scenario.csv')
+        except Exception as e:
+            print(f"[TDM] Warning: could not derive num_uavs from scenario.csv: {e}")
+            print("[TDM] Defaulting to num_uavs=1 (TDM effectively disabled)")
+            options.num_uavs = 1
+
+    return options
+
+
+def main(top_block_cls=CSwSNRTX, options=None):
+    if options is None:
+        options = argument_parser().parse_args()
+
+    options = _resolve_tdm_options(options)
+
+    tb = top_block_cls(
+        args=options.args, freq=options.freq, gaintx=options.gaintx,
+        offset=options.offset, samp_rate=options.samp_rate, sps=options.sps,
+        uav_id=options.uav_id, num_uavs=options.num_uavs,
+        slot_duration=options.slot_duration,
+        guard_interval=options.guard_interval)
+
+    tdm_sched = TdmScheduler(
+        tb.blocks_mute_0,
+        uav_id=options.uav_id,
+        num_uavs=options.num_uavs,
+        slot_duration=options.slot_duration,
+        guard_interval=options.guard_interval)
+
+    def sig_handler(sig=None, frame=None):
+        tdm_sched.stop()
+        tb.stop()
+        tb.wait()
+        sys.exit(0)
+
+    signal.signal(signal.SIGINT, sig_handler)
+    signal.signal(signal.SIGTERM, sig_handler)
+
+    tb.start()
+    tdm_sched.start()
+
+    try:
+        input('Press Enter to quit: ')
+    except EOFError:
+        pass
+    tdm_sched.stop()
+    tb.stop()
+    tb.wait()
+
+
+if __name__ == '__main__':
+    main()
+(END)
+
+

aerpaw/radio/startchannelsounderRXGRC.sh

@@ -0,0 +1,23 @@
+#!/bin/bash
+GAIN_RX=30
+OFFSET=250e3
+SAMP_RATE=2e6
+SPS=16
+
+# Custom
+RX_FREQ=3.32e9
+
+if [ "$LAUNCH_MODE" == "TESTBED" ]; then
+  #To select a specific device
+  #ARGS="serial=31E74A9"
+  ARGS=NULL
+elif [ "$LAUNCH_MODE" == "EMULATION" ]; then
+  #ARGS='type=zmq'
+  ARGS=NULL
+else
+  echo "Warning: LAUNCH_MODE not set (got '${LAUNCH_MODE}'). Defaulting to TESTBED."
+  ARGS=NULL
+fi
+
+cd $PROFILE_DIR"/SDR_control/Channel_Sounderv3"
+python3 CSwSNRRX.py --freq $RX_FREQ --gainrx $GAIN_RX --noise 0 --args $ARGS --offset $OFFSET --samp-rate $SAMP_RATE --sps $SPS

aerpaw/radio/startchannelsounderTXGRC.sh

@@ -0,0 +1,23 @@
+#!/bin/bash
+GAIN_TX=76
+OFFSET=250e3
+SAMP_RATE=2e6
+SPS=16
+
+# Custom
+TX_FREQ=3.32e9
+
+if [ "$LAUNCH_MODE" == "TESTBED" ]; then
+  #To select a specific device
+  #ARGS="serial=31E74A9"
+  ARGS=NULL
+elif [ "$LAUNCH_MODE" == "EMULATION" ]; then
+  #ARGS='type=zmq'
+  ARGS=NULL
+else
+  echo "Warning: LAUNCH_MODE not set (got '${LAUNCH_MODE}'). Defaulting to TESTBED."
+  ARGS=NULL
+fi
+
+cd $PROFILE_DIR"/SDR_control/Channel_Sounderv3"
+python3 CSwSNRTX.py --freq $TX_FREQ --gaintx $GAIN_TX --args $ARGS --offset $OFFSET --samp-rate $SAMP_RATE --sps $SPS

aerpaw/radio/updateScripts.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+cp startchannelsounderRXGRC.sh /root/Profiles/ProfileScripts/Radio/Helpers/.
+cp startchannelsounderTXGRC.sh /root/Profiles/ProfileScripts/Radio/Helpers/.
+
+cp CSwSNRRX.py /root/Profiles/SDR_control/Channel_Sounderv3/.
+cp CSwSNRTX.py /root/Profiles/SDR_control/Channel_Sounderv3/.

aerpaw/results/plotGpsLogs.m

@@ -0,0 +1,97 @@
+function [f, G] = plotGpsLogs(logDirs, seaToGroundLevel)
+    arguments (Input)
+        logDirs (1, 1) string;
+        seaToGroundLevel (1, 1) double = 110; % measured approximately from USGS national map viewer for the AERPAW test field
+    end
+    arguments (Output)
+        f (1, 1) matlab.ui.Figure;
+        G cell;
+    end
+    % Plot setup
+    f = uifigure;
+    gf = geoglobe(f);
+    hold(gf, "on");
+    c = ["g", "b", "m", "c"]; % plotting colors
+    
+    % paths
+    scenarioCsv = fullfile(matlab.project.rootProject().RootFolder, "aerpaw", "config", "scenario.csv");
+    
+    % configured data
+    params = readScenarioCsv(scenarioCsv);
+    
+    fID = fopen(fullfile(matlab.project.rootProject().RootFolder, "aerpaw", "config", "client1.yaml"), 'r');
+    yaml = fscanf(fID, '%s');
+    fclose(fID);
+    % origin (LLA)
+    lla0 = [str2double(yaml((strfind(yaml, 'lat:') + 4):(strfind(yaml, 'lon:') - 1))), str2double(yaml((strfind(yaml, 'lon:') + 4):(strfind(yaml, 'alt:') - 1))), seaToGroundLevel];
+    
+    logDirs = dir(logDirs);
+    logDirs = logDirs(3:end);
+    logDirs = logDirs([logDirs.isdir] == 1);
+
+    G = cell(size(logDirs));
+    for ii = 1:size(logDirs, 1)
+        % Find GPS log CSV
+        gpsCsv = dir(fullfile(logDirs(ii).folder, logDirs(ii).name));
+        gpsCsv = gpsCsv(endsWith({gpsCsv(:).name}, "_gps_log.csv"));
+        gpsCsv = fullfile(gpsCsv.folder, gpsCsv.name);
+    
+        % Read GPS log CSV
+        G{ii} = readGpsLogs(gpsCsv);
+    
+        % Find when algorithm begins/ends (using ENU altitude rate change)
+        verticalSpeed = movmean(abs(diff(G{ii}.Altitude)), [10, 0]);
+    
+        % Automatically detect start/stop of algorithm flight (ignore takeoff, setup, return to liftoff, landing segments of flight)
+        pctThreshold = 60; % pctThreshold may need adjusting depending on your flight
+        startIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, "first");
+        stopIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, "last");
+    
+        % % Plot whole flight, including setup/cleanup
+        % startIdx = 1;
+        % stopIdx = length(verticalSpeed);
+
+        % Convert LLA trajectory data to ENU for external analysis
+        % NaN out entries outside the algorithm flight range so they don't plot
+        enu = NaN(height(G{ii}), 3);
+        enu(startIdx:stopIdx, :) = lla2enu([G{ii}.Latitude(startIdx:stopIdx), G{ii}.Longitude(startIdx:stopIdx), G{ii}.Altitude(startIdx:stopIdx)], lla0, "flat");
+        enu = array2table(enu, 'VariableNames', ["East", "North", "Up"]);
+        G{ii} = [G{ii}, enu];
+    
+        % Plot recorded trajectory over specified range of indices
+        geoplot3(gf, G{ii}.Latitude(startIdx:stopIdx), G{ii}.Longitude(startIdx:stopIdx), G{ii}.Altitude(startIdx:stopIdx) + seaToGroundLevel, c(mod(ii, length(c))), 'LineWidth', 2, "MarkerSize", 5);
+    end
+    
+    % Plot domain
+    altOffset = 1; % to avoid clipping into the ground when displayed
+    domain = [lla0; enu2lla(params.domainMax, lla0, "flat")];
+    geoplot3(gf, [domain(1, 1), domain(2, 1), domain(2, 1), domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2), domain(2, 2), domain(2, 2), domain(1, 2)], repmat(domain(1, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'k');
+    geoplot3(gf, [domain(1, 1), domain(2, 1), domain(2, 1), domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2), domain(2, 2), domain(2, 2), domain(1, 2)], repmat(domain(2, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'k');
+    geoplot3(gf, [domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
+    geoplot3(gf, [domain(2, 1), domain(2, 1)], [domain(1, 2), domain(1, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
+    geoplot3(gf, [domain(1, 1), domain(1, 1)], [domain(2, 2), domain(2, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
+    geoplot3(gf, [domain(2, 1), domain(2, 1)], [domain(2, 2), domain(2, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
+    
+    % Plot floor (minimum altitude constraint)
+    floorAlt = params.minAlt;
+    geoplot3(gf, [domain(1, 1), domain(2, 1), domain(2, 1), domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2), domain(2, 2), domain(2, 2), domain(1, 2)], repmat(domain(1, 3) + altOffset + floorAlt, 1, 5), 'LineWidth', 3, 'Color', 'r');
+    
+    % Plot objective
+    objectivePos = [params.objectivePos, 0];
+    llaObj = enu2lla(objectivePos, lla0, "flat");
+    geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), domain(2, 3)], 'LineWidth', 3, "Color", 'y');
+    
+    % Plot obstacles
+    for ii = 1:params.numObstacles
+        obstacle = enu2lla([params.obstacleMin((1 + (ii - 1) * 3):(ii * 3)); params.obstacleMax((1 + (ii - 1) * 3):(ii * 3))], lla0, "flat");
+        geoplot3(gf, [obstacle(1, 1), obstacle(2, 1), obstacle(2, 1), obstacle(1, 1), obstacle(1, 1)], [obstacle(1, 2), obstacle(1, 2), obstacle(2, 2), obstacle(2, 2), obstacle(1, 2)], repmat(obstacle(1, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'r');
+        geoplot3(gf, [obstacle(1, 1), obstacle(2, 1), obstacle(2, 1), obstacle(1, 1), obstacle(1, 1)], [obstacle(1, 2), obstacle(1, 2), obstacle(2, 2), obstacle(2, 2), obstacle(1, 2)], repmat(obstacle(2, 3) + altOffset, 1, 5), 'LineWidth', 3, 'Color', 'r');
+        geoplot3(gf, [obstacle(1, 1), obstacle(1, 1)], [obstacle(1, 2), obstacle(1, 2)], obstacle(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'r');
+        geoplot3(gf, [obstacle(2, 1), obstacle(2, 1)], [obstacle(1, 2), obstacle(1, 2)], obstacle(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'r');
+        geoplot3(gf, [obstacle(1, 1), obstacle(1, 1)], [obstacle(2, 2), obstacle(2, 2)], obstacle(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'r');
+        geoplot3(gf, [obstacle(2, 1), obstacle(2, 1)], [obstacle(2, 2), obstacle(2, 2)], obstacle(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'r');
+    end
+    
+    % finish
+    hold(gf, "off");
+end

aerpaw/results/plotRadioLogs.m

@@ -0,0 +1,75 @@
+function [f, R] = plotRadioLogs(resultsPath)
+    arguments (Input)
+        resultsPath (1, 1) string;
+    end
+    arguments (Output)
+        f (1, 1) matlab.ui.Figure;
+        R cell;
+    end
+
+    logDirs = dir(resultsPath);
+    logDirs = logDirs(3:end);
+    logDirs = logDirs([logDirs.isdir] == 1);
+
+    R = cell(size(logDirs));
+    for ii = 1:size(logDirs, 1)
+        R{ii} = readRadioLogs(fullfile(logDirs(ii).folder, logDirs(ii).name));
+    end
+
+    % Discard rows where any non-NaN dB metric is below -200 (sentinel values)
+    for ii = 1:numel(R)
+        snr = R{ii}.SNR;
+        pwr = R{ii}.Power;
+        bad = (snr < -200 & ~isnan(snr)) | (pwr < -200 & ~isnan(pwr));
+        R{ii}(bad, :) = [];
+    end
+
+    % Build legend labels and color map for up to 4 UAVs
+    nUAV = numel(R);
+    colors = lines(nUAV * nUAV);
+    styles = ["-o", "-s", "-^", "-d", "-v", "-p", "-h", "-<", "->", "-+", "-x", "-*"];
+
+    metricNames = ["SNR", "Power", "Quality"];
+    yLabels     = ["SNR (dB)", "Power (dB)", "Quality"];
+
+    f = figure;
+    tl = tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'compact');
+
+    for mi = 1:numel(metricNames)
+        ax = nexttile(tl);
+        hold(ax, 'on');
+        grid(ax, 'on');
+
+        legendEntries = string.empty;
+        ci = 1;
+        for rxIdx = 1:nUAV
+            tbl = R{rxIdx};
+            txIDs = unique(tbl.TxUAVID);
+            for ti = 1:numel(txIDs)
+                txID = txIDs(ti);
+                rows = tbl(tbl.TxUAVID == txID, :);
+                vals = rows.(metricNames(mi));
+
+                % Skip if all NaN for this metric
+                if all(isnan(vals))
+                    continue;
+                end
+
+                si = mod(ci - 1, numel(styles)) + 1;
+                plot(ax, rows.Timestamp, vals, styles(si), ...
+                    'Color', colors(ci, :), 'MarkerSize', 3, 'LineWidth', 1);
+                legendEntries(end+1) = sprintf("TX %d → RX %d", txID, tbl.RxUAVID(1)); %#ok<AGROW>
+                ci = ci + 1;
+            end
+        end
+
+        ylabel(ax, yLabels(mi));
+        if mi == numel(metricNames)
+            xlabel(ax, 'Time');
+        end
+        legend(ax, legendEntries, 'Location', 'best');
+        hold(ax, 'off');
+    end
+
+    title(tl, 'Radio Channel Metrics');
+end

aerpaw/results/readGpsLogs.m

@@ -0,0 +1,32 @@
+function [G] = readGpsLogs(logPath)
+    arguments (Input)
+        logPath (1, 1) string {isfile(logPath)};
+    end
+    
+    arguments (Output)
+        G (:, 10) table;
+    end
+    
+    G = readtable(logPath, "ReadVariableNames", false);
+    
+    % first column is just index, meaningless, toss it
+    G = G(:, 2:end);
+    
+    % switch to the correct LLA convention (lat, lon, alt)
+    tmp = G(:, 2);
+    G(:, 2) = G(:, 1);
+    G(:, 1) = tmp;
+    
+    % Split pitch, yaw, roll data read in as one string per timestep into separate columns
+    PYR = cell2mat(cellfun(@(x) str2num(strip(strip(x, "left", "("), "right", ")")), table2cell(G(:, 5)), "UniformOutput", false)); %#ok<ST2NM>
+    % Reinsert to original table
+    G = [G(:, 1:3), table(PYR(:, 1), VariableNames="Pitch"), table(PYR(:, 2), VariableNames="Yaw"), table(PYR(:, 3), VariableNames="Roll"), G(:, 6:end)];
+    
+    % Clean up datetime entry
+    G = [table(datetime(G{:,8}, "InputFormat","yyyy-MM-dd HH:mm:ss.SSS", "TimeZone","America/New_York")), G(:, [1:7, 9:10])];
+    
+    % Fix variable names
+    G.Properties.VariableNames = ["Timestamp", "Latitude", "Longitude", "Altitude", "Pitch", "Yaw", "Roll", "Voltage", "GPS Status", "Satellites"];
+    G.Properties.VariableTypes = ["datetime", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
+    G.Properties.VariableUnits = ["yyyy-MM-dd HH:mm:ss.SSS (UTC+5)", "deg", "deg", "m", "deg", "deg", "deg", "Volts", "", ""];
+end

aerpaw/results/readRadioLogs.m

@@ -0,0 +1,65 @@
+function R = readRadioLogs(logPath)
+    arguments (Input)
+        logPath (1, 1) string {isfolder(logPath)};
+    end
+
+    arguments (Output)
+        R (:, 6) table;
+    end
+
+    % Extract receiving UAV ID from directory name (e.g. "uav0_..." → 0)
+    [~, dirName] = fileparts(logPath);
+    rxID = int32(sscanf(dirName, 'uav%d'));
+
+    metrics = ["quality", "snr", "power"];
+    logs = dir(logPath);
+    logs = logs(endsWith({logs(:).name}, metrics + "_log.txt"));
+
+    R = table(datetime.empty(0,1), zeros(0,1,'int32'), zeros(0,1,'int32'), zeros(0,1), zeros(0,1), zeros(0,1), ...
+        'VariableNames', ["Timestamp", "TxUAVID", "RxUAVID", "SNR", "Power", "Quality"]);
+
+    for ii = 1:numel(logs)
+        filepath = fullfile(logs(ii).folder, logs(ii).name);
+
+        % Determine which metric this file contains
+        metric = "";
+        for m = 1:numel(metrics)
+            if endsWith(logs(ii).name, metrics(m) + "_log.txt")
+                metric = metrics(m);
+                break;
+            end
+        end
+
+        fid = fopen(filepath, 'r');
+        % Skip 3 lines: 2 junk (tail errors) + 1 header (tx_uav_id,value)
+        for k = 1:3
+            fgetl(fid);
+        end
+        data = textscan(fid, '[%26c] %d,%f');
+        fclose(fid);
+
+        ts = datetime(data{1}, 'InputFormat', 'yyyy-MM-dd HH:mm:ss.SSSSSS');
+        txId = int32(data{2});
+        val = data{3};
+
+        n = numel(ts);
+        t = table(ts, txId, repmat(rxID, n, 1), NaN(n,1), NaN(n,1), NaN(n,1), ...
+            'VariableNames', ["Timestamp", "TxUAVID", "RxUAVID", "SNR", "Power", "Quality"]);
+
+        switch metric
+            case "snr",     t.SNR = val;
+            case "power",   t.Power = val;
+            case "quality", t.Quality = val;
+        end
+
+        R = [R; t]; %#ok<AGROW>
+    end
+
+    R = sortrows(R, "Timestamp");
+
+    % Remove rows during defined guard period between TDM shifts
+    R(R.TxUAVID == -1, :) = [];
+
+    % Remove self-reception rows (TX == RX)
+    R(R.TxUAVID == R.RxUAVID, :) = [];
+end

aerpaw/results/resultsAnalysis.m

@@ -0,0 +1,68 @@
+%% Plot AERPAW logs (trajectory, radio)
+resultsPath = fullfile(matlab.project.rootProject().RootFolder, "sandbox", "t1"); % Define path to results copied from AERPAW platform
+
+% Plot GPS logged data and scenario information (domain, objective, obstacles)
+seaToGroundLevel = 110; % measured approximately from USGS national map viewer
+[fGlobe, G] = plotGpsLogs(resultsPath, seaToGroundLevel);
+
+% Plot radio statistics
+[fRadio, R] = plotRadioLogs(resultsPath);
+
+%% Run simulation
+% Run miSim using same AERPAW scenario definition CSV
+csvPath = fullfile(matlab.project.rootProject().RootFolder, "aerpaw", "config", "scenario.csv");
+params = readScenarioCsv(csvPath);
+
+% Visualization settings
+plotCommsGeometry = false;
+makePlots = true;
+makeVideo = true;
+
+% Define scenario according to CSV specification
+domain = rectangularPrism;
+domain = domain.initialize([params.domainMin; params.domainMax], REGION_TYPE.DOMAIN, "Domain");
+domain.objective = domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos, reshape(params.objectiveVar, [2 2])), domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum);
+
+agents = cell(size(params.initialPositions, 2) / 3, 1);
+for ii = 1:size(agents, 1)
+    agents{ii} = agent;
+
+    sensorModel = sigmoidSensor;
+    sensorModel = sensorModel.initialize(params.alphaDist(ii), params.betaDist(ii), params.alphaTilt(ii), params.betaTilt(ii));
+
+    collisionGeometry = spherical;
+    collisionGeometry = collisionGeometry.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), params.collisionRadius(ii), REGION_TYPE.COLLISION, sprintf("Agent %d collision geometry", ii));
+
+    agents{ii} = agents{ii}.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), collisionGeometry, sensorModel, params.comRange(ii), params.maxIter, params.initialStepSize, sprintf("Agent %d", ii), plotCommsGeometry);
+end
+
+% Create obstacles
+obstacles = cell(params.numObstacles, 1);
+for ii = 1:size(obstacles, 1)
+    obstacles{ii} = rectangularPrism;
+    obstacles{ii} = obstacles{ii}.initialize([params.obstacleMin((((ii - 1) * 3) + 1):(ii * 3)); params.obstacleMax((((ii - 1) * 3) + 1):(ii * 3))], "OBSTACLE", sprintf("Obstacle %d", ii));
+end
+
+% Set up simulation
+sim = miSim;
+sim = sim.initialize(domain, agents, params.barrierGain, params.barrierExponent, params.minAlt, params.timestep, params.maxIter, obstacles, makePlots, makeVideo);
+
+% Save simulation parameters to output file
+sim.writeInits();
+
+% Run
+sim = sim.run();
+
+%% Plot AERPAW trajectory logs onto simulated result for comparison
+% Duplicate plot to overlay with logged trajectories
+comparison = figure;
+copyobj(sim.f.Children, comparison);
+
+% Plot trajectories on top
+for ii = 1:size(G, 1)
+    for jj = 1:size(sim.spatialPlotIndices, 2)
+        hold(comparison.Children.Children(sim.spatialPlotIndices(jj)), "on");
+        plot3(comparison.Children(1).Children(sim.spatialPlotIndices(jj)), G{ii}.East, G{ii}.North, G{ii}.Up + seaToGroundLevel, 'Color', 'r', 'LineWidth', 1);
+        hold(comparison.Children.Children(sim.spatialPlotIndices(jj)), "off");
+    end
+end

aerpaw/run_uav.sh

@@ -0,0 +1,65 @@
+#!/bin/bash
+# run_uav.sh - Wrapper for UAV runner
+# Launches UAV client with environment-specific configuration
+#
+# Usage:
+#   ./run_uav.sh local                          # defaults to config/client.yaml
+#   ./run_uav.sh testbed config/client2.yaml     # use a specific config file
+
+set -e
+
+# Change to aerpaw directory
+cd /root/miSim/aerpaw
+
+# Activate venv if it exists
+if [ -d "venv" ]; then
+    source venv/bin/activate
+fi
+
+# Determine environment from argument (required)
+if [ "$1" = "testbed" ]; then
+    ENV="testbed"
+elif [ "$1" = "local" ]; then
+    ENV="local"
+else
+    echo "Error: Environment not specified."
+    echo "Usage: $0 [local|testbed] [config_file]"
+    echo ""
+    echo "  local   - Use local/simulation configuration"
+    echo "  testbed - Use AERPAW testbed configuration"
+    echo ""
+    echo "  config_file - Path to client YAML (default: config/client.yaml)"
+    exit 1
+fi
+
+# Client config file (optional 2nd argument)
+CONFIG_FILE="${2:-config/client.yaml}"
+if [ ! -f "$CONFIG_FILE" ]; then
+    echo "Error: Config file not found: $CONFIG_FILE"
+    exit 1
+fi
+
+echo "[run_uav] Environment: $ENV"
+echo "[run_uav] Config file: $CONFIG_FILE"
+
+# Export for Python scripts to use
+export AERPAW_ENV="$ENV"
+export AERPAW_CLIENT_CONFIG="$(realpath "$CONFIG_FILE")"
+
+# Read MAVLink connection from config file using Python
+CONN=$(python3 -c "
+import yaml
+with open('$CONFIG_FILE') as f:
+    cfg = yaml.safe_load(f)
+env = cfg['environments']['$ENV']['mavlink']
+print(f\"udp:{env['ip']}:{env['port']}\")
+")
+
+echo "[run_uav] MAVLink connection: $CONN"
+
+# Run via aerpawlib
+echo "[run_uav] Starting UAV runner..."
+python3 -m aerpawlib \
+    --script client.uav_runner \
+    --conn "$CONN" \
+    --vehicle drone

geometries/@cone/initialize.m

@@ -1,6 +1,6 @@
 function obj = initialize(obj, center, radius, height, tag, label)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'cone')};
+        obj (1, 1) {mustBeA(obj, "cone")};
         center (1, 3) double;
         radius (1, 1) double;
         height (1, 1) double;
@@ -8,7 +8,7 @@ function obj = initialize(obj, center, radius, height, tag, label)
         label (1, 1) string = "";
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'cone')};
+        obj (1, 1) {mustBeA(obj, "cone")};
     end
 
     obj.center = center;

geometries/@cone/plot.m

@@ -1,13 +1,13 @@
 function [obj, f] = plot(obj, ind, f, maxAlt)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'cone')};
+        obj (1, 1) {mustBeA(obj, "cone")};
         ind (1, :) double = NaN;
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
         maxAlt (1, 1) = 10;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'cone')};
+        obj (1, 1) {mustBeA(obj, "cone")};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
     end
     
     % Create axes if they don't already exist
@@ -31,7 +31,7 @@ function [obj, f] = plot(obj, ind, f, maxAlt)
         o = surf(f.CurrentAxes, X, Y, Z);
     else
         hold(f.Children(1).Children(ind(1)), "on");
-        o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(obj.tag.color, 1, 1, 3), 'FaceAlpha', 0.25, 'EdgeColor', 'none');
+        o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(regionTypeColor(obj.tag), 1, 1, 3), "FaceAlpha", 0.25, "EdgeColor", "none");
         hold(f.Children(1).Children(ind(1)), "off");
     end
     

geometries/@rectangularPrism/closestToPoint.m

@@ -1,6 +1,6 @@
 function cPos = closestToPoint(obj, pos)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         pos (:, 3) double;
     end
     arguments (Output)

geometries/@rectangularPrism/contains.m

@@ -1,6 +1,6 @@
 function c = contains(obj, pos)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         pos (:, 3) double;
     end
     arguments (Output)

geometries/@rectangularPrism/containsLine.m

@@ -1,6 +1,6 @@
 function c = containsLine(obj, pos1, pos2)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         pos1 (1, 3) double;
         pos2 (1, 3) double;
     end
@@ -21,7 +21,7 @@ function c = containsLine(obj, pos1, pos2)
     tMax = 1;
     for ii = 1:3
         % line is parallel to geometry
-        if abs(d(ii)) < 1e-12
+        if abs(d(ii)) < 1e-9
             if pos1(ii) < obj.minCorner(ii) || pos1(ii) > obj.maxCorner(ii)
                 c = false;
                 return;

geometries/@rectangularPrism/distance.m

@@ -1,6 +1,6 @@
 function d = distance(obj, pos)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         pos (:, 3) double;
     end
     arguments (Output)

geometries/@rectangularPrism/distanceGradient.m

@@ -1,6 +1,6 @@
 function g = distanceGradient(obj, pos)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         pos (:, 3) double;
     end
     arguments (Output)

geometries/@rectangularPrism/initialize.m

@@ -1,6 +1,6 @@
 function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretizationStep)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         bounds (2, 3) double;
         tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
         label (1, 1) string = "";
@@ -8,7 +8,7 @@ function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretiza
         discretizationStep (1, 1) double = 1;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
     end
 
     obj.tag = tag;

geometries/@rectangularPrism/initializeRandom.m

@@ -1,6 +1,6 @@
 function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain, minAlt)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
         label (1, 1) string = "";
         minDimension (1, 1) double = 10;
@@ -9,7 +9,7 @@ function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, d
         minAlt (1, 1) double = 1;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
     end
     
     % Produce random bounds based on region type

geometries/@rectangularPrism/plotWireframe.m

@@ -1,12 +1,12 @@
 function [obj, f] = plotWireframe(obj, ind, f)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
         ind (1, :) double = NaN;
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
     end
 
     % Create axes if they don't already exist
@@ -19,10 +19,10 @@ function [obj, f] = plotWireframe(obj, ind, f)
 
     % Plot the boundaries of the geometry into 3D view
     if isnan(ind)
-        o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
+        o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 2);
     else
         hold(f.Children(1).Children(ind(1)), "on");
-        o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
+        o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 2);
         hold(f.Children(1).Children(ind(1)), "off");
     end
 

geometries/@rectangularPrism/random.m

@@ -1,6 +1,6 @@
 function r = random(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        obj (1, 1) {mustBeA(obj, "rectangularPrism")};
     end
     arguments (Output)
         r (1, 3) double

geometries/@rectangularPrism/rectangularPrism.m

@@ -28,6 +28,12 @@ classdef rectangularPrism
     end
 
     methods (Access = public)
+        function obj = rectangularPrism()
+            arguments (Output)
+                obj (1, 1) rectangularPrism
+            end
+            obj.objective = sensingObjective;
+        end
         [obj   ] = initialize(obj, bounds, tag, label, objectiveFunction, discretizationStep);
         [obj   ] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain);
         [r     ] = random(obj);

geometries/@spherical/contains.m

@@ -1,6 +1,6 @@
 function c = contains(obj, pos)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
+        obj (1, 1) {mustBeA(obj, "spherical")};
         pos (:, 3) double;
     end
     arguments (Output)

geometries/@spherical/containsLine.m

@@ -1,6 +1,6 @@
 function c = containsLine(obj, pos1, pos2)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
+        obj (1, 1) {mustBeA(obj, "spherical")};
         pos1 (1, 3) double;
         pos2 (1, 3) double;
     end

geometries/@spherical/initialize.m

@@ -1,13 +1,13 @@
 function obj = initialize(obj, center, radius, tag, label)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
+        obj (1, 1) {mustBeA(obj, "spherical")};
         center (1, 3) double;
         radius  (1, 1) double;
         tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
         label (1, 1) string = "";
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
+        obj (1, 1) {mustBeA(obj, "spherical")};
     end
 
     obj.tag = tag;

geometries/@spherical/plotWireframe.m

@@ -1,12 +1,12 @@
 function [obj, f] = plotWireframe(obj, ind, f)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
+        obj (1, 1) {mustBeA(obj, "spherical")};
         ind (1, :) double = NaN;
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")} = figure;
     end
     arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
+        obj (1, 1) {mustBeA(obj, "spherical")};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+        f (1, 1) {mustBeA(f, "matlab.ui.Figure")};
     end
 
     % Create axes if they don't already exist
@@ -25,10 +25,10 @@ function [obj, f] = plotWireframe(obj, ind, f)
 
     % Plot the boundaries of the geometry into 3D view
     if isnan(ind)
-        o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
+        o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 2);
     else
         hold(f.Children(1).Children(ind(1)), "on");
-        o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
+        o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 1);
         hold(f.Children(1).Children(ind(1)), "off");
     end
 

geometries/@spherical/random.m

@@ -1,6 +1,6 @@
 function r = random(obj)
     arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
+        obj (1, 1) {mustBeA(obj, "spherical")};
     end
     arguments (Output)
         r (1, 3) double

geometries/@spherical/spherical.m

@@ -2,12 +2,12 @@ classdef spherical
     % Rectangular prism geometry
     properties (SetAccess = private, GetAccess = public)
         % Spatial
-        center = NaN;
+        center = NaN(1, 3);
         radius = NaN;
         diameter = NaN;
 
-        vertices; % fake vertices
+        vertices = NaN(6, 3); % fake vertices
-        edges; % fake edges
+        edges = NaN(8, 2); % fake edges
 
         % Plotting
         lines;
@@ -22,6 +22,12 @@ classdef spherical
     end
 
     methods (Access = public)
+        function obj = spherical()
+            arguments (Output)
+                obj (1, 1) spherical
+            end
+            obj.objective = sensingObjective;
+        end
         [obj   ] = initialize(obj, center, radius, tag, label);
         [r     ] = random(obj);
         [c     ] = contains(obj, pos);

geometries/REGION_TYPE.m

@@ -1,20 +1,10 @@
-classdef REGION_TYPE
+classdef REGION_TYPE < uint8
-    properties
-        id
-        color
-    end
     enumeration
-        INVALID (0, [255, 127, 255]); % default value
+        INVALID   (0) % default value
-        DOMAIN (1, [0, 0, 0]); % domain region
+        DOMAIN    (1) % domain region
-        OBSTACLE (2, [255, 127, 127]); % obstacle region
+        OBSTACLE  (2) % obstacle region
-        COLLISION (3, [255, 255, 128]); % collision avoidance region
+        COLLISION (3) % collision avoidance region
-        FOV (4, [255, 165, 0]); % field of view region
+        FOV       (4) % field of view region
-        COMMS (5, [0, 255, 0]); % comunications region
+        COMMS     (5) % comunications region
-    end
-    methods
-        function obj = REGION_TYPE(id, color)
-            obj.id = id;
-            obj.color = color./ 255;
-        end
     end
 end

geometries/regionTypeColor.m

@@ -0,0 +1,24 @@
+function color = regionTypeColor(tag)
+% regionTypeColor  Return the RGB color (0-1 range) for a REGION_TYPE value.
+arguments (Input)
+    tag (1, 1) REGION_TYPE
+end
+arguments (Output)
+    color (1, 3) double
+end
+
+switch tag
+    case REGION_TYPE.DOMAIN
+        color = [0, 0, 0] / 255;
+    case REGION_TYPE.OBSTACLE
+        color = [255, 127, 127] / 255;
+    case REGION_TYPE.COLLISION
+        color = [255, 255, 128] / 255;
+    case REGION_TYPE.FOV
+        color = [255, 165, 0] / 255;
+    case REGION_TYPE.COMMS
+        color = [0, 255, 0] / 255;
+    otherwise % INVALID
+        color = [255, 127, 255] / 255;
+end
+end

resources/project/4sXto5Q6PQxNCRaZ_E3lobFdDW0/5kqAGPAcSSlbCfxgpmA59U0UWcUp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="controller_impl.cpp" type="File"/>

resources/project/4sXto5Q6PQxNCRaZ_E3lobFdDW0/DtKmIMiY6orJIYug9tOtsq_Fzhcd.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/4sXto5Q6PQxNCRaZ_E3lobFdDW0/DtKmIMiY6orJIYug9tOtsq_Fzhcp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="controller_impl.h" type="File"/>

resources/project/4sXto5Q6PQxNCRaZ_E3lobFdDW0/gsyStyyOVyV_ZZ2qqV11kkPreoEd.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/4sXto5Q6PQxNCRaZ_E3lobFdDW0/gsyStyyOVyV_ZZ2qqV11kkPreoEp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="tmwtypes.h" type="File"/>

resources/project/4sXto5Q6PQxNCRaZ_E3lobFdDW0/ob3_1xfVXe9xJyMpU2OoFU4cuiAp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/4sXto5Q6PQxNCRaZ_E3lobFdDW0/z4z4lM08j9WL4zP-VuDL0ahDO0Ud.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/4sXto5Q6PQxNCRaZ_E3lobFdDW0/z4z4lM08j9WL4zP-VuDL0ahDO0Up.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="controller_main.cpp" type="File"/>

resources/project/6V6Vr7QAsqxEmRj-ezOm1ygcTak/bJiTrGQ5GQ9DMI6whRMInBQY2yQd.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>

resources/project/6V6Vr7QAsqxEmRj-ezOm1ygcTak/bJiTrGQ5GQ9DMI6whRMInBQY2yQp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/71xh8lboMaPoa5Dmw3ABwzIozfw/bjgcAyD7dxJIgrUo3yHl5f1-iksd.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>

resources/project/71xh8lboMaPoa5Dmw3ABwzIozfw/bjgcAyD7dxJIgrUo3yHl5f1-iksp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/E2mMq2X73DyjKhlQAouGqrsyLgg/KW9h3Gs8ZTtGq070jCUxWZpaFvYd.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/E2mMq2X73DyjKhlQAouGqrsyLgg/KW9h3Gs8ZTtGq070jCUxWZpaFvYp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="readScenarioCsv.m" type="File"/>