removed prompt to continue
This commit is contained in:
@@ -113,6 +113,21 @@
|
|||||||
<Size type="coderapp.internal.codertype.Dimension"/>
|
<Size type="coderapp.internal.codertype.Dimension"/>
|
||||||
<Size type="coderapp.internal.codertype.Dimension"/>
|
<Size type="coderapp.internal.codertype.Dimension"/>
|
||||||
</Types>
|
</Types>
|
||||||
|
<Types id="23" type="coderapp.internal.codertype.PrimitiveType">
|
||||||
|
<ClassName>int32</ClassName>
|
||||||
|
<Size type="coderapp.internal.codertype.Dimension"/>
|
||||||
|
<Size type="coderapp.internal.codertype.Dimension"/>
|
||||||
|
</Types>
|
||||||
|
<Types id="24" type="coderapp.internal.codertype.PrimitiveType">
|
||||||
|
<ClassName>int32</ClassName>
|
||||||
|
<Size type="coderapp.internal.codertype.Dimension"/>
|
||||||
|
<Size type="coderapp.internal.codertype.Dimension"/>
|
||||||
|
</Types>
|
||||||
|
<Types id="25" type="coderapp.internal.codertype.PrimitiveType">
|
||||||
|
<ClassName>int32</ClassName>
|
||||||
|
<Size type="coderapp.internal.codertype.Dimension"/>
|
||||||
|
<Size type="coderapp.internal.codertype.Dimension"/>
|
||||||
|
</Types>
|
||||||
</Types>
|
</Types>
|
||||||
</coderapp.internal.interface.project.Interface>
|
</coderapp.internal.interface.project.Interface>
|
||||||
</MF0>
|
</MF0>
|
||||||
@@ -1080,7 +1095,7 @@
|
|||||||
</Artifacts>
|
</Artifacts>
|
||||||
<BuildFolder type="coderapp.internal.util.mfz.FileSpec"/>
|
<BuildFolder type="coderapp.internal.util.mfz.FileSpec"/>
|
||||||
<Success>true</Success>
|
<Success>true</Success>
|
||||||
<Timestamp>2026-03-02T18:23:05</Timestamp>
|
<Timestamp>2026-03-03T15:05:52</Timestamp>
|
||||||
</MainBuildResult>
|
</MainBuildResult>
|
||||||
</coderapp.internal.mlc.mfz.MatlabCoderProjectState>
|
</coderapp.internal.mlc.mfz.MatlabCoderProjectState>
|
||||||
</MF0>
|
</MF0>
|
||||||
|
|||||||
@@ -107,13 +107,6 @@ end
|
|||||||
MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations
|
MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations
|
||||||
GUIDANCE_RATE_MS = int32(5000); % ms between iterations (0.2 Hz default)
|
GUIDANCE_RATE_MS = int32(5000); % ms between iterations (0.2 Hz default)
|
||||||
|
|
||||||
% Wait for user input to start guidance loop
|
|
||||||
if coder.target('MATLAB')
|
|
||||||
input('Press Enter to start guidance loop: ', 's');
|
|
||||||
else
|
|
||||||
coder.ceval('waitForUserInput');
|
|
||||||
end
|
|
||||||
|
|
||||||
% Enter guidance mode on all clients
|
% Enter guidance mode on all clients
|
||||||
if ~coder.target('MATLAB')
|
if ~coder.target('MATLAB')
|
||||||
coder.ceval('sendGuidanceToggle', int32(numClients));
|
coder.ceval('sendGuidanceToggle', int32(numClients));
|
||||||
@@ -174,13 +167,6 @@ if ~coder.target('MATLAB')
|
|||||||
end
|
end
|
||||||
% --------------------------------------------------------------------------
|
% --------------------------------------------------------------------------
|
||||||
|
|
||||||
% Wait for user input before closing experiment (RTL + LAND)
|
|
||||||
if coder.target('MATLAB')
|
|
||||||
input('Press Enter to close experiment (RTL + LAND): ', 's');
|
|
||||||
else
|
|
||||||
coder.ceval('waitForUserInput');
|
|
||||||
end
|
|
||||||
|
|
||||||
% Send RTL command to all clients
|
% Send RTL command to all clients
|
||||||
for i = 1:numClients
|
for i = 1:numClients
|
||||||
if coder.target('MATLAB')
|
if coder.target('MATLAB')
|
||||||
|
|||||||
@@ -480,12 +480,6 @@ int waitForAllMessageType(int numClients, int expectedType) {
|
|||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Wait for user to press Enter
|
|
||||||
void waitForUserInput() {
|
|
||||||
std::cout << "Press Enter to continue...\n";
|
|
||||||
std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
|
|
||||||
}
|
|
||||||
|
|
||||||
// Broadcast GUIDANCE_TOGGLE to all clients
|
// Broadcast GUIDANCE_TOGGLE to all clients
|
||||||
void sendGuidanceToggle(int numClients) {
|
void sendGuidanceToggle(int numClients) {
|
||||||
for (int i = 1; i <= numClients; i++) {
|
for (int i = 1; i <= numClients; i++) {
|
||||||
|
|||||||
@@ -41,9 +41,6 @@ int loadObstacles(const char* filename,
|
|||||||
double* obstacleMax,
|
double* obstacleMax,
|
||||||
int maxObstacles);
|
int maxObstacles);
|
||||||
|
|
||||||
// User interaction
|
|
||||||
void waitForUserInput();
|
|
||||||
|
|
||||||
// Binary protocol operations
|
// Binary protocol operations
|
||||||
int sendMessageType(int clientId, int msgType);
|
int sendMessageType(int clientId, int msgType);
|
||||||
int sendTarget(int clientId, const double* coords);
|
int sendTarget(int clientId, const double* coords);
|
||||||
|
|||||||
@@ -4,12 +4,23 @@ gf = geoglobe(f);
|
|||||||
hold(gf, "on");
|
hold(gf, "on");
|
||||||
c = ["g", "b", "m", "c"]; % plotting colors
|
c = ["g", "b", "m", "c"]; % plotting colors
|
||||||
|
|
||||||
|
% paths
|
||||||
|
scenarioCsv = fullfile("aerpaw", "config", "scenario.csv");
|
||||||
|
|
||||||
|
% configured data
|
||||||
|
params = readScenarioCsv(scenarioCsv);
|
||||||
|
|
||||||
% coordinate system constants
|
% coordinate system constants
|
||||||
seaToGroundLevel = 110; % meters, measured approximately from USGS national map viewer
|
seaToGroundLevel = 110; % meters, measured approximately from USGS national map viewer
|
||||||
lla0 = [35.72550610629396, -78.70019657805574, seaToGroundLevel]; % origin (LLA)
|
|
||||||
|
fID = fopen(fullfile("aerpaw", "config", "client.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];
|
||||||
|
|
||||||
% Paths to logs
|
% Paths to logs
|
||||||
gpsCsvs = dir(fullfile("sandbox", "test6", "*.csv"));
|
gpsCsvs = dir(fullfile("sandbox", "test9", "*.csv"));
|
||||||
|
|
||||||
G = cell(size(gpsCsvs));
|
G = cell(size(gpsCsvs));
|
||||||
for ii = 1:size(gpsCsvs, 1)
|
for ii = 1:size(gpsCsvs, 1)
|
||||||
@@ -22,8 +33,9 @@ for ii = 1:size(gpsCsvs, 1)
|
|||||||
verticalSpeed = movmean(abs(diff(G{ii}.Altitude)), [10, 0]);
|
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)
|
% Automatically detect start/stop of algorithm flight (ignore takeoff, setup, return to liftoff, landing segments of flight)
|
||||||
startIdx = find(verticalSpeed <= prctile(verticalSpeed, 90), 1, 'first'); % 25 pct threshold may need adjusting
|
pctThreshold = 60; % pctThreshold may need adjusting depending on your flight
|
||||||
stopIdx = find(verticalSpeed <= prctile(verticalSpeed, 90), 1, 'last');
|
startIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, 'first');
|
||||||
|
stopIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, 'last');
|
||||||
|
|
||||||
% % Plot whole flight, including setup/cleanup
|
% % Plot whole flight, including setup/cleanup
|
||||||
% startIdx = 1;
|
% startIdx = 1;
|
||||||
@@ -34,13 +46,13 @@ for ii = 1:size(gpsCsvs, 1)
|
|||||||
end
|
end
|
||||||
|
|
||||||
% Plot objective
|
% Plot objective
|
||||||
objectivePos = [35, 35, 0];
|
objectivePos = [params.objectivePos, 0];
|
||||||
llaObj = enu2lla(objectivePos, lla0, 'flat');
|
llaObj = enu2lla(objectivePos, lla0, 'flat');
|
||||||
geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), llaObj(3) + 50], 'LineWidth', 3, "Color", 'y');
|
geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), llaObj(3) + 50], 'LineWidth', 3, "Color", 'y');
|
||||||
|
|
||||||
% Plot domain
|
% Plot domain
|
||||||
altOffset = 1; % to avoid clipping into the ground when displayed
|
altOffset = 1; % to avoid clipping into the ground when displayed
|
||||||
domain = [lla0; enu2lla([50, 50, 100], lla0, 'flat')];
|
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(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(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(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
|
||||||
@@ -49,7 +61,7 @@ geoplot3(gf, [domain(1, 1), domain(1, 1)], [domain(2, 2), domain(2, 2)], domain(
|
|||||||
geoplot3(gf, [domain(2, 1), domain(2, 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)
|
% Plot floor (minimum altitude constraint)
|
||||||
floorAlt = 30;
|
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');
|
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');
|
||||||
|
|
||||||
% finish
|
% finish
|
||||||
|
|||||||
Reference in New Issue
Block a user