From f476895a850dc31b1340600cac23187b19ac150e Mon Sep 17 00:00:00 2001
From: Mauco03 <marco.gaibotti@skywarder.eu>
Date: Wed, 29 May 2024 12:25:19 +0200
Subject: [PATCH] [refactoring-ode][descentParafoil] Implementing control for
parafoil
---
functions/odeFunctions/descentParafoil.m | 84 ++++++++++++++++--------
1 file changed, 57 insertions(+), 27 deletions(-)
diff --git a/functions/odeFunctions/descentParafoil.m b/functions/odeFunctions/descentParafoil.m
index 279a785..3f0cad0 100644
--- a/functions/odeFunctions/descentParafoil.m
+++ b/functions/odeFunctions/descentParafoil.m
@@ -1,12 +1,12 @@
-function [dY,parout] = descentParafoil(t, Y, rocket, environment, wind, ~, descentData, wrapper)
+function [dY,parout] = descentParafoil(t, Y, rocket, environment, wind, descentData, control, wrapper)
arguments
t
Y
rocket Rocket
environment Environment
wind % WindCustom {mustBeA(wind, {'WindCustom', 'WindMatlab'})}
- ~
descentData struct
+ control struct = []
wrapper = [] %DataWrapper = DataWrapper.empty
end
% ballistic - ode function of the 6DOF Rigid Rocket Model
@@ -45,23 +45,11 @@ qw = Y(10); % scalar first
qx = Y(11);
qy = Y(12);
qz = Y(13);
-deltaA = 0;
+deltaA = Y(14);
dY = zeros(13, 1);
parafoil = rocket.parachutes(descentData.para, descentData.stage);
-% saturation on servo angle, needed if the actuation is faster than the
-% integration step
-% if deltaA > contSettings.parafoil.uMax
-% deltaA = contSettings.parafoil.uMax;
-% flagAngleSaturation = true;
-% elseif deltaA < contSettings.parafoil.uMin
-% deltaA = contSettings.parafoil.uMin;
-% flagAngleSaturation = true;
-% else
-% flagAngleSaturation = false;
-% end
-
%% CONSTANTS
% environment
g = environment.g0/(1 + (altitude*1e-3/6371))^2; % [N/kg] module of gravitational field
@@ -91,6 +79,8 @@ clSurface = parafoil.clSurface;
% aerodynamic control coefficients - symmetric
deltaSMax = parafoil.deltaSMax; % max value
+isControlActive = ~isempty(control);
+isAngleSaturated = false;
%% ROTATIONS
Q = [qw qx qy qz]; % we want it scalar first
@@ -135,6 +125,19 @@ else
end
%% controls
+
+% Saturation on servo angle, needed if the actuation is faster than the
+% integration step
+if isControlActive
+ if deltaA > control.uMax
+ deltaA = control.uMax;
+ isAngleSaturated = true;
+ elseif deltaA < control.uMin
+ deltaA = control.uMin;
+ isAngleSaturated = true;
+ end
+end
+
deltaANormalized = deltaA / deltaSMax;
%% FORCES
@@ -174,11 +177,37 @@ dQQ = 1/2*OM*Q';
%% angular acceleration
angAcc = inverseInertia*(momentAero - cross(omega, inertia * omega));
-% %% actuator dynamics
-%
-% % set velocity of servo (controller)
-% if ~settings.identification
-% if length(deltaA_ref_vec)==2 % for the recallOdeFunction
+%% actuator dynamics
+
+dDeltaA = 0;
+if isControlActive && ~isAngleSaturated
+ deltaARef = control.deltaARef;
+ if ~control.identification
+ % set velocity of servo (air brakes)
+ if length(deltaARef) == 2 % for the recallOdeFunction
+ if theta < control.timeChangeRef
+ deltaARef = deltaARef(1);
+ else
+ deltaARef = deltaARef(2);
+ end
+ else
+ [~,idxDeltaA] = min(control.partialTime - theta);
+ deltaARef = deltaARef(idxDeltaA); % don't delete this unless you change how the recall ode works.
+ end
+ else
+ [~,idxDeltaA] = min(abs(theta-deltaARef(:,1))); % if we are trying to identify we need to have the same input of the flight
+ 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
+ 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
@@ -192,14 +221,14 @@ angAcc = inverseInertia*(momentAero - cross(omega, inertia * omega));
% [~,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)/contSettings.parafoil.deltaA_tau;
-% if abs(ddeltaA) >contSettings.parafoil.deltaA_maxSpeed
-% ddeltaA = sign(deltaA_ref-deltaA)*contSettings.parafoil.deltaA_maxSpeed; % concettualmente sta roba è sbagliata perchè dipende dal passo di integrazione, fixare
+%
+% 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 flagAngleSaturation
-% ddeltaA = 0;
+%
+% if isAngleSaturated
+% dDeltaA = 0;
% end
%% FINAL DERIVATIVE STATE ASSEMBLING
@@ -207,6 +236,7 @@ dY(1:3) = vNED;
dY(4:6) = bodyAcc;
dY(7:9) = angAcc;
dY(10:13) = dQQ;
+dY(14) = dDeltaA;
if nargout == 2 || ~isempty(wrapper)
parout.state = [];
--
GitLab