From 6e7d46481281086898aaab070ff65829e5bd7ff2 Mon Sep 17 00:00:00 2001
From: Mauco03 <marco.gaibotti@skywarder.eu>
Date: Thu, 30 May 2024 11:43:00 +0200
Subject: [PATCH] [refactoring-ode][functions] Added control to parafoil
---
classes/components/Parafoil.m | 6 ++
functions/eventFunctions/eventParaCut.m | 8 +--
functions/odeFunctions/descentParachute.m | 10 +---
functions/odeFunctions/descentParafoil.m | 55 ++++++-------------
functions/simulations/stdDescent.m | 7 ++-
.../config/paraConfig.m | 6 ++
6 files changed, 37 insertions(+), 55 deletions(-)
diff --git a/classes/components/Parafoil.m b/classes/components/Parafoil.m
index 68ce77f..d02cc7b 100644
--- a/classes/components/Parafoil.m
+++ b/classes/components/Parafoil.m
@@ -15,6 +15,12 @@ classdef Parafoil < Para
surface double % [m^2] Surface
deltaSMax double % [-] Aerodynamic control coefficient - symmetric, max value
+ uMax
+ uMin
+ identification
+ deltaATau
+ maxSpeed
+
mass double % [kg] Parafoil Mass
openingTime double % [s] Parafoil opening time (delay + time to get fully open)
diff --git a/functions/eventFunctions/eventParaCut.m b/functions/eventFunctions/eventParaCut.m
index d3055c3..7e7d3be 100644
--- a/functions/eventFunctions/eventParaCut.m
+++ b/functions/eventFunctions/eventParaCut.m
@@ -1,4 +1,4 @@
-function [value, isterminal, direction] = eventParaCut(~, Y, rocket, ~, ~, settings, descentData, ~)
+function [value, isterminal, direction] = eventParaCut(~, Y, rocket, ~, ~, descentData, varargin)
%{
stdDescent(ascent, rocket, environment, wind, parachute, newSettings)
eventParaCut - Event function to stop simulation at the chosen altitude to cut the
@@ -37,11 +37,7 @@ SPDX-License-Identifier: GPL-3.0-or-later
% y = Y(2);
altitude = -Y(3);
-if ~isfield(settings.simulator,'stochNumber')
- para = descentData.para;
-else
- para = settings.stoch.para;
-end
+para = descentData.para;
stage = descentData.stage;
value = altitude - rocket.parachutes(para, stage).finalAltitude;
diff --git a/functions/odeFunctions/descentParachute.m b/functions/odeFunctions/descentParachute.m
index 556fbbd..83720d0 100644
--- a/functions/odeFunctions/descentParachute.m
+++ b/functions/odeFunctions/descentParachute.m
@@ -1,11 +1,10 @@
-function [dY, parout] = descentParachute(~, Y, rocket, environment, wind, settings, descentData, wrapper)
+function [dY, parout] = descentParachute(~, Y, rocket, environment, wind, descentData, wrapper)
arguments
~
Y
rocket Rocket
environment Environment
wind % WindCustom {mustBeA(wind, {'WindCustom', 'WindMatlab'})}
- settings Settings
descentData struct
wrapper = [] %DataWrapper = DataWrapper.empty
end
@@ -44,12 +43,7 @@ w = Y(6);
dY = zeros(6, 1);
%% GETTING DATA
-
-if isfield(settings.simulator, 'stochNumber')
- para = settings.stoch.para; %!!!!!!!!!!
-else
- para = descentData.para; % defined in stdRun {para = i; settings.paraNumber = para;}
-end
+para = descentData.para; % defined in stdRun {para = i; settings.paraNumber = para;}
stage = descentData.stage;
S = rocket.parachutes(para, stage).surface; % [m^2] Surface
diff --git a/functions/odeFunctions/descentParafoil.m b/functions/odeFunctions/descentParafoil.m
index 3f0cad0..104a272 100644
--- a/functions/odeFunctions/descentParafoil.m
+++ b/functions/odeFunctions/descentParafoil.m
@@ -1,4 +1,4 @@
-function [dY,parout] = descentParafoil(t, Y, rocket, environment, wind, descentData, control, wrapper)
+function [dY,parout] = descentParafoil(t, Y, rocket, environment, wind, descentData, control, t0, wrapper)
arguments
t
Y
@@ -7,17 +7,19 @@ arguments
wind % WindCustom {mustBeA(wind, {'WindCustom', 'WindMatlab'})}
descentData struct
control struct = []
+ t0 double = 0
wrapper = [] %DataWrapper = DataWrapper.empty
end
% ballistic - ode function of the 6DOF Rigid Rocket Model
%
% INPUTS:
% - t, double [1, 1] integration time [s];
-% - Y, double [13, 1] state vector [ x y z | u v w | p q r | q0 q1 q2 q3 | angle]:
+% - Y, double [14, 1] state vector [ x y z | u v w | p q r | q0 q1 q2 q3 | angle]:
% * (x y z), NED{north, east, down} horizontal frame;
% * (u v w), body frame velocities;
% * (p q r), body frame angular rates;
% * (q0 q1 q2 q3), attitude unit quaternion;
+% * (deltaA), parafoil control
%
% - (rocket, environment, wind), data Classes
% - wrapper, handle to export data
@@ -50,7 +52,7 @@ deltaA = Y(14);
dY = zeros(13, 1);
parafoil = rocket.parachutes(descentData.para, descentData.stage);
-%% CONSTANTS
+%% GETTING DATA
% environment
g = environment.g0/(1 + (altitude*1e-3/6371))^2; % [N/kg] module of gravitational field
local = environment.local; % vector containing inputs for atmosphereData
@@ -79,6 +81,9 @@ clSurface = parafoil.clSurface;
% aerodynamic control coefficients - symmetric
deltaSMax = parafoil.deltaSMax; % max value
+% integration data
+theta = t-t0;
+
isControlActive = ~isempty(control);
isAngleSaturated = false;
@@ -88,7 +93,7 @@ Q = Q/norm(Q);
%% ADDING WIND (supposed to be added in NED axes);
if isa(wind, 'WindMatlab')
- [uw, vw, ww] = wind.getVels(altitude, t);
+ [uw, vw, ww] = wind.getVels(altitude, theta);
else
[uw, vw, ww] = wind.getVels(altitude);
end
@@ -129,11 +134,11 @@ end
% Saturation on servo angle, needed if the actuation is faster than the
% integration step
if isControlActive
- if deltaA > control.uMax
- deltaA = control.uMax;
+ if deltaA > parafoil.uMax
+ deltaA = parafoil.uMax;
isAngleSaturated = true;
- elseif deltaA < control.uMin
- deltaA = control.uMin;
+ elseif deltaA < parafoil.uMin
+ deltaA = parafoil.uMin;
isAngleSaturated = true;
end
end
@@ -182,7 +187,7 @@ angAcc = inverseInertia*(momentAero - cross(omega, inertia * omega));
dDeltaA = 0;
if isControlActive && ~isAngleSaturated
deltaARef = control.deltaARef;
- if ~control.identification
+ if ~parafoil.identification
% set velocity of servo (air brakes)
if length(deltaARef) == 2 % for the recallOdeFunction
if theta < control.timeChangeRef
@@ -199,38 +204,12 @@ if isControlActive && ~isAngleSaturated
deltaARef = deltaARef(idxDeltaA, 2);
end
- dDeltaA = (deltaARef-deltaA)/control.deltaATau;
- if abs(dDeltaA) > control.maxSpeed
- dDeltaA = sign(deltaARef-deltaA)*control.maxSpeed; % concettualmente sta roba è sbagliata perchè dipende dal passo di integrazione, fixare
+ dDeltaA = (deltaARef-deltaA)/parafoil.deltaATau;
+ if abs(dDeltaA) > parafoil.maxSpeed
+ dDeltaA = sign(deltaARef-deltaA)*parafoil.maxSpeed; % concettualmente sta roba è sbagliata perchè dipende dal passo di integrazione, fixare
end
end
-% set velocity of servo (controller)
-% if ~control.identification
-% if length(deltaARef)==2 % for the recallOdeFunction
-% if t < t_change_ref
-% deltaA_ref = deltaA_ref_vec(1);
-% else
-% deltaA_ref = deltaA_ref_vec(2);
-% end
-% else
-% [~,ind_deltaA] = min(settings.parout.partial_time-t);
-% deltaA_ref = deltaA_ref_vec(ind_deltaA); % don't delete this unless you change how the recall ode works.
-% end
-% else
-% [~,idx_deltaA] = min(abs(t-deltaA_ref_vec(:,1))); % if we are trying to identify we need to have the same input of the flight
-% deltaA_ref = deltaA_ref_vec(idx_deltaA,2);
-% end
-%
-% dDeltaA = (deltaA_ref-deltaA)/control.deltaA_tau;
-% if abs(dDeltaA) > control.deltaA_maxSpeed
-% dDeltaA = sign(deltaA_ref-deltaA)*control.deltaA_maxSpeed; % concettualmente sta roba è sbagliata perchè dipende dal passo di integrazione, fixare
-% end
-%
-% if isAngleSaturated
-% dDeltaA = 0;
-% end
-
%% FINAL DERIVATIVE STATE ASSEMBLING
dY(1:3) = vNED;
dY(4:6) = bodyAcc;
diff --git a/functions/simulations/stdDescent.m b/functions/simulations/stdDescent.m
index d78b45f..a1c27a0 100644
--- a/functions/simulations/stdDescent.m
+++ b/functions/simulations/stdDescent.m
@@ -118,7 +118,7 @@ for i = 1:nParachutes
t0 = prev.state.time(end);
solution = ode113(@descentParachute, [t0, tf], Y0, options, ...
- rocket, environment, wind, settings, descentData, wrapper);
+ rocket, environment, wind, descentData, wrapper);
case 'Parafoil'
if ~settings.simulator.parafoil, continue; end
if i == 1
@@ -134,11 +134,12 @@ for i = 1:nParachutes
Y0 = [prev.state.Y(1:3, end);
quatrotate(quat0, prev.state.Y(4:6, end)')';
0; 0; 0;
- quat0'];
+ quat0';
+ 0];
t0 = prev.state.time(end);
solution = ode113(@descentParafoil, [t0, tf], Y0, options, ...
- rocket, environment, wind, settings, descentData, wrapper);
+ rocket, environment, wind, descentData, [], 0, wrapper);
otherwise
error('Unrecognized para type for descent: %s', ...
class(rocket.parachutes(i, j)))
diff --git a/missions/2024_Lyra_Portugal_October/config/paraConfig.m b/missions/2024_Lyra_Portugal_October/config/paraConfig.m
index 59d2a43..6c8e927 100644
--- a/missions/2024_Lyra_Portugal_October/config/paraConfig.m
+++ b/missions/2024_Lyra_Portugal_October/config/paraConfig.m
@@ -67,6 +67,12 @@ para(2, 2).openingTime = 0; % [s] Parafoil opening delay
para(2, 2).surface = 0.11; % [m^2] Surface
para(2, 2).deltaSMax = 0.1; % max value
+uMax = 0;
+uMin = 0;
+identification = 0;
+deltaATau = 0;
+maxSpeed = 0;
+
para(2, 2).semiWingSpan = 2.55/2; % [m] settings.para(2, 2).b: semiwingspan - vela nuova: 2.55/2; - vela vecchia: 2.06/2;
para(2, 2).MAC = 0.8; % [m] mean aero chord
para(2, 2).surface = 2.04; % [m^2] parafoil surface - vela nuova 2.04; - vela vecchia: 1.64;
--
GitLab