diff --git a/functions/simulations/stdAscent.m b/functions/simulations/stdAscent.m new file mode 100644 index 0000000000000000000000000000000000000000..0035cae85ddde2b666f29b416d1d990b164eff5a --- /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 0000000000000000000000000000000000000000..d78b45fbf325fe6c4ce78dab29fce1b6911a7b82 --- /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 0000000000000000000000000000000000000000..9655db195ed3842ccbba49562678ff95a0a814dc --- /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