From 01f8bec73d55173e7098ccb82bde5dd3708fcb46 Mon Sep 17 00:00:00 2001
From: Mauco03 <marco.gaibotti@skywarder.eu>
Date: Thu, 23 May 2024 09:37:52 +0200
Subject: [PATCH] [refactoring-ode][simulations] Moved std Simulations to
simulations
---
functions/simulations/stdAscent.m | 76 ++++++++++++
functions/simulations/stdDescent.m | 163 ++++++++++++++++++++++++++
functions/simulations/stdDescentBal.m | 48 ++++++++
3 files changed, 287 insertions(+)
create mode 100644 functions/simulations/stdAscent.m
create mode 100644 functions/simulations/stdDescent.m
create mode 100644 functions/simulations/stdDescentBal.m
diff --git a/functions/simulations/stdAscent.m b/functions/simulations/stdAscent.m
new file mode 100644
index 0000000..0035cae
--- /dev/null
+++ b/functions/simulations/stdAscent.m
@@ -0,0 +1,76 @@
+function [solution, ascent] = stdAscent(rocket, env, wind, settings, wrapper)
+arguments
+ rocket Rocket
+ env Environment
+ wind WindCustom {mustBeA(wind, {'WindCustom', 'WindMatlab'})}
+ settings Settings
+ wrapper DataWrapper
+end
+% stdAscent - This function runs a nominal ascent
+%
+% INPUTS:
+% - rocket, environment, wind, settings
+%
+% OUTPUTS:
+% - solution, raw solution
+% - ascent, post processed solution
+% NOTE: if the simultation calculates the apogee only post processed
+% solution is used. Otherwise, for complete simulations (with descent) raw
+% solution is required.
+%
+%
+% CALLED FUNCTIONS: windConstGenerator, ascent, recallOdeFcn, descentParachute, descentParachute6Dof, delayControl.
+%
+% REVISIONS:
+% - 0 Release, Ruben Di Battista
+% - 1 Revision, Francesco Colombi
+% - 2 09/10/2019, Revision, Adriano Filippo Inno
+% - 3 16/04/2021, Update, Fiammetta Artioli, Davide Rosato
+% 6DOF descent update
+% - 4 03/10/2021, Update, Davide Rosato
+% Multiple smooth airbrakes opening update
+% Copyright © 2022, Skyward Experimental Rocketry, AFD department
+% All rights reserved
+%
+% SPDX-License-Identifier: GPL-3.0-or-later
+
+if rocket.airbrakes.enabled && length(rocket.airbrakes.extension) > 1 && ...
+ length(rocket.airbrakes.deltaTime) < length(rocket.airbrakes.extension) - 1
+ error('In airbrakes smooth opening simulations, AB configuration usage time vector must be at least of length length(airbrakes.extension)-1, check rocketConfig.m')
+end
+
+if any(rocket.airbrakes.extension > 3) || any(rocket.airbrakes.extension < 1)
+ error('At least a value in airbrakes.extension is set incorrectly in rocketConfig, they must be between 1 and 3')
+end
+
+%% DATA PREPARATION
+options = odeset(settings.ode.optAscent, 'OutputFcn', @storeData);
+tf = settings.ode.finalTime;
+
+%% INITIAL CONDITIONS
+%%% Attitude
+Q0 = angleToQuat(env.phi, env.omega, 180*pi/180)';
+
+%%% State
+X0 = [0; 0; 0];
+V0 = [0; 0; 0];
+W0 = [0; 0; 0];
+
+Y0 = [X0; V0; W0; Q0; 0];
+
+%% ASCENT
+t0 = 0;
+solution = ode113(@ballistic, [t0, tf], Y0, options, ...
+ rocket, env, wind, [], 0, wrapper);
+
+if settings.simulator.parachute && rocket.parachutes(1, 1).openingTime
+ solution = odextend(solution, [], solution.x(end) + rocket.parachutes(1, 1).openingTime);
+end
+
+if nargout == 2
+ ascent = getOdeFcn(solution);
+end
+% if not(isfield(simulator, 'unitTest')) || simulator.unitTest == false
+% save('ascent_plot.mat', 'ascent');
+% end
+end
\ No newline at end of file
diff --git a/functions/simulations/stdDescent.m b/functions/simulations/stdDescent.m
new file mode 100644
index 0000000..d78b45f
--- /dev/null
+++ b/functions/simulations/stdDescent.m
@@ -0,0 +1,163 @@
+function descent = stdDescent(solution, settings)
+arguments
+ solution struct
+ settings Settings
+end
+% stdRun - This function runs a nominal simulation
+%
+% INPUTS:
+% - ascent: this function restarts integrating from raw data ascent
+% - rocket, environment, wind, settings
+%
+% OUTPUTS:
+% - descent, post processed solution
+%
+% CALLED FUNCTIONS: windConstGenerator, ascent, recallOdeFcn, descentParachute, descentParachute6Dof, delayControl.
+%
+% REVISIONS:
+% - 0 Release, Ruben Di Battista
+% - 1 Revision, Francesco Colombi
+% - 2 09/10/2019, Revision, Adriano Filippo Inno
+% - 3 16/04/2021, Update, Fiammetta Artioli, Davide Rosato
+% 6DOF descent update
+% - 4 03/10/2021, Update, Davide Rosato
+% Multiple smooth airbrakes opening update
+% Copyright © 2022, Skyward Experimental Rocketry, AFD department
+% All rights reserved
+%
+% SPDX-License-Identifier: GPL-3.0-or-later
+
+%% PARACHUTES
+% Initial Condition are the last from ascent (need to rotate because
+% velocities are in body axes)
+
+% settings.mEndAscent = ascent.interp.mass(end);
+% settings.IxxEndAscent = ascent.interp.inertias(1, end);
+% settings.IyyEndAscent = ascent.interp.inertias(2, end);
+% settings.IzzEndAscent = ascent.interp.inertias(3, end);
+
+% if newSettings.simulator.descent6DOF
+% Yf = [Ya(:, 1:13), NaN*ones(size(Ya,1), 12)];
+% Tf = Ta;
+% for k = 1:newSettings.simulator.stagesNumber
+% settings.stage = k;
+% if k == 1
+% % simulationg 6DOF descen for the launcher only
+% if settings.timeEngineCut < settings.tb
+% settings.xcgEndAscent = interpLinear(settings.motor.expTime, settings.xcg, settings.timeEngineCut);
+% else
+% settings.xcgEndAscent = settings.xcg(end);
+% end
+%
+% [data_para6DOF, T6DOF, Y6DOF, bound_value] = descentParachute6Dof(Ta, Ya, settings);
+%
+% descent{k}.para{1} = data_para6DOF{1};
+% descent{k}.para{2} = data_para6DOF{2};
+% Yf = [Yf; Y6DOF];
+% Tf = [Ta; T6DOF];
+% else
+% % simulation 3DOF descent for secondary stages
+% Y0p = [Ya(end, 1:3) quatrotate(quatconj(Ya(end, 10:13)),Ya(end, 4:6))];
+% t0p = Ta(end);
+%
+% descent{k}.para = cell(newSettings.simulator.chutesNumber(k), 1);
+% for i = 1:newSettings.simulator.chutesNumber(k)
+% if i == 1
+% YaEndTrue = Ya(end, 4:6); % + [settings.para(i, k).Vexit 0 0];
+% Y0p = [Y0p(1:3) quatrotate(quatconj(Ya(end, 10:13)), YaEndTrue)];
+% end
+% para = i;
+% settings.paraNumber = para;
+% settings.stage = k;
+%
+% [Tp, Yp] = ode113(@descentParachute, [t0p, tf], Y0p, newSettings.ode.optParachute, settings);
+%
+% [data] = recallOdeFcn(@descentParachute, Tp, Yp, settings);
+% descent{k}.para{i} = data;
+% descent{k}.para{i}.state.Y = Yp;
+% descent{k}.para{i}.state.T = Tp;
+% end
+% end
+% end
+% else
+
+%% DATA PREPARATION
+rocket = solution.extdata.varargin{1};
+environment = solution.extdata.varargin{2};
+wind = solution.extdata.varargin{3};
+wrapper = solution.extdata.varargin{end};
+
+options = odeset(settings.ode.optParachute, 'OutputFcn', @storeData);
+
+ascent.state.time = solution.x;
+ascent.state.Y = solution.y;
+ascent.state.Y(4:6, end) = quatrotate(quatconj(ascent.state.Y(10:13, end)'), ...
+ ascent.state.Y(4:6, end)')'; % Rotating ascent end velocitites to NED
+
+tf = settings.ode.finalTime;
+
+nParachutes = size(rocket.parachutes, 1);
+nStages = size(rocket.parachutes, 2);
+
+descent(nParachutes, nStages) = wrapper.dummy;
+prev = ascent;
+
+%% PARACHUTE DESCENT
+for i = 1:nParachutes
+ descentData.para = i;
+ for j = 1:nStages
+ descentData.stage = j;
+ if isempty(rocket.parachutes(i, j).name), continue; end
+ if i ~= 1, prev = descent(i-1, j); end
+ wrapper.reset();
+
+ switch class(rocket.parachutes(i, j))
+ case 'Parachute'
+ Y0 = [prev.state.Y(1:3, end);
+ prev.state.Y(4:6, end)];
+ t0 = prev.state.time(end);
+
+ solution = ode113(@descentParachute, [t0, tf], Y0, options, ...
+ rocket, environment, wind, settings, descentData, wrapper);
+ case 'Parafoil'
+ if ~settings.simulator.parafoil, continue; end
+ if i == 1
+ error("Cannot simulate parafoil as fist descent stage");
+ end
+
+ windNED = prev.wind.NED(:, end);
+
+ azimuth = atan2(windNED(2), windNED(1));
+ elevation = asin(-windNED(3)/norm(windNED(1:2)));
+ quat0 = angleToQuat(azimuth, elevation, 0);
+
+ Y0 = [prev.state.Y(1:3, end);
+ quatrotate(quat0, prev.state.Y(4:6, end)')';
+ 0; 0; 0;
+ quat0'];
+ t0 = prev.state.time(end);
+
+ solution = ode113(@descentParafoil, [t0, tf], Y0, options, ...
+ rocket, environment, wind, settings, descentData, wrapper);
+ otherwise
+ error('Unrecognized para type for descent: %s', ...
+ class(rocket.parachutes(i, j)))
+ end
+
+ if i < nParachutes && rocket.parachutes(i+1, j).openingTime
+ solution = odextend(solution, [], solution.x(end) + rocket.parachutes(1, 1).openingTime);
+ end
+
+ if i < nParachutes && -solution.y(3, end) < rocket.parachutes(i+1, j).finalAltitude
+ error("Chute (%d, %d) has arrived at a lower altitude than Chute (%d, %d) target altitude." + ...
+ " Please lower Chute (%d, %d) opening delay or target altitude.", i, j, i+1, j);
+ end
+
+ descent(i, j) = getOdeFcn(solution, 1);
+ end
+end
+
+% if not(isfield(simulator, 'unitTest')) || newSettings.simulator.unitTest == false
+% save('descent_para_plot.mat', 'descent')
+% end
+end
\ No newline at end of file
diff --git a/functions/simulations/stdDescentBal.m b/functions/simulations/stdDescentBal.m
new file mode 100644
index 0000000..9655db1
--- /dev/null
+++ b/functions/simulations/stdDescentBal.m
@@ -0,0 +1,48 @@
+function descent = stdDescentBal(ascentSolution, settings)
+arguments
+ ascentSolution struct
+ settings Settings
+end
+%{
+
+stdRunBallistic - This function runs a standard ballistic (non-stochastic) simulation
+
+INTPUTS:
+ - settings, struct, stores data of the rocket and of the simulation.
+
+OUTPUTS:
+ - Tf, array, Total integration time vector;
+ - Yf, array, Total State Matrix;
+ - Ta, array, [n� variations, 1], Ascent Integration time vector;
+ - Ya, array, [n� variations, 16], Ascent State Matrix;
+ - bound_value, struct, Useful values for the plots;
+
+CALLED FUNCTIONS: windConstGenerator, ascent, recallOdeFcn, descentBal, delayControl.
+
+REVISIONS:
+- 0 Release, Ruben Di Battista
+- 1 Revision, Francesco Colombi
+- 2 09/10/2019, Revision, Adriano Filippo Inno
+- 3 03/10/2021, Update, Davide Rosato
+ Multiple and smooth airbrakes opening update
+
+Copyright © 2021, Skyward Experimental Rocketry, AFD department
+All rights reserved
+
+SPDX-License-Identifier: GPL-3.0-or-later
+
+%}
+
+%% DATA PREPARATION
+options = odeset(settings.ode.optDescent, 'OutputFcn', @storeData);
+
+%% DESCEND
+solution = odextend(ascentSolution, [], settings.ode.finalTime, [], options);
+
+apogeeIdx = find(solution.x == solution.xe(1), 1, "first");
+descent = getOdeFcn(solution, apogeeIdx);
+
+% if not(isfield(settings, 'unitTest')) || settings.unitTest == false
+% save('descent_plot.mat', 'descent');
+% end
+end
\ No newline at end of file
--
GitLab